File sample_ros_wrapper.hpp

// Copyright 2026 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.

#ifndef NEBULA_SAMPLE_ROS_WRAPPER_HPP
#define NEBULA_SAMPLE_ROS_WRAPPER_HPP

#include "nebula_sample_decoders/sample_decoder.hpp"
#include "nebula_sample_hw_interfaces/sample_hw_interface.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <nebula_core_common/util/expected.hpp>
#include <nebula_core_ros/diagnostics/liveness_monitor.hpp>
#include <nebula_core_ros/diagnostics/rate_bound_status.hpp>
#include <nebula_sample_common/sample_configuration.hpp>
#include <rclcpp/rclcpp.hpp>

#include <nebula_msgs/msg/nebula_packet.hpp>
#include <nebula_msgs/msg/nebula_packets.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/float64.hpp>

#include <cstdint>
#include <memory>
#include <optional>
#include <string>
#include <utility>
#include <variant>
#include <vector>

namespace nebula::ros
{

struct ConfigError
{
  enum class Code : uint8_t {
    PARAMETER_DECLARATION_FAILED,  
    PARAMETER_VALIDATION_FAILED,   
  };

  Code code;
  std::string message;
};

util::expected<drivers::SampleSensorConfiguration, ConfigError> load_config_from_ros_parameters(
  rclcpp::Node & node);

class SampleRosWrapper : public rclcpp::Node
{
public:
  struct Error
  {
    enum class Code : uint8_t {
      HW_INTERFACE_NOT_INITIALIZED,  
      HW_STREAM_START_FAILED,        
    };

    Code code;
    std::string message;
  };

  explicit SampleRosWrapper(const rclcpp::NodeOptions & options);
  ~SampleRosWrapper() override;

private:
  struct OnlineMode
  {
    explicit OnlineMode(drivers::ConnectionConfiguration connection_configuration)
    : hw_interface(std::move(connection_configuration))
    {
    }

    drivers::SampleHwInterface hw_interface;
    rclcpp::Publisher<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_pub;
    std::unique_ptr<nebula_msgs::msg::NebulaPackets> current_scan_packets_msg{
      std::make_unique<nebula_msgs::msg::NebulaPackets>()};
  };

  struct OfflineMode
  {
    rclcpp::Subscription<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_sub;
  };

  struct Publishers
  {
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr points;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr receive_duration_ms;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr decode_duration_ms;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publish_duration_ms;
  };

  struct Diagnostics
  {
    explicit Diagnostics(rclcpp::Node * node) : updater(node) {}

    diagnostic_updater::Updater updater;
    std::optional<custom_diagnostic_tasks::RateBoundStatus> publish_rate;
    std::optional<LivenessMonitor> packet_liveness;
  };

  void publish_pointcloud_callback(
    const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s);

  void receive_cloud_packet_callback(
    std::vector<uint8_t> & packet, const drivers::connections::UdpSocket::RxMetadata & metadata);

  void receive_packets_message_callback(
    std::unique_ptr<nebula_msgs::msg::NebulaPackets> packets_msg);

  void initialize_diagnostics();

  void process_packet(const std::vector<uint8_t> & packet, uint64_t receive_duration_ns);

  void publish_debug_durations(
    uint64_t receive_duration_ns, uint64_t decode_duration_ns, uint64_t publish_duration_ns) const;

  static const char * to_cstr(Error::Code code);

  drivers::SampleSensorConfiguration config_;
  std::string frame_id_;
  Publishers publishers_;
  Diagnostics diagnostics_;

  std::optional<drivers::SampleDecoder> decoder_;
  std::variant<std::monostate, OfflineMode, OnlineMode> runtime_mode_;
};

}  // namespace nebula::ros

#endif  // NEBULA_SAMPLE_ROS_WRAPPER_HPP