From a50dc29bbaf34bdcedac59a403d1230fd73cb633 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 15 May 2015 14:01:27 -0700 Subject: [PATCH] Added developer parameter API --- rclcpp/include/rclcpp/node.hpp | 30 ++--- rclcpp/include/rclcpp/node_impl.hpp | 34 +++-- rclcpp/include/rclcpp/parameter.hpp | 8 ++ rclcpp/include/rclcpp/parameter_client.hpp | 141 ++++++++++++++++++--- 4 files changed, 164 insertions(+), 49 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 5b76e7a..3661176 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -95,8 +95,8 @@ public: Node(std::string node_name, rclcpp::context::Context::SharedPtr context); /* Get the name of the node. */ - std::string - get_name(); + const std::string & + get_name() const {return name_; } /* Create and return a callback group. */ rclcpp::callback_group::CallbackGroup::SharedPtr @@ -148,23 +148,23 @@ public: FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - const std::vector set_parameters( - const std::vector & parameters); + std::vector set_parameters( + const std::vector & parameters); - const rcl_interfaces::SetParametersResult set_parameters_atomically( - const std::vector & parameters); + rcl_interfaces::SetParametersResult set_parameters_atomically( + const std::vector & parameters); - const std::vector get_parameters( - const std::vector & names); + std::vector get_parameters( + const std::vector & names) const; - const std::vector describe_parameters( - const std::vector & names); + std::vector describe_parameters( + const std::vector & names) const; - const std::vector get_parameter_types( - const std::vector & names); + std::vector get_parameter_types( + const std::vector & names) const; - const std::vector list_parameters( - const std::vector & prefixes, uint64_t depth); + rcl_interfaces::ListParametersResult list_parameters( + const std::vector & prefixes, uint64_t depth) const; private: RCLCPP_DISABLE_COPY(Node); @@ -186,7 +186,7 @@ private: size_t number_of_services_; size_t number_of_clients_; - std::mutex mutex_; + mutable std::mutex mutex_; std::map parameters_; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 381b573..185fd89 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -213,14 +213,14 @@ Node::create_service( return serv; } -const std::vector +std::vector Node::set_parameters( - const std::vector & 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); + parameters_[p.get_name()] = p; rcl_interfaces::SetParametersResult result; result.successful = true; // TODO: handle parameter constraints @@ -229,14 +229,14 @@ Node::set_parameters( return results; } -const rcl_interfaces::SetParametersResult +rcl_interfaces::SetParametersResult Node::set_parameters_atomically( - const std::vector & parameters) + 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[p.get_name()] = p; } tmp_map.insert(parameters_.begin(), parameters_.end()); std::swap(tmp_map, parameters_); @@ -246,9 +246,9 @@ Node::set_parameters_atomically( return result; } -const std::vector +std::vector Node::get_parameters( - const std::vector & names) + const std::vector & names) const { std::lock_guard lock(mutex_); std::vector results; @@ -263,9 +263,9 @@ Node::get_parameters( return results; } -const std::vector +std::vector Node::describe_parameters( - const std::vector & names) + const std::vector & names) const { std::lock_guard lock(mutex_); std::vector results; @@ -283,9 +283,9 @@ Node::describe_parameters( return results; } -const std::vector +std::vector Node::get_parameter_types( - const std::vector & names) + const std::vector & names) const { std::lock_guard lock(mutex_); std::vector results; @@ -302,12 +302,12 @@ Node::get_parameter_types( return results; } -const std::vector +rcl_interfaces::ListParametersResult Node::list_parameters( - const std::vector & prefixes, uint64_t depth) + const std::vector & prefixes, uint64_t depth) const { std::lock_guard lock(mutex_); - std::vector results; + rcl_interfaces::ListParametersResult result; // TODO: define parameter separator, use "." for now for (auto & kv : parameters_) { @@ -320,7 +320,6 @@ Node::list_parameters( 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); @@ -329,9 +328,8 @@ Node::list_parameters( { result.parameter_prefixes.push_back(prefix); } - results.push_back(result); } } - return results; + return result; } #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 26bba4f..30cde6f 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -190,6 +190,14 @@ public: } } + rcl_interfaces::Parameter to_parameter() + { + rcl_interfaces::Parameter parameter; + parameter.name = name_; + parameter.value = value_; + return parameter; + } + private: std::string name_; rcl_interfaces::ParameterValue value_; diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 1e17533..69aa99c 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -46,19 +46,25 @@ class AsyncParametersClient public: RCLCPP_MAKE_SHARED_DEFINITIONS(AsyncParametersClient); - AsyncParametersClient(const rclcpp::node::Node::SharedPtr & node) + AsyncParametersClient(const rclcpp::node::Node::SharedPtr & node, + const std::string & remote_node_name = "") : node_(node) { + if (remote_node_name != "") { + remote_node_name_ = remote_node_name; + } else { + remote_node_name_ = node_->get_name(); + } get_parameters_client_ = node_->create_client( - "get_parameters"); + remote_node_name_ + "__get_parameters"); get_parameter_types_client_ = node_->create_client( - "get_parameter_types"); + remote_node_name_ + "__get_parameter_types"); set_parameters_client_ = node_->create_client( - "set_parameters"); + remote_node_name_ + "__set_parameters"); list_parameters_client_ = node_->create_client( - "list_parameters"); + remote_node_name_ + "__list_parameters"); describe_parameters_client_ = node_->create_client( - "describe_parameters"); + remote_node_name_ + "__describe_parameters"); } std::shared_future> @@ -67,8 +73,35 @@ public: std::function>)> callback = nullptr) { - std::shared_future> f; - return f; + std::promise> promise_result; + auto future_result = promise_result.get_future().share(); + + auto request = std::make_shared(); + request->names = names; + + get_parameters_client_->async_send_request( + request, + [&names, &promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + std::vector parameter_variants; + auto & pvalues = cb_f.get()->values; + + std::transform(pvalues.begin(), pvalues.end(), std::back_inserter(parameter_variants), + [&names, &pvalues](rcl_interfaces::ParameterValue pvalue) { + auto i = &pvalue - &pvalues[0]; + rcl_interfaces::Parameter parameter; + parameter.name = names[i]; + parameter.value = pvalue; + return rclcpp::parameter::ParameterVariant::from_parameter(parameter); + }); + promise_result.set_value(parameter_variants); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; } std::shared_future> @@ -77,8 +110,28 @@ public: std::function>)> callback = nullptr) { - std::shared_future> f; - return f; + std::promise> promise_result; + auto future_result = promise_result.get_future().share(); + + auto request = std::make_shared(); + request->parameter_names = parameter_names; + + get_parameter_types_client_->async_send_request( + request, + [&promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + std::vector parameter_types; + auto & pts = cb_f.get()->parameter_types; + std::transform(pts.begin(), pts.end(), std::back_inserter(parameter_types), + [](uint8_t pt) {return static_cast(pt); }); + promise_result.set_value(parameter_types); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; } std::shared_future> @@ -87,8 +140,27 @@ public: std::function>)> callback = nullptr) { - std::shared_future> f; - return f; + std::promise> promise_result; + auto future_result = promise_result.get_future().share(); + + auto request = std::make_shared(); + + std::transform(parameters.begin(), parameters.end(), std::back_inserter( + request->parameters), []( + rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); }); + + set_parameters_client_->async_send_request( + request, + [&promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + promise_result.set_value(cb_f.get()->results); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; } std::shared_future @@ -97,8 +169,27 @@ public: std::function)> callback = nullptr) { - std::shared_future f; - return f; + std::promise promise_result; + auto future_result = promise_result.get_future().share(); + + auto request = std::make_shared(); + + std::transform(parameters.begin(), parameters.end(), std::back_inserter( + request->parameters), []( + rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); }); + + set_parameters_atomically_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 future_result; } std::shared_future @@ -108,8 +199,25 @@ public: std::function)> callback = nullptr) { - std::shared_future f; - return f; + std::promise promise_result; + auto future_result = promise_result.get_future().share(); + + auto request = std::make_shared(); + request->parameter_prefixes = parameter_prefixes; + request->depth = depth; + + list_parameters_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 future_result; } private: @@ -121,6 +229,7 @@ private: set_parameters_atomically_client_; rclcpp::client::Client::SharedPtr list_parameters_client_; rclcpp::client::Client::SharedPtr describe_parameters_client_; + std::string remote_node_name_; }; class SyncParametersClient