client.cpp

// 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/client.hpp"
 
#include <algorithm>
#include <chrono>
#include <cstdio>
#include <functional>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
 
#include "rcl/graph.h"
#include "rcl/node.h"
#include "rcl/wait.h"
 
#include "rmw/impl/cpp/demangle.hpp"
 
#include "rclcpp/detail/cpp_callback_trampoline.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/logging.hpp"
 
using rclcpp::ClientBase;
using rclcpp::exceptions::InvalidNodeError;
using rclcpp::exceptions::throw_from_rcl_error;
 
ClientBase::ClientBase(
  rclcpp::node_interfaces::NodeBaseInterface * node_base,
  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr & node_graph)
: node_graph_(node_graph),
  node_handle_(node_base->get_shared_rcl_node_handle()),
  context_(node_base->get_context()),
  node_logger_(rclcpp::get_node_logger(node_handle_.get()))
{
  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
  rcl_client_t * new_rcl_client = new rcl_client_t;
  *new_rcl_client = rcl_get_zero_initialized_client();
  client_handle_.reset(
    new_rcl_client, [weak_node_handle](rcl_client_t * client)
    {
      auto handle = weak_node_handle.lock();
      if (handle) {
        if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) {
          RCLCPP_ERROR(
            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
            "Error in destruction of rcl client handle: %s", rcl_get_error_string().str);
          rcl_reset_error();
        }
      } else {
        RCLCPP_ERROR(
          rclcpp::get_logger("rclcpp"),
          "Error in destruction of rcl client handle: "
          "the Node Handle was destructed too early. You will leak memory");
      }
      delete client;
    });
}
 
bool
ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out)
{
  rcl_ret_t ret = rcl_take_response(
    this->get_client_handle().get(),
    &request_header_out,
    response_out);
  if (RCL_RET_CLIENT_TAKE_FAILED == ret) {
    return false;
  } else if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
  return true;
}
 
const char *
ClientBase::get_service_name() const
{
  return rcl_client_get_service_name(this->get_client_handle().get());
}
 
std::shared_ptr<rcl_client_t>
ClientBase::get_client_handle()
{
  return client_handle_;
}
 
std::shared_ptr<const rcl_client_t>
ClientBase::get_client_handle() const
{
  return client_handle_;
}
 
bool
ClientBase::service_is_ready() const
{
  bool is_ready;
  rcl_ret_t ret = rcl_service_server_is_available(
    this->get_rcl_node_handle(),
    this->get_client_handle().get(),
    &is_ready);
  if (RCL_RET_NODE_INVALID == ret) {
    const rcl_node_t * node_handle = this->get_rcl_node_handle();
    if (node_handle && !rcl_context_is_valid(node_handle->context)) {
      // context is shutdown, do a soft failure
      return false;
    }
  }
  if (ret != RCL_RET_OK) {
    throw_from_rcl_error(ret, "rcl_service_server_is_available failed");
  }
  return is_ready;
}
 
bool
ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
{
  auto start = std::chrono::steady_clock::now();
  auto node_ptr = node_graph_.lock();
  if (!node_ptr) {
    throw InvalidNodeError();
  }
  // check to see if the server is ready immediately
  if (this->service_is_ready()) {
    return true;
  }
  if (timeout == std::chrono::nanoseconds(0)) {
    // check was non-blocking, return immediately
    return false;
  }
  // make an event to reuse, rather than create a new one each time
  auto event = node_ptr->get_graph_event();
  // update the time even on the first loop to account for time spent in the first call
  // to this->server_is_ready()
  std::chrono::nanoseconds time_to_wait =
    timeout > std::chrono::nanoseconds(0) ?
    timeout - (std::chrono::steady_clock::now() - start) :
    std::chrono::nanoseconds::max();
  if (time_to_wait < std::chrono::nanoseconds(0)) {
    // Do not allow the time_to_wait to become negative when timeout was originally positive.
    // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while.
    time_to_wait = std::chrono::nanoseconds(0);
  }
  do {
    if (!rclcpp::ok(this->context_)) {
      return false;
    }
    // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation.
    // A race condition means that graph changes for services becoming available may trigger the
    // wait set to wake up, but then not be reported as ready immediately after the wake up
    // (see https://github.com/ros2/rmw_connext/issues/201)
    // If no other graph events occur, the wait set will not be triggered again until the timeout
    // has been reached, despite the service being available, so we artificially limit the wait
    // time to limit the delay.
    node_ptr->wait_for_graph_change(
      event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100))));
    // Because of the aforementioned race condition, we check if the service is ready even if the
    // graph event wasn't triggered.
    event->check_and_clear();
    if (this->service_is_ready()) {
      return true;
    }
    // server is not ready, loop if there is time left
    if (timeout > std::chrono::nanoseconds(0)) {
      time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
    }
    // if timeout is negative, time_to_wait will never reach zero
  } while (time_to_wait > std::chrono::nanoseconds(0));
  return false;  // timeout exceeded while waiting for the server to be ready
}
 
