diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 888b962..5b76e7a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -24,11 +24,13 @@ #include #include #include +#include #include #include #include #include + // Forward declaration of ROS middleware class namespace rmw { @@ -146,6 +148,24 @@ public: FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + const std::vector set_parameters( + 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); @@ -166,10 +186,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/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index cb9a414..381b573 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,125 @@ Node::create_service( return serv; } +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); + 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::lock_guard lock(mutex_); + 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::lock_guard lock(mutex_); + 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::lock_guard lock(mutex_); + 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::lock_guard lock(mutex_); + 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.hpp b/rclcpp/include/rclcpp/parameter.hpp index 266eb45..26bba4f 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -19,10 +19,6 @@ #include -#include -#include -#include - #include #include #include @@ -174,158 +170,31 @@ 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_; }; -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/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_ */ 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