feat(planning_launch): separate surround_obstacle_checker from hierar…

…chical motion planning flow Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · May 9, 2022 · a516756 · a516756
1 parent fd09867
commit a516756
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 39 deletions.
diff --git a/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
@@ -92,20 +92,23 @@ def launch_setup(context, *args, **kwargs):
         surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     surround_obstacle_checker_component = ComposableNode(
         package="surround_obstacle_checker",
-        plugin="SurroundObstacleCheckerNode",
+        plugin="surround_obstacle_checker::SurroundObstacleCheckerNode",
         name="surround_obstacle_checker",
         namespace="",
         remappings=[
             ("~/output/no_start_reason", "/planning/scenario_planning/status/no_start_reason"),
             ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
-            ("~/output/trajectory", "surround_obstacle_checker/trajectory"),
+            ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"),
+            (
+                "~/output/velocity_limit_clear_command",
+                "/planning/scenario_planning/clear_velocity_limit",
+            ),
             (
                 "~/input/pointcloud",
                 "/perception/obstacle_segmentation/pointcloud",
             ),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
         ],
         parameters=[
             surround_obstacle_checker_param,
@@ -114,22 +117,6 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
-    # relay
-    relay_component = ComposableNode(
-        package="topic_tools",
-        plugin="topic_tools::RelayNode",
-        name="skip_surround_obstacle_check_relay",
-        namespace="",
-        parameters=[
-            {
-                "input_topic": "obstacle_avoidance_planner/trajectory",
-                "output_topic": "surround_obstacle_checker/trajectory",
-                "type": "autoware_auto_planning_msgs/msg/Trajectory",
-            }
-        ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-
     # obstacle stop planner
     obstacle_stop_planner_param_path = os.path.join(
         get_package_share_directory("tier4_planning_launch"),
@@ -173,7 +160,7 @@ def launch_setup(context, *args, **kwargs):
             ),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/trajectory", "surround_obstacle_checker/trajectory"),
+            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
         ],
         parameters=[
             common_param,
@@ -202,13 +189,7 @@ def launch_setup(context, *args, **kwargs):
         condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")),
     )
 
-    relay_loader = LoadComposableNodes(
-        composable_node_descriptions=[relay_component],
-        target_container=container,
-        condition=UnlessCondition(LaunchConfiguration("use_surround_obstacle_check")),
-    )
-
-    group = GroupAction([container, surround_obstacle_checker_loader, relay_loader])
+    group = GroupAction([container, surround_obstacle_checker_loader])
 
     return [group]
 

diff --git a/...g_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/...g_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -19,22 +19,11 @@
   <!-- surround obstacle checker (Do not start if there are pedestrian/bicycles around ego-car) -->
   <group if="$(var use_surround_obstacle_check)">
     <include file="$(find-pkg-share surround_obstacle_checker)/launch/surround_obstacle_checker.launch.xml">
-      <arg name="input_trajectory" value="obstacle_avoidance_planner/trajectory"/>
-      <arg name="output_trajectory" value="surround_obstacle_checker/trajectory"/>
       <arg name="param_path" value="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml"/>
       <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
     </include>
   </group>
 
-  <!-- skip surround obstacle checker -->
-  <group unless="$(var use_surround_obstacle_check)">
-    <node pkg="topic_tools" exec="relay" name="skip_surround_obstacle_check_relay" output="log">
-      <param name="input_topic" value="obstacle_avoidance_planner/trajectory"/>
-      <param name="output_topic" value="surround_obstacle_checker/trajectory"/>
-      <param name="type" value="autoware_auto_planning_msgs/msg/Trajectory"/>
-    </node>
-  </group>
-
   <!-- stop velocity planning with obstacle pointcloud info -->
   <group>
     <include file="$(find-pkg-share obstacle_stop_planner)/launch/obstacle_stop_planner.launch.xml">
@@ -45,7 +34,7 @@
       <arg name="obstacle_stop_planner_param_path" value="$(find-pkg-share tier4_planning_launch)/config/obstacle_stop_planner/obstacle_stop_planner.param.yaml"/>
       <arg name="enable_slow_down" value="false"/>
       <!-- remap topic name -->
-      <arg name="input_trajectory" value="surround_obstacle_checker/trajectory"/>
+      <arg name="input_trajectory" value="obstacle_avoidance_planner/trajectory"/>
       <arg name="input_pointcloud" value="/perception/obstacle_segmentation/pointcloud"/>
       <arg name="output_trajectory" value="/planning/scenario_planning/lane_driving/trajectory"/>
     </include>