text: // 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.
  
  #include "rclcpp/parameter_client.hpp"
  
  #include <algorithm>
  #include <memory>
  #include <string>
  #include <vector>
  
  #include "./parameter_service_names.hpp"
  
  using rclcpp::AsyncParametersClient;
  using rclcpp::SyncParametersClient;
  
  AsyncParametersClient::AsyncParametersClient(
    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
    const std::string & remote_node_name,
    const rmw_qos_profile_t & qos_profile)
  : node_topics_interface_(node_topics_interface)
  {
    if (remote_node_name != "") {
      remote_node_name_ = remote_node_name;
    } else {
      remote_node_name_ = node_base_interface->get_name();
    }
  
    rcl_client_options_t options = rcl_client_get_default_options();
    options.qos = qos_profile;
  
    using rclcpp::Client;
    using rclcpp::ClientBase;
  
    get_parameters_client_ = Client<rcl_interfaces::srv::GetParameters>::make_shared(
      node_base_interface.get(),
      node_graph_interface,
      remote_node_name_ + "/" + parameter_service_names::get_parameters,
      options);
    auto get_parameters_base = std::dynamic_pointer_cast<ClientBase>(get_parameters_client_);
    node_services_interface->add_client(get_parameters_base, nullptr);
  
    get_parameter_types_client_ = Client<rcl_interfaces::srv::GetParameterTypes>::make_shared(
      node_base_interface.get(),
      node_graph_interface,
      remote_node_name_ + "/" + parameter_service_names::get_parameter_types,
      options);
    auto get_parameter_types_base =
      std::dynamic_pointer_cast<ClientBase>(get_parameter_types_client_);
    node_services_interface->add_client(get_parameter_types_base, nullptr);
  
    set_parameters_client_ = Client<rcl_interfaces::srv::SetParameters>::make_shared(
      node_base_interface.get(),
      node_graph_interface,
      remote_node_name_ + "/" + parameter_service_names::set_parameters,
      options);
    auto set_parameters_base = std::dynamic_pointer_cast<ClientBase>(set_parameters_client_);
    node_services_interface->add_client(set_parameters_base, nullptr);
  
    set_parameters_atomically_client_ =
      Client<rcl_interfaces::srv::SetParametersAtomically>::make_shared(node_base_interface.get(),
        node_graph_interface,
        remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically,
        options);
    auto set_parameters_atomically_base = std::dynamic_pointer_cast<ClientBase>(
      set_parameters_atomically_client_);
    node_services_interface->add_client(set_parameters_atomically_base, nullptr);
  
    list_parameters_client_ = Client<rcl_interfaces::srv::ListParameters>::make_shared(
      node_base_interface.get(),
      node_graph_interface,
      remote_node_name_ + "/" + parameter_service_names::list_parameters,
      options);
    auto list_parameters_base = std::dynamic_pointer_cast<ClientBase>(list_parameters_client_);
    node_services_interface->add_client(list_parameters_base, nullptr);
  
    describe_parameters_client_ = Client<rcl_interfaces::srv::DescribeParameters>::make_shared(
      node_base_interface.get(),
      node_graph_interface,
      remote_node_name_ + "/" + parameter_service_names::describe_parameters,
      options);
    auto describe_parameters_base =
      std::dynamic_pointer_cast<ClientBase>(describe_parameters_client_);
    node_services_interface->add_client(describe_parameters_base, nullptr);
  }
  
  AsyncParametersClient::AsyncParametersClient(
    const rclcpp::Node::SharedPtr node,
    const std::string & remote_node_name,
    const rmw_qos_profile_t & qos_profile)
  : AsyncParametersClient(
      node->get_node_base_interface(),
      node->get_node_topics_interface(),
      node->get_node_graph_interface(),
      node->get_node_services_interface(),
      remote_node_name,
      qos_profile)
  {}
  
  AsyncParametersClient::AsyncParametersClient(
    rclcpp::Node * node,
    const std::string & remote_node_name,
    const rmw_qos_profile_t & qos_profile)
  : AsyncParametersClient(
      node->get_node_base_interface(),
      node->get_node_topics_interface(),
      node->get_node_graph_interface(),
      node->get_node_services_interface(),
      remote_node_name,
      qos_profile)
  {}
  
  std::shared_future<std::vector<rclcpp::Parameter>>
  AsyncParametersClient::get_parameters(
    const std::vector<std::string> & names,
    std::function<
      void(std::shared_future<std::vector<rclcpp::Parameter>>)
    > callback)
  {
    auto promise_result =
      std::make_shared<std::promise<std::vector<rclcpp::Parameter>>>();
    auto future_result = promise_result->get_future().share();
  
    auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
    request->names = names;
  
    get_parameters_client_->async_send_request(
      request,
      [request, promise_result, future_result, callback](
        rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f)
      {
        std::vector<rclcpp::Parameter> parameters;
        auto & pvalues = cb_f.get()->values;
  
        for (auto & pvalue : pvalues) {
          auto i = &pvalue - &pvalues[0];
          rcl_interfaces::msg::Parameter parameter;
          parameter.name = request->names[i];
          parameter.value = pvalue;
          parameters.push_back(rclcpp::Parameter::from_parameter_msg(
            parameter));
        }
  
        promise_result->set_value(parameters);
        if (callback != nullptr) {
          callback(future_result);
        }
      }
    );
  
    return future_result;
  }
  
  std::shared_future<std::vector<rclcpp::ParameterType>>
  AsyncParametersClient::get_parameter_types(
    const std::vector<std::string> & names,
    std::function<
      void(std::shared_future<std::vector<rclcpp::ParameterType>>)
    > callback)
  {
    auto promise_result =
      std::make_shared<std::promise<std::vector<rclcpp::ParameterType>>>();
    auto future_result = promise_result->get_future().share();
  
    auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
    request->names = names;
  
    get_parameter_types_client_->async_send_request(
      request,
      [promise_result, future_result, callback](
        rclcpp::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f)
      {
        std::vector<rclcpp::ParameterType> types;
        auto & pts = cb_f.get()->types;
        for (auto & pt : pts) {
          pts.push_back(static_cast<rclcpp::ParameterType>(pt));
        }
        promise_result->set_value(types);
        if (callback != nullptr) {
          callback(future_result);
        }
      }
    );
  
    return future_result;
  }
  
  std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
  AsyncParametersClient::set_parameters(
    const std::vector<rclcpp::Parameter> & parameters,
    std::function<
      void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
    > callback)
  {
    auto promise_result =
      std::make_shared<std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>>>();
    auto future_result = promise_result->get_future().share();
  
    auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
  
    std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
      [](rclcpp::Parameter p) {
        return p.to_parameter_msg();
      }
    );
  
    set_parameters_client_->async_send_request(
      request,
      [promise_result, future_result, callback](
        rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture cb_f)
      {
        promise_result->set_value(cb_f.get()->results);
        if (callback != nullptr) {
          callback(future_result);
        }
      }
    );
  
    return future_result;
  }
  
  std::shared_future<rcl_interfaces::msg::SetParametersResult>
  AsyncParametersClient::set_parameters_atomically(
    const std::vector<rclcpp::Parameter> & parameters,
    std::function<
      void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
    > callback)
  {
    auto promise_result =
      std::make_shared<std::promise<rcl_interfaces::msg::SetParametersResult>>();
    auto future_result = promise_result->get_future().share();
  
    auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
  
    std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
      [](rclcpp::Parameter p) {
        return p.to_parameter_msg();
      }
    );
  
    set_parameters_atomically_client_->async_send_request(
      request,
      [promise_result, future_result, callback](
        rclcpp::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedFuture cb_f)
      {
        promise_result->set_value(cb_f.get()->result);
        if (callback != nullptr) {
          callback(future_result);
        }
      }
    );
  
    return future_result;
  }
  
  std::shared_future<rcl_interfaces::msg::ListParametersResult>
  AsyncParametersClient::list_parameters(
    const std::vector<std::string> & prefixes,
    uint64_t depth,
    std::function<
      void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
    > callback)
  {
    auto promise_result =
      std::make_shared<std::promise<rcl_interfaces::msg::ListParametersResult>>();
    auto future_result = promise_result->get_future().share();
  
    auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
    request->prefixes = prefixes;
    request->depth = depth;
  
    list_parameters_client_->async_send_request(
      request,
      [promise_result, future_result, callback](
        rclcpp::Client<rcl_interfaces::srv::ListParameters>::SharedFuture cb_f)
      {
        promise_result->set_value(cb_f.get()->result);
        if (callback != nullptr) {
          callback(future_result);
        }
      }
    );
  
    return future_result;
  }
  
  bool
  AsyncParametersClient::service_is_ready() const
  {
    return
      get_parameters_client_->service_is_ready() &&
      get_parameter_types_client_->service_is_ready() &&
      set_parameters_client_->service_is_ready() &&
      list_parameters_client_->service_is_ready() &&
      describe_parameters_client_->service_is_ready();
  }
  
  bool
  AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
  {
    const std::vector<std::shared_ptr<rclcpp::ClientBase>> clients = {
      get_parameters_client_,
      get_parameter_types_client_,
      set_parameters_client_,
      list_parameters_client_,
      describe_parameters_client_
    };
    for (auto & client : clients) {
      auto stamp = std::chrono::steady_clock::now();
      if (!client->wait_for_service(timeout)) {
        return false;
      }
      if (timeout > std::chrono::nanoseconds::zero()) {
        timeout -= std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::steady_clock::now() - stamp);
        if (timeout < std::chrono::nanoseconds::zero()) {
          timeout = std::chrono::nanoseconds::zero();
        }
      }
    }
    return true;
  }
  
  SyncParametersClient::SyncParametersClient(
    rclcpp::Node::SharedPtr node,
    const std::string & remote_node_name,
    const rmw_qos_profile_t & qos_profile)
  : SyncParametersClient(
      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
      node,
      remote_node_name,
      qos_profile)
  {}
  
  SyncParametersClient::SyncParametersClient(
    rclcpp::executor::Executor::SharedPtr executor,
    rclcpp::Node::SharedPtr node,
    const std::string & remote_node_name,
    const rmw_qos_profile_t & qos_profile)
  : executor_(executor), node_(node)
  {
    async_parameters_client_ =
      std::make_shared<AsyncParametersClient>(node, remote_node_name, qos_profile);
  }
  
  std::vector<rclcpp::Parameter>
  SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
  {
    auto f = async_parameters_client_->get_parameters(parameter_names);
    using rclcpp::executors::spin_node_until_future_complete;
    if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
      rclcpp::executor::FutureReturnCode::SUCCESS)
    {
      return f.get();
    }
    // Return an empty vector if unsuccessful
    return std::vector<rclcpp::Parameter>();
  }
  
  bool
  SyncParametersClient::has_parameter(const std::string & parameter_name)
  {
    std::vector<std::string> names;
    names.push_back(parameter_name);
    auto vars = list_parameters(names, 1);
    return vars.names.size() > 0;
  }
  
  std::vector<rclcpp::ParameterType>
  SyncParametersClient::get_parameter_types(const std::vector<std::string> & parameter_names)
  {
    auto f = async_parameters_client_->get_parameter_types(parameter_names);
  
    using rclcpp::executors::spin_node_until_future_complete;
    if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
      rclcpp::executor::FutureReturnCode::SUCCESS)
    {
      return f.get();
    }
    return std::vector<rclcpp::ParameterType>();
  }
  
  std::vector<rcl_interfaces::msg::SetParametersResult>
  SyncParametersClient::set_parameters(
    const std::vector<rclcpp::Parameter> & parameters)
  {
    auto f = async_parameters_client_->set_parameters(parameters);
  
    auto node_base_interface = node_->get_node_base_interface();
    using rclcpp::executors::spin_node_until_future_complete;
    if (spin_node_until_future_complete(*executor_, node_base_interface, f) ==
      rclcpp::executor::FutureReturnCode::SUCCESS)
    {
      return f.get();
    }
    return std::vector<rcl_interfaces::msg::SetParametersResult>();
  }
  
  rcl_interfaces::msg::SetParametersResult
  SyncParametersClient::set_parameters_atomically(
    const std::vector<rclcpp::Parameter> & parameters)
  {
    auto f = async_parameters_client_->set_parameters_atomically(parameters);
  
    using rclcpp::executors::spin_node_until_future_complete;
    if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
      rclcpp::executor::FutureReturnCode::SUCCESS)
    {
      return f.get();
    }
  
    throw std::runtime_error("Unable to get result of set parameters service call.");
  }
  
  rcl_interfaces::msg::ListParametersResult
  SyncParametersClient::list_parameters(
    const std::vector<std::string> & parameter_prefixes,
    uint64_t depth)
  {
    auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
  
    using rclcpp::executors::spin_node_until_future_complete;
    if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
      rclcpp::executor::FutureReturnCode::SUCCESS)
    {
      return f.get();
    }
  
    throw std::runtime_error("Unable to get result of list parameters service call.");
  }
  