From 7f320aa56e3057dfe9260a026b9dd9750a3061ba Mon Sep 17 00:00:00 2001
From: veqcc <ryuta.kambe@tier4.jp>
Date: Thu, 29 Feb 2024 13:47:47 +0900
Subject: [PATCH] fix operation mode availability timeout
Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
---
.../include/mrm_handler/mrm_handler_core.hpp | 2 ++
.../src/mrm_handler/mrm_handler_core.cpp | 31 ++++++++++++-------
2 files changed, 21 insertions(+), 12 deletions(-)
diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
index 8a6ec81cbafff..4b9df373401c5 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -143,6 +143,8 @@ class MrmHandler : public rclcpp::Node
// Heartbeat
rclcpp::Time stamp_operation_mode_availability_;
std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt;
+ bool is_operation_mode_availability_timeout;
+ void checkOperationModeAvailabilityTimeout();
// Algorithm
rclcpp::Time takeover_requested_time_;
diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
index 976f5b3164abd..9ebdfaa0e43b0 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -93,6 +93,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
mrm_state_.stamp = this->now();
mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL;
mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
+ is_operation_mode_availability_timeout = false;
// Timer
const auto update_period_ns = rclcpp::Rate(param_.update_rate).period();
@@ -369,22 +370,26 @@ bool MrmHandler::isDataReady()
return true;
}
+void MrmHandler::checkOperationModeAvailabilityTimeout() {
+ if ((this->now() - stamp_operation_mode_availability_).seconds() >
+ param_.timeout_operation_mode_availability) {
+ is_operation_mode_availability_timeout = true;
+ RCLCPP_WARN_THROTTLE(
+ this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
+ "operation_mode_availability is timeout");
+ } else {
+ is_operation_mode_availability_timeout = false;
+ }
+}
+
void MrmHandler::onTimer()
{
if (!isDataReady()) {
return;
}
- const bool is_operation_mode_availability_timeout =
- (this->now() - stamp_operation_mode_availability_).seconds() >
- param_.timeout_operation_mode_availability;
- if (is_operation_mode_availability_timeout) {
- RCLCPP_WARN_THROTTLE(
- this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
- "heartbeat operation_mode_availability is timeout");
- mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING;
- publishControlCommands();
- return;
- }
+
+ // Check whether operation_mode_availability is timeout
+ checkOperationModeAvailabilityTimeout();
// Update Emergency State
updateMrmState();
@@ -576,7 +581,9 @@ bool MrmHandler::isStopped()
bool MrmHandler::isEmergency() const
{
- return !operation_mode_availability_->autonomous || is_emergency_holding_;
+ return !operation_mode_availability_->autonomous ||
+ is_emergency_holding_ ||
+ is_operation_mode_availability_timeout;
}
bool MrmHandler::isArrivedAtGoal()