From 88c5f81c5a8086a9fec81ed2e9e1ff2056ddb338 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 6 Jun 2023 15:14:20 +0300 Subject: [PATCH] control launch Signed-off-by: Berkay Karaman --- .../launch/control.launch.py | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 8cb9c726fcb05..c0657c6cc636d 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -61,6 +61,8 @@ def launch_setup(context, *args, **kwargs): obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("aeb_param_path").perform(context), "r") as f: aeb_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("predicted_path_checker_param_path").perform(context), "r") as f: + predicted_path_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( package="trajectory_follower_node", @@ -157,6 +159,34 @@ def launch_setup(context, *args, **kwargs): target_container="/control/control_container", ) + # autonomous emergency braking + predicted_path_checker = ComposableNode( + package="predicted_path_checker", + plugin="autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode", + name="predicted_path_checker", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ("~/input/current_accel", "/localization/acceleration"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + vehicle_info_param, + predicted_path_checker_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + predicted_path_checker_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_predicted_path_checker")), + composable_node_descriptions=[predicted_path_checker], + target_container="/control/control_container", + ) + # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( package="vehicle_cmd_gate", @@ -310,6 +340,7 @@ def launch_setup(context, *args, **kwargs): external_cmd_converter_loader, obstacle_collision_checker_loader, autonomous_emergency_braking_loader, + predicted_path_checker_loader, ] ) @@ -343,6 +374,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("obstacle_collision_checker_param_path") add_launch_arg("external_cmd_selector_param_path") add_launch_arg("aeb_param_path") + add_launch_arg("predicted_path_checker_param_path") add_launch_arg("enable_autonomous_emergency_braking") add_launch_arg("check_external_emergency_heartbeat")