Skip to content

Commit

Permalink
fix: rename use_external_emergency_stop to `check_external_emergenc…
Browse files Browse the repository at this point in the history
…y_heartbeat` (#2455) (#2490)

* fix: rename use_external_emergency_stop to check_external_emergency_heartbeat

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
shmpwk and pre-commit-ci[bot] authored Dec 19, 2022
1 parent 35b0dfb commit ec35bb4
Show file tree
Hide file tree
Showing 5 changed files with 14 additions and 11 deletions.
6 changes: 3 additions & 3 deletions control/vehicle_cmd_gate/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
Expand All @@ -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.
4 changes: 2 additions & 2 deletions control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<launch>
<arg name="config_file" default="$(find-pkg-share vehicle_cmd_gate)/config/vehicle_cmd_gate.param.yaml"/>
<arg name="use_emergency_handling" default="false"/>
<arg name="use_external_emergency_stop" default="true"/>
<arg name="check_external_emergency_heartbeat" default="true"/>
<arg name="use_start_request" default="false"/>

<!-- vehicle info -->
Expand Down Expand Up @@ -48,7 +48,7 @@
<param from="$(var config_file)"/>
<param from="$(var vehicle_info_param_file)"/>
<param name="use_emergency_handling" value="$(var use_emergency_handling)"/>
<param name="use_external_emergency_stop" value="$(var use_external_emergency_stop)"/>
<param name="check_external_emergency_heartbeat" value="$(var check_external_emergency_heartbeat)"/>
<param name="use_start_request" value="$(var use_start_request)"/>
</node>
</launch>
7 changes: 4 additions & 3 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_ =
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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_);

Expand Down
2 changes: 1 addition & 1 deletion control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
6 changes: 4 additions & 2 deletions launch/tier4_control_launch/launch/control.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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"),
},
],
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit ec35bb4

Please sign in to comment.