Update launch files to run in the motion_planner + add launch config

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

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,74 @@ 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")}],
+    )
+
+    # obstacle cruise planner
+    obstacle_cruise_planner_param_path = os.path.join(
+        LaunchConfiguration("tier4_planning_launch_param_path").perform(context),
+        "scenario_planning",
+        "lane_driving",
+        "motion_planning",
+        "obstacle_cruise_planner",
+        "obstacle_cruise_planner.param.yaml",
+    )
+    with open(obstacle_cruise_planner_param_path, "r") as f:
+        obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    obstacle_cruise_planner_component = ComposableNode(
+        package="obstacle_cruise_planner",
+        plugin="motion_planning::ObstacleCruisePlannerNode",
+        name="obstacle_cruise_planner",
+        namespace="",
+        remappings=[
+            ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"),
+            ("~/input/odometry", "/localization/kinematic_state"),
+            ("~/input/acceleration", "/localization/acceleration"),
+            ("~/input/objects", "/perception/object_recognition/objects"),
+            ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"),
+            ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"),
+            ("~/output/clear_velocity_limit", "/planning/scenario_planning/clear_velocity_limit"),
+            ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
+        ],
+        parameters=[
+            nearest_search_param,
+            common_param,
+            obstacle_cruise_planner_param,
+        ],
+        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 +235,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,
@@ -180,47 +248,13 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
-    # obstacle cruise planner
-    obstacle_cruise_planner_param_path = os.path.join(
-        LaunchConfiguration("tier4_planning_launch_param_path").perform(context),
-        "scenario_planning",
-        "lane_driving",
-        "motion_planning",
-        "obstacle_cruise_planner",
-        "obstacle_cruise_planner.param.yaml",
-    )
-    with open(obstacle_cruise_planner_param_path, "r") as f:
-        obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    obstacle_cruise_planner_component = ComposableNode(
-        package="obstacle_cruise_planner",
-        plugin="motion_planning::ObstacleCruisePlannerNode",
-        name="obstacle_cruise_planner",
-        namespace="",
-        remappings=[
-            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
-            ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/acceleration", "/localization/acceleration"),
-            ("~/input/objects", "/perception/object_recognition/objects"),
-            ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"),
-            ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"),
-            ("~/output/clear_velocity_limit", "/planning/scenario_planning/clear_velocity_limit"),
-            ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
-        ],
-        parameters=[
-            nearest_search_param,
-            common_param,
-            obstacle_cruise_planner_param,
-        ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-
     obstacle_cruise_planner_relay_component = ComposableNode(
         package="topic_tools",
         plugin="topic_tools::RelayNode",
         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,
         ],
     )
 

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -23,15 +23,6 @@
     </include>
   </group>
 
-  <!-- obstacle velocity limiter -->
-  <group>
-    <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
-    <set_remap from="~/output/trajectory" to="/planning/scenario_planning/obstacle_velocity_limiter/trajectory"/>
-    <include file="$(find-pkg-share obstacle_velocity_limiter)/launch/obstacle_velocity_limiter.launch.xml">
-      <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
-    </include>
-  </group>
-
   <!-- velocity planning with max velocity, acceleration, jerk, stop point constraint -->
   <group>
     <arg name="param_path" default="$(var tier4_planning_launch_param_path)/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml"/>
@@ -45,7 +36,7 @@
     <!-- motion velocity smoother -->
     <group>
       <arg name="smoother_type" default="JerkFiltered" description="options: JerkFiltered, L2, Analytical, Linf(Unstable)"/>
-      <set_remap from="~/input/trajectory" to="/planning/scenario_planning/obstacle_velocity_limiter/trajectory"/>
+      <set_remap from="~/input/trajectory" to="/planning/scenario_planning/lane_driving/trajectory"/>
       <set_remap from="~/output/trajectory" to="/planning/scenario_planning/trajectory"/>
       <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
         <arg name="smoother_type" value="$(var smoother_type)"/>

planning/obstacle_velocity_limiter/script/velocity_visualizer.py

@@ -31,13 +31,13 @@ def __init__(self):
 
         self.sub_original_traj = self.create_subscription(
             Trajectory,
-            "/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/trajectory",
+            "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory",
             self.plotOriginalTrajectory,
             1,
         )
         self.sub_adjusted_traj = self.create_subscription(
             Trajectory,
-            "/planning/scenario_planning/lane_driving/trajectory",
+            "/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/trajectory",
             self.plotAdjustedTrajectory,
             1,
         )