feat: change state machine and add isArrivedAtGoal

system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

@@ -30,6 +30,7 @@
 #include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
 #include <tier4_system_msgs/msg/operation_mode_availability.hpp>
 #include <tier4_system_msgs/srv/operate_mrm.hpp>
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 
 // ROS 2 core
 #include <rclcpp/create_timer.hpp>
@@ -77,6 +78,8 @@ class EmergencyHandler : public rclcpp::Node
     sub_mrm_comfortable_stop_status_;
   rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
     sub_mrm_emergency_stop_status_;
+  rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
+    sub_operation_mode_state_;
 
   autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_;
   nav_msgs::msg::Odometry::ConstSharedPtr odom_;
@@ -85,6 +88,7 @@ class EmergencyHandler : public rclcpp::Node
   tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_;
   tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_;
   tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_;
+  autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_;
 
   void onPrevControlCommand(
     const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
@@ -97,6 +101,8 @@ class EmergencyHandler : public rclcpp::Node
     const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
   void onMrmEmergencyStopStatus(
     const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
+  void onOperationModeState(
+    const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg);
 
   // Publisher
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
@@ -156,6 +162,7 @@ class EmergencyHandler : public rclcpp::Node
   autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
   bool isEmergency() const;
+  bool isArrivedAtGoal();
 };
 
 #endif  // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_

system/emergency_handler/launch/emergency_handler.launch.xml

@@ -7,6 +7,7 @@
   <arg name="input_mrm_pull_over_state" default="/system/mrm/pull_over/status"/>
   <arg name="input_mrm_comfortable_stop_state" default="/system/mrm/comfortable_stop/status"/>
   <arg name="input_mrm_emergency_stop_state" default="/system/mrm/emergency_stop/status"/>
+  <arg name="input_api_operation_mode_state" default="/api/operation_mode/state"/>
 
   <arg name="output_gear" default="/system/emergency/gear_cmd"/>
   <arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
@@ -26,6 +27,7 @@
     <remap from="~/input/mrm/pull_over/status" to="$(var input_mrm_pull_over_state)"/>
     <remap from="~/input/mrm/comfortable_stop/status" to="$(var input_mrm_comfortable_stop_state)"/>
     <remap from="~/input/mrm/emergency_stop/status" to="$(var input_mrm_emergency_stop_state)"/>
+    <remap from="~/input/api/operation_mode/state" to="$(var input_api_operation_mode_state)"/>
 
     <remap from="~/output/gear" to="$(var output_gear)"/>
     <remap from="~/output/hazard" to="$(var output_hazard)"/>

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

@@ -58,6 +58,9 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   sub_mrm_emergency_stop_status_ = create_subscription<tier4_system_msgs::msg::MrmBehaviorStatus>(
     "~/input/mrm/emergency_stop/status", rclcpp::QoS{1},
     std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1));
+  sub_operation_mode_state_ = create_subscription<autoware_adapi_v1_msgs::msg::OperationModeState>(
+    "~/input/api/operation_mode/state", rclcpp::QoS{1},
+    std::bind(&EmergencyHandler::onOperationModeState, this, _1));
 
   // Publisher
   pub_control_command_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
@@ -94,6 +97,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   mrm_comfortable_stop_status_ =
     std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>();
   mrm_emergency_stop_status_ = std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>();
+  operation_mode_state_ = std::make_shared<const autoware_adapi_v1_msgs::msg::OperationModeState>();
   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;
@@ -170,6 +174,12 @@ void EmergencyHandler::onMrmEmergencyStopStatus(
   mrm_emergency_stop_status_ = msg;
 }
 
+void EmergencyHandler::onOperationModeState(
+    const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg)
+{
+  operation_mode_state_ = msg;
+}
+
 autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg()
 {
   using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
@@ -481,6 +491,12 @@ void EmergencyHandler::updateMrmState()
       }
     } else if (mrm_state_.state == MrmState::MRM_OPERATING) {
       // TODO(Kenji Miyake): Check MRC is accomplished
+      if (mrm_state_.behavior == MrmState::PULL_OVER) {
+        if (isStopped() && isArrivedAtGoal()) {
+          transitionTo(MrmState::MRM_SUCCEEDED);
+          return;
+        }
+      }
       if (isStopped()) {
         transitionTo(MrmState::MRM_SUCCEEDED);
         return;
@@ -519,20 +535,47 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre
     return MrmState::EMERGENCY_STOP;
   }
   if (mrm_state_.behavior == MrmState::PULL_OVER) {
-    if (!operation_mode_availability_->pull_over) {
-      if (!operation_mode_availability_->emergency_stop) {
-        RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
+    if (operation_mode_availability_->pull_over) {
+      if (param_.use_pull_over) {
+        return MrmState::PULL_OVER;
       }
-      return MrmState::EMERGENCY_STOP;
     }
+    if (operation_mode_availability_->comfortable_stop) {
+      if (param_.use_comfortable_stop) {
+        return MrmState::COMFORTABLE_STOP;
+      }
+    }
+    if (!operation_mode_availability_->emergency_stop) {
+      RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
+    }
+    return MrmState::EMERGENCY_STOP;
   }
   if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) {
-    if (!operation_mode_availability_->comfortable_stop) {
-      if (!operation_mode_availability_->emergency_stop) {
-        RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
+    if (isStopped() && operation_mode_availability_->pull_over) {
+      if (param_.use_pull_over) {
+        return MrmState::PULL_OVER;
       }
-      return MrmState::EMERGENCY_STOP;
     }
+    if (operation_mode_availability_->comfortable_stop) {
+      if (param_.use_comfortable_stop) {
+        return MrmState::COMFORTABLE_STOP;
+      }
+    }
+    if (!operation_mode_availability_->emergency_stop) {
+      RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
+    }
+    return MrmState::EMERGENCY_STOP;
+  }
+  if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) {
+    if (isStopped() && operation_mode_availability_->pull_over) {
+      if (param_.use_pull_over) {
+        return MrmState::PULL_OVER;
+      }
+    }
+    if (!operation_mode_availability_->emergency_stop) {
+      RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
+    }
+    return MrmState::EMERGENCY_STOP;
   }
 
   return mrm_state_.behavior;
@@ -552,3 +595,14 @@ bool EmergencyHandler::isEmergency() const
 {
   return !operation_mode_availability_->autonomous | is_emergency_holding_;
 }
+
+bool EmergencyHandler::isArrivedAtGoal()
+{
+  using autoware_adapi_v1_msgs::msg::OperationModeState;
+
+  if (operation_mode_state_->mode == OperationModeState::STOP){
+    return true;
+  }
+
+  return false;
+}