From 6534a2734f8a2fef585d0d714f9105a0f9db9995 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 29 May 2015 08:46:15 -0700 Subject: [PATCH] refactor message generation (ros2/ros2#48) --- rclcpp/include/rclcpp/node.hpp | 12 ++- rclcpp/include/rclcpp/node_impl.hpp | 22 ++--- rclcpp/include/rclcpp/parameter.hpp | 60 ++++++-------- rclcpp/include/rclcpp/parameter_client.hpp | 92 +++++++++++---------- rclcpp/include/rclcpp/parameter_service.hpp | 67 +++++++-------- 5 files changed, 125 insertions(+), 128 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 508efdc..1e50ae6 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -20,6 +20,10 @@ #include #include +#include +#include +#include + #include #include #include @@ -148,22 +152,22 @@ public: FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - std::vector set_parameters( + std::vector set_parameters( const std::vector & parameters); - rcl_interfaces::SetParametersResult set_parameters_atomically( + rcl_interfaces::msg::SetParametersResult set_parameters_atomically( const std::vector & parameters); std::vector get_parameters( const std::vector & names) const; - std::vector describe_parameters( + std::vector describe_parameters( const std::vector & names) const; std::vector get_parameter_types( const std::vector & names) const; - rcl_interfaces::ListParametersResult list_parameters( + rcl_interfaces::msg::ListParametersResult list_parameters( const std::vector & prefixes, uint64_t depth) const; private: diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 185fd89..c126073 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -213,15 +213,15 @@ Node::create_service( return serv; } -std::vector +std::vector Node::set_parameters( const std::vector & parameters) { std::lock_guard lock(mutex_); - std::vector results; + std::vector results; for (auto p : parameters) { parameters_[p.get_name()] = p; - rcl_interfaces::SetParametersResult result; + rcl_interfaces::msg::SetParametersResult result; result.successful = true; // TODO: handle parameter constraints results.push_back(result); @@ -229,7 +229,7 @@ Node::set_parameters( return results; } -rcl_interfaces::SetParametersResult +rcl_interfaces::msg::SetParametersResult Node::set_parameters_atomically( const std::vector & parameters) { @@ -241,7 +241,7 @@ Node::set_parameters_atomically( tmp_map.insert(parameters_.begin(), parameters_.end()); std::swap(tmp_map, parameters_); // TODO: handle parameter constraints - rcl_interfaces::SetParametersResult result; + rcl_interfaces::msg::SetParametersResult result; result.successful = true; return result; } @@ -263,18 +263,18 @@ Node::get_parameters( return results; } -std::vector +std::vector Node::describe_parameters( const std::vector & names) const { std::lock_guard lock(mutex_); - std::vector results; + 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; + rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; parameter_descriptor.name = kv.first; parameter_descriptor.parameter_type = kv.second.get_type(); results.push_back(parameter_descriptor); @@ -296,18 +296,18 @@ Node::get_parameter_types( { results.push_back(kv.second.get_type()); } else { - results.push_back(rcl_interfaces::ParameterType::PARAMETER_NOT_SET); + results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); } } return results; } -rcl_interfaces::ListParametersResult +rcl_interfaces::msg::ListParametersResult Node::list_parameters( const std::vector & prefixes, uint64_t depth) const { std::lock_guard lock(mutex_); - rcl_interfaces::ListParametersResult result; + rcl_interfaces::msg::ListParametersResult result; // TODO: define parameter separator, use "." for now for (auto & kv : parameters_) { diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index c802db9..d7b420d 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -19,15 +19,9 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include namespace rclcpp { @@ -37,12 +31,12 @@ namespace parameter enum ParameterType { - PARAMETER_NOT_SET=rcl_interfaces::ParameterType::PARAMETER_NOT_SET, - PARAMETER_BOOL=rcl_interfaces::ParameterType::PARAMETER_BOOL, - PARAMETER_INTEGER=rcl_interfaces::ParameterType::PARAMETER_INTEGER, - PARAMETER_DOUBLE=rcl_interfaces::ParameterType::PARAMETER_DOUBLE, - PARAMETER_STRING=rcl_interfaces::ParameterType::PARAMETER_STRING, - PARAMETER_BYTES=rcl_interfaces::ParameterType::PARAMETER_BYTES, + PARAMETER_NOT_SET=rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, + PARAMETER_BOOL=rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + PARAMETER_INTEGER=rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + PARAMETER_DOUBLE=rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + PARAMETER_STRING=rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + PARAMETER_BYTES=rcl_interfaces::msg::ParameterType::PARAMETER_BYTES, }; // Structure to store an arbitrary parameter with templated get/set methods @@ -52,56 +46,56 @@ public: ParameterVariant() : name_("") { - value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_NOT_SET; + value_.parameter_type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; } 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; + value_.parameter_type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; } 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; + value_.parameter_type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; } 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; + value_.parameter_type = rcl_interfaces::msg::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; + value_.parameter_type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; } 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; + value_.parameter_type = rcl_interfaces::msg::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; + value_.parameter_type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; } explicit ParameterVariant(const std::string & name, const std::vector & bytes_value) : name_(name) { value_.bytes_value = bytes_value; - value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_BYTES; + value_.parameter_type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES; } inline ParameterType get_type() const {return static_cast(value_.parameter_type); } inline std::string get_name() const & {return name_; } - inline rcl_interfaces::ParameterValue get_parameter_value() const + inline rcl_interfaces::msg::ParameterValue get_parameter_value() const { return value_; } @@ -110,7 +104,7 @@ public: typename std::enable_if::type get_value() const { - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_INTEGER) { + if (value_.parameter_type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } @@ -120,7 +114,7 @@ public: typename std::enable_if::type get_value() const { - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_DOUBLE) { + if (value_.parameter_type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } @@ -130,7 +124,7 @@ public: typename std::enable_if::type get_value() const { - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_STRING) { + if (value_.parameter_type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } @@ -140,7 +134,7 @@ public: typename std::enable_if::type get_value() const { - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BOOL) { + if (value_.parameter_type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } @@ -151,7 +145,7 @@ public: const std::vector &>::type get_value() const { - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BYTES) { + if (value_.parameter_type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) { // TODO: use custom exception throw std::runtime_error("Invalid type"); } @@ -171,7 +165,7 @@ public: return get_value(); } - static ParameterVariant from_parameter(const rcl_interfaces::Parameter & parameter) + static ParameterVariant from_parameter(const rcl_interfaces::msg::Parameter & parameter) { switch (parameter.value.parameter_type) { case PARAMETER_BOOL: @@ -191,9 +185,9 @@ public: } } - rcl_interfaces::Parameter to_parameter() + rcl_interfaces::msg::Parameter to_parameter() { - rcl_interfaces::Parameter parameter; + rcl_interfaces::msg::Parameter parameter; parameter.name = name_; parameter.value = value_; return parameter; @@ -201,7 +195,7 @@ public: private: std::string name_; - rcl_interfaces::ParameterValue value_; + rcl_interfaces::msg::ParameterValue value_; }; } /* namespace parameter */ diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 7e4d086..5e7c611 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -24,15 +24,14 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include namespace rclcpp { @@ -55,15 +54,15 @@ public: } else { remote_node_name_ = node_->get_name(); } - get_parameters_client_ = node_->create_client( + get_parameters_client_ = node_->create_client( remote_node_name_ + "__get_parameters"); - get_parameter_types_client_ = node_->create_client( + get_parameter_types_client_ = node_->create_client( remote_node_name_ + "__get_parameter_types"); - set_parameters_client_ = node_->create_client( + set_parameters_client_ = node_->create_client( remote_node_name_ + "__set_parameters"); - list_parameters_client_ = node_->create_client( + list_parameters_client_ = node_->create_client( remote_node_name_ + "__list_parameters"); - describe_parameters_client_ = node_->create_client( + describe_parameters_client_ = node_->create_client( remote_node_name_ + "__describe_parameters"); } @@ -76,20 +75,20 @@ public: std::promise> promise_result; auto future_result = promise_result.get_future().share(); - auto request = std::make_shared(); + 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) { + 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) { + [&names, &pvalues](rcl_interfaces::msg::ParameterValue pvalue) { auto i = &pvalue - &pvalues[0]; - rcl_interfaces::Parameter parameter; + rcl_interfaces::msg::Parameter parameter; parameter.name = names[i]; parameter.value = pvalue; return rclcpp::parameter::ParameterVariant::from_parameter(parameter); @@ -113,13 +112,13 @@ public: std::promise> promise_result; auto future_result = promise_result.get_future().share(); - auto request = std::make_shared(); + 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) { + 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), @@ -134,16 +133,17 @@ public: return future_result; } - std::shared_future> + std::shared_future> set_parameters( std::vector parameters, - std::function>)> callback = nullptr) + std::function< + void(std::shared_future>) + > callback = nullptr) { - std::promise> promise_result; + std::promise> promise_result; auto future_result = promise_result.get_future().share(); - auto request = std::make_shared(); + auto request = std::make_shared(); std::transform(parameters.begin(), parameters.end(), std::back_inserter( request->parameters), []( @@ -152,7 +152,7 @@ public: set_parameters_client_->async_send_request( request, [&promise_result, &future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { + rclcpp::client::Client::SharedFuture cb_f) { promise_result.set_value(cb_f.get()->results); if (callback != nullptr) { callback(future_result); @@ -163,16 +163,16 @@ public: return future_result; } - std::shared_future + std::shared_future set_parameters_atomically( std::vector parameters, std::function)> callback = nullptr) + std::shared_future)> callback = nullptr) { - std::promise promise_result; + std::promise promise_result; auto future_result = promise_result.get_future().share(); - auto request = std::make_shared(); + auto request = std::make_shared(); std::transform(parameters.begin(), parameters.end(), std::back_inserter( request->parameters), []( @@ -181,7 +181,7 @@ public: set_parameters_atomically_client_->async_send_request( request, [&promise_result, &future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { + rclcpp::client::Client::SharedFuture cb_f) { promise_result.set_value(cb_f.get()->result); if (callback != nullptr) { callback(future_result); @@ -192,24 +192,24 @@ public: return future_result; } - 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::promise promise_result; + std::promise promise_result; auto future_result = promise_result.get_future().share(); - auto request = std::make_shared(); + 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) { + rclcpp::client::Client::SharedFuture cb_f) { promise_result.set_value(cb_f.get()->result); if (callback != nullptr) { callback(future_result); @@ -222,13 +222,15 @@ public: 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 + 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_; + rclcpp::client::Client::SharedPtr list_parameters_client_; + rclcpp::client::Client::SharedPtr + describe_parameters_client_; std::string remote_node_name_; }; @@ -267,21 +269,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::SetParametersResult + rcl_interfaces::msg::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 + rcl_interfaces::msg::ListParametersResult list_parameters( std::vector parameter_prefixes, uint64_t depth) diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index c2b6ceb..e4f369f 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -24,15 +24,12 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace rclcpp { @@ -49,11 +46,11 @@ public: ParameterService(const rclcpp::node::Node::SharedPtr & node) : node_(node) { - get_parameters_service_ = node_->create_service( + get_parameters_service_ = node_->create_service( node_->get_name() + "__get_parameters", [&node]( const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { auto values = node->get_parameters(request->names); std::transform(values.cbegin(), values.cend(), std::back_inserter(response->values), @@ -64,11 +61,11 @@ public: } ); - get_parameter_types_service_ = node_->create_service( + get_parameter_types_service_ = node_->create_service( node_->get_name() + "__get_parameter_types", [&node]( const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { auto types = node->get_parameter_types(request->parameter_names); std::transform(types.cbegin(), types.cend(), @@ -78,16 +75,16 @@ public: } ); - set_parameters_service_ = node_->create_service( + set_parameters_service_ = node_->create_service( node_->get_name() + "__set_parameters", [&node]( const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { std::vector pvariants; std::transform(request->parameters.cbegin(), request->parameters.cend(), std::back_inserter(pvariants), - [](const rcl_interfaces::Parameter & p) { + [](const rcl_interfaces::msg::Parameter & p) { return rclcpp::parameter::ParameterVariant:: from_parameter(p); }); @@ -97,16 +94,16 @@ public: ); set_parameters_atomically_service_ = - node_->create_service( + node_->create_service( node_->get_name() + "__set_parameters_atomically", [&node]( const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { std::vector pvariants; std::transform(request->parameters.cbegin(), request->parameters.cend(), std::back_inserter(pvariants), - [](const rcl_interfaces::Parameter & p) { + [](const rcl_interfaces::msg::Parameter & p) { return rclcpp::parameter::ParameterVariant:: from_parameter(p); }); @@ -115,22 +112,22 @@ public: } ); - describe_parameters_service_ = node_->create_service( + describe_parameters_service_ = node_->create_service( node_->get_name() + "__describe_parameters", [&node]( const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { auto descriptors = node->describe_parameters(request->names); response->descriptors = descriptors; } ); - list_parameters_service_ = node_->create_service( + list_parameters_service_ = node_->create_service( node_->get_name() + "__describe_parameters", [&node]( const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { auto result = node->list_parameters(request->parameter_prefixes, request->depth); response->result = result; @@ -142,15 +139,15 @@ public: private: const rclcpp::node::Node::SharedPtr node_; - rclcpp::service::Service::SharedPtr get_parameters_service_; - rclcpp::service::Service::SharedPtr + rclcpp::service::Service::SharedPtr get_parameters_service_; + rclcpp::service::Service::SharedPtr get_parameter_types_service_; - rclcpp::service::Service::SharedPtr set_parameters_service_; - rclcpp::service::Service::SharedPtr + rclcpp::service::Service::SharedPtr set_parameters_service_; + rclcpp::service::Service::SharedPtr set_parameters_atomically_service_; - rclcpp::service::Service::SharedPtr + rclcpp::service::Service::SharedPtr describe_parameters_service_; - rclcpp::service::Service::SharedPtr list_parameters_service_; + rclcpp::service::Service::SharedPtr list_parameters_service_; }; } /* namespace parameter_service */