From 54972ac55f85e3b0dd560d0d8d808d7f2a5075ad Mon Sep 17 00:00:00 2001
From: Shohei Sakai <saka1s.jp@gmail.com>
Date: Wed, 12 Jun 2024 11:35:26 +0900
Subject: [PATCH] daynamic reconfigurable mrm_holding parameter

---
 .../include/mrm_handler/mrm_handler_core.hpp     |  8 ++++++++
 system/mrm_handler/package.xml                   |  1 +
 .../src/mrm_handler/mrm_handler_core.cpp         | 16 ++++++++++++++++
 3 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 fef9cd6f92387..286151b90fdf5 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -20,8 +20,11 @@
 #include <optional>
 #include <string>
 #include <variant>
+#include <vector>
 
 // Autoware
+#include <tier4_autoware_utils/ros/update_param.hpp>
+
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
@@ -119,6 +122,11 @@ class MrmHandler : public rclcpp::Node
   autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
   void publishMrmState();
 
+  // parameter callback
+  rcl_interfaces::msg::SetParametersResult onParam(
+    const std::vector<rclcpp::Parameter> & parameters);
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
   // Clients
   rclcpp::CallbackGroup::SharedPtr client_mrm_pull_over_group_;
   rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_pull_over_;
diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml
index e62091f2984e6..3666cdf35f37b 100644
--- a/system/mrm_handler/package.xml
+++ b/system/mrm_handler/package.xml
@@ -20,6 +20,7 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>tier4_autoware_utils</depend>
   <depend>tier4_system_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
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 a0929ee95cc5b..9d31c68f161b5 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -103,6 +103,9 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
   const auto update_period_ns = rclcpp::Rate(param_.update_rate).period();
   timer_ = rclcpp::create_timer(
     this, get_clock(), update_period_ns, std::bind(&MrmHandler::onTimer, this));
+
+  // set parameter callback
+  set_param_res_ = this->add_on_set_parameters_callback(std::bind(&MrmHandler::onParam, this, _1));
 }
 
 void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
@@ -168,6 +171,19 @@ void MrmHandler::onOperationModeState(
   operation_mode_state_ = msg;
 }
 
+rcl_interfaces::msg::SetParametersResult MrmHandler::onParam(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  using tier4_autoware_utils::updateParam;
+  updateParam<bool>(parameters, "is_mrm_recoverable", param_.is_mrm_recoverable);
+  if (param_.is_mrm_recoverable) is_mrm_holding_ = false;
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+
+  return result;
+}
+
 void MrmHandler::publishHazardCmd()
 {
   using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;