From 333cfe6ccd5bb18d9d88e270bb66a265edae3217 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 15 Apr 2015 17:17:16 -0700 Subject: [PATCH] Refactored get_param to use get_params. Renamed param to parameter. Update code to latest rcl_interfaces --- rclcpp/include/rclcpp/node.hpp | 37 +++--- rclcpp/include/rclcpp/node_impl.hpp | 169 +++++++++++++++------------- rclcpp/include/rclcpp/parameter.hpp | 108 +++++++++--------- 3 files changed, 161 insertions(+), 153 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 4a1929f541..9b3cfab028 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -122,29 +122,30 @@ class Node template std::shared_future - async_get_param( - const std::string& node_name, const parameter::ParamName& key, + async_get_parameter( + const std::string& node_name, const parameter::ParameterName& key, std::function)> callback=nullptr); - std::shared_future< std::vector > - async_get_params( - const std::string& node_name, const std::vector& parameter_names, - std::function >)> callback=nullptr); + std::shared_future< std::vector > + async_get_parameters( + const std::string& node_name, const std::vector& parameter_names, + std::function >)> callback=nullptr); std::shared_future - async_has_param(const std::string& node_name, const parameter::ParamQuery& query, - std::function)> callback=nullptr); + async_has_parameter( + const std::string& node_name, const parameter::ParameterQuery& query, + std::function)> callback=nullptr); template std::shared_future - async_set_param( - const std::string& node_name, const parameter::ParamName& key, + async_set_parameter( + const std::string& node_name, const parameter::ParameterName& key, const ParamTypeT& value, std::function)> callback=nullptr); std::shared_future - async_set_params( + async_set_parameters( const std::string& node_name, - const std::vector& key_values, + const std::vector& key_values, std::function)> callback=nullptr); private: @@ -167,19 +168,19 @@ class Node size_t number_of_services_; size_t number_of_clients_; - std::map params_; + std::map params_; template - ParamTypeT get_param(const parameter::ParamName& key) const; + ParamTypeT get_parameter(const parameter::ParameterName& key) const; - std::vector - get_params(const std::vector& query) const; + std::vector + get_parameters(const std::vector& query) const; bool - has_param(const parameter::ParamQuery& query) const; + has_parameter(const parameter::ParameterQuery& query) const; template - void set_param(const parameter::ParamName& key, const ParamTypeT& value); + void set_parameter(const parameter::ParameterName& key, const ParamTypeT& value); }; } /* namespace node */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index f1653f341a..d27b329403 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -26,11 +26,11 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #ifndef RCLCPP_RCLCPP_NODE_HPP_ #include "node.hpp" @@ -229,35 +229,35 @@ Node::create_service( template ParamTypeT -Node::get_param(const parameter::ParamName& key) const +Node::get_parameter(const parameter::ParameterName& key) const { - const parameter::ParamContainer pc(this->params_.at(key)); + const parameter::ParameterContainer pc(this->params_.at(key)); return pc.get_value(); } template void -Node::set_param(const parameter::ParamName& key, const ParamTypeT& value) +Node::set_parameter(const parameter::ParameterName& key, const ParamTypeT& value) { - parameter::ParamContainer pc(key, value); + parameter::ParameterContainer pc(key, value); params_[key] = pc; } bool -Node::has_param(const parameter::ParamQuery& query) const +Node::has_parameter(const parameter::ParameterQuery& query) const { - const parameter::ParamName key = query.get_name(); + const parameter::ParameterName key = query.get_name(); return (params_.find(key) != params_.end()); } -std::vector -Node::get_params(const std::vector& queries) const +std::vector +Node::get_parameters(const std::vector& queries) const { - std::vector result; + std::vector result; for(auto& kv: params_) { if(std::any_of(queries.cbegin(), queries.cend(), - [&kv](const parameter::ParamQuery& query) { + [&kv](const parameter::ParameterQuery& query) { return kv.first == query.get_name();})) { result.push_back(kv.second); } @@ -267,49 +267,53 @@ Node::get_params(const std::vector& queries) const template std::shared_future -Node::async_get_param( - const std::string& node_name, const parameter::ParamName& key, +Node::async_get_parameter( + const std::string& node_name, const parameter::ParameterName& key, std::function)> callback) { std::promise promise_result; auto future_result = promise_result.get_future().share(); - std::vector param_names = {{ key }}; + std::vector param_names = {{ key }}; - this->async_get_params( + this->async_get_parameters( node_name, param_names, [&promise_result, &future_result, &callback, ¶m_names]( - std::shared_future >) {}); - + std::shared_future > cb_f) { + promise_result.set_value(cb_f.get()[0].get_value()); + if(callback != nullptr) { + callback(future_result); + } + }); return future_result; } -std::shared_future< std::vector > -Node::async_get_params( - const std::string& node_name, const std::vector& parameter_names, - std::function >)> callback) +std::shared_future< std::vector > +Node::async_get_parameters( + const std::string& node_name, const std::vector& parameter_names, + std::function >)> callback) { - std::promise< std::vector > promise_result; + std::promise< std::vector > promise_result; auto future_result = promise_result.get_future().share(); if (node_name == this->get_name()) { - std::vector value; + std::vector value; for(auto pn : parameter_names) { - if (this->has_param(pn)) { + if (this->has_parameter(pn)) { try { - parameter::ParamContainer param_container(pn, this->get_param(pn)); + parameter::ParameterContainer param_container(pn, this->get_parameter(pn)); value.push_back(param_container); // TODO: use custom exception } catch(...) { try { - parameter::ParamContainer param_container(pn, this->get_param(pn)); + parameter::ParameterContainer param_container(pn, this->get_parameter(pn)); value.push_back(param_container); } catch(...) { try { - parameter::ParamContainer param_container(pn, this->get_param(pn)); + parameter::ParameterContainer param_container(pn, this->get_parameter(pn)); value.push_back(param_container); } catch(...) { - parameter::ParamContainer param_container(pn, this->get_param(pn)); + parameter::ParameterContainer param_container(pn, this->get_parameter(pn)); value.push_back(param_container); } } @@ -317,39 +321,42 @@ Node::async_get_params( } } promise_result.set_value(value); + if (callback != nullptr) { + callback(future_result); + } } else { - auto client = this->create_client("get_params"); - auto request = std::make_shared(); + auto client = this->create_client("get_params"); + auto request = std::make_shared(); for(auto parameter_name : parameter_names) { - rcl_interfaces::ParamDescription description; + rcl_interfaces::ParameterDescription description; description.name = parameter_name; - request->param_descriptions.push_back(description); + request->parameter_descriptions.push_back(description); } client->async_send_request( request, [&promise_result, &future_result, &callback] ( - rclcpp::client::Client::SharedFuture cb_f) { - std::vector value; + rclcpp::client::Client::SharedFuture cb_f) { + std::vector value; auto parameters = cb_f.get()->parameters; for(auto parameter : parameters) { - switch(parameter.description.param_type) { - case rcl_interfaces::ParamDescription::BOOL_PARAM: - value.push_back(parameter::ParamContainer( + switch(parameter.description.parameter_type) { + case rcl_interfaces::ParameterDescription::BOOL_PARAMETER: + value.push_back(parameter::ParameterContainer( parameter.description.name, rclcpp::parameter::get_parameter_value(parameter))); break; - case rcl_interfaces::ParamDescription::STRING_PARAM: - value.push_back(parameter::ParamContainer( + case rcl_interfaces::ParameterDescription::STRING_PARAMETER: + value.push_back(parameter::ParameterContainer( parameter.description.name, rclcpp::parameter::get_parameter_value(parameter))); break; - case rcl_interfaces::ParamDescription::DOUBLE_PARAM: - value.push_back(parameter::ParamContainer( + case rcl_interfaces::ParameterDescription::DOUBLE_PARAMETER: + value.push_back(parameter::ParameterContainer( parameter.description.name, rclcpp::parameter::get_parameter_value(parameter))); break; - case rcl_interfaces::ParamDescription::INTEGER_PARAM: - value.push_back(parameter::ParamContainer( + case rcl_interfaces::ParameterDescription::INTEGER_PARAMETER: + value.push_back(parameter::ParameterContainer( parameter.description.name, rclcpp::parameter::get_parameter_value(parameter))); break; @@ -369,9 +376,9 @@ Node::async_get_params( } std::shared_future -Node::async_set_params( +Node::async_set_parameters( const std::string& node_name, - const std::vector& key_values, + const std::vector& key_values, std::function)> callback) { std::promise promise_result; @@ -379,43 +386,46 @@ Node::async_set_params( if (node_name == this->get_name()) { for(auto kv : key_values) { switch(kv.get_typeID()) { - case parameter::ParamDataType::INT_PARAM: - this->set_param(kv.get_name(), kv.get_value()); + case parameter::ParameterDataType::INTEGER_PARAMETER: + this->set_parameter(kv.get_name(), kv.get_value()); break; - case parameter::ParamDataType::DOUBLE_PARAM: - this->set_param(kv.get_name(), kv.get_value()); + case parameter::ParameterDataType::DOUBLE_PARAMETER: + this->set_parameter(kv.get_name(), kv.get_value()); break; - case parameter::ParamDataType::STRING_PARAM: - this->set_param(kv.get_name(), kv.get_value()); + case parameter::ParameterDataType::STRING_PARAMETER: + this->set_parameter(kv.get_name(), kv.get_value()); break; - case parameter::ParamDataType::BOOL_PARAM: - this->set_param(kv.get_name(), kv.get_value()); + case parameter::ParameterDataType::BOOL_PARAMETER: + this->set_parameter(kv.get_name(), kv.get_value()); break; default: throw std::runtime_error("Invalid parameter type"); } } promise_result.set_value(true); + if (callback != nullptr) { + callback(future_result); + } } else { - auto client = this->create_client("set_params"); - auto request = std::make_shared(); + auto client = this->create_client("set_params"); + auto request = std::make_shared(); for (auto kv: key_values) { - rcl_interfaces::Param parameter; + rcl_interfaces::Parameter parameter; parameter.description.name = kv.get_name(); switch(kv.get_typeID()) { - case parameter::ParamDataType::INT_PARAM: + case parameter::ParameterDataType::INTEGER_PARAMETER: rclcpp::parameter::set_parameter_value( parameter, kv.get_value()); break; - case parameter::ParamDataType::DOUBLE_PARAM: + case parameter::ParameterDataType::DOUBLE_PARAMETER: rclcpp::parameter::set_parameter_value( parameter, kv.get_value()); break; - case parameter::ParamDataType::STRING_PARAM: + case parameter::ParameterDataType::STRING_PARAMETER: rclcpp::parameter::set_parameter_value( parameter, kv.get_value()); break; - case parameter::ParamDataType::BOOL_PARAM: + case parameter::ParameterDataType::BOOL_PARAMETER: rclcpp::parameter::set_parameter_value( parameter, kv.get_value()); break; @@ -427,7 +437,7 @@ Node::async_set_params( client->async_send_request( request, [&promise_result, &future_result, &callback] ( - rclcpp::client::Client::SharedFuture cb_f) { + rclcpp::client::Client::SharedFuture cb_f) { promise_result.set_value(cb_f.get()->success); if (callback != nullptr) { callback(future_result); @@ -440,33 +450,34 @@ Node::async_set_params( template std::shared_future -Node::async_set_param( - const std::string& node_name, const parameter::ParamName& key, const ParamTypeT& value, +Node::async_set_parameter( + const std::string& node_name, const parameter::ParameterName& key, const ParamTypeT& value, std::function)> callback) { - std::vector param_containers; - parameter::ParamContainer param_container(key, value); - param_containers.push_back(param_container); - return this->async_set_params(node_name, param_containers, callback); + std::vector param_containers = {{ + parameter::ParameterContainer(key, value) }}; + return this->async_set_parameters(node_name, param_containers, callback); } std::shared_future -Node::async_has_param( - const std::string& node_name, const parameter::ParamQuery& query, +Node::async_has_parameter( + const std::string& node_name, const parameter::ParameterQuery& query, std::function)> callback) { std::promise promise_result; auto future_result = promise_result.get_future().share(); if (node_name == this->get_name()) { - promise_result.set_value(this->has_param(query)); + promise_result.set_value(this->has_parameter(query)); } else { - auto client = this->create_client("has_param"); - auto request = std::make_shared(); - request->param_description.name = query.get_name(); + auto client = this->create_client("has_params"); + auto request = std::make_shared(); + rcl_interfaces::ParameterDescription parameter_description; + parameter_description.name = query.get_name(); + request->parameter_descriptions.push_back(parameter_description); client->async_send_request( request, [&promise_result, &future_result, &callback] ( - rclcpp::client::Client::SharedFuture cb_f) { - promise_result.set_value(cb_f.get()->result); + rclcpp::client::Client::SharedFuture cb_f) { + promise_result.set_value(cb_f.get()->result[0]); if (callback != nullptr) { callback(future_result); } diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index cba4329933..b81bd6a580 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -21,8 +21,8 @@ #include -#include -#include +#include +#include namespace rclcpp { @@ -30,46 +30,42 @@ namespace rclcpp namespace parameter { // Datatype for parameter names - typedef std::string ParamName; + typedef std::string ParameterName; // Datatype for storing parameter types - enum ParamDataType {INVALID_PARAM, INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_PARAM}; + enum ParameterDataType { + INVALID_PARAMETER, INTEGER_PARAMETER, DOUBLE_PARAMETER, STRING_PARAMETER, BOOL_PARAMETER}; // Structure to store an arbitrary parameter with templated get/set methods - class ParamContainer + class ParameterContainer { public: - ParamContainer() : typeID_(INVALID_PARAM) {} - ParamContainer( + ParameterContainer() : typeID_(INVALID_PARAMETER) {} + ParameterContainer( const std::string& name, const int64_t int_value) : - name_(name), typeID_(INT_PARAM), int_value_(int_value) {} - ParamContainer( + name_(name), typeID_(INTEGER_PARAMETER), int_value_(int_value) {} + ParameterContainer( const std::string& name, const double double_value) : - name_(name), typeID_(DOUBLE_PARAM), double_value_(double_value) {} - ParamContainer( + name_(name), typeID_(DOUBLE_PARAMETER), double_value_(double_value) {} + ParameterContainer( const std::string& name, const std::string& string_value) : - name_(name), typeID_(STRING_PARAM), string_value_(string_value) {} - ParamContainer( + name_(name), typeID_(STRING_PARAMETER), string_value_(string_value) {} + ParameterContainer( const std::string& name, const bool bool_value) : - name_(name), typeID_(BOOL_PARAM), bool_value_(bool_value) {} + name_(name), typeID_(BOOL_PARAMETER), bool_value_(bool_value) {} - ParamName name_; - ParamDataType typeID_; + ParameterName name_; + ParameterDataType typeID_; /* Templated getter */ template T get_value() const; - inline ParamName get_name() const { return name_; }; + inline ParameterName get_name() const { return name_; }; - inline ParamDataType get_typeID() const { return typeID_; }; - - /* Templated setter */ - template - void - set_value(const ParamName& name, const T& value); + inline ParameterDataType get_typeID() const { return typeID_; }; private: int64_t int_value_; @@ -79,72 +75,72 @@ namespace parameter }; template <> - inline int64_t ParamContainer::get_value() const + inline int64_t ParameterContainer::get_value() const { - if (typeID_!= INT_PARAM) { + if (typeID_!= INTEGER_PARAMETER) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } return int_value_; } template <> - inline double ParamContainer::get_value() const + inline double ParameterContainer::get_value() const { - if (typeID_!= DOUBLE_PARAM) { + if (typeID_!= DOUBLE_PARAMETER) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } return double_value_; } template <> - inline std::string ParamContainer::get_value() const + inline std::string ParameterContainer::get_value() const { - if (typeID_!= STRING_PARAM) { + if (typeID_!= STRING_PARAMETER) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } return string_value_; } template <> - inline bool ParamContainer::get_value() const + inline bool ParameterContainer::get_value() const { - if (typeID_!= BOOL_PARAM) { + if (typeID_!= BOOL_PARAMETER) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } return bool_value_; } - class ParamQuery + class ParameterQuery { public: - ParamQuery(const std::string& name) : typeID_(INVALID_PARAM), name_(name) {} - ParamQuery(const ParamDataType typeID) : typeID_(typeID), name_("") {} + ParameterQuery(const std::string& name) : typeID_(INVALID_PARAMETER), name_(name) {} + ParameterQuery(const ParameterDataType typeID) : typeID_(typeID), name_("") {} // TODO: make this extendable for potential regex or other dynamic queryies // Possibly use a generator pattern? // For now just store a single datatype and provide accessors. - inline ParamDataType get_type() const + inline ParameterDataType get_type() const { return typeID_; } - inline ParamName get_name() const + inline ParameterName get_name() const { return name_; } private: - ParamDataType typeID_; - ParamName name_; + ParameterDataType typeID_; + ParameterName name_; }; template - T get_parameter_value(rcl_interfaces::Param& parameter); + T get_parameter_value(rcl_interfaces::Parameter& parameter); template <> - bool get_parameter_value(rcl_interfaces::Param& parameter) { - if(parameter.description.param_type != rcl_interfaces::ParamDescription::BOOL_PARAM) { + bool get_parameter_value(rcl_interfaces::Parameter& parameter) { + if(parameter.description.parameter_type != rcl_interfaces::ParameterDescription::BOOL_PARAMETER) { // TODO: use custom exception throw std::runtime_error("Parameter value is not a boolean"); } @@ -152,8 +148,8 @@ namespace parameter } template <> - int64_t get_parameter_value(rcl_interfaces::Param& parameter) { - if(parameter.description.param_type != rcl_interfaces::ParamDescription::INTEGER_PARAM) { + int64_t get_parameter_value(rcl_interfaces::Parameter& parameter) { + if(parameter.description.parameter_type != rcl_interfaces::ParameterDescription::INTEGER_PARAMETER) { // TODO: use custom exception throw std::runtime_error("Parameter value is not an integer"); } @@ -161,8 +157,8 @@ namespace parameter } template <> - double get_parameter_value(rcl_interfaces::Param& parameter) { - if(parameter.description.param_type != rcl_interfaces::ParamDescription::DOUBLE_PARAM) { + double get_parameter_value(rcl_interfaces::Parameter& parameter) { + if(parameter.description.parameter_type != rcl_interfaces::ParameterDescription::DOUBLE_PARAMETER) { // TODO: use custom exception throw std::runtime_error("Parameter value is not a double"); } @@ -170,8 +166,8 @@ namespace parameter } template <> - std::string get_parameter_value(rcl_interfaces::Param& parameter) { - if(parameter.description.param_type != rcl_interfaces::ParamDescription::STRING_PARAM) { + std::string get_parameter_value(rcl_interfaces::Parameter& parameter) { + if(parameter.description.parameter_type != rcl_interfaces::ParameterDescription::STRING_PARAMETER) { // TODO: use custom exception throw std::runtime_error("Parameter value is not a string"); } @@ -179,29 +175,29 @@ namespace parameter } template - void set_parameter_value(rcl_interfaces::Param& parameter, const T& value); + void set_parameter_value(rcl_interfaces::Parameter& parameter, const T& value); template<> - void set_parameter_value(rcl_interfaces::Param& parameter, const bool& value) { - parameter.description.param_type = rcl_interfaces::ParamDescription::BOOL_PARAM; + void set_parameter_value(rcl_interfaces::Parameter& parameter, const bool& value) { + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::BOOL_PARAMETER; parameter.bool_value = value; } template<> - void set_parameter_value(rcl_interfaces::Param& parameter, const int64_t& value) { - parameter.description.param_type = rcl_interfaces::ParamDescription::INTEGER_PARAM; + void set_parameter_value(rcl_interfaces::Parameter& parameter, const int64_t& value) { + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::INTEGER_PARAMETER; parameter.integer_value = value; } template<> - void set_parameter_value(rcl_interfaces::Param& parameter, const double& value) { - parameter.description.param_type = rcl_interfaces::ParamDescription::DOUBLE_PARAM; + void set_parameter_value(rcl_interfaces::Parameter& parameter, const double& value) { + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::DOUBLE_PARAMETER; parameter.double_value = value; } template<> - void set_parameter_value(rcl_interfaces::Param& parameter, const std::string& value) { - parameter.description.param_type = rcl_interfaces::ParamDescription::STRING_PARAM; + void set_parameter_value(rcl_interfaces::Parameter& parameter, const std::string& value) { + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::STRING_PARAMETER; parameter.string_value = value; } } /* namespace parameter */