diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index e8661322215fe..89d04bc5b3a00 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_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/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 0fa8707e4fda9..3d195185d9bd6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -19,22 +19,11 @@ - - - - - - - - - - - @@ -45,7 +34,7 @@ - +