callback_draft.cpp

// Copyright 2019 Dan Rose
// 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__ANY_SUBSCRIPTION_CALLBACK_HPP_
#define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_

#include <rmw/types.h>

#include <functional>
#include <memory>
#include <stdexcept>
#include <type_traits>
#include <utility>

#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/function_traits.hpp"
#include "rclcpp/visibility_control.hpp"

namespace rclcpp
{

template<typename MessageT, typename Alloc>
class AbstractSubscriptionCallback{
public:
  virtual bool use_take_shared_method() = 0;
  virtual void dispatch(std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info) = 0;
  virtual void dispatch_intra_process(ConstMessageSharedPtr message, const rmw_message_info_t & message_info) = 0;
  virtual void dispatch_intra_process(MessageUniquePtr message, const rmw_message_info_t & message_info) = 0;
}

// backport of cpp17 standard library function
template <typename R, typename F, typename... Args>
struct is_invocable_r :
std::is_constructible<
std::function<R(Args ...)>,
std::reference_wrapper<typename std::remove_reference<F>::type>
>
{};

template<typename MessageT, typename Alloc, typename CallbackT>
class AnySubscriptionCallback
{
protected:
  CallbackT callback;

  std::shared_ptr<MessageAlloc> message_allocator_;
  MessageDeleter message_deleter_;

protected:
  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
  using MessageAlloc = typename MessageAllocTraits::allocator_type;
  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
  using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;

  using ConstSharedPtrCallback = std::function<void (ConstMessageSharedPtr const &)>;
  using ConstSharedPtrWithInfoCallback =  std::function<void (ConstMessageSharedPtr const &, const rmw_message_info_t &)>;

  // note the overlap. Every ConstSharedPtrCallback can also be called as a UniquePtrCallback, since a unique_ptr
  // rvalue can be implicitly promoted to a shared_ptr
  using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
  using UniquePtrWithInfoCallback = std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;

  using ConstRefCallback = std::function<void (const MessageT &)>;
  using ConstRefWithInfoCallback = std::function<void (const MessageT &, const rmw_message_info_t &)>;

public:
  explicit AnySubscriptionCallback(std::shared_ptr<Alloc> allocator, CallbackT callback)
  : callback(callback) {
    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
  }

  AnySubscriptionCallback(const AnySubscriptionCallback &) = default;

  virtual void dispatch(const MessageT & msg, const rmw_message_info_t & info) const override {
    dispatch_impl(callback, msg, info);
  }

  static void dispatch_impl(ConstRefCallback f, Msg const &msg, rmw_message_info_t const &)
  { f(msg); }

  static void dispatch_impl(ConstRefWithInfoCallback f, Msg const &msg, rmw_message_info_t const &)
  { f(msg, info); }

  static void dispatch_impl(UniquePtrCallback f,
    Msg const &msg,
    const rmw_message_info_t & info)
  {
    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message);
    f(std::unique_ptr<Msg>(msg,message_deleter_));
  }

  static void dispatch_impl(UniquePtrWithInfoCallback f,
    Msg const &msg,
    const rmw_message_info_t & info)
  {
    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message);
    f(std::unique_ptr<Msg>(msg,message_deleter_), info);
  }

  template <typename F_>
  static constexpr can_call_shared() {
    return 
      is_invocable_r<void, F_, std::shared_ptr<const Msg> const &> ||
      is_invocable_r<void, F_, std::shared_ptr<const Msg, rmw_message_info_t const &> const &>;
  };

  virtual bool use_take_shared_method()  override {
    return CanCallShared;
  };

  static void dispatch_shared_impl(
    ConstSharedPtrCallback fn,
    ConstMessageSharedPtr const & msg,
    rmw_message_info_t const &)
  { 
    fn(msg);
  }

  static void dispatch_shared_impl(
    ConstSharedPtrWithInfoCallback fn,
    ConstMessageSharedPtr const & msg,
    rmw_message_info_t const & info)
  { 
    fn(msg, info);
  }

  template<typename T_=T, std::enable_if(!can_call_shared(T_))>
  static void dispatch_shared_impl(
    T_ && t,
    ConstMessageSharedPtr const & msg,
    rmw_message_info_t const & info)
  {
    dispatch_impl(std::forward(t), *msg, info);
  }
};

}  // namespace rclcpp

#endif  // RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_