From 346f17aff0883688159433547e18af663228afc3 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 5 May 2015 17:01:34 -0700 Subject: [PATCH 1/6] Split parameter.hpp into parameter_client.hpp --- rclcpp/include/rclcpp/node.hpp | 24 ++++- rclcpp/include/rclcpp/parameter.hpp | 151 ---------------------------- rclcpp/include/rclcpp/rclcpp.hpp | 1 + 3 files changed, 21 insertions(+), 155 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 888b962..d31ad89 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -29,6 +29,9 @@ #include #include + +#include + // Forward declaration of ROS middleware class namespace rmw { @@ -146,6 +149,20 @@ public: FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + const std::vector set_parameters( + const std::vector & parameters) + { + std::vector results; + for (auto p : parameters) { +// parameters_[p.name] = ParameterVariant::from_parameter_value(p.value); + rcl_interfaces::SetParametersResult result; + result.successful = true; + // TODO: handle parameter constraints + results.push_back(result); + } + return results; + } + private: RCLCPP_DISABLE_COPY(Node); @@ -166,10 +183,9 @@ private: size_t number_of_services_; size_t number_of_clients_; - void register_service( - const std::string & service_name, - std::shared_ptr serv_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group); + std::mutex mutex_; + +// std::map parameters_; template< typename ServiceT, diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 266eb45..bb27281 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -19,10 +19,6 @@ #include -#include -#include -#include - #include #include #include @@ -179,153 +175,6 @@ private: rcl_interfaces::ParameterValue value_; }; -class AsyncParametersClient -{ - -public: - RCLCPP_MAKE_SHARED_DEFINITIONS(AsyncParametersClient); - - AsyncParametersClient(const rclcpp::node::Node::SharedPtr & node) - : node_(node) - { - get_parameters_client_ = node_->create_client( - "get_parameters"); - get_parameter_types_client_ = node_->create_client( - "get_parameter_types"); - set_parameters_client_ = node_->create_client( - "set_parameters"); - list_parameters_client_ = node_->create_client( - "list_parameters"); - describe_parameters_client_ = node_->create_client( - "describe_parameters"); - } - - std::shared_future> - get_parameters( - std::vector names, - std::function>)> callback = nullptr) - { - std::shared_future> f; - return f; - } - - std::shared_future> - get_parameter_types( - std::vector parameter_names, - std::function>)> callback = nullptr) - { - std::shared_future> f; - return f; - } - - std::shared_future> - set_parameters( - std::vector parameters, - std::function>)> callback = nullptr) - { - std::shared_future> f; - return f; - } - - std::shared_future - set_parameters_atomically( - std::vector parameters, - std::function)> callback = nullptr) - { - std::shared_future f; - return f; - } - - std::shared_future - list_parameters( - std::vector parameter_prefixes, - uint64_t depth, - std::function)> callback = nullptr) - { - std::shared_future f; - return f; - } - -private: - const rclcpp::node::Node::SharedPtr node_; - rclcpp::client::Client::SharedPtr get_parameters_client_; - rclcpp::client::Client::SharedPtr get_parameter_types_client_; - rclcpp::client::Client::SharedPtr set_parameters_client_; - rclcpp::client::Client::SharedPtr - set_parameters_atomically_client_; - rclcpp::client::Client::SharedPtr list_parameters_client_; - rclcpp::client::Client::SharedPtr describe_parameters_client_; -}; - -class SyncParametersClient -{ -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::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::ListParametersResult - 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 */ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index ac907d6..a9d484f 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include From 9a499cf726ad7a0e8a8b24abdff917051ca0c9dc Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 5 May 2015 17:34:12 -0700 Subject: [PATCH 2/6] Added from_parameter --- rclcpp/include/rclcpp/node.hpp | 5 +++-- rclcpp/include/rclcpp/parameter.hpp | 18 ++++++++++++++++++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index d31ad89..3324e97 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -154,7 +155,7 @@ public: { std::vector results; for (auto p : parameters) { -// parameters_[p.name] = ParameterVariant::from_parameter_value(p.value); + parameters_[p.name] = rclcpp::parameter::ParameterVariant::from_parameter(p); rcl_interfaces::SetParametersResult result; result.successful = true; // TODO: handle parameter constraints @@ -185,7 +186,7 @@ private: std::mutex mutex_; -// std::map parameters_; + std::map parameters_; template< typename ServiceT, diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index bb27281..bc0a4b3 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -170,6 +170,24 @@ public: return get_value(); } + static ParameterVariant from_parameter(const rcl_interfaces::Parameter & parameter) { + switch(parameter.value.parameter_type) { + case PARAMETER_BOOL: + return ParameterVariant(parameter.name, parameter.value.bool_value); + case PARAMETER_INTEGER: + return ParameterVariant(parameter.name, parameter.value.integer_value); + case PARAMETER_DOUBLE: + return ParameterVariant(parameter.name, parameter.value.double_value); + case PARAMETER_STRING: + return ParameterVariant(parameter.name, parameter.value.string_value); + case PARAMETER_BYTES: + return ParameterVariant(parameter.name, parameter.value.bytes_value); + case PARAMETER_NOT_SET: + default: + // TODO: use custom exception + throw std::runtime_error("Invalid type from ParameterValue"); + } + } private: std::string name_; rcl_interfaces::ParameterValue value_; From 0ddefd1efdb38efbda3d4b1640e7ca2c6f1fad14 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 5 May 2015 17:34:32 -0700 Subject: [PATCH 3/6] Fix code style --- rclcpp/include/rclcpp/parameter.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index bc0a4b3..26bba4f 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -170,8 +170,9 @@ public: return get_value(); } - static ParameterVariant from_parameter(const rcl_interfaces::Parameter & parameter) { - switch(parameter.value.parameter_type) { + static ParameterVariant from_parameter(const rcl_interfaces::Parameter & parameter) + { + switch (parameter.value.parameter_type) { case PARAMETER_BOOL: return ParameterVariant(parameter.name, parameter.value.bool_value); case PARAMETER_INTEGER: @@ -188,6 +189,7 @@ public: throw std::runtime_error("Invalid type from ParameterValue"); } } + private: std::string name_; rcl_interfaces::ParameterValue value_; From 72a9287185739b49498db06bba6838c14454d21e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 7 May 2015 17:35:04 -0700 Subject: [PATCH 4/6] Added internal parameters API. --- rclcpp/include/rclcpp/node.hpp | 28 +-- rclcpp/include/rclcpp/node_impl.hpp | 117 +++++++++++++ rclcpp/include/rclcpp/parameter_client.hpp | 194 +++++++++++++++++++++ 3 files changed, 327 insertions(+), 12 deletions(-) create mode 100644 rclcpp/include/rclcpp/parameter_client.hpp diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 3324e97..f8e060a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -151,18 +151,22 @@ public: rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); const std::vector set_parameters( - const std::vector & parameters) - { - std::vector results; - for (auto p : parameters) { - parameters_[p.name] = rclcpp::parameter::ParameterVariant::from_parameter(p); - rcl_interfaces::SetParametersResult result; - result.successful = true; - // TODO: handle parameter constraints - results.push_back(result); - } - return results; - } + const std::vector & parameters); + + const rcl_interfaces::SetParametersResult set_parameters_atomically( + const std::vector & parameters); + + const std::vector get_parameters( + const std::vector & names); + + const std::vector describe_parameters( + const std::vector & names); + + const std::vector get_parameter_types( + const std::vector & names); + + const std::vector list_parameters( + const std::vector & prefixes, uint64_t depth); private: RCLCPP_DISABLE_COPY(Node); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index cb9a414..bfd9fe2 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 @@ -212,4 +213,120 @@ Node::create_service( return serv; } +const std::vector +Node::set_parameters( + const std::vector & parameters) +{ + std::vector results; + for (auto p : parameters) { + parameters_[p.name] = rclcpp::parameter::ParameterVariant::from_parameter(p); + rcl_interfaces::SetParametersResult result; + result.successful = true; + // TODO: handle parameter constraints + results.push_back(result); + } + return results; +} + +const rcl_interfaces::SetParametersResult +Node::set_parameters_atomically( + const std::vector & parameters) +{ + std::lock_guard lock(mutex_); + std::map tmp_map; + for (auto p : parameters) { + tmp_map[p.name] = rclcpp::parameter::ParameterVariant::from_parameter(p); + } + tmp_map.insert(parameters_.begin(), parameters_.end()); + std::swap(tmp_map, parameters_); + // TODO: handle parameter constraints + rcl_interfaces::SetParametersResult result; + result.successful = true; + return result; +} + +const std::vector +Node::get_parameters( + const std::vector & names) +{ + std::vector results; + for (auto & kv : parameters_) { + if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { + return name == kv.first; + })) + { + results.push_back(kv.second); + } + } + return results; +} + +const std::vector +Node::describe_parameters( + const std::vector & names) +{ + std::vector results; + for (auto & kv : parameters_) { + if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { + return name == kv.first; + })) + { + rcl_interfaces::ParameterDescriptor parameter_descriptor; + parameter_descriptor.name = kv.first; + parameter_descriptor.parameter_type = kv.second.get_type(); + results.push_back(parameter_descriptor); + } + } + return results; +} + +const std::vector +Node::get_parameter_types( + const std::vector & names) +{ + std::vector results; + for (auto & kv : parameters_) { + if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { + return name == kv.first; + })) + { + results.push_back(kv.second.get_type()); + } else { + results.push_back(rcl_interfaces::ParameterType::PARAMETER_NOT_SET); + } + } + return results; +} + +const std::vector +Node::list_parameters( + const std::vector & prefixes, uint64_t depth) +{ + std::vector results; + + // TODO: define parameter separator, use "." for now + for (auto & kv : parameters_) { + if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) { + if (kv.first.find(prefix + ".") == 0) { + size_t length = prefix.length(); + std::string substr = kv.first.substr(length); + return std::count(substr.begin(), substr.end(), '.') < depth; + } + return false; + })) + { + rcl_interfaces::ListParametersResult result; + result.parameter_names.push_back(kv.first); + size_t last_separator = kv.first.find_last_of('.'); + std::string prefix = kv.first.substr(0, last_separator); + if (std::find(result.parameter_prefixes.cbegin(), result.parameter_prefixes.cend(), + prefix) == result.parameter_prefixes.cend()) + { + result.parameter_prefixes.push_back(prefix); + } + results.push_back(result); + } + } + return results; +} #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp new file mode 100644 index 0000000..1e17533 --- /dev/null +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -0,0 +1,194 @@ +// Copyright 2015 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_CLIENT_HPP_ +#define RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ + +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ + +namespace parameter_client +{ + +class AsyncParametersClient +{ + +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(AsyncParametersClient); + + AsyncParametersClient(const rclcpp::node::Node::SharedPtr & node) + : node_(node) + { + get_parameters_client_ = node_->create_client( + "get_parameters"); + get_parameter_types_client_ = node_->create_client( + "get_parameter_types"); + set_parameters_client_ = node_->create_client( + "set_parameters"); + list_parameters_client_ = node_->create_client( + "list_parameters"); + describe_parameters_client_ = node_->create_client( + "describe_parameters"); + } + + std::shared_future> + get_parameters( + std::vector names, + std::function>)> callback = nullptr) + { + std::shared_future> f; + return f; + } + + std::shared_future> + get_parameter_types( + std::vector parameter_names, + std::function>)> callback = nullptr) + { + std::shared_future> f; + return f; + } + + std::shared_future> + set_parameters( + std::vector parameters, + std::function>)> callback = nullptr) + { + std::shared_future> f; + return f; + } + + std::shared_future + set_parameters_atomically( + std::vector parameters, + std::function)> callback = nullptr) + { + std::shared_future f; + return f; + } + + std::shared_future + list_parameters( + std::vector parameter_prefixes, + uint64_t depth, + std::function)> callback = nullptr) + { + std::shared_future f; + return f; + } + +private: + const rclcpp::node::Node::SharedPtr node_; + rclcpp::client::Client::SharedPtr get_parameters_client_; + rclcpp::client::Client::SharedPtr get_parameter_types_client_; + rclcpp::client::Client::SharedPtr set_parameters_client_; + rclcpp::client::Client::SharedPtr + set_parameters_atomically_client_; + rclcpp::client::Client::SharedPtr list_parameters_client_; + rclcpp::client::Client::SharedPtr describe_parameters_client_; +}; + +class SyncParametersClient +{ +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::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::ListParametersResult + 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_client */ + +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ */ From 49fc07dab3b16081702accaccea90ce00868be29 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 15 May 2015 12:43:52 -0700 Subject: [PATCH 5/6] Lock the mutex for every method --- rclcpp/include/rclcpp/node_impl.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index bfd9fe2..381b573 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -217,6 +217,7 @@ const std::vector Node::set_parameters( const std::vector & parameters) { + std::lock_guard lock(mutex_); std::vector results; for (auto p : parameters) { parameters_[p.name] = rclcpp::parameter::ParameterVariant::from_parameter(p); @@ -249,6 +250,7 @@ const std::vector Node::get_parameters( const std::vector & names) { + std::lock_guard lock(mutex_); std::vector results; for (auto & kv : parameters_) { if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { @@ -265,6 +267,7 @@ const std::vector Node::describe_parameters( const std::vector & names) { + std::lock_guard lock(mutex_); std::vector results; for (auto & kv : parameters_) { if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { @@ -284,6 +287,7 @@ const std::vector Node::get_parameter_types( const std::vector & names) { + std::lock_guard lock(mutex_); std::vector results; for (auto & kv : parameters_) { if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { @@ -302,6 +306,7 @@ const std::vector Node::list_parameters( const std::vector & prefixes, uint64_t depth) { + std::lock_guard lock(mutex_); std::vector results; // TODO: define parameter separator, use "." for now From 1e516426d051b5c5605c1ca2d5b8fcd17206b270 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 15 May 2015 16:06:05 -0700 Subject: [PATCH 6/6] Removed unused include --- rclcpp/include/rclcpp/node.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f8e060a..5b76e7a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -31,8 +31,6 @@ #include -#include - // Forward declaration of ROS middleware class namespace rmw {