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 <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · Jul 2, 2024 · d5882c9 · d5882c9
1 parent ce09907
commit d5882c9
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 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
@@ -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
@@ -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,14 +538,22 @@ 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();
   if (mode == nullptr) return false;
   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();