Program Listing for File agnocast.hpp

↰ Return to documentation for file (include/agnocast/agnocast.hpp)

#pragma once

#include "agnocast/agnocast_callback_info.hpp"
#include "agnocast/agnocast_callback_isolated_executor.hpp"
#include "agnocast/agnocast_client.hpp"
#include "agnocast/agnocast_multi_threaded_executor.hpp"
#include "agnocast/agnocast_public_api.hpp"
#include "agnocast/agnocast_publisher.hpp"
#include "agnocast/agnocast_service.hpp"
#include "agnocast/agnocast_single_threaded_executor.hpp"
#include "agnocast/agnocast_subscription.hpp"
#include "agnocast/agnocast_tracepoint_wrapper.h"
#include "agnocast/bridge/agnocast_bridge_node.hpp"
#include "agnocast/bridge/performance/agnocast_performance_bridge_plugin_api.hpp"
#include "agnocast/message_filters/pass_through.hpp"
#include "agnocast/message_filters/subscriber.hpp"
#include "agnocast/message_filters/sync_policies/approximate_time.hpp"
#include "agnocast/message_filters/sync_policies/exact_time.hpp"
#include "agnocast/message_filters/synchronizer.hpp"
#include "agnocast/node/agnocast_context.hpp"
#include "agnocast/node/agnocast_node.hpp"
#include "agnocast/node/agnocast_only_callback_isolated_executor.hpp"
#include "agnocast/node/agnocast_only_executor.hpp"
#include "agnocast/node/agnocast_only_multi_threaded_executor.hpp"
#include "agnocast/node/agnocast_only_single_threaded_executor.hpp"
#include "rclcpp/rclcpp.hpp"

#include <fcntl.h>
#include <mqueue.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/syscall.h>
#include <unistd.h>

#include <cstring>
#include <functional>
#include <memory>
#include <type_traits>

namespace agnocast
{

// Type trait to extract the pointed-to node type from raw pointers and smart pointers.
template <typename T, typename = void>
struct node_pointer_traits
{
  using type = std::remove_pointer_t<T>;
};

template <typename T>
struct node_pointer_traits<T, std::void_t<typename T::element_type>>
{
  using type = typename T::element_type;
};

struct initialize_agnocast_result
{
  void * mempool_ptr;
  uint64_t mempool_size;
};

extern "C" struct initialize_agnocast_result initialize_agnocast(
  const unsigned char * heaphook_version_ptr, const size_t heaphook_version_str_len);

AGNOCAST_PUBLIC
template <typename MessageT, typename NodeT>
typename Publisher<MessageT>::SharedPtr create_publisher(
  NodeT * node, const std::string & topic_name, const rclcpp::QoS & qos,
  const PublisherOptions & options = PublisherOptions{})
{
  static_assert(
    std::is_base_of_v<rclcpp::Node, NodeT> || std::is_base_of_v<agnocast::Node, NodeT>,
    "NodeT must be rclcpp::Node or agnocast::Node (or derived from them)");
  return std::make_shared<BasicPublisher<MessageT, AgnocastToRosRequestPolicy>>(
    node, topic_name, qos, options);
}

AGNOCAST_PUBLIC
template <typename MessageT, typename NodeT>
typename Publisher<MessageT>::SharedPtr create_publisher(
  NodeT * node, const std::string & topic_name, const size_t qos_history_depth,
  const PublisherOptions & options = PublisherOptions{})
{
  static_assert(
    std::is_base_of_v<rclcpp::Node, NodeT> || std::is_base_of_v<agnocast::Node, NodeT>,
    "NodeT must be rclcpp::Node or agnocast::Node (or derived from them)");
  return std::make_shared<BasicPublisher<MessageT, AgnocastToRosRequestPolicy>>(
    node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), options);
}

AGNOCAST_PUBLIC
template <typename MessageT, typename NodeT, typename Func>
typename Subscription<MessageT>::SharedPtr create_subscription(
  NodeT * node, const std::string & topic_name, const rclcpp::QoS & qos, Func && callback,
  agnocast::SubscriptionOptions options = agnocast::SubscriptionOptions{})
{
  static_assert(
    std::is_base_of_v<rclcpp::Node, NodeT> || std::is_base_of_v<agnocast::Node, NodeT>,
    "NodeT must be rclcpp::Node or agnocast::Node (or derived from them)");
  return std::make_shared<BasicSubscription<MessageT, RosToAgnocastRequestPolicy>>(
    node, topic_name, qos, std::forward<Func>(callback), options);
}

