diff --git a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp index 19e41397fe4da..5b91f56973ae9 100644 --- a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp +++ b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp @@ -27,6 +27,7 @@ // ROS 2 core #include +#include 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 & parameters); // Subscriber rclcpp::Subscription::SharedPtr sub_control_cmd_; diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml index 251e79a07a73d..6ca4e3a815f72 100644 --- a/system/mrm_emergency_stop_operator/package.xml +++ b/system/mrm_emergency_stop_operator/package.xml @@ -17,6 +17,7 @@ rclcpp_components std_msgs std_srvs + tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index 08814dbbd710d..9ee9d7a685df3 100644 --- a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/mrm_emergency_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 + 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 & parameters) +{ + using tier4_autoware_utils::updateParam; + updateParam(parameters, "target_acceleration", params_.target_acceleration); + updateParam(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)