implement service for clear MRM behavior

saka1-s · Sep 4, 2024 · 5c229fd · 5c229fd
1 parent 9d91137
commit 5c229fd
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 0 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
@@ -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,8 @@ 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 +138,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
@@ -86,6 +86,13 @@ 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 +626,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;
+  }
+}