feat: use mrm in any operation mode

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -135,7 +135,7 @@ class MrmHandler : public rclcpp::Node
 
   // Heartbeat
   rclcpp::Time stamp_operation_mode_availability_;
-  std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt;
+  std::optional<rclcpp::Time> stamp_current_operation_mode_become_unavailable_ = std::nullopt;
   bool is_operation_mode_availability_timeout;
   void checkOperationModeAvailabilityTimeout();
 
@@ -148,13 +148,14 @@ class MrmHandler : public rclcpp::Node
   void handleFailedRequest();
   autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
-  bool isEmergency() const;
+  bool isEmergency();
   bool isControlModeAutonomous();
-  bool isOperationModeAutonomous();
   bool isPullOverStatusAvailable();
   bool isComfortableStopStatusAvailable();
   bool isEmergencyStopStatusAvailable();
   bool isArrivedAtGoal();
+  bool isAvailableCurrentOperationMode();
+  autoware_adapi_v1_msgs::msg::OperationModeState::_mode_type getCurrentOperationMode();
 };
 
 #endif  // MRM_HANDLER__MRM_HANDLER_CORE_HPP_

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

@@ -83,27 +83,27 @@ void MrmHandler::onOperationModeAvailability(
 {
   stamp_operation_mode_availability_ = this->now();
   operation_mode_availability_ = msg;
-  const bool skip_emergency_holding_check =
-    !param_.use_emergency_holding || is_emergency_holding_ || !isOperationModeAutonomous();
+  const bool skip_emergency_holding_check = !param_.use_emergency_holding || is_emergency_holding_;
 
   if (skip_emergency_holding_check) {
     return;
   }
 
-  if (msg->autonomous) {
-    stamp_autonomous_become_unavailable_.reset();
+  if (isAvailableCurrentOperationMode()) {
+    stamp_current_operation_mode_become_unavailable_.reset();
     return;
   }
 
   // If no timestamp is available, the ego autonomous mode just became unavailable and the current
   // time is recorded.
-  stamp_autonomous_become_unavailable_ = (!stamp_autonomous_become_unavailable_.has_value())
-                                           ? this->now()
-                                           : stamp_autonomous_become_unavailable_;
+  stamp_current_operation_mode_become_unavailable_ =
+    (!stamp_current_operation_mode_become_unavailable_.has_value())
+      ? this->now()
+      : stamp_current_operation_mode_become_unavailable_;
 
   // Check if autonomous mode unavailable time is larger than timeout threshold.
   const auto emergency_duration =
-    (this->now() - stamp_autonomous_become_unavailable_.value()).seconds();
+    (this->now() - stamp_current_operation_mode_become_unavailable_.value()).seconds();
   is_emergency_holding_ = (emergency_duration > param_.timeout_emergency_recovery);
 }
 
@@ -399,12 +399,11 @@ void MrmHandler::updateMrmState()
 
   // Get mode
   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_control_mode_autonomous && is_operation_mode_autonomous) {
+      if (is_control_mode_autonomous) {
         transitionTo(MrmState::MRM_OPERATING);
       }
       return;
@@ -526,9 +525,9 @@ bool MrmHandler::isStopped()
   return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity);
 }
 
-bool MrmHandler::isEmergency() const
+bool MrmHandler::isEmergency()
 {
-  return !operation_mode_availability_->autonomous || is_emergency_holding_ ||
+  return !isAvailableCurrentOperationMode() || is_emergency_holding_ ||
          is_operation_mode_availability_timeout;
 }
 
@@ -540,14 +539,6 @@ bool MrmHandler::isControlModeAutonomous()
   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();
@@ -577,5 +568,32 @@ bool MrmHandler::isArrivedAtGoal()
   return state->mode == OperationModeState::STOP;
 }
 
+bool MrmHandler::isAvailableCurrentOperationMode()
+{
+  auto operation_mode = getCurrentOperationMode();
+  switch (operation_mode) {
+    case autoware_adapi_v1_msgs::msg::OperationModeState::UNKNOWN:
+      return false;
+    case autoware_adapi_v1_msgs::msg::OperationModeState::STOP:
+      return operation_mode_availability_->stop;
+    case autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS:
+      return operation_mode_availability_->autonomous;
+    case autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL:
+      return operation_mode_availability_->local;
+    case autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE:
+      return operation_mode_availability_->remote;
+    default:
+      RCLCPP_WARN(this->get_logger(), "invalid operation mode: %d", operation_mode);
+      return false;
+  }
+}
+
+autoware_adapi_v1_msgs::msg::OperationModeState::_mode_type MrmHandler::getCurrentOperationMode()
+{
+  auto state = sub_operation_mode_state_.takeData();
+  if (state == nullptr) return autoware_adapi_v1_msgs::msg::OperationModeState::UNKNOWN;
+  return state->mode;
+}
+
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(MrmHandler)