feat(planning_launch): add behavior_velocity_limiter to motion_planni…

…ng launch file (autowarefoundation#596) (autowarefoundation#597)

Signed-off-by: Maxime CLEMENT <[email protected]>

Signed-off-by: Maxime CLEMENT <[email protected]>

Signed-off-by: Maxime CLEMENT <[email protected]>
Co-authored-by: Maxime CLEMENT <[email protected]>

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

planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -94,6 +94,40 @@ def add_launch_arg(name: str, default_value=None, description=None):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # obstacle velocity limiter
+    obstacle_velocity_limiter_param_path = os.path.join(
+        get_package_share_directory("planning_launch"),
+        "config",
+        "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,
+            {"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(
         get_package_share_directory("planning_launch"),
@@ -151,7 +185,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
         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"),
@@ -212,7 +246,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
             ),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
+            ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"),
         ],
         parameters=[
             common_param,
@@ -230,7 +264,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
         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"},
         ],
@@ -244,6 +278,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
             obstacle_avoidance_planner_component,
+            obstacle_velocity_limiter_component,
         ],
     )