From 23b53562eacf62f31753858303535b59436abf1b Mon Sep 17 00:00:00 2001
From: Shohei Sakai <saka1s.jp@gmail.com>
Date: Fri, 6 Sep 2024 15:28:09 +0900
Subject: [PATCH] feat: implement service for clear MRM behavior (#1511)

* implement service for clear MRM behavior

* clang format
---
 .../include/mrm_handler/mrm_handler_core.hpp   |  7 +++++++
 .../src/mrm_handler/mrm_handler_core.cpp       | 18 ++++++++++++++++++
 2 files changed, 25 insertions(+)

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 286151b90fdf5..c64a2f338f6ef 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -40,6 +40,7 @@
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 struct HazardLampPolicy
 {
@@ -105,6 +106,9 @@ class MrmHandler : public rclcpp::Node
     const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
   void onOperationModeState(
     const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg);
+  void onRecoverMrm(
+    const std_srvs::srv::Trigger::Request::SharedPtr,
+    const std_srvs::srv::Trigger::Response::SharedPtr response);
 
   // Publisher
 
@@ -135,6 +139,9 @@ class MrmHandler : public rclcpp::Node
   rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_;
   rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
 
+  // Services
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_recover_mrm_;
+
   bool requestMrmBehavior(
     const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
     RequestType request_type) const;
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 9d31c68f161b5..8569d31288139 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -86,6 +86,12 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
     "~/output/mrm/emergency_stop/operate", rmw_qos_profile_services_default,
     client_mrm_emergency_stop_group_);
 
+  // Services
+  service_recover_mrm_ = create_service<std_srvs::srv::Trigger>(
+    "/system/clear_mrm",
+    std::bind(&MrmHandler::onRecoverMrm, this, std::placeholders::_1, std::placeholders::_2),
+    rmw_qos_profile_services_default);
+
   // Initialize
   odom_ = std::make_shared<const nav_msgs::msg::Odometry>();
   control_mode_ = std::make_shared<const autoware_auto_vehicle_msgs::msg::ControlModeReport>();
@@ -619,3 +625,15 @@ bool MrmHandler::isArrivedAtGoal()
 
   return operation_mode_state_->mode == OperationModeState::STOP;
 }
+
+void MrmHandler::onRecoverMrm(
+  const std_srvs::srv::Trigger::Request::SharedPtr,
+  const std_srvs::srv::Trigger::Response::SharedPtr response)
+{
+  if (!param_.is_mrm_recoverable) {
+    is_mrm_holding_ = false;
+    response->success = true;
+  } else {
+    response->success = false;
+  }
+}