From d5882c9cd528b3ef4d02e3c4b355837c884ec986 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 2 Jul 2024 21:45:32 +0900 Subject: [PATCH] feat(mrm_handler): operate mrm only when autonomous operation mode (#7784) * feat: add isOperationModeAutonomous() function to MRM handler core The code changes add a new function `isOperationModeAutonomous()` to the MRM handler core. This function is used to check if the operation mode is set to autonomous. Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../include/mrm_handler/mrm_handler_core.hpp | 3 ++- .../src/mrm_handler/mrm_handler_core.cpp | 15 ++++++++++++--- 2 files changed, 14 insertions(+), 4 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 c908ca1a62ee2..7d7deb8c4a504 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -148,7 +148,8 @@ class MrmHandler : public rclcpp::Node bool isStopped(); bool isDrivingBackwards(); bool isEmergency() const; - bool isAutonomous(); + bool isControlModeAutonomous(); + bool isOperationModeAutonomous(); bool isPullOverStatusAvailable(); bool isComfortableStopStatusAvailable(); bool isEmergencyStopStatusAvailable(); 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 b50bdc4f6a766..f2531a0556a0d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -396,12 +396,13 @@ void MrmHandler::updateMrmState() } // Get mode - const bool is_auto_mode = isAutonomous(); + const bool is_control_mode_autonomous = isControlModeAutonomous(); + const bool is_operation_mode_autonomous = isOperationModeAutonomous(); // State Machine switch (mrm_state_.state) { case MrmState::NORMAL: - if (is_auto_mode) { + if (is_control_mode_autonomous && is_operation_mode_autonomous) { transitionTo(MrmState::MRM_OPERATING); } return; @@ -537,7 +538,7 @@ bool MrmHandler::isEmergency() const is_operation_mode_availability_timeout; } -bool MrmHandler::isAutonomous() +bool MrmHandler::isControlModeAutonomous() { using autoware_vehicle_msgs::msg::ControlModeReport; auto mode = sub_control_mode_.takeData(); @@ -545,6 +546,14 @@ bool MrmHandler::isAutonomous() return mode->mode == ControlModeReport::AUTONOMOUS; } +bool MrmHandler::isOperationModeAutonomous() +{ + using autoware_adapi_v1_msgs::msg::OperationModeState; + auto state = sub_operation_mode_state_.takeData(); + if (state == nullptr) return false; + return state->mode == OperationModeState::AUTONOMOUS; +} + bool MrmHandler::isPullOverStatusAvailable() { auto status = sub_mrm_pull_over_status_.takeData();