From 4ecf5f9c0b077dfbf83d5ed2c2d90928c7de4f44 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

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 .../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 b2d835778e93e..1ea48d8820fc1 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -39,6 +39,7 @@
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 struct HazardLampPolicy
 {
@@ -95,6 +96,9 @@ class MrmHandler : public rclcpp::Node
 
   void onOperationModeAvailability(
     const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg);
+  void onRecoverMrm(
+    const std_srvs::srv::Trigger::Request::SharedPtr,
+    const std_srvs::srv::Trigger::Response::SharedPtr response);
 
   // Publisher
 
@@ -119,6 +123,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 e8c057152ad94..aff0adc3c555f 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -69,6 +69,12 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : 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
   mrm_state_.stamp = this->now();
   mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL;
@@ -582,5 +588,17 @@ bool MrmHandler::isArrivedAtGoal()
   return 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;
+  }
+}
+
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(MrmHandler)