diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index d9194f5bc0..d059cf178a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -3,8 +3,10 @@ cmake_minimum_required(VERSION 2.8.3) project(rclcpp) find_package(ament_cmake REQUIRED) +find_package(rcl_interfaces REQUIRED) ament_export_dependencies(rmw) +ament_export_dependencies(rcl_interfaces) ament_export_include_directories(include) diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 8428aa65d0..3fedaabbea 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -26,6 +26,20 @@ namespace executors using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; +template +std::shared_future & +spin_node_until_future_complete( + rclcpp::executor::Executor & executor, Node::SharedPtr & node_ptr, + std::shared_future & future) +{ + std::future_status status; + do { + executor.spin_node_some(node_ptr); + status = future.wait_for(std::chrono::seconds(0)); + } while (status != std::future_status::ready && rclcpp::utilities::ok()); + return future; +} + } // namespace executors } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 701609843a..d3ced5ef42 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -18,11 +18,13 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -64,7 +66,7 @@ class Node /* Get the name of the node. */ std::string - get_name(); + get_name() const {return this->name_; } /* Create and return a callback group. */ rclcpp::callback_group::CallbackGroup::SharedPtr @@ -124,6 +126,47 @@ class Node typename rclcpp::service::Service::CallbackWithHeaderType callback_with_header, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + template + std::shared_future + async_get_parameter( + const std::string & node_name, const parameter::ParameterName & key, + std::function)> callback = nullptr); + + std::shared_future> + async_get_parameters( + const std::string & node_name, const std::vector & parameter_names, + std::function>)> callback = + nullptr); + + std::shared_future + async_has_parameter( + const std::string & node_name, const parameter::ParameterName & parameter_name, + std::function)> callback = nullptr); + + std::shared_future> + async_has_parameters( + const std::string & node_name, const std::vector & parameter_names, + std::function>)> callback = nullptr); + + template + std::shared_future + async_set_parameter( + const std::string & node_name, const parameter::ParameterName & key, + const ParamTypeT & value, std::function)> callback = nullptr); + + std::shared_future + async_set_parameters( + const std::string & node_name, + const std::vector & key_values, + std::function)> callback = nullptr); + + std::shared_future> + async_list_parameters( + const std::string & node_name, + const std::vector & parameter_groups, bool recursive, + std::function>)> callback = + nullptr); + private: RCLCPP_DISABLE_COPY(Node); @@ -148,6 +191,24 @@ class Node const std::string & service_name, std::shared_ptr serv_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); + + std::map parameters_; + + template + ParamTypeT get_parameter(const parameter::ParameterName & key) const; + + std::vector + get_parameters(const std::vector & query) const; + + bool + has_parameter(const parameter::ParameterName & parameter_name) const; + + template + void set_parameter(const parameter::ParameterName & key, const ParamTypeT & value); + + std::vector + list_parameters( + const std::vector & parameter_groups, bool recursive) const; }; } /* namespace node */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 363d5e7729..f40482f650 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -15,8 +15,10 @@ #ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_ #define RCLCPP_RCLCPP_NODE_IMPL_HPP_ +#include #include #include +#include #include #include @@ -24,6 +26,11 @@ #include +#include +#include +#include +#include + #ifndef RCLCPP_RCLCPP_NODE_HPP_ #include "node.hpp" #endif @@ -246,4 +253,354 @@ Node::register_service( number_of_services_++; } +template +ParamTypeT +Node::get_parameter(const parameter::ParameterName & key) const +{ + const parameter::ParameterContainer pc(this->parameters_.at(key)); + return pc.get_value(); +} + +template +void +Node::set_parameter(const parameter::ParameterName & key, const ParamTypeT & value) +{ + parameter::ParameterContainer pc(key, value); + parameters_[key] = pc; +} + +bool +Node::has_parameter(const parameter::ParameterName & parameter_name) const +{ + return parameters_.find(parameter_name) != parameters_.end(); +} + +std::vector +Node::get_parameters(const std::vector & queries) const +{ + std::vector result; + + for (auto & kv: parameters_) { + if (std::any_of(queries.cbegin(), queries.cend(), + [&kv](const parameter::ParameterQuery & query) { + return kv.first == query.get_name(); + })) + { + result.push_back(kv.second); + } + } + return result; +} + +std::vector +Node::list_parameters( + const std::vector & parameter_groups, bool recursive) const +{ + std::vector result; + + // TODO: define parameter separator + for (auto & kv: parameters_) { + if (std::any_of(parameter_groups.cbegin(), parameter_groups.cend(), + [&kv, &recursive](const parameter::ParameterName & parameter_group) { + if (kv.first.find(parameter_group + ".") == 0) { + if (recursive) { + return true; + } else { + size_t length = parameter_group.length(); + std::string substr = kv.first.substr(length); + return substr.find_first_of(".") == substr.find_last_of("."); + } + } + return false; + })) + { + result.push_back(kv.first); + } + } + return result; +} + +template +std::shared_future +Node::async_get_parameter( + const std::string & node_name, const parameter::ParameterName & key, + std::function)> callback) +{ + std::promise promise_result; + auto future_result = promise_result.get_future().share(); + std::vector param_names = {{key}}; + + this->async_get_parameters( + node_name, param_names, + [&promise_result, &future_result, &callback, ¶m_names]( + std::shared_future> cb_f) { + promise_result.set_value(cb_f.get()[0].get_value()); + if (callback != nullptr) { + callback(future_result); + } + }); + + return future_result; +} + + +std::shared_future> +Node::async_get_parameters( + const std::string & node_name, const std::vector & parameter_names, + std::function>)> callback) +{ + std::promise> promise_result; + auto future_result = promise_result.get_future().share(); + if (node_name == this->get_name()) { + std::vector value; + for (auto pn : parameter_names) { + if (this->has_parameter(pn)) { + try { + parameter::ParameterContainer param_container(pn, this->get_parameter(pn)); + value.push_back(param_container); + // TODO: use custom exception + } catch (...) { + try { + parameter::ParameterContainer param_container(pn, this->get_parameter(pn)); + value.push_back(param_container); + } catch (...) { + try { + parameter::ParameterContainer param_container(pn, + this->get_parameter(pn)); + value.push_back(param_container); + } catch (...) { + parameter::ParameterContainer param_container(pn, this->get_parameter(pn)); + value.push_back(param_container); + } + } + } + } + } + promise_result.set_value(value); + if (callback != nullptr) { + callback(future_result); + } + } else { + auto client = this->create_client("get_parameters"); + auto request = std::make_shared(); + for (auto parameter_name : parameter_names) { + rcl_interfaces::ParameterDescription description; + description.name = parameter_name; + request->parameter_descriptions.push_back(description); + } + + client->async_send_request( + request, [&promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + std::vector value; + auto parameters = cb_f.get()->parameters; + for (auto parameter : parameters) { + switch (parameter.description.parameter_type) { + case rcl_interfaces::ParameterDescription::BOOL_PARAMETER: + value.push_back(parameter::ParameterContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + case rcl_interfaces::ParameterDescription::STRING_PARAMETER: + value.push_back(parameter::ParameterContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + case rcl_interfaces::ParameterDescription::DOUBLE_PARAMETER: + value.push_back(parameter::ParameterContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + case rcl_interfaces::ParameterDescription::INTEGER_PARAMETER: + value.push_back(parameter::ParameterContainer( + parameter.description.name, + rclcpp::parameter::get_parameter_value(parameter))); + break; + default: + // TODO: use custom exception + throw std::runtime_error("Invalid value type"); + } + } + promise_result.set_value(value); + if (callback != nullptr) { + callback(future_result); + } + } + ); + } + return future_result; +} + +std::shared_future +Node::async_set_parameters( + const std::string & node_name, + const std::vector & key_values, + std::function)> callback) +{ + std::promise promise_result; + auto future_result = promise_result.get_future().share(); + if (node_name == this->get_name()) { + for (auto kv : key_values) { + switch (kv.get_typeID()) { + case parameter::ParameterDataType::INTEGER_PARAMETER: + this->set_parameter(kv.get_name(), kv.get_value()); + break; + case parameter::ParameterDataType::DOUBLE_PARAMETER: + this->set_parameter(kv.get_name(), kv.get_value()); + break; + case parameter::ParameterDataType::STRING_PARAMETER: + this->set_parameter(kv.get_name(), kv.get_value()); + break; + case parameter::ParameterDataType::BOOL_PARAMETER: + this->set_parameter(kv.get_name(), kv.get_value()); + break; + default: + throw std::runtime_error("Invalid parameter type"); + } + } + promise_result.set_value(true); + if (callback != nullptr) { + callback(future_result); + } + } else { + auto client = this->create_client("set_parameters"); + auto request = std::make_shared(); + for (auto kv: key_values) { + rcl_interfaces::Parameter parameter; + parameter.description.name = kv.get_name(); + switch (kv.get_typeID()) { + case parameter::ParameterDataType::INTEGER_PARAMETER: + rclcpp::parameter::set_parameter_value( + parameter, kv.get_value()); + break; + case parameter::ParameterDataType::DOUBLE_PARAMETER: + rclcpp::parameter::set_parameter_value( + parameter, kv.get_value()); + break; + case parameter::ParameterDataType::STRING_PARAMETER: + rclcpp::parameter::set_parameter_value( + parameter, kv.get_value()); + break; + case parameter::ParameterDataType::BOOL_PARAMETER: + rclcpp::parameter::set_parameter_value( + parameter, kv.get_value()); + break; + default: + throw std::runtime_error("Invalid parameter type"); + } + request->parameters.push_back(parameter); + } + + client->async_send_request( + request, [&promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + promise_result.set_value(cb_f.get()->success); + if (callback != nullptr) { + callback(future_result); + } + } + ); + } + return future_result; +} + +template +std::shared_future +Node::async_set_parameter( + const std::string & node_name, const parameter::ParameterName & key, const ParamTypeT & value, + std::function)> callback) +{ + std::vector param_containers = {{ + parameter::ParameterContainer( + key, value) + }}; + return this->async_set_parameters(node_name, param_containers, callback); +} + +std::shared_future +Node::async_has_parameter( + const std::string & node_name, const parameter::ParameterName & parameter_name, + std::function)> callback) +{ + std::promise promise_result; + auto future_result = promise_result.get_future().share(); + this->async_has_parameters(node_name, {{parameter_name}}, + [&promise_result, &future_result, &callback]( + std::shared_future> cb_f) { + promise_result.set_value(cb_f.get()[0]); + if (callback != nullptr) { + callback(future_result); + } + } + ); + return future_result; +} + +std::shared_future> +Node::async_has_parameters( + const std::string & node_name, const std::vector & parameter_names, + std::function>)> callback) +{ + std::promise> promise_result; + auto future_result = promise_result.get_future().share(); + if (node_name == this->get_name()) { + std::vector value; + for (auto parameter_name: parameter_names) { + value.push_back(this->has_parameter(parameter_name)); + } + promise_result.set_value(value); + if (callback != nullptr) { + callback(future_result); + } + } else { + auto client = this->create_client("has_parameters"); + auto request = std::make_shared(); + for (auto parameter_name: parameter_names) { + rcl_interfaces::ParameterDescription parameter_description; + parameter_description.name = parameter_name; + request->parameter_descriptions.push_back(parameter_description); + } + 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> +Node::async_list_parameters( + const std::string & node_name, + const std::vector & parameter_groups, + bool recursive, + std::function>)> callback) +{ + std::promise> promise_result; + auto future_result = promise_result.get_future().share(); + if (node_name == this->get_name()) { + promise_result.set_value(this->list_parameters(parameter_groups, recursive)); + if (callback != nullptr) { + callback(future_result); + } + } else { + auto client = this->create_client("list_parameters"); + auto request = std::make_shared(); + request->parameter_group = parameter_groups; + request->recursive = recursive; + client->async_send_request( + request, [&promise_result, &future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { + promise_result.set_value(cb_f.get()->sub_groups); + if (callback != nullptr) { + callback(future_result); + } + }); + } + return future_result; +} #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp new file mode 100644 index 0000000000..eab0a21670 --- /dev/null +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -0,0 +1,253 @@ +// Copyright 2014 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_HPP_ +#define RCLCPP_RCLCPP_PARAMETER_HPP_ + +#include + +#include + +#include + +#include +#include + +namespace rclcpp +{ + +namespace parameter +{ +// Datatype for parameter names +typedef std::string ParameterName; + +// Datatype for storing parameter types +enum ParameterDataType { + INVALID_PARAMETER, INTEGER_PARAMETER, DOUBLE_PARAMETER, STRING_PARAMETER, BOOL_PARAMETER +}; + +// Structure to store an arbitrary parameter with templated get/set methods +class ParamContainer +{ +public: + ParamContainer() + : typeID_(INVALID_PARAM) {} + ParamContainer( + const std::string & name, const int64_t int_value) + : name_(name), typeID_(INT_PARAM), int_value_(int_value) {} + ParamContainer( + const std::string & name, const double double_value) + : name_(name), typeID_(DOUBLE_PARAM), double_value_(double_value) {} + ParamContainer( + const std::string & name, const std::string & string_value) + : name_(name), typeID_(STRING_PARAM), string_value_(string_value) {} + ParamContainer( + const std::string & name, const bool bool_value) + : name_(name), typeID_(BOOL_PARAM), bool_value_(bool_value) {} + + ParamName name_; + ParamDataType typeID_; + + /* Templated getter */ + template + T + get_value() const; + +// Structure to store an arbitrary parameter with templated get/set methods + class ParameterContainer + { +public: + ParameterContainer() + : typeID_(INVALID_PARAMETER) {} + ParameterContainer( + const std::string & name, const int64_t int_value) + : name_(name), typeID_(INTEGER_PARAMETER), int_value_(int_value) {} + ParameterContainer( + const std::string & name, const double double_value) + : name_(name), typeID_(DOUBLE_PARAMETER), double_value_(double_value) {} + ParameterContainer( + const std::string & name, const std::string & string_value) + : name_(name), typeID_(STRING_PARAMETER), string_value_(string_value) {} + ParameterContainer( + const std::string & name, const bool bool_value) + : name_(name), typeID_(BOOL_PARAMETER), bool_value_(bool_value) {} + + ParameterName name_; + ParameterDataType typeID_; + + /* Templated getter */ + template + T + get_value() const; + + inline ParameterName get_name() const {return name_; } + + inline ParameterDataType get_typeID() const {return typeID_; } + +private: + int64_t int_value_; + double double_value_; + std::string string_value_; + bool bool_value_; + }; + + template<> + inline int64_t ParameterContainer::get_value() const + { + if (typeID_ != INTEGER_PARAMETER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return int_value_; + } + template<> + inline double ParameterContainer::get_value() const + { + if (typeID_ != DOUBLE_PARAMETER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return double_value_; + } + template<> + inline std::string ParameterContainer::get_value() const + { + if (typeID_ != STRING_PARAMETER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return string_value_; + } + template<> + inline bool ParameterContainer::get_value() const + { + if (typeID_ != BOOL_PARAMETER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return bool_value_; + } + + class ParameterQuery + { +public: + ParameterQuery(const std::string & name) + : typeID_(INVALID_PARAMETER), name_(name) {} + ParameterQuery(const ParameterDataType typeID) + : typeID_(typeID), name_("") {} + + // TODO: make this extendable for potential regex or other dynamic queryies + // Possibly use a generator pattern? + // For now just store a single datatype and provide accessors. + + inline ParameterDataType get_type() const + { + return typeID_; + } + inline ParameterName get_name() const + { + return name_; + } + +private: + ParameterDataType typeID_; + ParameterName name_; + }; + + template + T get_parameter_value(rcl_interfaces::Parameter & parameter); + + template<> + bool get_parameter_value(rcl_interfaces::Parameter & parameter) + { + if (parameter.description.parameter_type != + rcl_interfaces::ParameterDescription::BOOL_PARAMETER) + { + // TODO: use custom exception + throw std::runtime_error("Parameter value is not a boolean"); + } + return parameter.bool_value; + } + + template<> + int64_t get_parameter_value(rcl_interfaces::Parameter & parameter) + { + if (parameter.description.parameter_type != + rcl_interfaces::ParameterDescription::INTEGER_PARAMETER) + { + // TODO: use custom exception + throw std::runtime_error("Parameter value is not an integer"); + } + return parameter.integer_value; + } + + template<> + double get_parameter_value(rcl_interfaces::Parameter & parameter) + { + if (parameter.description.parameter_type != + rcl_interfaces::ParameterDescription::DOUBLE_PARAMETER) + { + // TODO: use custom exception + throw std::runtime_error("Parameter value is not a double"); + } + return parameter.double_value; + } + + template<> + std::string get_parameter_value(rcl_interfaces::Parameter & parameter) + { + if (parameter.description.parameter_type != + rcl_interfaces::ParameterDescription::STRING_PARAMETER) + { + // TODO: use custom exception + throw std::runtime_error("Parameter value is not a string"); + } + return parameter.string_value; + } + + template + void set_parameter_value(rcl_interfaces::Parameter & parameter, const T & value); + + template<> + void set_parameter_value(rcl_interfaces::Parameter & parameter, const bool & value) + { + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::BOOL_PARAMETER; + parameter.bool_value = value; + } + + template<> + void set_parameter_value(rcl_interfaces::Parameter & parameter, const int64_t & value) + { + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::INTEGER_PARAMETER; + parameter.integer_value = value; + } + + template<> + void set_parameter_value(rcl_interfaces::Parameter & parameter, const double & value) + { + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::DOUBLE_PARAMETER; + parameter.double_value = value; + } + + template<> + void set_parameter_value(rcl_interfaces::Parameter & parameter, const std::string & value) + { + parameter.description.parameter_type = rcl_interfaces::ParameterDescription::STRING_PARAMETER; + parameter.string_value = value; + } +}; +} /* namespace parameter */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 040cf3897e..02d835a37f 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -76,6 +76,17 @@ void spin(Node::SharedPtr & node_ptr) executor.spin(); } +template +std::shared_future & +spin_until_future_complete( + Node::SharedPtr & node_ptr, std::shared_future & future) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::spin_node_until_future_complete( + executor, node_ptr, future); + return future; +} + } /* namespace rclcpp */ #endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */ diff --git a/rclcpp/package.xml b/rclcpp/package.xml index f393a38136..5c7e924641 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -10,6 +10,9 @@ rmw + rcl_interfaces + rcl_interfaces + ament_lint_auto ament_lint_common