AGNOCAST_PUBLIC
template <typename MessageT, typename NodeT, typename Func>
typename Subscription<MessageT>::SharedPtr create_subscription(
  NodeT * node, const std::string & topic_name, const size_t qos_history_depth, Func && callback,
  agnocast::SubscriptionOptions options = agnocast::SubscriptionOptions{})
{
  static_assert(
    std::is_base_of_v<rclcpp::Node, NodeT> || std::is_base_of_v<agnocast::Node, NodeT>,
    "NodeT must be rclcpp::Node or agnocast::Node (or derived from them)");
  return std::make_shared<BasicSubscription<MessageT, RosToAgnocastRequestPolicy>>(
    node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)),
    std::forward<Func>(callback), options);
}

AGNOCAST_PUBLIC
template <typename MessageT, typename NodeT>
typename PollingSubscriber<MessageT>::SharedPtr create_subscription(
  NodeT * node, const std::string & topic_name, const size_t qos_history_depth)
{
  static_assert(
    std::is_base_of_v<rclcpp::Node, NodeT> || std::is_base_of_v<agnocast::Node, NodeT>,
    "NodeT must be rclcpp::Node or agnocast::Node (or derived from them)");
  return std::make_shared<BasicPollingSubscriber<MessageT, RosToAgnocastRequestPolicy>>(
    node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)));
}

AGNOCAST_PUBLIC
template <typename MessageT, typename NodeT>
typename PollingSubscriber<MessageT>::SharedPtr create_subscription(
  NodeT * node, const std::string & topic_name, const rclcpp::QoS & qos)
{
  static_assert(
    std::is_base_of_v<rclcpp::Node, NodeT> || std::is_base_of_v<agnocast::Node, NodeT>,
    "NodeT must be rclcpp::Node or agnocast::Node (or derived from them)");
  return std::make_shared<BasicPollingSubscriber<MessageT, RosToAgnocastRequestPolicy>>(
    node, topic_name, qos);
}

// AGNOCAST_PUBLIC
template <typename ServiceT>
typename Client<ServiceT>::SharedPtr create_client(
  rclcpp::Node * node, const std::string & service_name,
  const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr)
{
  return std::make_shared<Client<ServiceT>>(node, service_name, qos, group);
}

// AGNOCAST_PUBLIC
template <typename ServiceT, typename Func>
typename Service<ServiceT>::SharedPtr create_service(
  rclcpp::Node * node, const std::string & service_name, Func && callback,
  const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr)
{
  return std::make_shared<Service<ServiceT>>(
    node, service_name, std::forward<Func>(callback), qos, group);
}

AGNOCAST_PUBLIC
template <typename NodeT, typename CallbackT>
TimerBase::SharedPtr create_timer(
  NodeT node, rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback,
  rclcpp::CallbackGroup::SharedPtr group = nullptr, bool autostart = true)
{
  using NodePtrT = typename node_pointer_traits<std::decay_t<NodeT>>::type;
  static_assert(
    std::is_base_of_v<rclcpp::Node, NodePtrT> || std::is_base_of_v<agnocast::Node, NodePtrT>,
    "NodeT must be rclcpp::Node or agnocast::Node (or derived from them)");
  static_assert(
    std::is_invocable_v<CallbackT, TimerBase &> || std::is_invocable_v<CallbackT>,
    "Callback must be callable with void() or void(TimerBase&)");

  if (!autostart) {
    RCLCPP_WARN(
      node->get_logger(),
      "autostart=false is not yet supported in agnocast. Timer will autostart.");
  }

  if (!group) {
    group = node->get_node_base_interface()->get_default_callback_group();
  }

  const uint32_t timer_id = allocate_timer_id();
  const auto period_ns = period.to_chrono<std::chrono::nanoseconds>();

  const void * callback_addr = static_cast<const void *>(&callback);
  const char * callback_symbol = tracetools::get_symbol(callback);

  auto timer = std::make_shared<GenericTimer<CallbackT>>(
    timer_id, period_ns, clock, std::forward<CallbackT>(callback));

  register_timer_info(timer_id, timer, period_ns, group, clock);

  const void * node_handle;
  if constexpr (std::is_base_of_v<rclcpp::Node, NodePtrT>) {
    node_handle = static_cast<const void *>(
      node->get_node_base_interface()->get_shared_rcl_node_handle().get());
  } else {
    node_handle = static_cast<const void *>(node->get_node_base_interface().get());
  }

  TRACEPOINT(
    agnocast_timer_init, static_cast<const void *>(timer.get()), node_handle, callback_addr,
    static_cast<const void *>(group.get()), callback_symbol, period_ns.count());

  return timer;
}

}  // namespace agnocast