daynamic reconfigurable mrm_holding parameter

saka1-s · Jun 12, 2024 · 54972ac · 54972ac
1 parent 4388a51
commit 54972ac
Show file tree

Hide file tree

Showing 3 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
@@ -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
@@ -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
@@ -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;