rcl_node_t *
ClientBase::get_rcl_node_handle()
{
  return node_handle_.get();
}
 
const rcl_node_t *
ClientBase::get_rcl_node_handle() const
{
  return node_handle_.get();
}
 
bool
ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state)
{
  return in_use_by_wait_set_.exchange(in_use_state);
}
 
rclcpp::QoS
ClientBase::get_request_publisher_actual_qos() const
{
  const rmw_qos_profile_t * qos =
    rcl_client_request_publisher_get_actual_qos(client_handle_.get());
  if (!qos) {
    auto msg =
      std::string("failed to get client's request publisher qos settings: ") +
      rcl_get_error_string().str;
    rcl_reset_error();
    throw std::runtime_error(msg);
  }
 
  rclcpp::QoS request_publisher_qos =
    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
 
  return request_publisher_qos;
}
 
rclcpp::QoS
ClientBase::get_response_subscription_actual_qos() const
{
  const rmw_qos_profile_t * qos =
    rcl_client_response_subscription_get_actual_qos(client_handle_.get());
  if (!qos) {
    auto msg =
      std::string("failed to get client's response subscription qos settings: ") +
      rcl_get_error_string().str;
    rcl_reset_error();
    throw std::runtime_error(msg);
  }
 
  rclcpp::QoS response_subscription_qos =
    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
 
  return response_subscription_qos;
}
 
void
ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data)
{
  rcl_ret_t ret = rcl_client_set_on_new_response_callback(
    client_handle_.get(),
    callback,
    user_data);
 
  if (RCL_RET_OK != ret) {
    throw_from_rcl_error(ret, "failed to set the on new response callback for client");
  }
}
 
void
ClientBase::set_on_new_response_callback(const std::function<void(size_t)> & callback)
{
  if (!callback) {
    throw std::invalid_argument(
            "The callback passed to set_on_new_response_callback "
            "is not callable.");
  }
 
  auto new_callback =
    [callback, this](size_t number_of_responses) {
      try {
        callback(number_of_responses);
      } catch (const std::exception & exception) {
        RCLCPP_ERROR_STREAM(
          node_logger_,
          "rclcpp::ClientBase@" << this <<
            " caught " << rmw::impl::cpp::demangle(exception) <<
            " exception in user-provided callback for the 'on new response' callback: " <<
            exception.what());
      } catch (...) {
        RCLCPP_ERROR_STREAM(
          node_logger_,
          "rclcpp::ClientBase@" << this <<
            " caught unhandled exception in user-provided callback " <<
            "for the 'on new response' callback");
      }
    };
 
  std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
 
  // Set it temporarily to the new callback, while we replace the old one.
  // This two-step setting, prevents a gap where the old std::function has
  // been replaced but the middleware hasn't been told about the new one yet.
  set_on_new_response_callback(
    rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
    static_cast<const void *>(&new_callback));
 
  // Store the std::function to keep it in scope, also overwrites the existing one.
  on_new_response_callback_ = new_callback;
 
  // Set it again, now using the permanent storage.
  set_on_new_response_callback(
    rclcpp::detail::cpp_callback_trampoline<
      decltype(on_new_response_callback_), const void *, size_t>,
    static_cast<const void *>(&on_new_response_callback_));
}
 
void
ClientBase::clear_on_new_response_callback()
{
  std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
  if (on_new_response_callback_) {
    set_on_new_response_callback(nullptr, nullptr);
    on_new_response_callback_ = nullptr;
  }
}