diff --git a/control/external_cmd_selector/CMakeLists.txt b/control/external_cmd_selector/CMakeLists.txt index 8785c5c5ae811..5c4860b7441f3 100644 --- a/control/external_cmd_selector/CMakeLists.txt +++ b/control/external_cmd_selector/CMakeLists.txt @@ -15,4 +15,5 @@ rclcpp_components_register_node(external_cmd_selector_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/control/external_cmd_selector/config/external_cmd_selector.param.yaml b/control/external_cmd_selector/config/external_cmd_selector.param.yaml new file mode 100644 index 0000000000000..6556656cc8938 --- /dev/null +++ b/control/external_cmd_selector/config/external_cmd_selector.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + update_rate: 10.0 + initial_selector_mode: "local" # ["local", "remote"] diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.py b/control/external_cmd_selector/launch/external_cmd_selector.launch.py index 5e67627207ad3..358cc135996f7 100644 --- a/control/external_cmd_selector/launch/external_cmd_selector.launch.py +++ b/control/external_cmd_selector/launch/external_cmd_selector.launch.py @@ -14,25 +14,74 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ComposableNode +import yaml def _create_mapping_tuple(name): return ("~/" + name, LaunchConfiguration(name)) -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): + with open(LaunchConfiguration("external_cmd_selector_param_path").perform(context), "r") as f: + external_cmd_selector_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + component = ComposableNode( + package="external_cmd_selector", + plugin="ExternalCmdSelector", + name="external_cmd_selector", + remappings=[ + _create_mapping_tuple("service/select_external_command"), + _create_mapping_tuple("input/local/control_cmd"), + _create_mapping_tuple("input/local/shift_cmd"), + _create_mapping_tuple("input/local/turn_signal_cmd"), + _create_mapping_tuple("input/local/heartbeat"), + _create_mapping_tuple("input/remote/control_cmd"), + _create_mapping_tuple("input/remote/shift_cmd"), + _create_mapping_tuple("input/remote/turn_signal_cmd"), + _create_mapping_tuple("input/remote/heartbeat"), + _create_mapping_tuple("output/control_cmd"), + _create_mapping_tuple("output/gear_cmd"), + _create_mapping_tuple("output/turn_indicators_cmd"), + _create_mapping_tuple("output/hazard_lights_cmd"), + _create_mapping_tuple("output/heartbeat"), + _create_mapping_tuple("output/current_selector_mode"), + ], + parameters=[ + external_cmd_selector_param, + ], + extra_arguments=[ + { + "use_intra_process_comms": LaunchConfiguration("use_intra_process"), + } + ], + ) + + loader = LoadComposableNodes( + composable_node_descriptions=[component], + target_container=LaunchConfiguration("target_container"), + ) + + group = GroupAction( + [ + PushRosNamespace(""), + loader, + ] + ) + + return [group] + +def generate_launch_description(): arguments = [ # component DeclareLaunchArgument("use_intra_process"), DeclareLaunchArgument("target_container"), - # settings - DeclareLaunchArgument( - "initial_selector_mode", default_value="local", choices=["local", "remote"] - ), # service DeclareLaunchArgument( "service/select_external_command", default_value="~/select_external_command" @@ -82,42 +131,4 @@ def generate_launch_description(): ), ] - component = ComposableNode( - package="external_cmd_selector", - plugin="ExternalCmdSelector", - name="external_cmd_selector", - remappings=[ - _create_mapping_tuple("service/select_external_command"), - _create_mapping_tuple("input/local/control_cmd"), - _create_mapping_tuple("input/local/shift_cmd"), - _create_mapping_tuple("input/local/turn_signal_cmd"), - _create_mapping_tuple("input/local/heartbeat"), - _create_mapping_tuple("input/remote/control_cmd"), - _create_mapping_tuple("input/remote/shift_cmd"), - _create_mapping_tuple("input/remote/turn_signal_cmd"), - _create_mapping_tuple("input/remote/heartbeat"), - _create_mapping_tuple("output/control_cmd"), - _create_mapping_tuple("output/gear_cmd"), - _create_mapping_tuple("output/turn_indicators_cmd"), - _create_mapping_tuple("output/hazard_lights_cmd"), - _create_mapping_tuple("output/heartbeat"), - _create_mapping_tuple("output/current_selector_mode"), - ], - parameters=[ - { - "initial_selector_mode": LaunchConfiguration("initial_selector_mode"), - } - ], - extra_arguments=[ - { - "use_intra_process_comms": LaunchConfiguration("use_intra_process"), - } - ], - ) - - loader = LoadComposableNodes( - composable_node_descriptions=[component], - target_container=LaunchConfiguration("target_container"), - ) - - return LaunchDescription(arguments + [loader]) + return LaunchDescription(arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp index b775a24346bff..0039ec9cd1547 100644 --- a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp +++ b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp @@ -28,8 +28,8 @@ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_option using std::placeholders::_2; // Parameter - double update_rate = declare_parameter("update_rate", 10.0); - std::string initial_selector_mode = declare_parameter("initial_selector_mode", "local"); + double update_rate = declare_parameter("update_rate"); + std::string initial_selector_mode = declare_parameter("initial_selector_mode"); // Publisher pub_current_selector_mode_ = diff --git a/control/trajectory_follower_node/CMakeLists.txt b/control/trajectory_follower_node/CMakeLists.txt index 07bc45c2933c8..a2316a6db30d2 100644 --- a/control/trajectory_follower_node/CMakeLists.txt +++ b/control/trajectory_follower_node/CMakeLists.txt @@ -44,7 +44,7 @@ if(BUILD_TESTING) find_package(autoware_testing REQUIRED) add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe PARAM_FILENAMES "lateral_controller_defaults.param.yaml longitudinal_controller_defaults.param.yaml -test_vehicle_info.param.yaml test_nearest_search.param.yaml" +test_vehicle_info.param.yaml test_nearest_search.param.yaml trajectory_follower_node.param.yaml" ) endif() diff --git a/control/trajectory_follower_node/param/trajectory_follower_node.param.yaml b/control/trajectory_follower_node/param/trajectory_follower_node.param.yaml new file mode 100644 index 0000000000000..dcd985bb4a6b1 --- /dev/null +++ b/control/trajectory_follower_node/param/trajectory_follower_node.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + ctrl_period: 0.03 + timeout_thr_sec: 0.5 diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index b8a2aa03864bd..7b98a69a7c3a2 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -37,11 +37,11 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control { using std::placeholders::_1; - const double ctrl_period = declare_parameter("ctrl_period", 0.03); - timeout_thr_sec_ = declare_parameter("timeout_thr_sec", 0.5); + const double ctrl_period = declare_parameter("ctrl_period"); + timeout_thr_sec_ = declare_parameter("timeout_thr_sec"); const auto lateral_controller_mode = - getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc")); + getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc")); switch (lateral_controller_mode) { case LateralControllerMode::MPC: { lateral_controller_ = std::make_shared(*this); @@ -55,8 +55,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control throw std::domain_error("[LateralController] invalid algorithm"); } - const auto longitudinal_controller_mode = - getLongitudinalControllerMode(declare_parameter("longitudinal_controller_mode", "pid")); + const auto longitudinal_controller_mode = getLongitudinalControllerMode( + declare_parameter("longitudinal_controller_mode", "pid")); switch (longitudinal_controller_mode) { case LongitudinalControllerMode::PID: { longitudinal_controller_ = diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index 306b971adb22e..d3b39a018652c 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -56,6 +56,8 @@ rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_c rclcpp::NodeOptions node_options; node_options.append_parameter_override("ctrl_period", 0.03); node_options.append_parameter_override("timeout_thr_sec", 0.5); + node_options.append_parameter_override("lateral_controller_mode", "mpc"); + node_options.append_parameter_override("longitudinal_controller_mode", "pid"); node_options.append_parameter_override( "enable_keep_stopped_until_steer_convergence", enable_keep_stopped_until_steer_convergence); // longitudinal @@ -64,7 +66,8 @@ rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_c lateral_share_dir + "/param/lateral_controller_defaults.param.yaml", "--params-file", longitudinal_share_dir + "/param/longitudinal_controller_defaults.param.yaml", "--params-file", share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", - share_dir + "/param/test_nearest_search.param.yaml"}); + share_dir + "/param/test_nearest_search.param.yaml", "--params-file", + share_dir + "/param/trajectory_follower_node.param.yaml"}); return node_options; } diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 4876699351ffd..a4ac84f5b43e0 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -2,6 +2,9 @@ ros__parameters: update_rate: 10.0 system_emergency_heartbeat_timeout: 0.5 + use_emergency_handling: false + check_external_emergency_heartbeat: false + use_start_request: false external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 36e12ab28a3a6..e38299a6cfa6e 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -126,40 +126,40 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) "input/emergency/gear_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyShiftCmd, this, _1)); // Parameter - update_period_ = 1.0 / declare_parameter("update_rate", 10.0); - use_emergency_handling_ = declare_parameter("use_emergency_handling", false); + update_period_ = 1.0 / declare_parameter("update_rate"); + use_emergency_handling_ = declare_parameter("use_emergency_handling"); check_external_emergency_heartbeat_ = - declare_parameter("check_external_emergency_heartbeat", false); + declare_parameter("check_external_emergency_heartbeat"); system_emergency_heartbeat_timeout_ = - declare_parameter("system_emergency_heartbeat_timeout", 0.5); + declare_parameter("system_emergency_heartbeat_timeout"); external_emergency_stop_heartbeat_timeout_ = - declare_parameter("external_emergency_stop_heartbeat_timeout", 0.5); - stop_hold_acceleration_ = declare_parameter("stop_hold_acceleration", -1.5); - emergency_acceleration_ = declare_parameter("emergency_acceleration", -2.4); + declare_parameter("external_emergency_stop_heartbeat_timeout"); + stop_hold_acceleration_ = declare_parameter("stop_hold_acceleration"); + emergency_acceleration_ = declare_parameter("emergency_acceleration"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); { VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; - p.vel_lim = declare_parameter("nominal.vel_lim", 25.0); - p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim", 5.0); - p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim", 5.0); - p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim", 5.0); - p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim", 5.0); - p.actual_steer_diff_lim = declare_parameter("nominal.actual_steer_diff_lim", 1.0); + p.vel_lim = declare_parameter("nominal.vel_lim"); + p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim"); + p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim"); + p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim"); + p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim"); + p.actual_steer_diff_lim = declare_parameter("nominal.actual_steer_diff_lim"); filter_.setParam(p); } { VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; - p.vel_lim = declare_parameter("on_transition.vel_lim", 25.0); - p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim", 0.5); - p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim", 0.25); - p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim", 0.5); - p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim", 0.25); - p.actual_steer_diff_lim = declare_parameter("on_transition.actual_steer_diff_lim", 0.05); + p.vel_lim = declare_parameter("on_transition.vel_lim"); + p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim"); + p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim"); + p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim"); + p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim"); + p.actual_steer_diff_lim = declare_parameter("on_transition.actual_steer_diff_lim"); filter_on_transition_.setParam(p); } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 8d513b83b0a82..d65614a39be7c 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -37,6 +37,10 @@ def launch_setup(context, *args, **kwargs): with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open( + LaunchConfiguration("trajectory_follower_node_param_path").perform(context), "r" + ) as f: + trajectory_follower_node_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("lat_controller_param_path").perform(context), "r") as f: lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("lon_controller_param_path").perform(context), "r") as f: @@ -74,10 +78,10 @@ def launch_setup(context, *args, **kwargs): ], parameters=[ { - "ctrl_period": 0.03, "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"), }, nearest_search_param, + trajectory_follower_node_param, lon_controller_param, lat_controller_param, vehicle_info_param, @@ -162,13 +166,6 @@ def launch_setup(context, *args, **kwargs): parameters=[ vehicle_cmd_gate_param, vehicle_info_param, - { - "use_emergency_handling": LaunchConfiguration("use_emergency_handling"), - "check_external_emergency_heartbeat": LaunchConfiguration( - "check_external_emergency_heartbeat" - ), - "use_start_request": LaunchConfiguration("use_start_request"), - }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -205,7 +202,10 @@ def launch_setup(context, *args, **kwargs): launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), ("target_container", "/control/control_container"), - ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + ( + "external_cmd_selector_param_path", + LaunchConfiguration("external_cmd_selector_param_path"), + ), ], ) @@ -286,15 +286,14 @@ def add_launch_arg(name: str, default_value=None, description=None): # option add_launch_arg("vehicle_param_file") - add_launch_arg("vehicle_param_file") add_launch_arg("vehicle_id") add_launch_arg("enable_obstacle_collision_checker") - add_launch_arg("check_external_emergency_heartbeat") add_launch_arg("lateral_controller_mode") add_launch_arg("longitudinal_controller_mode") # common param path add_launch_arg("nearest_search_param_path") # package param path + add_launch_arg("trajectory_follower_node_param_path") add_launch_arg("lat_controller_param_path") add_launch_arg("lon_controller_param_path") add_launch_arg("vehicle_cmd_gate_param_path") @@ -302,18 +301,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("operation_mode_transition_manager_param_path") add_launch_arg("shift_decider_param_path") add_launch_arg("obstacle_collision_checker_param_path") - - # velocity controller - add_launch_arg("show_debug_info", "false", "show debug information") - add_launch_arg("enable_pub_debug", "true", "enable to publish debug information") - - # vehicle cmd gate - add_launch_arg("use_emergency_handling", "false", "use emergency handling") - add_launch_arg("check_external_emergency_heartbeat", "true", "use external emergency stop") - add_launch_arg("use_start_request", "false", "use start request service") - - # external cmd selector - add_launch_arg("initial_selector_mode", "remote", "local or remote") + add_launch_arg("external_cmd_selector_param_path") # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication")