diff --git a/system/system_error_monitor/README.md b/system/system_error_monitor/README.md index d1bfbdc95488d..a62d4b0f00576 100644 --- a/system/system_error_monitor/README.md +++ b/system/system_error_monitor/README.md @@ -108,12 +108,13 @@ endif ### Node Parameters -| Name | Type | Default Value | Explanation | -| ---------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `ignore_missing_diagnostics` | bool | `false` | If this parameter is turned off, it will be ignored if required modules have not been received. | -| `add_leaf_diagnostics` | bool | `true` | Required to use children diagnostics. | -| `diag_timeout_sec` | double | `1.0` (sec) | If required diagnostic is not received for a `diag_timeout_sec`, the diagnostic state become STALE state. | -| `data_ready_timeout` | double | `30.0` | If input topics required for system_error_monitor are not available for `data_ready_timeout` seconds, autoware_state will translate to emergency state. | +| Name | Type | Default Value | Explanation | +| ---------------------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `ignore_missing_diagnostics` | bool | `false` | If this parameter is turned off, it will be ignored if required modules have not been received. | +| `add_leaf_diagnostics` | bool | `true` | Required to use children diagnostics. | +| `diag_timeout_sec` | double | `1.0` (sec) | If required diagnostic is not received for a `diag_timeout_sec`, the diagnostic state become STALE state. | +| `data_ready_timeout` | double | `30.0` | If input topics required for system_error_monitor are not available for `data_ready_timeout` seconds, autoware_state will translate to emergency state. | +| `data_heartbeat_timeout` | double | `1.0` | If input topics required for system_error_monitor are not no longer subscribed for `data_heartbeat_timeout` seconds, autoware_state will translate to emergency state. | ### Core Parameters diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 19c94cd21d45c..a170fa8b90742 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -71,6 +71,7 @@ class AutowareErrorMonitor : public rclcpp::Node bool ignore_missing_diagnostics; bool add_leaf_diagnostics; double data_ready_timeout; + double data_heartbeat_timeout; double diag_timeout_sec; double hazard_recovery_timeout; int emergency_hazard_level; @@ -92,6 +93,7 @@ class AutowareErrorMonitor : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; bool isDataReady(); + bool isDataHeartbeatTimeout(); void onTimer(); // Subscriber @@ -125,6 +127,12 @@ class AutowareErrorMonitor : public rclcpp::Node [[maybe_unused]] std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response); + // for Heartbeat + rclcpp::Time diag_array_stamp_; + rclcpp::Time autoware_state_stamp_; + rclcpp::Time current_gate_mode_stamp_; + rclcpp::Time control_mode_stamp_; + // Algorithm boost::optional getLatestDiag(const std::string & diag_name) const; uint8_t getHazardLevel(const DiagConfig & required_module, const int diag_level) const; diff --git a/system/system_error_monitor/launch/system_error_monitor.launch.xml b/system/system_error_monitor/launch/system_error_monitor.launch.xml index 76d89c915a8e4..9eb805b0eb2a6 100644 --- a/system/system_error_monitor/launch/system_error_monitor.launch.xml +++ b/system/system_error_monitor/launch/system_error_monitor.launch.xml @@ -3,6 +3,7 @@ + @@ -45,6 +46,7 @@ + diff --git a/system/system_error_monitor/launch/system_error_monitor_node.launch.xml b/system/system_error_monitor/launch/system_error_monitor_node.launch.xml index 161f29f5ee626..6741b4f22ce48 100644 --- a/system/system_error_monitor/launch/system_error_monitor_node.launch.xml +++ b/system/system_error_monitor/launch/system_error_monitor_node.launch.xml @@ -3,6 +3,7 @@ + @@ -23,6 +24,7 @@ + diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 1a0b696bb36f4..9240563741015 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -234,6 +234,7 @@ AutowareErrorMonitor::AutowareErrorMonitor() get_parameter_or("ignore_missing_diagnostics", params_.ignore_missing_diagnostics, false); get_parameter_or("add_leaf_diagnostics", params_.add_leaf_diagnostics, true); get_parameter_or("data_ready_timeout", params_.data_ready_timeout, 30.0); + get_parameter_or("data_heartbeat_timeout", params_.data_heartbeat_timeout, 1.0); get_parameter_or("diag_timeout_sec", params_.diag_timeout_sec, 1.0); get_parameter_or("hazard_recovery_timeout", params_.hazard_recovery_timeout, 5.0); get_parameter_or( @@ -363,24 +364,36 @@ void AutowareErrorMonitor::onDiagArray( diag_buffer.pop_front(); } } + + // for Heartbeat + diag_array_stamp_ = this->now(); } void AutowareErrorMonitor::onCurrentGateMode( const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) { current_gate_mode_ = msg; + + // for Heartbeat + current_gate_mode_stamp_ = this->now(); } void AutowareErrorMonitor::onAutowareState( const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg) { autoware_state_ = msg; + + // for Heartbeat + autoware_state_stamp_ = this->now(); } void AutowareErrorMonitor::onControlMode( const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; + + // for Heartbeat + control_mode_stamp_ = this->now(); } bool AutowareErrorMonitor::isDataReady() @@ -408,6 +421,37 @@ bool AutowareErrorMonitor::isDataReady() return true; } +bool AutowareErrorMonitor::isDataHeartbeatTimeout() +{ + auto isTimeout = [this](const rclcpp::Time & last_stamp, const double threshold) { + const auto time_diff = this->now() - last_stamp; + return time_diff.seconds() > threshold; + }; + + if (isTimeout(diag_array_stamp_, params_.data_heartbeat_timeout)) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "diag_array msg is timeout..."); + return true; + } + + if (isTimeout(current_gate_mode_stamp_, params_.data_heartbeat_timeout)) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "current_gate_mode msg is timeout..."); + return true; + } + + if (isTimeout(autoware_state_stamp_, params_.data_heartbeat_timeout)) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "autoware_state msg is timeout..."); + return true; + } + + if (isTimeout(control_mode_stamp_, params_.data_heartbeat_timeout)) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "vehicle_state_report msg is timeout..."); + return true; + } + + return false; +} + void AutowareErrorMonitor::onTimer() { if (!isDataReady()) { @@ -420,6 +464,11 @@ void AutowareErrorMonitor::onTimer() return; } + if (isDataHeartbeatTimeout()) { + publishHazardStatus(createTimeoutHazardStatus()); + return; + } + current_mode_ = current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO ? KeyName::autonomous_driving : KeyName::external_control;