File generic_subscription.hpp

// Copyright 2025 TIER IV, 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.

#pragma once

// Type-erased ("generic") subscription abstraction for the Agnocast build.

#include <rclcpp/qos_overriding_options.hpp>
#include <rclcpp/serialized_message.hpp>

#include <functional>
#include <memory>
#include <stdexcept>
#include <string>

namespace nebula::agnocast_wrapper
{

using GenericSubscriptionCallback =
  std::function<void(std::shared_ptr<const rclcpp::SerializedMessage>)>;

namespace detail
{

inline void check_generic_subscription_qos_overriding_options(
  const rclcpp::QosOverridingOptions & qos_overriding_options, const std::string & topic_name)
{
  if (!qos_overriding_options.get_policy_kinds().empty()) {
    throw std::invalid_argument(
      "create_generic_subscription(" + topic_name +
      "): qos_overriding_options is not supported for a generic subscription (honored by "
      "Agnocast's GenericSubscription but silently ignored by rclcpp's, so it cannot behave "
      "consistently across backends or builds)");
  }
}

}  // namespace detail

}  // namespace nebula::agnocast_wrapper

#ifdef USE_AGNOCAST_ENABLED

#include "nebula_core_ros/agnocast_wrapper/runtime.hpp"
#include "nebula_core_ros/agnocast_wrapper/subscription.hpp"

#include <agnocast/agnocast.hpp>
#include <rclcpp/rclcpp.hpp>

#include <cstddef>
#include <utility>

namespace nebula::agnocast_wrapper
{

class GenericSubscription
{
public:
  using SharedPtr = std::shared_ptr<GenericSubscription>;

  virtual ~GenericSubscription() = default;

  virtual const char * get_topic_name() const = 0;

  virtual rclcpp::QoS get_actual_qos() const = 0;
};

class AgnocastGenericSubscription : public GenericSubscription
{
  agnocast::GenericSubscription::SharedPtr subscription_;

public:
  template <typename NodeT>
  explicit AgnocastGenericSubscription(
    NodeT * node, const std::string & topic_name, const std::string & topic_type,
    const rclcpp::QoS & qos, GenericSubscriptionCallback callback,
    const agnocast::SubscriptionOptions & options)
  {
    detail::check_generic_subscription_qos_overriding_options(
      options.qos_overriding_options, topic_name);
    subscription_ = agnocast::create_generic_subscription(
      node, topic_name, topic_type, qos, std::move(callback), options);
  }

  const char * get_topic_name() const override { return subscription_->get_topic_name(); }
  rclcpp::QoS get_actual_qos() const override { return subscription_->get_actual_qos(); }
};

class ROS2GenericSubscription : public GenericSubscription
{
  rclcpp::GenericSubscription::SharedPtr subscription_;

public:
  explicit ROS2GenericSubscription(
    rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
    const rclcpp::QoS & qos, GenericSubscriptionCallback callback,
    const agnocast::SubscriptionOptions & options)
  {
    detail::check_generic_subscription_qos_overriding_options(
      options.qos_overriding_options, topic_name);
    subscription_ = node->create_generic_subscription(
      topic_name, topic_type, qos, std::move(callback), to_rclcpp_subscription_options(options));
  }

  const char * get_topic_name() const override { return subscription_->get_topic_name(); }
  rclcpp::QoS get_actual_qos() const override { return subscription_->get_actual_qos(); }
};

inline GenericSubscription::SharedPtr create_generic_subscription(
  rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
  const rclcpp::QoS & qos, GenericSubscriptionCallback callback,
  const agnocast::SubscriptionOptions & options = agnocast::SubscriptionOptions{})
{
  if (use_agnocast()) {
    return std::make_shared<AgnocastGenericSubscription>(
      node, topic_name, topic_type, qos, std::move(callback), options);
  } else {
    return std::make_shared<ROS2GenericSubscription>(
      node, topic_name, topic_type, qos, std::move(callback), options);
  }
}

inline GenericSubscription::SharedPtr create_generic_subscription(
  rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
  const size_t qos_history_depth, GenericSubscriptionCallback callback,
  const agnocast::SubscriptionOptions & options = agnocast::SubscriptionOptions{})
{
  return create_generic_subscription(
    node, topic_name, topic_type, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)),
    std::move(callback), options);
}

}  // namespace nebula::agnocast_wrapper

#else

#include <rclcpp/rclcpp.hpp>

#include <cstddef>
#include <utility>

namespace nebula::agnocast_wrapper
{

inline rclcpp::GenericSubscription::SharedPtr create_generic_subscription(
  rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
  const rclcpp::QoS & qos, GenericSubscriptionCallback callback,
  const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions{})
{
  detail::check_generic_subscription_qos_overriding_options(
    options.qos_overriding_options, topic_name);
  return node->create_generic_subscription(
    topic_name, topic_type, qos, std::move(callback), options);
}

inline rclcpp::GenericSubscription::SharedPtr create_generic_subscription(
  rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
  const size_t qos_history_depth, GenericSubscriptionCallback callback)
{
  return create_generic_subscription(
    node, topic_name, topic_type, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)),
    std::move(callback));
}

}  // namespace nebula::agnocast_wrapper

#endif