diff --git a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp index 7856c23fa3158..eb84191f481f9 100644 --- a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp +++ b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp @@ -17,6 +17,7 @@ // Core #include +#include // Autoware #include @@ -55,6 +56,9 @@ class MrmComfortableStopOperator : public rclcpp::Node const tier4_system_msgs::srv::OperateMrm::Request::SharedPtr request, const tier4_system_msgs::srv::OperateMrm::Response::SharedPtr response); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + // Publisher rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; @@ -72,6 +76,9 @@ class MrmComfortableStopOperator : public rclcpp::Node // States tier4_system_msgs::msg::MrmBehaviorStatus status_; + + // Parameter callback + OnSetParametersCallbackHandle::SharedPtr set_param_res_; }; } // namespace mrm_comfortable_stop_operator diff --git a/system/mrm_comfortable_stop_operator/package.xml b/system/mrm_comfortable_stop_operator/package.xml index 67211c95d4bdf..92a9569bf329a 100644 --- a/system/mrm_comfortable_stop_operator/package.xml +++ b/system/mrm_comfortable_stop_operator/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_universe_utils rclcpp rclcpp_components std_msgs diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp index 5c9562463f891..6b18555053582 100644 --- a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +++ b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp @@ -14,6 +14,8 @@ #include "mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp" +#include + namespace mrm_comfortable_stop_operator { @@ -48,6 +50,10 @@ MrmComfortableStopOperator::MrmComfortableStopOperator(const rclcpp::NodeOptions // Initialize status_.state = tier4_system_msgs::msg::MrmBehaviorStatus::AVAILABLE; + + // Parameter Callback + set_param_res_ = add_on_set_parameters_callback( + std::bind(&MrmComfortableStopOperator::onParameter, this, std::placeholders::_1)); } void MrmComfortableStopOperator::operateComfortableStop( @@ -65,6 +71,20 @@ void MrmComfortableStopOperator::operateComfortableStop( } } +rcl_interfaces::msg::SetParametersResult MrmComfortableStopOperator::onParameter( + const std::vector & parameters) +{ + using autoware::universe_utils::updateParam; + updateParam(parameters, "min_acceleration", params_.min_acceleration); + updateParam(parameters, "max_jerk", params_.max_jerk); + updateParam(parameters, "min_jerk", params_.min_jerk); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + void MrmComfortableStopOperator::publishStatus() const { auto status = status_;