From 9bf3d0839a940279f34389995539cdd9dbfc9ea1 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 30 Apr 2015 16:38:12 -0700 Subject: [PATCH 1/7] Added SyncParametersClient --- rclcpp/include/rclcpp/parameter.hpp | 69 +++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index e0f01a8..6c318fa 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -19,6 +19,7 @@ #include +#include #include #include @@ -156,6 +157,8 @@ class AsyncParametersClient { public: + RCLCPP_MAKE_SHARED_DEFINITIONS(AsyncParametersClient); + AsyncParametersClient(const rclcpp::node::Node::SharedPtr & node) : node_(node) { @@ -233,6 +236,72 @@ private: rclcpp::client::Client::SharedPtr describe_parameters_client_; }; +class SyncParametersClient +{ + friend class rclcpp::executor::Executor; + +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(SyncParametersClient); + + SyncParametersClient( + rclcpp::node::Node::SharedPtr & node) + : node_(node) + { + executor_ = std::make_shared(); + async_parameters_client_ = std::make_shared(node); + } + + SyncParametersClient( + rclcpp::executor::Executor::SharedPtr & executor, + rclcpp::node::Node::SharedPtr & node) + : executor_(executor), node_(node) + { + async_parameters_client_ = std::make_shared(node); + } + + std::vector + get_parameters(std::vector parameter_names) + { + auto f = async_parameters_client_->get_parameters(parameter_names); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + + std::vector + get_parameter_types(std::vector parameter_names) + { + auto f = async_parameters_client_->get_parameter_types(parameter_names); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + + std::vector + set_parameters(std::vector parameters) + { + auto f = async_parameters_client_->set_parameters(parameters); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + + rcl_interfaces::ParameterSetResult + set_parameters_atomically(std::vector parameters) + { + auto f = async_parameters_client_->set_parameters_atomically(parameters); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + + rcl_interfaces::ParameterListResult + list_parameters( + std::vector parameter_prefixes, + uint64_t depth) + { + auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + +private: + rclcpp::executor::Executor::SharedPtr executor_; + rclcpp::node::Node::SharedPtr node_; + AsyncParametersClient::SharedPtr async_parameters_client_; +}; + } /* namespace parameter */ } /* namespace rclcpp */ From 7c54d94cbd3acf04768bcb2328d3855bebfa8344 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 1 May 2015 12:17:37 -0700 Subject: [PATCH 2/7] Added explicit constructors for int and float --- rclcpp/include/rclcpp/parameter.hpp | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 6c318fa..bdacfe9 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -57,31 +57,43 @@ public: { value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_NOT_SET; } - ParameterVariant(const std::string & name, const bool bool_value) + explicit ParameterVariant(const std::string & name, const bool bool_value) : name_(name) { value_.bool_value = bool_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_BOOL; } - ParameterVariant(const std::string & name, const int64_t int_value) + explicit ParameterVariant(const std::string & name, const int int_value) : name_(name) { value_.integer_value = int_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_INTEGER; } - ParameterVariant(const std::string & name, const double double_value) + explicit ParameterVariant(const std::string & name, const int64_t int_value) + : name_(name) + { + value_.integer_value = int_value; + value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_INTEGER; + } + explicit ParameterVariant(const std::string & name, const float double_value) : name_(name) { value_.double_value = double_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_DOUBLE; } - ParameterVariant(const std::string & name, const std::string & string_value) + explicit ParameterVariant(const std::string & name, const double double_value) + : name_(name) + { + value_.double_value = double_value; + value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_DOUBLE; + } + explicit ParameterVariant(const std::string & name, const std::string & string_value) : name_(name) { value_.string_value = string_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_STRING; } - ParameterVariant(const std::string & name, const std::vector & bytes_value) + explicit ParameterVariant(const std::string & name, const std::vector & bytes_value) : name_(name) { value_.bytes_value = bytes_value; From 9943576fe795678f9d58e48bc93ed9a3284e8993 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 4 May 2015 10:12:44 -0700 Subject: [PATCH 3/7] Use unsigned int8 to match the byte primitive type --- 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 bdacfe9..f5cba37 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -93,7 +93,7 @@ public: value_.string_value = string_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_STRING; } - explicit ParameterVariant(const std::string & name, const std::vector & bytes_value) + explicit ParameterVariant(const std::string & name, const std::vector & bytes_value) : name_(name) { value_.bytes_value = bytes_value; @@ -156,7 +156,7 @@ inline bool ParameterVariant::get_value() const return value_.bool_value; } template<> -inline std::vector ParameterVariant::get_value() const +inline std::vector ParameterVariant::get_value() const { if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BYTES) { // TODO: use custom exception From e7ca3d3698a0f0fca490fa1de7a1e74c05104371 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 4 May 2015 10:29:33 -0700 Subject: [PATCH 4/7] Removed friend --- rclcpp/include/rclcpp/parameter.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index f5cba37..8bb998d 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -250,8 +250,6 @@ private: class SyncParametersClient { - friend class rclcpp::executor::Executor; - public: RCLCPP_MAKE_SHARED_DEFINITIONS(SyncParametersClient); From db0e117c7269dc80ce00f8c380fe350fbe514202 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 4 May 2015 11:01:15 -0700 Subject: [PATCH 5/7] Use ListParametersResult and SetParametersResult --- 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 8bb998d..8dc9ac6 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -206,34 +206,34 @@ public: return f; } - std::shared_future> + std::shared_future> set_parameters( std::vector parameters, std::function>)> callback = nullptr) + std::shared_future>)> callback = nullptr) { - std::shared_future> f; + std::shared_future> f; return f; } - std::shared_future + std::shared_future set_parameters_atomically( std::vector parameters, std::function)> callback = nullptr) + std::shared_future)> callback = nullptr) { - std::shared_future f; + std::shared_future f; return f; } - std::shared_future + std::shared_future list_parameters( std::vector parameter_prefixes, uint64_t depth, std::function)> callback = nullptr) + std::shared_future)> callback = nullptr) { - std::shared_future f; + std::shared_future f; return f; } @@ -283,21 +283,21 @@ public: return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); } - std::vector + std::vector set_parameters(std::vector parameters) { auto f = async_parameters_client_->set_parameters(parameters); return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); } - rcl_interfaces::ParameterSetResult + rcl_interfaces::SetParametersResult set_parameters_atomically(std::vector parameters) { auto f = async_parameters_client_->set_parameters_atomically(parameters); return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); } - rcl_interfaces::ParameterListResult + rcl_interfaces::ListParametersResult list_parameters( std::vector parameter_prefixes, uint64_t depth) From d82aa219d3a570e06e8d9cdcbc6dc9726664c805 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 4 May 2015 14:34:38 -0700 Subject: [PATCH 6/7] Use enum value to return appropriate values for get_value<> --- rclcpp/include/rclcpp/parameter.hpp | 102 ++++++++++++++-------------- 1 file changed, 51 insertions(+), 51 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 8dc9ac6..afb7ce1 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -100,11 +100,6 @@ public: value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_BYTES; } - /* Templated getter */ - template - T - get_value() const; - inline ParameterType get_type() const {return static_cast(value_.parameter_type); } inline std::string get_name() const & {return name_; } @@ -114,57 +109,62 @@ public: return value_; } + template + typename std::enable_if::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_INTEGER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.integer_value; + } + template + typename std::enable_if::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_DOUBLE) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.double_value; + } + template + typename std::enable_if::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_STRING) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.string_value; + } + template + typename std::enable_if::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BOOL) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.bool_value; + } + template + typename std::enable_if>::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BYTES) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.bytes_value; + } + private: std::string name_; rcl_interfaces::ParameterValue value_; }; -template<> -inline int64_t ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_INTEGER) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.integer_value; -} -template<> -inline double ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_DOUBLE) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.double_value; -} -template<> -inline std::string ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_STRING) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.string_value; -} -template<> -inline bool ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BOOL) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.bool_value; -} -template<> -inline std::vector ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BYTES) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.bytes_value; -} - class AsyncParametersClient { From cf0bc5822e1b6f135ff5f434389eff59c02b9b03 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 4 May 2015 17:31:57 -0700 Subject: [PATCH 7/7] Added as_* methods --- rclcpp/include/rclcpp/parameter.hpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index afb7ce1..3da59bb 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -160,6 +160,16 @@ public: return value_.bytes_value; } + int64_t as_int() const {return get_value(); } + + double as_double() const {return get_value(); } + + const std::string & as_string() const {return get_value(); } + + bool as_bool() const {return get_value(); } + + std::vector as_bytes() const {return get_value(); } + private: std::string name_; rcl_interfaces::ParameterValue value_;