Added internal parameters API.

rclcpp/include/rclcpp/node.hpp

@@ -139,18 +139,22 @@ class Node
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   const std::vector<rcl_interfaces::SetParametersResult> set_parameters(
-    const std::vector<rcl_interfaces::Parameter> & parameters)
-  {
-    std::vector<rcl_interfaces::SetParametersResult> 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<rcl_interfaces::Parameter> & parameters);
+
+  const rcl_interfaces::SetParametersResult set_parameters_atomically(
+    const std::vector<rcl_interfaces::Parameter> & parameters);
+
+  const std::vector<rclcpp::parameter::ParameterVariant> get_parameters(
+    const std::vector<std::string> & names);
+
+  const std::vector<rcl_interfaces::ParameterDescriptor> describe_parameters(
+    const std::vector<std::string> & names);
+
+  const std::vector<uint8_t> get_parameter_types(
+    const std::vector<std::string> & names);
+
+  const std::vector<rcl_interfaces::ListParametersResult> list_parameters(
+    const std::vector<std::string> & prefixes, uint64_t depth);
 
 private:
   RCLCPP_DISABLE_COPY(Node);

rclcpp/include/rclcpp/node_impl.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
 #define RCLCPP_RCLCPP_NODE_IMPL_HPP_
 
+#include <algorithm>
 #include <memory>
 #include <string>
 
@@ -212,4 +213,120 @@ Node::create_service(
   return serv;
 }
 
+const std::vector<rcl_interfaces::SetParametersResult>
+Node::set_parameters(
+  const std::vector<rcl_interfaces::Parameter> & parameters)
+{
+  std::vector<rcl_interfaces::SetParametersResult> 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<rcl_interfaces::Parameter> & parameters)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  std::map<std::string, rclcpp::parameter::ParameterVariant> 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<rclcpp::parameter::ParameterVariant>
+Node::get_parameters(
+  const std::vector<std::string> & names)
+{
+  std::vector<rclcpp::parameter::ParameterVariant> 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<rcl_interfaces::ParameterDescriptor>
+Node::describe_parameters(
+  const std::vector<std::string> & names)
+{
+  std::vector<rcl_interfaces::ParameterDescriptor> 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<uint8_t>
+Node::get_parameter_types(
+  const std::vector<std::string> & names)
+{
+  std::vector<uint8_t> 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<rcl_interfaces::ListParametersResult>
+Node::list_parameters(
+  const std::vector<std::string> & prefixes, uint64_t depth)
+{
+  std::vector<rcl_interfaces::ListParametersResult> 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_ */

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 <string>
+
+#include <rmw/rmw.h>
+
+#include <rclcpp/executors.hpp>
+#include <rclcpp/macros.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/parameter.hpp>
+
+#include <rcl_interfaces/GetParameters.h>
+#include <rcl_interfaces/GetParameterTypes.h>
+#include <rcl_interfaces/Parameter.h>
+#include <rcl_interfaces/ParameterDescriptor.h>
+#include <rcl_interfaces/ParameterType.h>
+#include <rcl_interfaces/SetParameters.h>
+#include <rcl_interfaces/SetParametersAtomically.h>
+#include <rcl_interfaces/ListParameters.h>
+#include <rcl_interfaces/DescribeParameters.h>
+
+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<rcl_interfaces::GetParameters>(
+      "get_parameters");
+    get_parameter_types_client_ = node_->create_client<rcl_interfaces::GetParameterTypes>(
+      "get_parameter_types");
+    set_parameters_client_ = node_->create_client<rcl_interfaces::SetParameters>(
+      "set_parameters");
+    list_parameters_client_ = node_->create_client<rcl_interfaces::ListParameters>(
+      "list_parameters");
+    describe_parameters_client_ = node_->create_client<rcl_interfaces::DescribeParameters>(
+      "describe_parameters");
+  }
+
+  std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
+  get_parameters(
+    std::vector<std::string> names,
+    std::function<void(
+      std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)> callback = nullptr)
+  {
+    std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>> f;
+    return f;
+  }
+
+  std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
+  get_parameter_types(
+    std::vector<std::string> parameter_names,
+    std::function<void(
+      std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)> callback = nullptr)
+  {
+    std::shared_future<std::vector<rclcpp::parameter::ParameterType>> f;
+    return f;
+  }
+
+  std::shared_future<std::vector<rcl_interfaces::SetParametersResult>>
+  set_parameters(
+    std::vector<rclcpp::parameter::ParameterVariant> parameters,
+    std::function<void(
+      std::shared_future<std::vector<rcl_interfaces::SetParametersResult>>)> callback = nullptr)
+  {
+    std::shared_future<std::vector<rcl_interfaces::SetParametersResult>> f;
+    return f;
+  }
+
+  std::shared_future<rcl_interfaces::SetParametersResult>
+  set_parameters_atomically(
+    std::vector<rclcpp::parameter::ParameterVariant> parameters,
+    std::function<void(
+      std::shared_future<rcl_interfaces::SetParametersResult>)> callback = nullptr)
+  {
+    std::shared_future<rcl_interfaces::SetParametersResult> f;
+    return f;
+  }
+
+  std::shared_future<rcl_interfaces::ListParametersResult>
+  list_parameters(
+    std::vector<std::string> parameter_prefixes,
+    uint64_t depth,
+    std::function<void(
+      std::shared_future<rcl_interfaces::ListParametersResult>)> callback = nullptr)
+  {
+    std::shared_future<rcl_interfaces::ListParametersResult> f;
+    return f;
+  }
+
+private:
+  const rclcpp::node::Node::SharedPtr node_;
+  rclcpp::client::Client<rcl_interfaces::GetParameters>::SharedPtr get_parameters_client_;
+  rclcpp::client::Client<rcl_interfaces::GetParameterTypes>::SharedPtr get_parameter_types_client_;
+  rclcpp::client::Client<rcl_interfaces::SetParameters>::SharedPtr set_parameters_client_;
+  rclcpp::client::Client<rcl_interfaces::SetParametersAtomically>::SharedPtr
+    set_parameters_atomically_client_;
+  rclcpp::client::Client<rcl_interfaces::ListParameters>::SharedPtr list_parameters_client_;
+  rclcpp::client::Client<rcl_interfaces::DescribeParameters>::SharedPtr describe_parameters_client_;
+};
+
+class SyncParametersClient
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(SyncParametersClient);
+
+  SyncParametersClient(
+    rclcpp::node::Node::SharedPtr & node)
+  : node_(node)
+  {
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
+  }
+
+  SyncParametersClient(
+    rclcpp::executor::Executor::SharedPtr & executor,
+    rclcpp::node::Node::SharedPtr & node)
+  : executor_(executor), node_(node)
+  {
+    async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
+  }
+
+  std::vector<rclcpp::parameter::ParameterVariant>
+  get_parameters(std::vector<std::string> 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<rclcpp::parameter::ParameterType>
+  get_parameter_types(std::vector<std::string> 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<rcl_interfaces::SetParametersResult>
+  set_parameters(std::vector<rclcpp::parameter::ParameterVariant> 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<rclcpp::parameter::ParameterVariant> 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<std::string> 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_ */