@@ -83,6 +83,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
83
83
mrm_state_.stamp = this ->now ();
84
84
mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL;
85
85
mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
86
+ is_hazard_status_timeout_ = false ;
86
87
87
88
// Timer
88
89
const auto update_period_ns = rclcpp::Rate (param_.update_rate ).period ();
@@ -135,7 +136,7 @@ autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHaz
135
136
HazardLightsCommand msg;
136
137
137
138
// Check emergency
138
- const bool is_emergency = isEmergency (hazard_status_stamped_-> status );
139
+ const bool is_emergency = isEmergency ();
139
140
140
141
if (hazard_status_stamped_->status .emergency_holding ) {
141
142
// turn hazard on during emergency holding
@@ -309,22 +310,27 @@ bool EmergencyHandler::isDataReady()
309
310
return true ;
310
311
}
311
312
312
- void EmergencyHandler::onTimer ()
313
+ void EmergencyHandler::checkHazardStatusTimeout ()
313
314
{
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 ;
320
317
RCLCPP_WARN_THROTTLE (
321
318
this ->get_logger (), *this ->get_clock (), std::chrono::milliseconds (1000 ).count (),
322
319
" 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 ()) {
325
328
return ;
326
329
}
327
330
331
+ // Check whether heartbeat hazard_status is timeout
332
+ checkHazardStatusTimeout ();
333
+
328
334
// Update Emergency State
329
335
updateMrmState ();
330
336
@@ -369,7 +375,7 @@ void EmergencyHandler::updateMrmState()
369
375
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
370
376
371
377
// Check emergency
372
- const bool is_emergency = isEmergency (hazard_status_stamped_-> status );
378
+ const bool is_emergency = isEmergency ();
373
379
374
380
// Get mode
375
381
const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
@@ -412,7 +418,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre
412
418
using autoware_auto_system_msgs::msg::HazardStatus;
413
419
414
420
// 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
+ }
416
425
417
426
// State machine
418
427
if (mrm_state_.behavior == MrmState::NONE) {
@@ -435,10 +444,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre
435
444
return mrm_state_.behavior ;
436
445
}
437
446
438
- bool EmergencyHandler::isEmergency (
439
- const autoware_auto_system_msgs::msg::HazardStatus & hazard_status)
447
+ bool EmergencyHandler::isEmergency ()
440
448
{
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_;
442
451
}
443
452
444
453
bool EmergencyHandler::isStopped ()
0 commit comments