From 346f17aff0883688159433547e18af663228afc3 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 5 May 2015 17:01:34 -0700 Subject: [PATCH] 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