Merge branch 'main' into fix/ignore_spell_check

Author: h-ohta

.github/workflows/build-and-test-differential.yaml

@@ -24,7 +24,6 @@ jobs:
         rosdistro:
           - humble
         container-suffix:
-          - ""
           - -cuda
         include:
           - rosdistro: humble
@@ -74,38 +73,3 @@ jobs:
 
       - name: Check disk space after build
         run: df -h
-
-  clang-tidy-differential:
-    runs-on: [self-hosted, linux, X64]
-    container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda
-    needs: build-and-test-differential
-    steps:
-      - name: Check out repository
-        uses: actions/checkout@v3
-        with:
-          fetch-depth: 0
-
-      - name: Remove exec_depend
-        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
-
-      - name: Get modified packages
-        id: get-modified-packages
-        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
-
-      - name: Get modified files
-        id: get-modified-files
-        uses: tj-actions/changed-files@v35
-        with:
-          files: |
-            **/*.cpp
-            **/*.hpp
-
-      - name: Run clang-tidy
-        if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }}
-        uses: autowarefoundation/autoware-github-actions/clang-tidy@v1
-        with:
-          rosdistro: humble
-          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-          target-files: ${{ steps.get-modified-files.outputs.all_changed_files }}
-          clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy
-          build-depends-repos: build_depends.repos

README.md

@@ -1,7 +1,16 @@
-# autoware.universe
+# Autoware Universe
 
-For Autoware's general documentation, see [Autoware Documentation](https://autowarefoundation.github.io/autoware-documentation/).
+## Welcome to Autoware Universe
 
