diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 3d6a81df08..63d1791800 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -139,18 +139,22 @@ class Node 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 cb9a414add..bfd9fe2249 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 0000000000..1e175332ba --- /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_ */