Skip to content

Commit c96a7f1

Browse files
authored
fix(emergency_handler): fix hazard status timeout (autowarefoundation#6399)
* fix(emergency_handler): fix hazard status timeout Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
1 parent f9dd38c commit c96a7f1

File tree

2 files changed

+27
-16
lines changed

2 files changed

+27
-16
lines changed

system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -131,14 +131,16 @@ class EmergencyHandler : public rclcpp::Node
131131

132132
// Heartbeat
133133
rclcpp::Time stamp_hazard_status_;
134+
bool is_hazard_status_timeout_;
135+
void checkHazardStatusTimeout();
134136

135137
// Algorithm
136138
void transitionTo(const int new_state);
137139
void updateMrmState();
138140
void operateMrm();
139141
autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
140142
bool isStopped();
141-
bool isEmergency(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status);
143+
bool isEmergency();
142144
};
143145

144146
#endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

+24-15
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
8383
mrm_state_.stamp = this->now();
8484
mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL;
8585
mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
86+
is_hazard_status_timeout_ = false;
8687

8788
// Timer
8889
const auto update_period_ns = rclcpp::Rate(param_.update_rate).period();
@@ -135,7 +136,7 @@ autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHaz
135136
HazardLightsCommand msg;
136137

137138
// Check emergency
138-
const bool is_emergency = isEmergency(hazard_status_stamped_->status);
139+
const bool is_emergency = isEmergency();
139140

140141
if (hazard_status_stamped_->status.emergency_holding) {
141142
// turn hazard on during emergency holding
@@ -309,22 +310,27 @@ bool EmergencyHandler::isDataReady()
309310
return true;
310311
}
311312

312-
void EmergencyHandler::onTimer()
313+
void EmergencyHandler::checkHazardStatusTimeout()
313314
{
314-
if (!isDataReady()) {
315-
return;
316-
}
317-
const bool is_hazard_status_timeout =
318-
(this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status;
319-
if (is_hazard_status_timeout) {
315+
if ((this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status) {
316+
is_hazard_status_timeout_ = true;
320317
RCLCPP_WARN_THROTTLE(
321318
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
322319
"heartbeat_hazard_status is timeout");
323-
mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING;
324-
publishControlCommands();
320+
} else {
321+
is_hazard_status_timeout_ = false;
322+
}
323+
}
324+
325+
void EmergencyHandler::onTimer()
326+
{
327+
if (!isDataReady()) {
325328
return;
326329
}
327330

331+
// Check whether heartbeat hazard_status is timeout
332+
checkHazardStatusTimeout();
333+
328334
// Update Emergency State
329335
updateMrmState();
330336

@@ -369,7 +375,7 @@ void EmergencyHandler::updateMrmState()
369375
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
370376

371377
// Check emergency
372-
const bool is_emergency = isEmergency(hazard_status_stamped_->status);
378+
const bool is_emergency = isEmergency();
373379

374380
// Get mode
375381
const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
@@ -412,7 +418,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre
412418
using autoware_auto_system_msgs::msg::HazardStatus;
413419

414420
// Get hazard level
415-
const auto level = hazard_status_stamped_->status.level;
421+
auto level = hazard_status_stamped_->status.level;
422+
if (is_hazard_status_timeout_) {
423+
level = HazardStatus::SINGLE_POINT_FAULT;
424+
}
416425

417426
// State machine
418427
if (mrm_state_.behavior == MrmState::NONE) {
@@ -435,10 +444,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre
435444
return mrm_state_.behavior;
436445
}
437446

438-
bool EmergencyHandler::isEmergency(
439-
const autoware_auto_system_msgs::msg::HazardStatus & hazard_status)
447+
bool EmergencyHandler::isEmergency()
440448
{
441-
return hazard_status.emergency || hazard_status.emergency_holding;
449+
return hazard_status_stamped_->status.emergency ||
450+
hazard_status_stamped_->status.emergency_holding || is_hazard_status_timeout_;
442451
}
443452

444453
bool EmergencyHandler::isStopped()

0 commit comments

Comments
 (0)