diff --git a/control/pure_pursuit/CMakeLists.txt b/control/pure_pursuit/CMakeLists.txt index bd0f25b206c67..ab8f65f9fc8d5 100644 --- a/control/pure_pursuit/CMakeLists.txt +++ b/control/pure_pursuit/CMakeLists.txt @@ -56,4 +56,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index c49c280e2647b..05305eaf59e6e 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -31,6 +31,7 @@ def launch_setup(context, *args, **kwargs): + lateral_controller_mode = LaunchConfiguration("lateral_controller_mode").perform(context) vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -43,7 +44,9 @@ def launch_setup(context, *args, **kwargs): latlon_muxer_param_path = LaunchConfiguration("latlon_muxer_param_path").perform(context) with open(latlon_muxer_param_path, "r") as f: latlon_muxer_param = yaml.safe_load(f)["/**"]["ros__parameters"] - + pure_pursuit_param_path = LaunchConfiguration("pure_pursuit_param_path").perform(context) + with open(pure_pursuit_param_path, "r") as f: + pure_pursuit_param = yaml.safe_load(f)["/**"]["ros__parameters"] vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform( context ) @@ -76,6 +79,22 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + pure_pursuit_component = ComposableNode( + package="pure_pursuit", + plugin="pure_pursuit::PurePursuitNode", + name="pure_pursuit_node_exe", + namespace="trajectory_follower", + remappings=[ + ("input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ("input/current_odometry", "/localization/kinematic_state"), + ("output/control_raw", "lateral/control_cmd"), + ], + parameters=[ + pure_pursuit_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + # longitudinal controller lon_controller_component = ComposableNode( package="trajectory_follower_nodes", @@ -238,11 +257,19 @@ def launch_setup(context, *args, **kwargs): ) # lateral controller is separated since it may be another controller (e.g. pure pursuit) - lat_controller_loader = LoadComposableNodes( - composable_node_descriptions=[lat_controller_component], - target_container=container, - # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"), - ) + if lateral_controller_mode == "mpc_follower": + lat_controller_loader = LoadComposableNodes( + composable_node_descriptions=[lat_controller_component], + target_container=container, + # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"), + ) + elif lateral_controller_mode == "pure_pursuit": + lat_controller_loader = LoadComposableNodes( + composable_node_descriptions=[pure_pursuit_component], + target_container=container, + # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"), + ) + group = GroupAction( [ @@ -271,6 +298,9 @@ def add_launch_arg(name: str, default_value=None, description=None): # "lateral controller mode: `mpc_follower` or `pure_pursuit`", # ) + add_launch_arg("lateral_controller_mode", "mpc_follower", "lateral controller mode: `mpc_follower` or `pure_pursuit`") + + add_launch_arg( "vehicle_info_param_file", [ @@ -288,6 +318,14 @@ def add_launch_arg(name: str, default_value=None, description=None): ], "path to the parameter file of lateral controller", ) + add_launch_arg( + "pure_pursuit_param_path", + [ + FindPackageShare("pure_pursuit"), + "/config/pure_pursuit.param.yaml", + ], + "path to the parameter file of lateral controller", + ) add_launch_arg( "lon_controller_param_path", [ @@ -325,6 +363,8 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg("enable_pub_debug", "true", "enable to publish debug information") + # lateral controller + # vehicle cmd gate add_launch_arg("use_emergency_handling", "false", "use emergency handling") add_launch_arg("use_external_emergency_stop", "true", "use external emergency stop")