service.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/service.hpp"
 
#include <functional>
#include <memory>
#include <mutex>
#include <sstream>
#include <stdexcept>
#include <string>
 
#include "rclcpp/any_service_callback.hpp"
#include "rclcpp/detail/cpp_callback_trampoline.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/qos.hpp"
#include "rmw/error_handling.h"
#include "rmw/impl/cpp/demangle.hpp"
#include "rmw/rmw.h"
 
using rclcpp::ServiceBase;
 
ServiceBase::ServiceBase(const std::shared_ptr<rcl_node_t> & node_handle)
: node_handle_(node_handle),
  node_logger_(rclcpp::get_node_logger(node_handle_.get()))
{}
 
 
bool
ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out)
{
  rcl_ret_t ret = rcl_take_request(
    this->get_service_handle().get(),
    &request_id_out,
    request_out);
  if (RCL_RET_SERVICE_TAKE_FAILED == ret) {
    return false;
  } else if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
  return true;
}
 
const char *
ServiceBase::get_service_name()
{
  return rcl_service_get_service_name(this->get_service_handle().get());
}
 
std::shared_ptr<rcl_service_t>
ServiceBase::get_service_handle()
{
  return service_handle_;
}
 
std::shared_ptr<const rcl_service_t>
ServiceBase::get_service_handle() const
{
  return service_handle_;
}
 
rcl_node_t *
ServiceBase::get_rcl_node_handle()
{
  return node_handle_.get();
}
 
const rcl_node_t *
ServiceBase::get_rcl_node_handle() const
{
  return node_handle_.get();
}
 
bool
ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state)
{
  return in_use_by_wait_set_.exchange(in_use_state);
}
 
rclcpp::QoS
ServiceBase::get_response_publisher_actual_qos() const
{
  const rmw_qos_profile_t * qos =
    rcl_service_response_publisher_get_actual_qos(service_handle_.get());
  if (!qos) {
    auto msg =
      std::string("failed to get service's response publisher qos settings: ") +
      rcl_get_error_string().str;
    rcl_reset_error();
    throw std::runtime_error(msg);
  }
 
  rclcpp::QoS response_publisher_qos =
    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
 
  return response_publisher_qos;
}
 
rclcpp::QoS
ServiceBase::get_request_subscription_actual_qos() const
{
  const rmw_qos_profile_t * qos =
    rcl_service_request_subscription_get_actual_qos(service_handle_.get());
  if (!qos) {
    auto msg =
      std::string("failed to get service's request subscription qos settings: ") +
      rcl_get_error_string().str;
    rcl_reset_error();
    throw std::runtime_error(msg);
  }
 
  rclcpp::QoS request_subscription_qos =
    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
 
  return request_subscription_qos;
}
 
void
ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data)
{
  rcl_ret_t ret = rcl_service_set_on_new_request_callback(
    service_handle_.get(),
    callback,
    user_data);
 
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(
      ret, "failed to set the on new request callback for service");
  }
}
 
void
ServiceBase::set_on_new_request_callback(const std::function<void(size_t)> & callback)
{
  if (!callback) {
    throw std::invalid_argument(
            "The callback passed to set_on_new_request_callback "
            "is not callable.");
  }
 
  auto new_callback =
    [callback, this](size_t number_of_requests) {
      try {
        callback(number_of_requests);
      } catch (const std::exception & exception) {
        RCLCPP_ERROR_STREAM(
          node_logger_,
          "rclcpp::ServiceBase@" << this <<
            " caught " << rmw::impl::cpp::demangle(exception) <<
            " exception in user-provided callback for the 'on new request' callback: " <<
            exception.what());
      } catch (...) {
        RCLCPP_ERROR_STREAM(
          node_logger_,
          "rclcpp::ServiceBase@" << this <<
            " caught unhandled exception in user-provided callback " <<
            "for the 'on new request' 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_request_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_request_callback_ = new_callback;
 
  // Set it again, now using the permanent storage.
  set_on_new_request_callback(
    rclcpp::detail::cpp_callback_trampoline<
      decltype(on_new_request_callback_), const void *, size_t>,
    static_cast<const void *>(&on_new_request_callback_));
}
 
void
ServiceBase::clear_on_new_request_callback()
{
  std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
  if (on_new_request_callback_) {
    set_on_new_request_callback(nullptr, nullptr);
    on_new_request_callback_ = nullptr;
  }
}