feat(mrm_emergency_stop_operator): add support for real time param re…

…configure for mrm_emergency_stop (autowarefoundation#6994) add support for real time param reconfigure for mrm_emergency_stop Signed-off-by: Daniel Sanchez <[email protected]>
Interplai · Jun 3, 2024 · 2496e35 · 2496e35
1 parent 801c361
commit 2496e35
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 0 deletions.
diff --git a/...cy_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp b/...cy_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp
@@ -27,6 +27,7 @@
 // ROS 2 core
 #include <rclcpp/rclcpp.hpp>
 
+#include <vector>
 namespace mrm_emergency_stop_operator
 {
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
@@ -48,6 +49,10 @@ class MrmEmergencyStopOperator : public rclcpp::Node
 private:
   // Parameters
   Parameters params_;
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  rcl_interfaces::msg::SetParametersResult onParameter(
+    const std::vector<rclcpp::Parameter> & parameters);
 
   // Subscriber
   rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_control_cmd_;

diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml
@@ -17,6 +17,7 @@
   <depend>rclcpp_components</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/...rgency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/...rgency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp
@@ -14,6 +14,8 @@
 
 #include "mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp"
 
+#include <tier4_autoware_utils/ros/update_param.hpp>
+
 namespace mrm_emergency_stop_operator
 {
 
@@ -49,6 +51,23 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n
   // Initialize
   status_.state = MrmBehaviorStatus::AVAILABLE;
   is_prev_control_cmd_subscribed_ = false;
+
+  // Parameter Callback
+  set_param_res_ = add_on_set_parameters_callback(
+    std::bind(&MrmEmergencyStopOperator::onParameter, this, std::placeholders::_1));
+}
+
+rcl_interfaces::msg::SetParametersResult MrmEmergencyStopOperator::onParameter(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  using tier4_autoware_utils::updateParam;
+  updateParam<double>(parameters, "target_acceleration", params_.target_acceleration);
+  updateParam<double>(parameters, "target_jerk", params_.target_jerk);
+
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+  return result;
 }
 
 void MrmEmergencyStopOperator::onControlCommand(AckermannControlCommand::ConstSharedPtr msg)