-For detailed documents of Autoware Universe components, see [Autoware Universe Documentation](https://autowarefoundation.github.io/autoware.universe/).
+Autoware Universe serves as a foundational pillar within the Autoware ecosystem, playing a critical role in enhancing the core functionalities of autonomous driving technologies.
+This repository is a pivotal element of the Autoware Core/Universe concept, managing a wide array of packages that significantly extend the capabilities of autonomous vehicles.
 
----
+![autoware_universe_front](docs/assets/images/autoware_universe_front.png)
+
+## Getting Started
+
+To dive into the vast world of Autoware and understand how Autoware Universe fits into the bigger picture, we recommend starting with the [Autoware Documentation](https://autowarefoundation.github.io/autoware-documentation/). This resource provides a thorough overview of the Autoware ecosystem, guiding you through its components, functionalities, and how to get started with development.
+
+### Explore Autoware Universe documentation
+
+For those looking to explore the specifics of Autoware Universe components, the [Autoware Universe Documentation](https://autowarefoundation.github.io/autoware.universe/), deployed with MKDocs, offers detailed insights.

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml

@@ -23,7 +23,7 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

common/tensorrt_common/package.xml

@@ -6,7 +6,6 @@
 
   <author email="taichi.higashide@tier4.jp">Taichi Higashide</author>
   <author email="daisuke.nishimatsu@tier4.jp">Daisuke Nishimatsu</author>
-  <maintainer email="daisuke.nishimatsu@tier4.jp">Daisuke Nishimatsu</maintainer>
   <maintainer email="dan.umeda@tier4.jp">Dan Umeda</maintainer>
   <maintainer email="manato.hirabayashi@tier4.jp">Manato Hirabayashi</maintainer>
 

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp

@@ -18,6 +18,7 @@
 #include <boost/geometry/core/cs.hpp>
 #include <boost/geometry/geometries/geometries.hpp>
 #include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/geometries/register/ring.hpp>
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
@@ -98,5 +99,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(                                       // NOLIN
   tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y())       // NOLINT
 BOOST_GEOMETRY_REGISTER_POINT_3D(                                       // NOLINT
   tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z())  // NOLINT
+BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d)        // NOLINT
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_

common/traffic_light_utils/package.xml

@@ -4,7 +4,7 @@
   <name>traffic_light_utils</name>
   <version>0.1.0</version>
   <description>The traffic_light_utils package</description>
-  <maintainer email="mingyu.li@tier4.jp">Mingyu Li</maintainer>
+  <maintainer email="kotaro.uetake@tier4.jp">Kotaro Uetake</maintainer>
   <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
   <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <license>Apache License 2.0</license>

control/joy_controller/README.md

@@ -4,6 +4,21 @@
 
 `joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle.
 
+## Usage
+
+### ROS 2 launch
+
+```bash
+# With default config (ds4)
+ros2 launch joy_controller joy_controller.launch.xml
+
+# Default config but select from the existing parameter files
+ros2 launch joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox
+
+# Override the param file
+ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml
+```
+
 ## Input / Output
 
 ### Input topics

control/joy_controller/config/joy_controller.param.yaml control/joy_controller/config/joy_controller_ds4.param.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    joy_type: $(var joy_type)
+    joy_type: DS4
     update_rate: 10.0
     accel_ratio: 3.0
     brake_ratio: 5.0

control/joy_controller/config/joy_controller_g29.param.yaml

@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    joy_type: G29
+    update_rate: 10.0
+    accel_ratio: 3.0
+    brake_ratio: 5.0
+    steer_ratio: 0.5
+    steering_angle_velocity: 0.1
+    accel_sensitivity: 1.0
+    brake_sensitivity: 1.0
+    control_command:
+      raw_control: false
+      velocity_gain: 3.0
+      max_forward_velocity: 20.0
+      max_backward_velocity: 3.0
+      backward_accel_ratio: 1.0

control/joy_controller/config/joy_controller_p65.param.yaml

@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    joy_type: P65
+    update_rate: 10.0
+    accel_ratio: 3.0
+    brake_ratio: 5.0
+    steer_ratio: 0.5
+    steering_angle_velocity: 0.1
+    accel_sensitivity: 1.0
+    brake_sensitivity: 1.0
+    control_command:
+      raw_control: false
+      velocity_gain: 3.0
+      max_forward_velocity: 20.0
+      max_backward_velocity: 3.0
+      backward_accel_ratio: 1.0

control/joy_controller/config/joy_controller_xbox.param.yaml

@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    joy_type: XBOX
+    update_rate: 10.0
+    accel_ratio: 3.0
+    brake_ratio: 5.0
+    steer_ratio: 0.5
+    steering_angle_velocity: 0.1
+    accel_sensitivity: 1.0
+    brake_sensitivity: 1.0
+    control_command:
+      raw_control: false
+      velocity_gain: 3.0
+      max_forward_velocity: 20.0
+      max_backward_velocity: 3.0
+      backward_accel_ratio: 1.0

control/joy_controller/launch/joy_controller.launch.xml

@@ -1,5 +1,4 @@
 <launch>
-  <arg name="joy_type" default="DS4" description="options: DS4, G29, P65, XBOX"/>
   <arg name="external_cmd_source" default="remote" description="options: local, remote"/>
 
   <arg name="input_joy" default="/joy"/>
@@ -13,7 +12,7 @@
   <arg name="output_gate_mode" default="/control/gate_mode_cmd"/>
   <arg name="output_vehicle_engage" default="/vehicle/engage"/>
 
-  <arg name="config_file" default="$(find-pkg-share joy_controller)/config/joy_controller.param.yaml"/>
+  <arg name="config_file" default="$(find-pkg-share joy_controller)/config/joy_controller_ds4.param.yaml"/>
 
   <node pkg="joy_controller" exec="joy_controller" name="joy_controller" output="screen">
     <remap from="input/joy" to="$(var input_joy)"/>

control/joy_controller/launch/joy_controller_param_selection.launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="joy_type" default="ds4" description="options: ds4, g29, p65, xbox"/>
+
+  <include file="$(find-pkg-share joy_controller)/launch/joy_controller.launch.xml">
+    <arg name="config_file" value="$(find-pkg-share joy_controller)/config/joy_controller_$(var joy_type).param.yaml"/>
+  </include>
+</launch>

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -29,14 +29,20 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <boost/geometry/algorithms/envelope.hpp>
+#include <boost/geometry/algorithms/union.hpp>
 #include <boost/geometry/index/rtree.hpp>
-#include <boost/optional.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Polygon.h>
 
 #include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace lane_departure_checker
@@ -112,6 +118,19 @@ class LaneDepartureChecker
   bool checkPathWillLeaveLane(
     const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
 
+  std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  bool checkPathWillLeaveLane(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  PathWithLaneId cropPointsOutsideOfLanes(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
+    const size_t end_index);
+
   static bool isOutOfLane(
     const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint);
 

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -34,6 +34,7 @@ using motion_utils::calcArcLength;
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::LineString2d;
 using tier4_autoware_utils::MultiPoint2d;
+using tier4_autoware_utils::MultiPolygon2d;
 using tier4_autoware_utils::Point2d;
 
 namespace
@@ -92,6 +93,7 @@ lanelet::ConstLanelets getCandidateLanelets(
 
   return candidate_lanelets;
 }
+
 }  // namespace
 
 namespace lane_departure_checker
@@ -298,6 +300,92 @@ bool LaneDepartureChecker::willLeaveLane(
   return false;
 }
 
+std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  // Get Footprint Hull basic polygon
+  std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);
+  auto to_basic_polygon = [](const LinearRing2d & footprint_hull) -> lanelet::BasicPolygon2d {
+    lanelet::BasicPolygon2d basic_polygon;
+    for (const auto & point : footprint_hull) {
+      Eigen::Vector2d p(point.x(), point.y());
+      basic_polygon.push_back(p);
+    }
+    return basic_polygon;
+  };
+  lanelet::BasicPolygon2d footprint_hull_basic_polygon = to_basic_polygon(footprint_hull);
+
+  // Find all lanelets that intersect the footprint hull
+  return lanelet::geometry::findWithin2d(
+    lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
+}
+
+std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
+  // Fuse lanelets into a single BasicPolygon2d
+  auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
+    if (lanelets_distance_pair.empty()) return std::nullopt;
+    if (lanelets_distance_pair.size() == 1)
+      return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+
+    lanelet::BasicPolygon2d merged_polygon =
+      lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+    for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
+      const auto & route_lanelet = lanelets_distance_pair.at(i).second;
+      const auto & poly = route_lanelet.polygon2d().basicPolygon();
+
+      std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
+      boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
+
+      // Update merged_polygon by accumulating all merged results
+      merged_polygon.clear();
+      for (const auto & temp_poly : lanelet_union_temp) {
+        merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
+      }
+    }
+    if (merged_polygon.empty()) return std::nullopt;
+    return merged_polygon;
+  }();
+
+  return fused_lanelets;
+}
+
+bool LaneDepartureChecker::checkPathWillLeaveLane(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  // check if the footprint is not fully contained within the fused lanelets polygon
+  const std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
+  if (!fused_lanelets_polygon) return true;
+  return !std::all_of(
+    vehicle_footprints.begin(), vehicle_footprints.end(),
+    [&fused_lanelets_polygon](const auto & footprint) {
+      return boost::geometry::within(footprint, fused_lanelets_polygon.value());
+    });
+}
+
+PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
+{
+  PathWithLaneId temp_path;
+  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
+  if (path.points.empty() || !fused_lanelets_polygon) return temp_path;
+  const auto vehicle_footprints = createVehicleFootprints(path);
+  size_t idx = 0;
+  std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
+    if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
+      temp_path.points.push_back(path.points.at(idx));
+    }
+    ++idx;
+  });
+  PathWithLaneId cropped_path = path;
+  cropped_path.points = temp_path.points;
+  return cropped_path;
+}
+
 bool LaneDepartureChecker::isOutOfLane(
   const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint)
 {
@@ -364,4 +452,5 @@ bool LaneDepartureChecker::willCrossBoundary(
   }
   return false;
 }
+
 }  // namespace lane_departure_checker