feat: add 'obstacle_velocity_limiter' package

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

@@ -35,8 +35,8 @@ using LineString2d = boost::geometry::model::linestring<Point2d>;
 using LinearRing2d = boost::geometry::model::ring<Point2d>;
 using Polygon2d = boost::geometry::model::polygon<Point2d>;
 using MultiPoint2d = boost::geometry::model::multi_point<Point2d>;
-using MultiLineString2d = boost::geometry::model::multi_linestring<Point2d>;
-using MultiPolygon2d = boost::geometry::model::multi_polygon<Point2d>;
+using MultiLineString2d = boost::geometry::model::multi_linestring<LineString2d>;
+using MultiPolygon2d = boost::geometry::model::multi_polygon<Polygon2d>;
 
 // 3D
 struct Point3d;
@@ -46,8 +46,8 @@ using LineString3d = boost::geometry::model::linestring<Point3d>;
 using LinearRing3d = boost::geometry::model::ring<Point3d>;
 using Polygon3d = boost::geometry::model::polygon<Point3d>;
 using MultiPoint3d = boost::geometry::model::multi_point<Point3d>;
-using MultiLineString3d = boost::geometry::model::multi_linestring<Point3d>;
-using MultiPolygon3d = boost::geometry::model::multi_polygon<Point3d>;
+using MultiLineString3d = boost::geometry::model::multi_linestring<LineString3d>;
+using MultiPolygon3d = boost::geometry::model::multi_polygon<Polygon3d>;
 
 struct Point2d : public Eigen::Vector2d
 {

launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml

@@ -0,0 +1,33 @@
+/**:
+  ros__parameters:
+    min_ttc: 1.0  # [s] minimum time without collision when doing projection at each trajectory point
+    distance_buffer: 0.0  # [m] extra distance to add to a projection (in addition to the vehicle overhang)
+    min_adjusted_velocity: 2.5  # [m/s] minimum velocity that the module can set
+    max_deceleration: 2.0  # [m/s²] maximum deceleration caused by the adjusted velocity
+
+    trajectory_preprocessing:
+      start_distance: 0.0  # [m] distance ahead of ego from which to start modifying the trajectory
+      max_length: 100.0 # [m] maximum distance (from the start point) where the velocity is adjusted
+      max_duration: 10.0 # [s] maximum duration (from the start point) where the velocity is adjusted
+      downsample_factor: 10  # factor by which to downsample the input trajectory for calculation
+      calculate_steering_angles: false # if true, the steering angle at each trajectory point is calculated from the change in heading
+
+    simulation:
+      model: particle # model to use for forward projection at each trajectory point. Either "particle" or "bicycle"
+      distance_method: exact  # distance calculation method. Either "exact" or "approximation".
+      # parameters used only with the bicycle model
+      steering_offset: 0.01 # [rad] steering angle offset used to model uncertainty in the forward projection
+      nb_points: 5  # number of points representing the curved projections
+
+    obstacles:
+      dynamic_source: static_only # source of dynamic obstacles. Must be 'pointcloud', 'occupancy_grid', or 'static_only'.
+      ignore_obstacles_on_path: false # if true, obstacles on the ego paths are ignored
+      ignore_extra_distance: 1.0 # [m] extra lateral distance where obstacles along the path are ignored
+      occupancy_grid_threshold: 60 # occupancy grid values higher than this are considered to be obstacles
+      dynamic_obstacles_buffer: 1.5 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection
+      dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module
+      static_map_tags: # linestring tags in the lanelet maps that will be used as static obstacles
+        - guard_rail
+      filter_envelope : false # whether to calculate the apparent safety envelope and use it to filter obstacles
+      rtree_min_points: 500 # from this number of obstacle points, a rtree is used for collision detection
+      rtree_min_segments: 1600 # from this number of obstacle segments, a rtree is used for collision detection

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -87,6 +87,40 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # obstacle velocity limiter
+    obstacle_velocity_limiter_param_path = os.path.join(
+        LaunchConfiguration("tier4_planning_launch_param_path").perform(context),
+        "scenario_planning",
+        "lane_driving",
+        "motion_planning",
+        "obstacle_velocity_limiter",
+        "obstacle_velocity_limiter.param.yaml",
+    )
+    with open(obstacle_velocity_limiter_param_path, "r") as f:
+        obstacle_velocity_limiter_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    obstacle_velocity_limiter_component = ComposableNode(
+        package="obstacle_velocity_limiter",
+        plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode",
+        name="obstacle_velocity_limiter",
+        namespace="",
+        remappings=[
+            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
+            ("~/input/odometry", "/localization/kinematic_state"),
+            ("~/input/dynamic_obstacles", "/perception/object_recognition/objects"),
+            ("~/input/occupancy_grid", "/perception/occupancy_grid_map/map"),
+            ("~/input/obstacle_pointcloud", "/perception/obstacle_segmentation/pointcloud"),
+            ("~/input/map", "/map/vector_map"),
+            ("~/output/debug_markers", "debug_markers"),
+            ("~/output/trajectory", "obstacle_velocity_limiter/trajectory"),
+        ],
+        parameters=[
+            obstacle_velocity_limiter_param,
+            vehicle_info_param,
+            {"obstacles.dynamic_source": "static_only"},
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
     # surround obstacle checker
     surround_obstacle_checker_param_path = os.path.join(
         LaunchConfiguration("tier4_planning_launch_param_path").perform(context),
@@ -167,7 +201,7 @@ def launch_setup(context, *args, **kwargs):
             ),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
+            ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"),
         ],
         parameters=[
             nearest_search_param,
@@ -197,7 +231,7 @@ def launch_setup(context, *args, **kwargs):
         name="obstacle_cruise_planner",
         namespace="",
         remappings=[
-            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
+            ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/input/acceleration", "/localization/acceleration"),
             ("~/input/objects", "/perception/object_recognition/objects"),
@@ -220,7 +254,7 @@ def launch_setup(context, *args, **kwargs):
         name="obstacle_cruise_planner_relay",
         namespace="",
         parameters=[
-            {"input_topic": "obstacle_avoidance_planner/trajectory"},
+            {"input_topic": "obstacle_velocity_limiter/trajectory"},
             {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"},
             {"type": "autoware_auto_planning_msgs/msg/Trajectory"},
         ],
@@ -234,6 +268,7 @@ def launch_setup(context, *args, **kwargs):
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
             obstacle_avoidance_planner_component,
+            obstacle_velocity_limiter_component,
         ],
     )
 

planning/obstacle_velocity_limiter/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.5)
+project(obstacle_velocity_limiter)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(PCL REQUIRED COMPONENTS common)
+find_package(pcl_conversions REQUIRED)
+
+ament_auto_add_library(obstacle_velocity_limiter_node SHARED
+  DIRECTORY src
+)
+
+ament_target_dependencies(obstacle_velocity_limiter_node PCL)
+target_include_directories(obstacle_velocity_limiter_node
+  SYSTEM PUBLIC
+    "${PCL_INCLUDE_DIRS}"
+)
+
+# Disable warnings due to external dependencies
+get_target_property(lanelet2_core_INCLUDE_DIR
+  lanelet2_core::lanelet2_core INTERFACE_INCLUDE_DIRECTORIES
+)
+
+rclcpp_components_register_node(obstacle_velocity_limiter_node
+  PLUGIN "obstacle_velocity_limiter::ObstacleVelocityLimiterNode"
+  EXECUTABLE obstacle_velocity_limiter
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_forward_projection.cpp
+    test/test_obstacles.cpp
+    test/test_collision_distance.cpp
+    test/test_occupancy_grid_utils.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    obstacle_velocity_limiter_node
+  )
+endif()
+
+add_executable(collision_benchmark
+  benchmarks/collision_checker_benchmark.cpp
+)
+target_link_libraries(collision_benchmark
+  obstacle_velocity_limiter_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+    script
+)