File generic_publisher.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") publisher abstraction for the Agnocast build.

#include <rclcpp/qos_overriding_options.hpp>

#include <stdexcept>
#include <string>

namespace nebula::agnocast_wrapper::detail
{

inline void check_generic_publisher_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_publisher(" + topic_name +
      "): qos_overriding_options is not supported for a generic publisher (honored by "
      "Agnocast's GenericPublisher but silently ignored by rclcpp's, so it cannot behave "
      "consistently across backends or builds)");
  }
}

}  // namespace nebula::agnocast_wrapper::detail

#ifdef USE_AGNOCAST_ENABLED

#include "nebula_core_ros/agnocast_wrapper/runtime.hpp"

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

#include <cstddef>
#include <cstdint>
#include <memory>

namespace nebula::agnocast_wrapper
{

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

  virtual ~GenericPublisher() = default;

  virtual void publish(const rclcpp::SerializedMessage & message) = 0;

  virtual uint32_t get_subscription_count() const = 0;
  virtual uint32_t get_intra_process_subscription_count() const = 0;
  virtual const char * get_topic_name() const = 0;

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

class AgnocastGenericPublisher : public GenericPublisher
{
  agnocast::GenericPublisher::SharedPtr publisher_;

public:
  template <typename NodeT>
  explicit AgnocastGenericPublisher(
    NodeT * node, const std::string & topic_name, const std::string & topic_type,
    const rclcpp::QoS & qos, const agnocast::PublisherOptions & options)
  {
    detail::check_generic_publisher_qos_overriding_options(
      options.qos_overriding_options, topic_name);
    publisher_ = agnocast::create_generic_publisher(node, topic_name, topic_type, qos, options);
  }

  void publish(const rclcpp::SerializedMessage & message) override { publisher_->publish(message); }

  uint32_t get_subscription_count() const override { return publisher_->get_subscription_count(); }
  uint32_t get_intra_process_subscription_count() const override
  {
    return publisher_->get_intra_subscription_count();
  }
  const char * get_topic_name() const override { return publisher_->get_topic_name(); }
  rclcpp::QoS get_actual_qos() const override { return publisher_->get_actual_qos(); }
};

class ROS2GenericPublisher : public GenericPublisher
{
  rclcpp::GenericPublisher::SharedPtr publisher_;

public:
  explicit ROS2GenericPublisher(
    rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
    const rclcpp::QoS & qos, const agnocast::PublisherOptions & options)
  {
    detail::check_generic_publisher_qos_overriding_options(
      options.qos_overriding_options, topic_name);
    publisher_ = node->create_generic_publisher(topic_name, topic_type, qos);
  }

  void publish(const rclcpp::SerializedMessage & message) override { publisher_->publish(message); }

  uint32_t get_subscription_count() const override { return publisher_->get_subscription_count(); }
  uint32_t get_intra_process_subscription_count() const override
  {
    return publisher_->get_intra_process_subscription_count();
  }
  const char * get_topic_name() const override { return publisher_->get_topic_name(); }
  rclcpp::QoS get_actual_qos() const override { return publisher_->get_actual_qos(); }
};

inline GenericPublisher::SharedPtr create_generic_publisher(
  rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
  const rclcpp::QoS & qos,
  const agnocast::PublisherOptions & options = agnocast::PublisherOptions{})
{
  if (use_agnocast()) {
    return std::make_shared<AgnocastGenericPublisher>(node, topic_name, topic_type, qos, options);
  } else {
    return std::make_shared<ROS2GenericPublisher>(node, topic_name, topic_type, qos, options);
  }
}

inline GenericPublisher::SharedPtr create_generic_publisher(
  rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
  const size_t qos_history_depth,
  const agnocast::PublisherOptions & options = agnocast::PublisherOptions{})
{
  return create_generic_publisher(
    node, topic_name, topic_type, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), options);
}

}  // namespace nebula::agnocast_wrapper

#else

#include <rclcpp/rclcpp.hpp>

#include <cstddef>
#include <memory>

namespace nebula::agnocast_wrapper
{

inline rclcpp::GenericPublisher::SharedPtr create_generic_publisher(
  rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
  const rclcpp::QoS & qos, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions{})
{
  detail::check_generic_publisher_qos_overriding_options(
    options.qos_overriding_options, topic_name);
  return node->create_generic_publisher(topic_name, topic_type, qos, options);
}

inline rclcpp::GenericPublisher::SharedPtr create_generic_publisher(
  rclcpp::Node * node, const std::string & topic_name, const std::string & topic_type,
  const size_t qos_history_depth)
{
  return create_generic_publisher(
    node, topic_name, topic_type, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)));
}

}  // namespace nebula::agnocast_wrapper

#endif