From 4126d1c2404469983c49796eb0d2818d14c6ab0f Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 3 Apr 2015 16:36:36 -0700 Subject: [PATCH 01/26] Added union-like parameter container --- rclcpp/include/rclcpp/node.hpp | 3 ++ rclcpp/include/rclcpp/parameter.hpp | 43 +++++++++++++++++++++++++++++ 2 files changed, 46 insertions(+) create mode 100644 rclcpp/include/rclcpp/parameter.hpp diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 701609843a..44e0b0fac6 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -124,6 +124,9 @@ class Node typename rclcpp::service::Service::CallbackWithHeaderType callback_with_header, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + template + ParamTypeT getParam(const std::string& key); + private: RCLCPP_DISABLE_COPY(Node); diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp new file mode 100644 index 0000000000..4a8e49363a --- /dev/null +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -0,0 +1,43 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_RCLCPP_PARAMETER_HPP_ +#define RCLCPP_RCLCPP_PARAMETER_HPP_ + +#include + +#include + +#include + + +namespace rclcpp +{ + +namespace parameter +{ + struct ParamContainer + { + enum {INT, DOUBLE, STRING, BOOL} typeID; + union { + int64_t i; + double d; + std::string s; + bool b; + } + }; +} /* namespace parameter */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */ From d56f9dee43fb187ce79b96f2f3c138b52a0dafce Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Mon, 6 Apr 2015 16:06:31 -0700 Subject: [PATCH 02/26] filling out developer API --- rclcpp/include/rclcpp/node.hpp | 14 ++++- rclcpp/include/rclcpp/parameter.hpp | 89 +++++++++++++++++++++++++++-- 2 files changed, 96 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 44e0b0fac6..3532cc28f1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -125,7 +126,18 @@ class Node rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); template - ParamTypeT getParam(const std::string& key); + ParamTypeT getParam(const parameter::param_name_t& key); + + std::vector + getParams(const std::vector& query); + + bool + hasParam(const parameter::ParamQuery& query); + + template + bool setParam(const parameter::param_name_t& key, const ParamTypeT& value); + + private: RCLCPP_DISABLE_COPY(Node); diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 4a8e49363a..fb429c1bf9 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -27,15 +27,92 @@ namespace rclcpp namespace parameter { + // Datatype for parameter names + typedef std::string param_name_t; + + // Datatype for storing parameter types + enum ParamDataType {INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_PARAM}; + + + // Structure to store an arbitrary parameter with templated get/set methods struct ParamContainer { - enum {INT, DOUBLE, STRING, BOOL} typeID; + ParamDataType typeID_; union { - int64_t i; - double d; - std::string s; - bool b; - } + int64_t i_; + double d_; + std::string s_; + bool b_; + }; + param_name_t name_; + + /* Templated getter */ + template + T& + getValue(T& val); + + inline param_name_t getName() { return name_; }; + /* Templated setter */ + template + bool + setValue(const param_name_t& name, T& value); + }; + + template <> + inline int64_t& ParamContainer::getValue(int64_t& val) + { + if (typeID_!= INT_PARAM) + { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + val = i_; + return val; + } + template <> + inline double& ParamContainer::getValue(double& val) + { + if (typeID_!= DOUBLE_PARAM) + { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + val = i_; + return val; + } + template <> + inline std::string& ParamContainer::getValue(std::string& val) + { + if (typeID_!= STRING_PARAM) + { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + val = i_; + return val; + } + template <> + inline bool& ParamContainer::getValue(bool& val) + { + if (typeID_!= BOOL_PARAM) + { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + val = i_; + return val; + } + struct ParamQuery + { + // 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 getType(void){ return typeID_;}; + inline param_name_t getName(void){ return name_;}; + + ParamDataType typeID_; + param_name_t name_; }; } /* namespace parameter */ } /* namespace rclcpp */ From 92f28e617f0a7641052db75be50335d14e22276a Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 6 Apr 2015 17:50:03 -0700 Subject: [PATCH 03/26] Implemented get_param --- rclcpp/include/rclcpp/node.hpp | 12 ++++---- rclcpp/include/rclcpp/node_impl.hpp | 9 ++++++ rclcpp/include/rclcpp/parameter.hpp | 47 +++++++++++++++-------------- 3 files changed, 39 insertions(+), 29 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 3532cc28f1..9cb6e52c1a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -126,18 +126,16 @@ class Node rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); template - ParamTypeT getParam(const parameter::param_name_t& key); + ParamTypeT get_param(const parameter::param_name_t& key); std::vector - getParams(const std::vector& query); + get_params(const std::vector& query); bool - hasParam(const parameter::ParamQuery& query); + has_param(const parameter::ParamQuery& query); template - bool setParam(const parameter::param_name_t& key, const ParamTypeT& value); - - + bool set_param(const parameter::param_name_t& key, const ParamTypeT& value); private: RCLCPP_DISABLE_COPY(Node); @@ -163,6 +161,8 @@ class Node const std::string & service_name, std::shared_ptr serv_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); + + std::map params_; }; } /* namespace node */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 363d5e7729..b952735dc7 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -246,4 +246,13 @@ Node::register_service( number_of_services_++; } +template +ParamTypeT +Node::get_param(const parameter::param_name_t& key) +{ + parameter::ParamContainer pc = this->params_[key]; + ParamTypeT value; + return pc.get_value(value); +} + #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index fb429c1bf9..7fb21b4639 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -35,31 +35,32 @@ namespace parameter // Structure to store an arbitrary parameter with templated get/set methods - struct ParamContainer + class ParamContainer { - ParamDataType typeID_; - union { + public: + ParamDataType typeID_; + param_name_t name_; + + /* Templated getter */ + template + T& + get_value(T& val); + + inline param_name_t getName() { return name_; }; + /* Templated setter */ + template + bool + set_value(const param_name_t& name, T& value); + + private: int64_t i_; double d_; std::string s_; bool b_; - }; - param_name_t name_; - - /* Templated getter */ - template - T& - getValue(T& val); - - inline param_name_t getName() { return name_; }; - /* Templated setter */ - template - bool - setValue(const param_name_t& name, T& value); }; template <> - inline int64_t& ParamContainer::getValue(int64_t& val) + inline int64_t& ParamContainer::get_value(int64_t& val) { if (typeID_!= INT_PARAM) { @@ -70,36 +71,36 @@ namespace parameter return val; } template <> - inline double& ParamContainer::getValue(double& val) + inline double& ParamContainer::get_value(double& val) { if (typeID_!= DOUBLE_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - val = i_; + val = d_; return val; } template <> - inline std::string& ParamContainer::getValue(std::string& val) + inline std::string& ParamContainer::get_value(std::string& val) { if (typeID_!= STRING_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - val = i_; + val = s_; return val; } template <> - inline bool& ParamContainer::getValue(bool& val) + inline bool& ParamContainer::get_value(bool& val) { if (typeID_!= BOOL_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - val = i_; + val = b_; return val; } struct ParamQuery From 1c413ea8b1ac917cb6850a63ea309e6392861f15 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 7 Apr 2015 17:46:37 -0700 Subject: [PATCH 04/26] Basic infrastructure to support the parameters API --- rclcpp/include/rclcpp/node.hpp | 2 +- rclcpp/include/rclcpp/node_impl.hpp | 31 ++++++++++ rclcpp/include/rclcpp/parameter.hpp | 90 +++++++++++++++++++++-------- 3 files changed, 97 insertions(+), 26 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 9cb6e52c1a..c19f2b8958 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -135,7 +135,7 @@ class Node has_param(const parameter::ParamQuery& query); template - bool set_param(const parameter::param_name_t& key, const ParamTypeT& value); + void set_param(const parameter::param_name_t& key, const ParamTypeT& value); private: RCLCPP_DISABLE_COPY(Node); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index b952735dc7..4a070c7f4a 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_ #define RCLCPP_RCLCPP_NODE_IMPL_HPP_ +#include #include #include @@ -255,4 +256,34 @@ Node::get_param(const parameter::param_name_t& key) return pc.get_value(value); } +template +void +Node::set_param(const parameter::param_name_t& key, const ParamTypeT& value) +{ + parameter::ParamContainer pc; + pc.set_value(key, value); + params_[key] = pc; +} + +bool +Node::has_param(const parameter::ParamQuery& query) +{ + const parameter::param_name_t key = query.get_name(); + return (params_.find(key) != params_.end()); +} + +std::vector +Node::get_params(const std::vector& queries) +{ + std::vector result; + + for(auto& kv: params_) { + if(std::any_of(queries.cbegin(), queries.cend(), + [&kv](const parameter::ParamQuery& query) { + return kv.first == query.get_name();})) { + result.push_back(kv.second); + } + } + return result; +} #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 7fb21b4639..56d1792f95 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -42,15 +42,15 @@ namespace parameter param_name_t name_; /* Templated getter */ - template + template T& - get_value(T& val); + get_value(T& value); inline param_name_t getName() { return name_; }; /* Templated setter */ - template - bool - set_value(const param_name_t& name, T& value); + template + void + set_value(const param_name_t& name, const T& value); private: int64_t i_; @@ -60,60 +60,100 @@ namespace parameter }; template <> - inline int64_t& ParamContainer::get_value(int64_t& val) + inline int64_t& ParamContainer::get_value(int64_t& value) { if (typeID_!= INT_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - val = i_; - return val; + value = i_; + return value; } template <> - inline double& ParamContainer::get_value(double& val) + inline double& ParamContainer::get_value(double& value) { if (typeID_!= DOUBLE_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - val = d_; - return val; + value = d_; + return value; } template <> - inline std::string& ParamContainer::get_value(std::string& val) + inline std::string& ParamContainer::get_value(std::string& value) { if (typeID_!= STRING_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - val = s_; - return val; + value = s_; + return value; } template <> - inline bool& ParamContainer::get_value(bool& val) + inline bool& ParamContainer::get_value(bool& value) { if (typeID_!= BOOL_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - val = b_; - return val; + value = b_; + return value; } - struct ParamQuery + + template <> + inline void ParamContainer::set_value(const param_name_t& name, const int64_t& value) + { + typeID_ = INT_PARAM; + i_ = value; + } + + template <> + inline void ParamContainer::set_value(const param_name_t& name, const double& value) + { + typeID_ = DOUBLE_PARAM; + d_ = value; + } + + template <> + inline void ParamContainer::set_value(const param_name_t& name, const std::string& value) + { + typeID_ = STRING_PARAM; + s_ = value; + } + + template <> + inline void ParamContainer::set_value(const param_name_t& name, const bool& value) { - // 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. + typeID_ = BOOL_PARAM; + b_ = value; + } - inline ParamDataType getType(void){ return typeID_;}; - inline param_name_t getName(void){ return name_;}; + class ParamQuery + { + public: + ParamQuery(const std::string& name) : name_(name) {} + ParamQuery(const ParamDataType typeID) : typeID_(typeID) {} - ParamDataType typeID_; - param_name_t 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(void) const + { + return typeID_; + } + inline param_name_t get_name(void) const + { + return name_; + } + + private: + ParamDataType typeID_; + param_name_t name_; }; } /* namespace parameter */ } /* namespace rclcpp */ From 22a6b64b54cf02486991416a98e7072fc457e017 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 8 Apr 2015 11:37:18 -0700 Subject: [PATCH 05/26] Use correct naming style --- rclcpp/include/rclcpp/parameter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 56d1792f95..520dfde5ce 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -46,7 +46,7 @@ namespace parameter T& get_value(T& value); - inline param_name_t getName() { return name_; }; + inline param_name_t get_name() { return name_; }; /* Templated setter */ template void From 6811cc9cd521ef6d99f5536fd940ea2131302cec Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 8 Apr 2015 13:08:48 -0700 Subject: [PATCH 06/26] Use const on getters --- rclcpp/include/rclcpp/node.hpp | 6 +++--- rclcpp/include/rclcpp/node_impl.hpp | 6 +++--- rclcpp/include/rclcpp/parameter.hpp | 30 ++++++++++++++--------------- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index c19f2b8958..49904edb52 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -126,7 +126,7 @@ class Node rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); template - ParamTypeT get_param(const parameter::param_name_t& key); + ParamTypeT get_param(const parameter::ParamName& key); std::vector get_params(const std::vector& query); @@ -135,7 +135,7 @@ class Node has_param(const parameter::ParamQuery& query); template - void set_param(const parameter::param_name_t& key, const ParamTypeT& value); + void set_param(const parameter::ParamName& key, const ParamTypeT& value); private: RCLCPP_DISABLE_COPY(Node); @@ -162,7 +162,7 @@ class Node std::shared_ptr serv_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); - std::map params_; + std::map params_; }; } /* namespace node */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 4a070c7f4a..41abd5416f 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -249,7 +249,7 @@ Node::register_service( template ParamTypeT -Node::get_param(const parameter::param_name_t& key) +Node::get_param(const parameter::ParamName& key) { parameter::ParamContainer pc = this->params_[key]; ParamTypeT value; @@ -258,7 +258,7 @@ Node::get_param(const parameter::param_name_t& key) template void -Node::set_param(const parameter::param_name_t& key, const ParamTypeT& value) +Node::set_param(const parameter::ParamName& key, const ParamTypeT& value) { parameter::ParamContainer pc; pc.set_value(key, value); @@ -268,7 +268,7 @@ Node::set_param(const parameter::param_name_t& key, const ParamTypeT& value) bool Node::has_param(const parameter::ParamQuery& query) { - const parameter::param_name_t key = query.get_name(); + const parameter::ParamName key = query.get_name(); return (params_.find(key) != params_.end()); } diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 520dfde5ce..ac4f6444b7 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -28,7 +28,7 @@ namespace rclcpp namespace parameter { // Datatype for parameter names - typedef std::string param_name_t; + typedef std::string ParamName; // Datatype for storing parameter types enum ParamDataType {INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_PARAM}; @@ -39,18 +39,18 @@ namespace parameter { public: ParamDataType typeID_; - param_name_t name_; + ParamName name_; /* Templated getter */ template T& - get_value(T& value); + get_value (T& value) const; - inline param_name_t get_name() { return name_; }; + inline ParamName get_name() const { return name_; }; /* Templated setter */ template void - set_value(const param_name_t& name, const T& value); + set_value(const ParamName& name, const T& value); private: int64_t i_; @@ -60,7 +60,7 @@ namespace parameter }; template <> - inline int64_t& ParamContainer::get_value(int64_t& value) + inline int64_t& ParamContainer::get_value(int64_t& value) const { if (typeID_!= INT_PARAM) { @@ -71,7 +71,7 @@ namespace parameter return value; } template <> - inline double& ParamContainer::get_value(double& value) + inline double& ParamContainer::get_value(double& value) const { if (typeID_!= DOUBLE_PARAM) { @@ -82,7 +82,7 @@ namespace parameter return value; } template <> - inline std::string& ParamContainer::get_value(std::string& value) + inline std::string& ParamContainer::get_value(std::string& value) const { if (typeID_!= STRING_PARAM) { @@ -93,7 +93,7 @@ namespace parameter return value; } template <> - inline bool& ParamContainer::get_value(bool& value) + inline bool& ParamContainer::get_value(bool& value) const { if (typeID_!= BOOL_PARAM) { @@ -105,28 +105,28 @@ namespace parameter } template <> - inline void ParamContainer::set_value(const param_name_t& name, const int64_t& value) + inline void ParamContainer::set_value(const ParamName& name, const int64_t& value) { typeID_ = INT_PARAM; i_ = value; } template <> - inline void ParamContainer::set_value(const param_name_t& name, const double& value) + inline void ParamContainer::set_value(const ParamName& name, const double& value) { typeID_ = DOUBLE_PARAM; d_ = value; } template <> - inline void ParamContainer::set_value(const param_name_t& name, const std::string& value) + inline void ParamContainer::set_value(const ParamName& name, const std::string& value) { typeID_ = STRING_PARAM; s_ = value; } template <> - inline void ParamContainer::set_value(const param_name_t& name, const bool& value) + inline void ParamContainer::set_value(const ParamName& name, const bool& value) { typeID_ = BOOL_PARAM; b_ = value; @@ -146,14 +146,14 @@ namespace parameter { return typeID_; } - inline param_name_t get_name(void) const + inline ParamName get_name(void) const { return name_; } private: ParamDataType typeID_; - param_name_t name_; + ParamName name_; }; } /* namespace parameter */ } /* namespace rclcpp */ From 8b473d369f7f0e7b05cee1e5853a522430a4950a Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 8 Apr 2015 13:13:43 -0700 Subject: [PATCH 07/26] Use longer variable names --- rclcpp/include/rclcpp/parameter.hpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index ac4f6444b7..b2a3e818f8 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -53,10 +53,10 @@ namespace parameter set_value(const ParamName& name, const T& value); private: - int64_t i_; - double d_; - std::string s_; - bool b_; + int64_t int_value_; + double double_value_; + std::string string_value_; + bool bool_value_; }; template <> @@ -67,7 +67,7 @@ namespace parameter // TODO: use custom exception throw std::runtime_error("Invalid type"); } - value = i_; + value = int_value_; return value; } template <> @@ -78,7 +78,7 @@ namespace parameter // TODO: use custom exception throw std::runtime_error("Invalid type"); } - value = d_; + value = double_value_; return value; } template <> @@ -89,7 +89,7 @@ namespace parameter // TODO: use custom exception throw std::runtime_error("Invalid type"); } - value = s_; + value = string_value_; return value; } template <> @@ -100,7 +100,7 @@ namespace parameter // TODO: use custom exception throw std::runtime_error("Invalid type"); } - value = b_; + value = bool_value_; return value; } @@ -108,28 +108,28 @@ namespace parameter inline void ParamContainer::set_value(const ParamName& name, const int64_t& value) { typeID_ = INT_PARAM; - i_ = value; + int_value_ = value; } template <> inline void ParamContainer::set_value(const ParamName& name, const double& value) { typeID_ = DOUBLE_PARAM; - d_ = value; + double_value_ = value; } template <> inline void ParamContainer::set_value(const ParamName& name, const std::string& value) { typeID_ = STRING_PARAM; - s_ = value; + string_value_ = value; } template <> inline void ParamContainer::set_value(const ParamName& name, const bool& value) { typeID_ = BOOL_PARAM; - b_ = value; + bool_value_ = value; } class ParamQuery From 0c06fbf7aff7582142d7b7a9c5a4a3454f9f8270 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 8 Apr 2015 13:15:57 -0700 Subject: [PATCH 08/26] Added INVALID_PARAM --- rclcpp/include/rclcpp/parameter.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index b2a3e818f8..5dbe79ab8e 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -31,7 +31,7 @@ namespace parameter typedef std::string ParamName; // Datatype for storing parameter types - enum ParamDataType {INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_PARAM}; + enum ParamDataType {INVALID_PARAM, INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_PARAM}; // Structure to store an arbitrary parameter with templated get/set methods @@ -135,8 +135,8 @@ namespace parameter class ParamQuery { public: - ParamQuery(const std::string& name) : name_(name) {} - ParamQuery(const ParamDataType typeID) : typeID_(typeID) {} + ParamQuery(const std::string& name) : typeID_(INVALID_PARAM), name_(name) {} + ParamQuery(const ParamDataType typeID) : typeID_(typeID), name_("") {} // TODO: make this extendable for potential regex or other dynamic queryies // Possibly use a generator pattern? From 67d088b22d486cf00cf80d5656deab819d03047d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 8 Apr 2015 13:17:07 -0700 Subject: [PATCH 09/26] Removed void argument --- rclcpp/include/rclcpp/parameter.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 5dbe79ab8e..5247dc2c72 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -142,11 +142,11 @@ namespace parameter // Possibly use a generator pattern? // For now just store a single datatype and provide accessors. - inline ParamDataType get_type(void) const + inline ParamDataType get_type() const { return typeID_; } - inline ParamName get_name(void) const + inline ParamName get_name() const { return name_; } From 45ed589f88bb28fddf848206767821e939fa8c51 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 8 Apr 2015 13:18:37 -0700 Subject: [PATCH 10/26] Use const on getters --- rclcpp/include/rclcpp/node.hpp | 6 +++--- rclcpp/include/rclcpp/node_impl.hpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 49904edb52..e2b7290e94 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -126,13 +126,13 @@ class Node rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); template - ParamTypeT get_param(const parameter::ParamName& key); + ParamTypeT get_param(const parameter::ParamName& key) const; std::vector - get_params(const std::vector& query); + get_params(const std::vector& query) const; bool - has_param(const parameter::ParamQuery& query); + has_param(const parameter::ParamQuery& query) const; template void set_param(const parameter::ParamName& key, const ParamTypeT& value); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 41abd5416f..067dcee181 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -249,9 +249,9 @@ Node::register_service( template ParamTypeT -Node::get_param(const parameter::ParamName& key) +Node::get_param(const parameter::ParamName& key) const { - parameter::ParamContainer pc = this->params_[key]; + const parameter::ParamContainer pc = this->params_.at(key); ParamTypeT value; return pc.get_value(value); } @@ -266,14 +266,14 @@ Node::set_param(const parameter::ParamName& key, const ParamTypeT& value) } bool -Node::has_param(const parameter::ParamQuery& query) +Node::has_param(const parameter::ParamQuery& query) const { const parameter::ParamName key = query.get_name(); return (params_.find(key) != params_.end()); } std::vector -Node::get_params(const std::vector& queries) +Node::get_params(const std::vector& queries) const { std::vector result; From 97dd34665b3720c9e6f7ac833fee2383fef98d79 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Apr 2015 14:33:08 -0700 Subject: [PATCH 11/26] Fix code style --- rclcpp/include/rclcpp/node.hpp | 12 +- rclcpp/include/rclcpp/node_impl.hpp | 24 +-- rclcpp/include/rclcpp/parameter.hpp | 224 ++++++++++++++-------------- 3 files changed, 130 insertions(+), 130 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e2b7290e94..a9a3df8c6c 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -125,17 +125,17 @@ class Node typename rclcpp::service::Service::CallbackWithHeaderType callback_with_header, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - template - ParamTypeT get_param(const parameter::ParamName& key) const; + template + ParamTypeT get_param(const parameter::ParamName & key) const; std::vector - get_params(const std::vector& query) const; + get_params(const std::vector & query) const; bool - has_param(const parameter::ParamQuery& query) const; + has_param(const parameter::ParamQuery & query) const; - template - void set_param(const parameter::ParamName& key, const ParamTypeT& value); + template + void set_param(const parameter::ParamName & key, const ParamTypeT & value); private: RCLCPP_DISABLE_COPY(Node); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 067dcee181..cd2b9423d1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -247,18 +247,18 @@ Node::register_service( number_of_services_++; } -template +template ParamTypeT -Node::get_param(const parameter::ParamName& key) const +Node::get_param(const parameter::ParamName & key) const { const parameter::ParamContainer pc = this->params_.at(key); ParamTypeT value; return pc.get_value(value); } -template +template void -Node::set_param(const parameter::ParamName& key, const ParamTypeT& value) +Node::set_param(const parameter::ParamName & key, const ParamTypeT & value) { parameter::ParamContainer pc; pc.set_value(key, value); @@ -266,21 +266,23 @@ Node::set_param(const parameter::ParamName& key, const ParamTypeT& value) } bool -Node::has_param(const parameter::ParamQuery& query) const +Node::has_param(const parameter::ParamQuery & query) const { const parameter::ParamName key = query.get_name(); - return (params_.find(key) != params_.end()); + return params_.find(key) != params_.end(); } std::vector -Node::get_params(const std::vector& queries) const +Node::get_params(const std::vector & queries) const { std::vector result; - for(auto& kv: params_) { - if(std::any_of(queries.cbegin(), queries.cend(), - [&kv](const parameter::ParamQuery& query) { - return kv.first == query.get_name();})) { + for (auto & kv: params_) { + if (std::any_of(queries.cbegin(), queries.cend(), + [&kv](const parameter::ParamQuery & query) { + return kv.first == query.get_name(); + })) + { result.push_back(kv.second); } } diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 5247dc2c72..7a65c61dfa 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -27,134 +27,132 @@ namespace rclcpp namespace parameter { - // Datatype for parameter names - typedef std::string ParamName; +// Datatype for parameter names +typedef std::string ParamName; - // Datatype for storing parameter types - enum ParamDataType {INVALID_PARAM, INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_PARAM}; +// Datatype for storing parameter types +enum ParamDataType {INVALID_PARAM, INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_PARAM}; - // Structure to store an arbitrary parameter with templated get/set methods - class ParamContainer - { - public: - ParamDataType typeID_; - ParamName name_; - - /* Templated getter */ - template - T& - get_value (T& value) const; - - inline ParamName get_name() const { return name_; }; - /* Templated setter */ - template - void - set_value(const ParamName& name, const T& value); - - private: - int64_t int_value_; - double double_value_; - std::string string_value_; - bool bool_value_; - }; - - template <> - inline int64_t& ParamContainer::get_value(int64_t& value) const - { - if (typeID_!= INT_PARAM) - { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - value = int_value_; - return value; +// Structure to store an arbitrary parameter with templated get/set methods +class ParamContainer +{ +public: + ParamDataType typeID_; + ParamName name_; + + /* Templated getter */ + template + T & + get_value(T & value) const; + + inline ParamName get_name() const {return name_; } + /* Templated setter */ + template + void + set_value(const ParamName & name, const T & value); + +private: + int64_t int_value_; + double double_value_; + std::string string_value_; + bool bool_value_; +}; + +template<> +inline int64_t & ParamContainer::get_value(int64_t & value) const +{ + if (typeID_ != INT_PARAM) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); } - template <> - inline double& ParamContainer::get_value(double& value) const - { - if (typeID_!= DOUBLE_PARAM) - { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - value = double_value_; - return value; + value = int_value_; + return value; +} +template<> +inline double & ParamContainer::get_value(double & value) const +{ + if (typeID_ != DOUBLE_PARAM) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); } - template <> - inline std::string& ParamContainer::get_value(std::string& value) const - { - if (typeID_!= STRING_PARAM) - { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - value = string_value_; - return value; + value = double_value_; + return value; +} +template<> +inline std::string & ParamContainer::get_value(std::string & value) const +{ + if (typeID_ != STRING_PARAM) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); } - template <> - inline bool& ParamContainer::get_value(bool& value) const - { - if (typeID_!= BOOL_PARAM) - { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - value = bool_value_; - return value; + value = string_value_; + return value; +} +template<> +inline bool & ParamContainer::get_value(bool & value) const +{ + if (typeID_ != BOOL_PARAM) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); } + value = bool_value_; + return value; +} - template <> - inline void ParamContainer::set_value(const ParamName& name, const int64_t& value) - { - typeID_ = INT_PARAM; - int_value_ = value; - } +template<> +inline void ParamContainer::set_value(const ParamName & name, const int64_t & value) +{ + typeID_ = INT_PARAM; + int_value_ = value; +} - template <> - inline void ParamContainer::set_value(const ParamName& name, const double& value) - { - typeID_ = DOUBLE_PARAM; - double_value_ = value; - } +template<> +inline void ParamContainer::set_value(const ParamName & name, const double & value) +{ + typeID_ = DOUBLE_PARAM; + double_value_ = value; +} + +template<> +inline void ParamContainer::set_value(const ParamName & name, const std::string & value) +{ + typeID_ = STRING_PARAM; + string_value_ = value; +} + +template<> +inline void ParamContainer::set_value(const ParamName & name, const bool & value) +{ + typeID_ = BOOL_PARAM; + bool_value_ = value; +} + +class ParamQuery +{ +public: + ParamQuery(const std::string & name) + : typeID_(INVALID_PARAM), name_(name) {} + ParamQuery(const ParamDataType typeID) + : typeID_(typeID), name_("") {} - template <> - inline void ParamContainer::set_value(const ParamName& name, const std::string& value) + // 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 { - typeID_ = STRING_PARAM; - string_value_ = value; + return typeID_; } - - template <> - inline void ParamContainer::set_value(const ParamName& name, const bool& value) + inline ParamName get_name() const { - typeID_ = BOOL_PARAM; - bool_value_ = value; + return name_; } - class ParamQuery - { - public: - ParamQuery(const std::string& name) : typeID_(INVALID_PARAM), name_(name) {} - ParamQuery(const ParamDataType 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 - { - return typeID_; - } - inline ParamName get_name() const - { - return name_; - } - - private: - ParamDataType typeID_; - ParamName name_; - }; +private: + ParamDataType typeID_; + ParamName name_; +}; } /* namespace parameter */ } /* namespace rclcpp */ From 7df3f25528964e9326740f704d6c3759b84bfd1d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 24 Apr 2015 14:59:22 -0700 Subject: [PATCH 12/26] Made typeID_ and name_ private --- rclcpp/include/rclcpp/parameter.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 7a65c61dfa..17dbbef0e4 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -38,9 +38,6 @@ enum ParamDataType {INVALID_PARAM, INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_P class ParamContainer { public: - ParamDataType typeID_; - ParamName name_; - /* Templated getter */ template T & @@ -53,6 +50,8 @@ class ParamContainer set_value(const ParamName & name, const T & value); private: + ParamDataType typeID_; + ParamName name_; int64_t int_value_; double double_value_; std::string string_value_; From 4f3f1400f0af9d4f36d704a6e2bad8f239a39ae9 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 9 Apr 2015 16:39:40 -0700 Subject: [PATCH 13/26] Basic support for developer API --- rclcpp/include/rclcpp/node.hpp | 24 +++++++++++++++------- rclcpp/include/rclcpp/node_impl.hpp | 31 +++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index a9a3df8c6c..f1e279b7db 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -65,7 +65,7 @@ class Node /* Get the name of the node. */ std::string - get_name(); + get_name() const {return this->name_; } /* Create and return a callback group. */ rclcpp::callback_group::CallbackGroup::SharedPtr @@ -126,16 +126,14 @@ class Node rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); template - ParamTypeT get_param(const parameter::ParamName & key) const; - - std::vector - get_params(const std::vector & query) const; + ParamTypeT get_param(const std::string & node_name, const parameter::ParamName & key) const; bool - has_param(const parameter::ParamQuery & query) const; + has_param(const std::string & node_name, const parameter::ParamQuery & query) const; template - void set_param(const parameter::ParamName & key, const ParamTypeT & value); + void set_param(const std::string & node_name, const parameter::ParamName & key, + const ParamTypeT & value); private: RCLCPP_DISABLE_COPY(Node); @@ -163,6 +161,18 @@ class Node rclcpp::callback_group::CallbackGroup::SharedPtr group); std::map params_; + + template + ParamTypeT get_param(const parameter::ParamName & key) const; + + std::vector + get_params(const std::vector & query) const; + + bool + has_param(const parameter::ParamQuery & query) const; + + template + void set_param(const parameter::ParamName & key, const ParamTypeT & value); }; } /* namespace node */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index cd2b9423d1..b31eda4745 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -288,4 +288,35 @@ Node::get_params(const std::vector & queries) const } return result; } + +template +ParamTypeT +Node::get_param(const std::string& node_name, const parameter::ParamName& key) const +{ + if (node_name == this->get_name()) { + return this->get_param(key); + } + ParamTypeT result; + return result; +} + +template +void +Node::set_param(const std::string& node_name, const parameter::ParamName& key, + const ParamTypeT& value) +{ + if (node_name == this->get_name()) { + this->set_param(key, value); + } +} + +bool +Node::has_param(const std::string& node_name, const parameter::ParamQuery& query) const +{ + if (node_name == this->get_name()) { + return this->has_param(query); + } + return false; +} + #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ From 2528bfb1666ca3bca6a033a37ac79d30b6e1d644 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 9 Apr 2015 17:20:32 -0700 Subject: [PATCH 14/26] Depend on rcl_interfaces --- rclcpp/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index f393a38136..1b41b12501 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -10,6 +10,8 @@ rmw + rcl_interfaces + ament_lint_auto ament_lint_common From eba2fb01838e8f75f447f2cf2e8f39e36af35971 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 10 Apr 2015 16:52:57 -0700 Subject: [PATCH 15/26] Added get_param, set_param and has_param --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/node.hpp | 17 +++++-- rclcpp/include/rclcpp/node_impl.hpp | 79 ++++++++++++++++++++++++----- rclcpp/include/rclcpp/parameter.hpp | 71 +++++++++++++++++++++++++- 4 files changed, 149 insertions(+), 19 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index d9194f5bc0..25600ca367 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 2.8.3) project(rclcpp) find_package(ament_cmake REQUIRED) +find_package(rcl_interfaces REQUIRED) ament_export_dependencies(rmw) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f1e279b7db..ed0feb8d5b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -126,14 +127,20 @@ class Node rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); template - ParamTypeT get_param(const std::string & node_name, const parameter::ParamName & key) const; + std::shared_future + async_get_param( + const std::string & node_name, const parameter::ParamName & key, + std::function)> callback = nullptr); - bool - has_param(const std::string & node_name, const parameter::ParamQuery & query) const; + std::shared_future + async_has_param(const std::string & node_name, const parameter::ParamQuery & query, + std::function)> callback = nullptr); template - void set_param(const std::string & node_name, const parameter::ParamName & key, - const ParamTypeT & value); + std::shared_future + async_set_param( + const std::string & node_name, const parameter::ParamName & key, + const ParamTypeT & value, std::function)> callback = nullptr); private: RCLCPP_DISABLE_COPY(Node); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index b31eda4745..42add641d8 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -25,6 +25,10 @@ #include +#include +#include +#include + #ifndef RCLCPP_RCLCPP_NODE_HPP_ #include "node.hpp" #endif @@ -290,33 +294,84 @@ Node::get_params(const std::vector & queries) const } template -ParamTypeT -Node::get_param(const std::string& node_name, const parameter::ParamName& key) const +std::shared_future +Node::async_get_param( + const std::string& node_name, const parameter::ParamName& key, + std::function)> callback) { + std::promise promise_result; + std::shared_future future_result(promise_result.get_future()); if (node_name == this->get_name()) { - return this->get_param(key); + promise_result.set_value(this->get_param(key)); + } else { + auto client = this->create_client("get_params"); + auto request = std::make_shared(); + client->async_send_request( + request, [&promise_result, &future_result, &callback] ( + rclcpp::client::Client::SharedFuture cb_f) { + rcl_interfaces::Param parameter = cb_f.get()->parameters[0]; + ParamTypeT value = rclcpp::parameter::get_parameter_value(parameter); + promise_result.set_value(value); + if (callback != nullptr) { + callback(future_result); + } + } + ); } - ParamTypeT result; - return result; + return future_result; } template -void -Node::set_param(const std::string& node_name, const parameter::ParamName& key, - const ParamTypeT& value) +std::shared_future +Node::async_set_param( + const std::string& node_name, const parameter::ParamName& key, const ParamTypeT& value, + std::function)> callback) { + std::promise promise_result; + std::shared_future future_result(promise_result.get_future()); if (node_name == this->get_name()) { this->set_param(key, value); + promise_result.set_value(); + } else { + auto client = this->create_client("set_params"); + auto request = std::make_shared(); + client->async_send_request( + request, [&promise_result, &future_result, &callback] ( + rclcpp::client::Client::SharedFuture cb_f) { + cb_f.get(); + promise_result.set_value(); + if (callback != nullptr) { + callback(future_result); + } + } + ); } + return future_result; } -bool -Node::has_param(const std::string& node_name, const parameter::ParamQuery& query) const +std::shared_future +Node::async_has_param( + const std::string& node_name, const parameter::ParamQuery& query, + std::function)> callback) { + std::promise promise_result; + std::shared_future future_result(promise_result.get_future()); if (node_name == this->get_name()) { - return this->has_param(query); + promise_result.set_value(this->has_param(query)); + } else { + auto client = this->create_client("has_param"); + auto request = std::make_shared(); + client->async_send_request( + request, [&promise_result, &future_result, &callback] ( + rclcpp::client::Client::SharedFuture cb_f) { + promise_result.set_value(cb_f.get()->result); + if (callback != nullptr) { + callback(future_result); + } + } + ); } - return false; + return future_result; } #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 17dbbef0e4..e98390b41b 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -21,6 +21,8 @@ #include +#include +#include namespace rclcpp { @@ -148,9 +150,74 @@ class ParamQuery return name_; } + class ParamQuery + { +public: + ParamQuery(const std::string & name) + : typeID_(INVALID_PARAM), name_(name) {} + ParamQuery(const ParamDataType 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 + { + return typeID_; + } + inline ParamName get_name() const + { + return name_; + } + private: - ParamDataType typeID_; - ParamName name_; + ParamDataType typeID_; + ParamName name_; + }; + + template + T get_parameter_value(rcl_interfaces::Param & parameter); + + template<> + bool get_parameter_value(rcl_interfaces::Param & parameter) + { + if (parameter.description.param_type != rcl_interfaces::ParamDescription::BOOL_PARAM) { + // TODO: use custom exception + throw std::runtime_error("Parameter value is not a boolean"); + } + return parameter.bool_value; + } + + template<> + int64_t get_parameter_value(rcl_interfaces::Param & parameter) + { + if (parameter.description.param_type != rcl_interfaces::ParamDescription::INTEGER_PARAM) { + // TODO: use custom exception + throw std::runtime_error("Parameter value is not an integer"); + } + return parameter.integer_value; + } + + template<> + double get_parameter_value(rcl_interfaces::Param & parameter) + { + if (parameter.description.param_type != rcl_interfaces::ParamDescription::DOUBLE_PARAM) { + // TODO: use custom exception + throw std::runtime_error("Parameter value is not a double"); + } + return parameter.double_value; + } + + template<> + std::string get_parameter_value(rcl_interfaces::Param & parameter) + { + if (parameter.description.param_type != rcl_interfaces::ParamDescription::STRING_PARAM) { + // TODO: use custom exception + throw std::runtime_error("Parameter value is not a string"); + } + return parameter.string_value; + } }; } /* namespace parameter */ } /* namespace rclcpp */ From 9208f245bb21b3d6082eb979904c65aa6c97396c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 10 Apr 2015 17:20:09 -0700 Subject: [PATCH 16/26] Set parameter values --- rclcpp/include/rclcpp/node_impl.hpp | 3 +++ rclcpp/include/rclcpp/parameter.hpp | 31 +++++++++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 42add641d8..85100aa9c0 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -335,6 +335,8 @@ Node::async_set_param( } else { auto client = this->create_client("set_params"); auto request = std::make_shared(); + request->parameters[0].description.name = key; + rclcpp::parameter::set_parameter_value(request->parameters[0], value); client->async_send_request( request, [&promise_result, &future_result, &callback] ( rclcpp::client::Client::SharedFuture cb_f) { @@ -361,6 +363,7 @@ Node::async_has_param( } else { auto client = this->create_client("has_param"); auto request = std::make_shared(); + request->param_description.name = query.get_name(); client->async_send_request( request, [&promise_result, &future_result, &callback] ( rclcpp::client::Client::SharedFuture cb_f) { diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index e98390b41b..ebd8392ba3 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -218,6 +218,37 @@ class ParamQuery } return parameter.string_value; } + + template + void set_parameter_value(rcl_interfaces::Param & 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; + 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; + 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; + 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; + parameter.string_value = value; + } }; } /* namespace parameter */ } /* namespace rclcpp */ From ce8597fe63ed5233c32fee13b33e6362027ce21f Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 13 Apr 2015 17:40:16 -0700 Subject: [PATCH 17/26] Added get_params and set_params --- rclcpp/include/rclcpp/node.hpp | 16 ++- rclcpp/include/rclcpp/node_impl.hpp | 174 ++++++++++++++++++++++++---- rclcpp/include/rclcpp/parameter.hpp | 97 +++++----------- 3 files changed, 195 insertions(+), 92 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index ed0feb8d5b..59b0824b33 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -132,15 +132,27 @@ class Node const std::string & node_name, const parameter::ParamName & key, std::function)> callback = nullptr); + std::shared_future> + async_get_params( + 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); template - std::shared_future + std::shared_future async_set_param( const std::string & node_name, const parameter::ParamName & key, - const ParamTypeT & value, std::function)> callback = nullptr); + const ParamTypeT & value, std::function)> callback = nullptr); + + std::shared_future + async_set_params( + const std::string & node_name, + const std::vector & key_values, + std::function)> callback = nullptr); private: RCLCPP_DISABLE_COPY(Node); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 85100aa9c0..01aca56481 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -27,6 +28,8 @@ #include #include +#include +#include #include #ifndef RCLCPP_RCLCPP_NODE_HPP_ @@ -255,17 +258,15 @@ template ParamTypeT Node::get_param(const parameter::ParamName & key) const { - const parameter::ParamContainer pc = this->params_.at(key); - ParamTypeT value; - return pc.get_value(value); + const parameter::ParamContainer pc(this->params_.at(key)); + return pc.get_value(); } template void Node::set_param(const parameter::ParamName & key, const ParamTypeT & value) { - parameter::ParamContainer pc; - pc.set_value(key, value); + parameter::ParamContainer pc(key, value); params_[key] = pc; } @@ -300,17 +301,92 @@ Node::async_get_param( std::function)> callback) { std::promise promise_result; - std::shared_future future_result(promise_result.get_future()); + auto future_result = promise_result.get_future().share(); + std::vector param_names = {{ key }}; + + this->async_get_params( + node_name, param_names, + [&promise_result, &future_result, &callback, ¶m_names]( + std::shared_future >) {}); + + + 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::promise< std::vector > promise_result; + auto future_result = promise_result.get_future().share(); if (node_name == this->get_name()) { - promise_result.set_value(this->get_param(key)); + std::vector value; + for(auto pn : parameter_names) { + if (this->has_param(pn)) { + try { + parameter::ParamContainer param_container(pn, this->get_param(pn)); + value.push_back(param_container); + // TODO: use custom exception + } catch(...) { + try { + parameter::ParamContainer param_container(pn, this->get_param(pn)); + value.push_back(param_container); + } catch(...) { + try { + parameter::ParamContainer param_container(pn, this->get_param(pn)); + value.push_back(param_container); + } catch(...) { + parameter::ParamContainer param_container(pn, this->get_param(pn)); + value.push_back(param_container); + } + } + } + } + } + promise_result.set_value(value); } else { auto client = this->create_client("get_params"); auto request = std::make_shared(); + for(auto parameter_name : parameter_names) { + rcl_interfaces::ParamDescription description; + description.name = parameter_name; + request->param_descriptions.push_back(description); + } + client->async_send_request( request, [&promise_result, &future_result, &callback] ( rclcpp::client::Client::SharedFuture cb_f) { - rcl_interfaces::Param parameter = cb_f.get()->parameters[0]; - ParamTypeT value = rclcpp::parameter::get_parameter_value(parameter); + 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( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + case rcl_interfaces::ParamDescription::STRING_PARAM: + value.push_back(parameter::ParamContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + case rcl_interfaces::ParamDescription::DOUBLE_PARAM: + value.push_back(parameter::ParamContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + case rcl_interfaces::ParamDescription::INTEGER_PARAM: + value.push_back(parameter::ParamContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + default: + // TODO: use custom exception + throw std::runtime_error("Invalid value type"); + } + } promise_result.set_value(value); if (callback != nullptr) { callback(future_result); @@ -321,27 +397,67 @@ Node::async_get_param( return future_result; } -template -std::shared_future -Node::async_set_param( - const std::string& node_name, const parameter::ParamName& key, const ParamTypeT& value, - std::function)> callback) +std::shared_future +Node::async_set_params( + const std::string& node_name, + const std::vector& key_values, + std::function)> callback) { - std::promise promise_result; - std::shared_future future_result(promise_result.get_future()); + std::promise promise_result; + auto future_result = promise_result.get_future().share(); if (node_name == this->get_name()) { - this->set_param(key, value); - promise_result.set_value(); + for(auto kv : key_values) { + switch(kv.get_typeID()) { + case parameter::ParamDataType::INT_PARAM: + this->set_param(kv.get_name(), kv.get_value()); + break; + case parameter::ParamDataType::DOUBLE_PARAM: + this->set_param(kv.get_name(), kv.get_value()); + break; + case parameter::ParamDataType::STRING_PARAM: + this->set_param(kv.get_name(), kv.get_value()); + break; + case parameter::ParamDataType::BOOL_PARAM: + this->set_param(kv.get_name(), kv.get_value()); + break; + default: + throw std::runtime_error("Invalid parameter type"); + } + } + promise_result.set_value(true); } else { auto client = this->create_client("set_params"); auto request = std::make_shared(); - request->parameters[0].description.name = key; - rclcpp::parameter::set_parameter_value(request->parameters[0], value); + for (auto kv: key_values) { + rcl_interfaces::Param parameter; + parameter.description.name = kv.get_name(); + switch(kv.get_typeID()) { + case parameter::ParamDataType::INT_PARAM: + rclcpp::parameter::set_parameter_value( + parameter, kv.get_value()); + break; + case parameter::ParamDataType::DOUBLE_PARAM: + rclcpp::parameter::set_parameter_value( + parameter, kv.get_value()); + break; + case parameter::ParamDataType::STRING_PARAM: + rclcpp::parameter::set_parameter_value( + parameter, kv.get_value()); + break; + case parameter::ParamDataType::BOOL_PARAM: + rclcpp::parameter::set_parameter_value( + parameter, kv.get_value()); + break; + default: + throw std::runtime_error("Invalid parameter type"); + } + request->parameters.push_back(parameter); + } + client->async_send_request( request, [&promise_result, &future_result, &callback] ( rclcpp::client::Client::SharedFuture cb_f) { - cb_f.get(); - promise_result.set_value(); + promise_result.set_value(cb_f.get()->success); if (callback != nullptr) { callback(future_result); } @@ -351,13 +467,25 @@ Node::async_set_param( return future_result; } +template +std::shared_future +Node::async_set_param( + const std::string& node_name, const parameter::ParamName& 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::shared_future Node::async_has_param( const std::string& node_name, const parameter::ParamQuery& query, std::function)> callback) { std::promise promise_result; - std::shared_future future_result(promise_result.get_future()); + auto future_result = promise_result.get_future().share(); if (node_name == this->get_name()) { promise_result.set_value(this->has_param(query)); } else { diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index ebd8392ba3..29e6fbf980 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -35,17 +35,34 @@ typedef std::string ParamName; // Datatype for storing parameter types enum ParamDataType {INVALID_PARAM, INT_PARAM, DOUBLE_PARAM, STRING_PARAM, BOOL_PARAM}; - // Structure to store an arbitrary parameter with templated get/set methods class ParamContainer { public: + ParamContainer() + : typeID_(INVALID_PARAM) {} + ParamContainer( + const std::string & name, const int64_t int_value) + : name_(name), typeID_(INT_PARAM), int_value_(int_value) {} + ParamContainer( + const std::string & name, const double double_value) + : name_(name), typeID_(DOUBLE_PARAM), double_value_(double_value) {} + ParamContainer( + const std::string & name, const std::string & string_value) + : name_(name), typeID_(STRING_PARAM), string_value_(string_value) {} + ParamContainer( + const std::string & name, const bool bool_value) + : name_(name), typeID_(BOOL_PARAM), bool_value_(bool_value) {} + /* Templated getter */ template - T & - get_value(T & value) const; + T + get_value() const; inline ParamName get_name() const {return name_; } + + inline ParamDataType get_typeID() const {return typeID_; } + /* Templated setter */ template void @@ -61,72 +78,40 @@ class ParamContainer }; template<> -inline int64_t & ParamContainer::get_value(int64_t & value) const +inline int64_t ParamContainer::get_value() const { if (typeID_ != INT_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - value = int_value_; - return value; + return int_value_; } template<> -inline double & ParamContainer::get_value(double & value) const +inline double ParamContainer::get_value() const { if (typeID_ != DOUBLE_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - value = double_value_; - return value; + return double_value_; } template<> -inline std::string & ParamContainer::get_value(std::string & value) const +inline std::string ParamContainer::get_value() const { if (typeID_ != STRING_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - value = string_value_; - return value; + return string_value_; } template<> -inline bool & ParamContainer::get_value(bool & value) const +inline bool ParamContainer::get_value() const { if (typeID_ != BOOL_PARAM) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } - value = bool_value_; - return value; -} - -template<> -inline void ParamContainer::set_value(const ParamName & name, const int64_t & value) -{ - typeID_ = INT_PARAM; - int_value_ = value; -} - -template<> -inline void ParamContainer::set_value(const ParamName & name, const double & value) -{ - typeID_ = DOUBLE_PARAM; - double_value_ = value; -} - -template<> -inline void ParamContainer::set_value(const ParamName & name, const std::string & value) -{ - typeID_ = STRING_PARAM; - string_value_ = value; -} - -template<> -inline void ParamContainer::set_value(const ParamName & name, const bool & value) -{ - typeID_ = BOOL_PARAM; - bool_value_ = value; + return bool_value_; } class ParamQuery @@ -150,31 +135,9 @@ class ParamQuery return name_; } - class ParamQuery - { -public: - ParamQuery(const std::string & name) - : typeID_(INVALID_PARAM), name_(name) {} - ParamQuery(const ParamDataType 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 - { - return typeID_; - } - inline ParamName get_name() const - { - return name_; - } - private: - ParamDataType typeID_; - ParamName name_; - }; + ParamDataType typeID_; + ParamName name_; template T get_parameter_value(rcl_interfaces::Param & parameter); From 4f76425969c236338ba803f226e2abcebf27ab3a Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 15 Apr 2015 17:17:16 -0700 Subject: [PATCH 18/26] Refactored get_param to use get_params. Renamed param to parameter. Update code to latest rcl_interfaces --- rclcpp/include/rclcpp/node.hpp | 35 ++-- rclcpp/include/rclcpp/node_impl.hpp | 274 +++++++++++++++------------- rclcpp/include/rclcpp/parameter.hpp | 213 ++++++++++++--------- 3 files changed, 285 insertions(+), 237 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 59b0824b33..7e3dc65a14 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -128,30 +128,31 @@ 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> - async_get_params( - const std::string & node_name, const std::vector & parameter_names, - std::function>)> callback = + std::shared_future> + 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, + 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: @@ -179,19 +180,19 @@ class Node std::shared_ptr serv_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); - 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 01aca56481..1ec83f6a65 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" @@ -256,35 +256,35 @@ Node::register_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(); })) { @@ -294,51 +294,56 @@ Node::get_params(const std::vector & queries) const return result; } -template +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> +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> promise_result; auto future_result = promise_result.get_future().share(); if (node_name == this->get_name()) { - std::vector value; - for(auto pn : parameter_names) { - if (this->has_param(pn)) { + std::vector value; + for (auto pn : parameter_names) { + 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(...) { + // 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(...) { + } 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)); + } catch (...) { + parameter::ParameterContainer param_container(pn, this->get_parameter(pn)); value.push_back(param_container); } } @@ -346,105 +351,111 @@ 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(); - for(auto parameter_name : parameter_names) { - rcl_interfaces::ParamDescription description; + auto client = this->create_client("get_params"); + auto request = std::make_shared(); + for (auto parameter_name : parameter_names) { + 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; - 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( - parameter.description.name, - rclcpp::parameter::get_parameter_value(parameter))); - break; - case rcl_interfaces::ParamDescription::STRING_PARAM: - value.push_back(parameter::ParamContainer( - parameter.description.name, - rclcpp::parameter::get_parameter_value(parameter))); - break; - case rcl_interfaces::ParamDescription::DOUBLE_PARAM: - value.push_back(parameter::ParamContainer( - parameter.description.name, - rclcpp::parameter::get_parameter_value(parameter))); - break; - case rcl_interfaces::ParamDescription::INTEGER_PARAM: - value.push_back(parameter::ParamContainer( - parameter.description.name, - rclcpp::parameter::get_parameter_value(parameter))); - break; - default: - // TODO: use custom exception - throw std::runtime_error("Invalid value type"); - } - } - promise_result.set_value(value); - if (callback != nullptr) { - callback(future_result); - } + request, [&promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + std::vector value; + auto parameters = cb_f.get()->parameters; + for (auto parameter : parameters) { + 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::ParameterDescription::STRING_PARAMETER: + value.push_back(parameter::ParameterContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + case rcl_interfaces::ParameterDescription::DOUBLE_PARAMETER: + value.push_back(parameter::ParameterContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + case rcl_interfaces::ParameterDescription::INTEGER_PARAMETER: + value.push_back(parameter::ParameterContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + default: + // TODO: use custom exception + throw std::runtime_error("Invalid value type"); } - ); + } + promise_result.set_value(value); + if (callback != nullptr) { + callback(future_result); + } + } + ); } return future_result; } std::shared_future -Node::async_set_params( - const std::string& node_name, - const std::vector& key_values, - std::function)> callback) +Node::async_set_parameters( + const std::string & node_name, + const std::vector & key_values, + std::function)> callback) { std::promise promise_result; auto future_result = promise_result.get_future().share(); 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()); + for (auto kv : key_values) { + switch (kv.get_typeID()) { + 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: + switch (kv.get_typeID()) { + 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; @@ -455,52 +466,55 @@ Node::async_set_params( } client->async_send_request( - request, [&promise_result, &future_result, &callback] ( - rclcpp::client::Client::SharedFuture cb_f) { - promise_result.set_value(cb_f.get()->success); - if (callback != nullptr) { - callback(future_result); - } - } - ); + request, [&promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + promise_result.set_value(cb_f.get()->success); + if (callback != nullptr) { + callback(future_result); + } + } + ); } return future_result; } -template +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); - if (callback != nullptr) { - callback(future_result); - } - } - ); + request, [&promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + promise_result.set_value(cb_f.get()->result[0]); + if (callback != nullptr) { + callback(future_result); + } + } + ); } return future_result; } diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 29e6fbf980..988c5b4cf7 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,10 +30,12 @@ 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 @@ -59,93 +61,118 @@ class ParamContainer T get_value() const; - inline ParamName get_name() const {return name_; } - - inline ParamDataType get_typeID() const {return typeID_; } - - /* Templated setter */ - template - void - set_value(const ParamName & name, const T & value); + // Structure to store an arbitrary parameter with templated get/set methods + class ParameterContainer + { +public: + ParameterContainer() + : typeID_(INVALID_PARAMETER) {} + ParameterContainer( + const std::string & name, const int64_t int_value) + : name_(name), typeID_(INTEGER_PARAMETER), int_value_(int_value) {} + ParameterContainer( + const std::string & name, const double double_value) + : name_(name), typeID_(DOUBLE_PARAMETER), double_value_(double_value) {} + ParameterContainer( + const std::string & name, const std::string & string_value) + : name_(name), typeID_(STRING_PARAMETER), string_value_(string_value) {} + ParameterContainer( + const std::string & name, const bool bool_value) + : name_(name), typeID_(BOOL_PARAMETER), bool_value_(bool_value) {} + + ParameterName name_; + ParameterDataType typeID_; + + /* Templated getter */ + template + T + get_value() const; + + inline ParameterName get_name() const {return name_; } + + inline ParameterDataType get_typeID() const {return typeID_; } private: - ParamDataType typeID_; - ParamName name_; - int64_t int_value_; - double double_value_; - std::string string_value_; - bool bool_value_; -}; + ParamDataType typeID_; + ParamName name_; + int64_t int_value_; + double double_value_; + std::string string_value_; + bool bool_value_; + }; -template<> -inline int64_t ParamContainer::get_value() const -{ - if (typeID_ != INT_PARAM) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return int_value_; -} -template<> -inline double ParamContainer::get_value() const -{ - if (typeID_ != DOUBLE_PARAM) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return double_value_; -} -template<> -inline std::string ParamContainer::get_value() const -{ - if (typeID_ != STRING_PARAM) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); + template<> + inline int64_t ParameterContainer::get_value() const + { + if (typeID_ != INTEGER_PARAMETER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return int_value_; } - return string_value_; -} -template<> -inline bool ParamContainer::get_value() const -{ - if (typeID_ != BOOL_PARAM) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); + template<> + inline double ParameterContainer::get_value() const + { + if (typeID_ != DOUBLE_PARAMETER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return double_value_; } - return bool_value_; -} - -class ParamQuery -{ -public: - ParamQuery(const std::string & name) - : typeID_(INVALID_PARAM), name_(name) {} - ParamQuery(const ParamDataType 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 + template<> + inline std::string ParameterContainer::get_value() const { - return typeID_; + if (typeID_ != STRING_PARAMETER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return string_value_; } - inline ParamName get_name() const + template<> + inline bool ParameterContainer::get_value() const { - return name_; + if (typeID_ != BOOL_PARAMETER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return bool_value_; } + class ParameterQuery + { +public: + 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 ParameterDataType get_type() const + { + return typeID_; + } + 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) + bool get_parameter_value(rcl_interfaces::Parameter & parameter) { - if (parameter.description.param_type != rcl_interfaces::ParamDescription::BOOL_PARAM) { + if (parameter.description.parameter_type != + rcl_interfaces::ParameterDescription::BOOL_PARAMETER) + { // TODO: use custom exception throw std::runtime_error("Parameter value is not a boolean"); } @@ -153,9 +180,11 @@ class ParamQuery } template<> - int64_t get_parameter_value(rcl_interfaces::Param & parameter) + int64_t get_parameter_value(rcl_interfaces::Parameter & parameter) { - if (parameter.description.param_type != rcl_interfaces::ParamDescription::INTEGER_PARAM) { + if (parameter.description.parameter_type != + rcl_interfaces::ParameterDescription::INTEGER_PARAMETER) + { // TODO: use custom exception throw std::runtime_error("Parameter value is not an integer"); } @@ -163,9 +192,11 @@ class ParamQuery } template<> - double get_parameter_value(rcl_interfaces::Param & parameter) + double get_parameter_value(rcl_interfaces::Parameter & parameter) { - if (parameter.description.param_type != rcl_interfaces::ParamDescription::DOUBLE_PARAM) { + if (parameter.description.parameter_type != + rcl_interfaces::ParameterDescription::DOUBLE_PARAMETER) + { // TODO: use custom exception throw std::runtime_error("Parameter value is not a double"); } @@ -173,9 +204,11 @@ class ParamQuery } template<> - std::string get_parameter_value(rcl_interfaces::Param & parameter) + std::string get_parameter_value(rcl_interfaces::Parameter & parameter) { - if (parameter.description.param_type != rcl_interfaces::ParamDescription::STRING_PARAM) { + if (parameter.description.parameter_type != + rcl_interfaces::ParameterDescription::STRING_PARAMETER) + { // TODO: use custom exception throw std::runtime_error("Parameter value is not a string"); } @@ -183,33 +216,33 @@ class ParamQuery } 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) + void set_parameter_value(rcl_interfaces::Parameter & parameter, const bool & value) { - parameter.description.param_type = rcl_interfaces::ParamDescription::BOOL_PARAM; + 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) + void set_parameter_value(rcl_interfaces::Parameter & parameter, const int64_t & value) { - parameter.description.param_type = rcl_interfaces::ParamDescription::INTEGER_PARAM; + 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) + void set_parameter_value(rcl_interfaces::Parameter & parameter, const double & value) { - parameter.description.param_type = rcl_interfaces::ParamDescription::DOUBLE_PARAM; + 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) + void set_parameter_value(rcl_interfaces::Parameter & parameter, const std::string & value) { - parameter.description.param_type = rcl_interfaces::ParamDescription::STRING_PARAM; + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::STRING_PARAMETER; parameter.string_value = value; } }; From 0e315e09da67d6ca4ff8a8379dc2b1d599e2bc97 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 16 Apr 2015 11:31:48 -0700 Subject: [PATCH 19/26] Refactored has_parameter to use has_parameters --- rclcpp/include/rclcpp/node.hpp | 5 ++++ rclcpp/include/rclcpp/node_impl.hpp | 44 +++++++++++++++++++++++------ 2 files changed, 41 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7e3dc65a14..7a9d3ec09b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -143,6 +143,11 @@ class Node const std::string & node_name, const parameter::ParameterQuery & query, std::function)> callback = nullptr); + std::shared_future> + async_has_parameters( + const std::string & node_name, const std::vector & queries, + std::function>)> callback = nullptr); + template std::shared_future async_set_parameter( diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 1ec83f6a65..7843cbe751 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -355,7 +355,7 @@ Node::async_get_parameters( callback(future_result); } } else { - auto client = this->create_client("get_params"); + auto client = this->create_client("get_parameters"); auto request = std::make_shared(); for (auto parameter_name : parameter_names) { rcl_interfaces::ParameterDescription description; @@ -437,7 +437,7 @@ Node::async_set_parameters( callback(future_result); } } else { - auto client = this->create_client("set_params"); + auto client = this->create_client("set_parameters"); auto request = std::make_shared(); for (auto kv: key_values) { rcl_interfaces::Parameter parameter; @@ -498,18 +498,46 @@ Node::async_has_parameter( { std::promise promise_result; auto future_result = promise_result.get_future().share(); + this->async_has_parameters(node_name, {{query}}, + [&promise_result, &future_result, &callback]( + std::shared_future> cb_f) { + promise_result.set_value(cb_f.get()[0]); + if (callback != nullptr) { + callback(future_result); + } + } + ); + return future_result; +} + +std::shared_future> +Node::async_has_parameters( + const std::string & node_name, const std::vector & queries, + 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_parameter(query)); + std::vector value; + for (auto query: queries) { + value.push_back(this->has_parameter(query)); + } + promise_result.set_value(value); + if (callback != nullptr) { + callback(future_result); + } } else { - auto client = this->create_client("has_params"); + auto client = this->create_client("has_parameters"); auto request = std::make_shared(); - rcl_interfaces::ParameterDescription parameter_description; - parameter_description.name = query.get_name(); - request->parameter_descriptions.push_back(parameter_description); + for (auto query: queries) { + 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[0]); + promise_result.set_value(cb_f.get()->result); if (callback != nullptr) { callback(future_result); } From a551911580f3961ca86eb4adfa5077038e5caa33 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Apr 2015 11:41:40 -0700 Subject: [PATCH 20/26] Renamed params_ to parameters_ --- rclcpp/include/rclcpp/node.hpp | 2 +- rclcpp/include/rclcpp/node_impl.hpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7a9d3ec09b..58628a84a2 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -185,7 +185,7 @@ class Node std::shared_ptr serv_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); - std::map params_; + std::map parameters_; template ParamTypeT get_parameter(const parameter::ParameterName & key) const; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 7843cbe751..28de4b68d1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -258,7 +258,7 @@ template ParamTypeT Node::get_parameter(const parameter::ParameterName & key) const { - const parameter::ParameterContainer pc(this->params_.at(key)); + const parameter::ParameterContainer pc(this->parameters_.at(key)); return pc.get_value(); } @@ -267,14 +267,14 @@ void Node::set_parameter(const parameter::ParameterName & key, const ParamTypeT & value) { parameter::ParameterContainer pc(key, value); - params_[key] = pc; + parameters_[key] = pc; } bool Node::has_parameter(const parameter::ParameterQuery & query) const { const parameter::ParameterName key = query.get_name(); - return params_.find(key) != params_.end(); + return parameters_.find(key) != parameters_.end(); } std::vector @@ -282,7 +282,7 @@ Node::get_parameters(const std::vector & queries) con { std::vector result; - for (auto & kv: params_) { + for (auto & kv: parameters_) { if (std::any_of(queries.cbegin(), queries.cend(), [&kv](const parameter::ParameterQuery & query) { return kv.first == query.get_name(); From d5b7d139e8e0d0e4338194ce2e9ac4a24421982e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Apr 2015 13:19:37 -0700 Subject: [PATCH 21/26] Export rcl_interfaces dependency --- rclcpp/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 25600ca367..d059cf178a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(ament_cmake REQUIRED) find_package(rcl_interfaces REQUIRED) ament_export_dependencies(rmw) +ament_export_dependencies(rcl_interfaces) ament_export_include_directories(include) From ad987b198082ac97297cd3fe208d7dfef6a672e6 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Apr 2015 13:39:40 -0700 Subject: [PATCH 22/26] Added run dependency for rcl_interfaces --- rclcpp/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 1b41b12501..5c7e924641 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -11,6 +11,7 @@ rmw rcl_interfaces + rcl_interfaces ament_lint_auto ament_lint_common From 7a8ef8463230a3343aa930706a1de7aae8c90679 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Apr 2015 14:34:45 -0700 Subject: [PATCH 23/26] Fix code style --- rclcpp/include/rclcpp/parameter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 988c5b4cf7..014bda15d9 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -61,7 +61,7 @@ class ParamContainer T get_value() const; - // Structure to store an arbitrary parameter with templated get/set methods +// Structure to store an arbitrary parameter with templated get/set methods class ParameterContainer { public: From 8a7dc46a96d6e9cd489b23afc4d709d7f91ea4a6 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 17 Apr 2015 15:02:50 -0700 Subject: [PATCH 24/26] Use only a ParameterName to check for parameter existence --- rclcpp/include/rclcpp/node.hpp | 6 +++--- rclcpp/include/rclcpp/node_impl.hpp | 19 +++++++++---------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 58628a84a2..9f14f8a481 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -140,12 +140,12 @@ class Node std::shared_future async_has_parameter( - const std::string & node_name, const parameter::ParameterQuery & query, + const std::string & node_name, const parameter::ParameterName & parameter_name, std::function)> callback = nullptr); std::shared_future> async_has_parameters( - const std::string & node_name, const std::vector & queries, + const std::string & node_name, const std::vector & parameter_names, std::function>)> callback = nullptr); template @@ -194,7 +194,7 @@ class Node get_parameters(const std::vector & query) const; bool - has_parameter(const parameter::ParameterQuery & query) const; + has_parameter(const parameter::ParameterName & parameter_name) const; template void set_parameter(const parameter::ParameterName & key, const ParamTypeT & value); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 28de4b68d1..1e4e530334 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -271,10 +271,9 @@ Node::set_parameter(const parameter::ParameterName & key, const ParamTypeT & val } bool -Node::has_parameter(const parameter::ParameterQuery & query) const +Node::has_parameter(const parameter::ParameterName & parameter_name) const { - const parameter::ParameterName key = query.get_name(); - return parameters_.find(key) != parameters_.end(); + return parameters_.find(parameter_name) != parameters_.end(); } std::vector @@ -493,12 +492,12 @@ Node::async_set_parameter( std::shared_future Node::async_has_parameter( - const std::string & node_name, const parameter::ParameterQuery & query, + const std::string & node_name, const parameter::ParameterName & parameter_name, std::function)> callback) { std::promise promise_result; auto future_result = promise_result.get_future().share(); - this->async_has_parameters(node_name, {{query}}, + this->async_has_parameters(node_name, {{parameter_name}}, [&promise_result, &future_result, &callback]( std::shared_future> cb_f) { promise_result.set_value(cb_f.get()[0]); @@ -512,15 +511,15 @@ Node::async_has_parameter( std::shared_future> Node::async_has_parameters( - const std::string & node_name, const std::vector & queries, + const std::string & node_name, const std::vector & parameter_names, std::function>)> callback) { std::promise> promise_result; auto future_result = promise_result.get_future().share(); if (node_name == this->get_name()) { std::vector value; - for (auto query: queries) { - value.push_back(this->has_parameter(query)); + for (auto parameter_name: parameter_names) { + value.push_back(this->has_parameter(parameter_name)); } promise_result.set_value(value); if (callback != nullptr) { @@ -529,9 +528,9 @@ Node::async_has_parameters( } else { auto client = this->create_client("has_parameters"); auto request = std::make_shared(); - for (auto query: queries) { + for (auto parameter_name: parameter_names) { rcl_interfaces::ParameterDescription parameter_description; - parameter_description.name = query.get_name(); + parameter_description.name = parameter_name; request->parameter_descriptions.push_back(parameter_description); } client->async_send_request( From 8f843d975fd58c7d7470bb3f7bf9be654c26ed19 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 20 Apr 2015 10:29:26 -0700 Subject: [PATCH 25/26] Added spin_until_future_complete and spin_node_until_future_complete helper functions --- rclcpp/include/rclcpp/executors.hpp | 14 ++++++++++++++ rclcpp/include/rclcpp/rclcpp.hpp | 11 +++++++++++ 2 files changed, 25 insertions(+) diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 8428aa65d0..8c02eb0ebc 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -26,6 +26,20 @@ namespace executors using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; +template +std::shared_future & +spin_node_until_future_complete( + rclcpp::executor::Executor & executor, Node::SharedPtr & node_ptr, + std::shared_future & future) +{ + std::future_status status; + do { + executor.spin_node_some(node_ptr); + status = future.wait_for(std::chrono::seconds(0)); + } while (status != std::future_status::ready && rclcpp::utilities::ok()); + return future; +} + } // namespace executors } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 040cf3897e..90c0cb986c 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -76,6 +76,17 @@ void spin(Node::SharedPtr & node_ptr) executor.spin(); } +template +std::shared_future & +spin_until_future_complete( + Node::SharedPtr & node_ptr, std::shared_future & future) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::spin_node_until_future_complete( + executor, node_ptr, future); + return future; +} + } /* namespace rclcpp */ #endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */ From ebfdfe04c9425b633680f6329ce514a9eafe879d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 20 Apr 2015 10:34:51 -0700 Subject: [PATCH 26/26] Fix code style --- rclcpp/include/rclcpp/executors.hpp | 2 +- rclcpp/include/rclcpp/rclcpp.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 8c02eb0ebc..3fedaabbea 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -26,7 +26,7 @@ namespace executors using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; -template +template std::shared_future & spin_node_until_future_complete( rclcpp::executor::Executor & executor, Node::SharedPtr & node_ptr, diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 90c0cb986c..02d835a37f 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -76,7 +76,7 @@ void spin(Node::SharedPtr & node_ptr) executor.spin(); } -template +template std::shared_future & spin_until_future_complete( Node::SharedPtr & node_ptr, std::shared_future & future)