diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index d31ad89..3324e97 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -154,7 +155,7 @@ public: { std::vector results; for (auto p : parameters) { -// parameters_[p.name] = ParameterVariant::from_parameter_value(p.value); + parameters_[p.name] = rclcpp::parameter::ParameterVariant::from_parameter(p); rcl_interfaces::SetParametersResult result; result.successful = true; // TODO: handle parameter constraints @@ -185,7 +186,7 @@ private: std::mutex mutex_; -// std::map parameters_; + std::map parameters_; template< typename ServiceT, diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index bb27281..bc0a4b3 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -170,6 +170,24 @@ public: return get_value(); } + static ParameterVariant from_parameter(const rcl_interfaces::Parameter & parameter) { + switch(parameter.value.parameter_type) { + case PARAMETER_BOOL: + return ParameterVariant(parameter.name, parameter.value.bool_value); + case PARAMETER_INTEGER: + return ParameterVariant(parameter.name, parameter.value.integer_value); + case PARAMETER_DOUBLE: + return ParameterVariant(parameter.name, parameter.value.double_value); + case PARAMETER_STRING: + return ParameterVariant(parameter.name, parameter.value.string_value); + case PARAMETER_BYTES: + return ParameterVariant(parameter.name, parameter.value.bytes_value); + case PARAMETER_NOT_SET: + default: + // TODO: use custom exception + throw std::runtime_error("Invalid type from ParameterValue"); + } + } private: std::string name_; rcl_interfaces::ParameterValue value_;