diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 09592c3fffac6..62a774f7e5028 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -48,7 +48,7 @@ | ------------------------------------------- | ------ | --------------------------------------------------------------------------- | | `update_period` | double | update period | | `use_emergency_handling` | bool | true when emergency handler is used | -| `use_external_emergency_stop` | bool | true when external emergency stop information is used | +| `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop | | `system_emergency_heartbeat_timeout` | double | timeout for system emergency | | `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | | `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | @@ -66,6 +66,6 @@ ## Assumptions / Known limits -The parameter `use_external_emergency_stop` (true by default) enables an emergency stop request from external modules. +The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. This feature requires a `~/input/external_emergency_stop_heartbeat` topic for health monitoring of the external module, and the vehicle_cmd_gate module will not start without the topic. -The `use_external_emergency_stop` parameter must be false when the "external emergency stop" function is not used. +The `check_external_emergency_heartbeat` parameter must be false when the "external emergency stop" function is not used. diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index 47aedbae68430..4ac592c1499f7 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -1,7 +1,7 @@ - + @@ -48,7 +48,7 @@ - + diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 2634949b8b577..36e12ab28a3a6 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -128,7 +128,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Parameter update_period_ = 1.0 / declare_parameter("update_rate", 10.0); use_emergency_handling_ = declare_parameter("use_emergency_handling", false); - use_external_emergency_stop_ = declare_parameter("use_external_emergency_stop", false); + check_external_emergency_heartbeat_ = + declare_parameter("check_external_emergency_heartbeat", false); system_emergency_heartbeat_timeout_ = declare_parameter("system_emergency_heartbeat_timeout", 0.5); external_emergency_stop_heartbeat_timeout_ = @@ -224,7 +225,7 @@ bool VehicleCmdGate::isDataReady() } } - if (use_external_emergency_stop_) { + if (check_external_emergency_heartbeat_) { if (!external_emergency_stop_heartbeat_received_time_) { RCLCPP_WARN(get_logger(), "external_emergency_stop_heartbeat_received_time_ is false"); return false; @@ -322,7 +323,7 @@ void VehicleCmdGate::onTimer() } // Check external emergency stop heartbeat - if (use_external_emergency_stop_) { + if (check_external_emergency_heartbeat_) { is_external_emergency_stop_heartbeat_timeout_ = isHeartbeatTimeout( external_emergency_stop_heartbeat_received_time_, external_emergency_stop_heartbeat_timeout_); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 2813af0261641..291d320efddf4 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -160,7 +160,7 @@ class VehicleCmdGate : public rclcpp::Node // Parameter double update_period_; bool use_emergency_handling_; - bool use_external_emergency_stop_; + bool check_external_emergency_heartbeat_; double system_emergency_heartbeat_timeout_; double external_emergency_stop_heartbeat_timeout_; double stop_hold_acceleration_; diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index e5754fce856e9..b02095623ae1d 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -209,7 +209,9 @@ def launch_setup(context, *args, **kwargs): vehicle_info_param, { "use_emergency_handling": LaunchConfiguration("use_emergency_handling"), - "use_external_emergency_stop": LaunchConfiguration("use_external_emergency_stop"), + "check_external_emergency_heartbeat": LaunchConfiguration( + "check_external_emergency_heartbeat" + ), "use_start_request": LaunchConfiguration("use_start_request"), }, ], @@ -374,7 +376,7 @@ def add_launch_arg(name: str, default_value=None, description=None): # 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") + 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