refactor(tier4_control_launch): remove parameter definition in contro…

…l.launch.py (autowarefoundation#2585) * refactor trajectory_follower_node's param Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * organize parameter definition in control_launch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix typo Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix failed test Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
sujithvemi · Dec 28, 2022 · 0f96564 · 0f96564
1 parent 0483c70
commit 0f96564
Show file tree

Hide file tree

Showing 11 changed files with 109 additions and 95 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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<double>("update_rate");
+  std::string initial_selector_mode = declare_parameter<std::string>("initial_selector_mode");
 
   // Publisher
   pub_current_selector_mode_ =

diff --git 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
@@ -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
@@ -37,11 +37,11 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
 {
   using std::placeholders::_1;
 
-  const double ctrl_period = declare_parameter<double>("ctrl_period", 0.03);
-  timeout_thr_sec_ = declare_parameter<double>("timeout_thr_sec", 0.5);
+  const double ctrl_period = declare_parameter<double>("ctrl_period");
+  timeout_thr_sec_ = declare_parameter<double>("timeout_thr_sec");
 
   const auto lateral_controller_mode =
-    getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc"));
+    getLateralControllerMode(declare_parameter<std::string>("lateral_controller_mode", "mpc"));
   switch (lateral_controller_mode) {
     case LateralControllerMode::MPC: {
       lateral_controller_ = std::make_shared<mpc_lateral_controller::MpcLateralController>(*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<std::string>("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
@@ -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
@@ -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
@@ -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<double>("update_rate");
+  use_emergency_handling_ = declare_parameter<bool>("use_emergency_handling");
   check_external_emergency_heartbeat_ =
-    declare_parameter("check_external_emergency_heartbeat", false);
+    declare_parameter<bool>("check_external_emergency_heartbeat");
   system_emergency_heartbeat_timeout_ =
-    declare_parameter("system_emergency_heartbeat_timeout", 0.5);
+    declare_parameter<double>("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<double>("external_emergency_stop_heartbeat_timeout");
+  stop_hold_acceleration_ = declare_parameter<double>("stop_hold_acceleration");
+  emergency_acceleration_ = declare_parameter<double>("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<double>("nominal.vel_lim");
+    p.lon_acc_lim = declare_parameter<double>("nominal.lon_acc_lim");
+    p.lon_jerk_lim = declare_parameter<double>("nominal.lon_jerk_lim");
+    p.lat_acc_lim = declare_parameter<double>("nominal.lat_acc_lim");
+    p.lat_jerk_lim = declare_parameter<double>("nominal.lat_jerk_lim");
+    p.actual_steer_diff_lim = declare_parameter<double>("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<double>("on_transition.vel_lim");
+    p.lon_acc_lim = declare_parameter<double>("on_transition.lon_acc_lim");
+    p.lon_jerk_lim = declare_parameter<double>("on_transition.lon_jerk_lim");
+    p.lat_acc_lim = declare_parameter<double>("on_transition.lat_acc_lim");
+    p.lat_jerk_lim = declare_parameter<double>("on_transition.lat_jerk_lim");
+    p.actual_steer_diff_lim = declare_parameter<double>("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
@@ -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,34 +286,22 @@ 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")
     add_launch_arg("lane_departure_checker_param_path")
     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")
-Original file line number
+Diff line change
@@ Expand Up / @@ -15,4 +15,5 @@ rclcpp_components_register_node(external_cmd_selector_node @@
     ament_auto_package(INSTALL_TO_SHARE
       launch
+      config
     )