camera.hpp

/****************************************************************************
 *
 * Copyright 2023-2026 Auterion AG. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 
#pragma once
 
#ifdef ROS_DISTRO_IS_ROLLING
#include <tf2_ros/transform_listener.hpp>
#else
#include <tf2_ros/transform_listener.h>
#endif
 
#include <Eigen/Eigen>
#include <auterion_core_msgs/msg/camera_info.hpp>
#include <auterion_core_msgs/msg/image8m.hpp>
#include <chrono>
#include <functional>
#include <opencv2/core/mat.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <string>
#include <utility>
 
#include "camera_control.hpp"
 
namespace auterion {
using namespace std::chrono_literals;
 
class SDK;
class CameraMonitorImpl;
class CameraImpl;
 
struct ImageHeader {
    ImageHeader() = default;
 
    enum class Encoding {
        Unknown,
        Y800,  
        Y16,   
        UYVY,  
        YUY2,  
        I420,  
        NV12,  
        RGB8,
        BGR8,
        BA81,  
        GBRG,  
        GRBG,  
        RGGB,  
        RGBA   
    };
 
    ImageHeader(int width_, int height_, Encoding encoding_, int step_ = 0);
 
    template <typename ImageT = auterion_core_msgs::msg::Image8m>
    explicit ImageHeader(const ImageT& image)
        : width(image.width), height(image.height), step(image.step), timestamp(image.stamp) {
        const int encoding_len =
            strnlen(reinterpret_cast<const char*>(image.encoding.data()), image.encoding.size());
        const std::string encoding_str(reinterpret_cast<const char*>(image.encoding.data()),
                                       encoding_len);
        if (encoding_str == "mono8") {
            encoding = Encoding::Y800;
        } else if (encoding_str == "mono16") {
            encoding = Encoding::Y16;
        } else if (encoding_str == "uyvy") {
            encoding = Encoding::UYVY;
        } else if (encoding_str == "yuyv") {
            encoding = Encoding::YUY2;
        } else if (encoding_str == "i420") {
            // (This does not exist in ROS:
            // https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/include/sensor_msgs/image_encodings.hpp)
            encoding = Encoding::I420;
        } else if (encoding_str == "nv12") {
            // Does not exist in ROS 2
            encoding = Encoding::NV12;
        } else if (encoding_str == "rgb8") {
            encoding = Encoding::RGB8;
        } else if (encoding_str == "rgba8") {
            encoding = Encoding::RGBA;
        } else if (encoding_str == "bgr8") {
            encoding = Encoding::BGR8;
        } else if (encoding_str == "bayer_bggr8") {
            encoding = Encoding::BA81;
        } else if (encoding_str == "bayer_gbrg8") {
            encoding = Encoding::GBRG;
        } else if (encoding_str == "bayer_grbg8") {
            encoding = Encoding::GRBG;
        } else if (encoding_str == "bayer_rggb8") {
            encoding = Encoding::RGGB;
        }
    }
 
    template <typename ImageT = auterion_core_msgs::msg::Image8m>
    void toImage(ImageT& image) {
        image.width = width;
        image.height = height;
        image.step = step;
        image.stamp = timestamp;
        switch (encoding) {
            case Encoding::Unknown:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "unknown",
                        image.encoding.size());
                break;
            case Encoding::Y800:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "mono8",
                        image.encoding.size());
                break;
            case Encoding::Y16:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "mono16",
                        image.encoding.size());
                break;
            case Encoding::UYVY:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "uyvy",
                        image.encoding.size());
                break;
            case Encoding::YUY2:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "yuyv",
                        image.encoding.size());
                break;
            case Encoding::I420:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "i420",
                        image.encoding.size());
                break;
            case Encoding::NV12:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "nv12",
                        image.encoding.size());
                break;
            case Encoding::RGB8:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "rgb8",
                        image.encoding.size());
                break;
            case Encoding::BGR8:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bgr8",
                        image.encoding.size());
                break;
            case Encoding::RGBA:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "rgba8",
                        image.encoding.size());
                break;
            case Encoding::BA81:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bayer_bggr8",
                        image.encoding.size());
                break;
            case Encoding::GBRG:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bayer_gbrg8",
                        image.encoding.size());
                break;
            case Encoding::GRBG:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bayer_grbg8",
                        image.encoding.size());
                break;
            case Encoding::RGGB:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bayer_rggb8",
                        image.encoding.size());
                break;
        }
    }
 
    size_t planeOffset(int plane) const;
    size_t planeSizeBytes(int plane) const;
    size_t imageSizeBytes() const;
 
    int width{};
    int height{};
    int step{};  
    Encoding encoding{Encoding::Unknown};
 
    rclcpp::Time timestamp{};
    std::optional<uint64_t> monotonic_timestamp_us{std::nullopt};  
    uint32_t frame_id{};                                           
 
    float framerate{0.f};
 
    sensor_msgs::msg::CameraInfo camera_info{};
};
 
std::ostream& operator<<(std::ostream& os, const ImageHeader::Encoding& encoding);
 
class Image {
   public:
    Image(ImageHeader header, uint8_t* data) : _header(std::move(header)), _data(data) {}
 
    const ImageHeader& header() const { return _header; }
 
    uint8_t* data() { return _data; }
    const uint8_t* data() const { return _data; }
 
    int width() const { return _header.width; }
    int height() const { return _header.height; }
 
    cv::Mat asOpenCVMat(int plane = -1) const;
 
   private:
    ImageHeader _header{};
    uint8_t* _data{nullptr};
};
 
class ImageFD {
   public:
    ImageFD(ImageHeader header, int fd, uint64_t buffer_size,
            std::function<void()> release_function, uint8_t* buffer = nullptr,
            std::function<void()> discard_function = nullptr)
        : _header(std::move(header)),
          _fd(fd),
          _buffer_size(buffer_size),
          _buffer(buffer),
          _release_function(std::move(release_function)),
          _discard_function(std::move(discard_function)) {}
 
    ~ImageFD() { release(); }
 
    // Delete copy operations
    // Copying an ImageFD is not allowed since it contains an FD that should be uniquely owned.
    ImageFD(const ImageFD&) = delete;
    ImageFD& operator=(const ImageFD&) = delete;
 
    // Move constructor
    ImageFD(ImageFD&& other) noexcept
        : _header(std::move(other._header)),
          _fd(other._fd),
          _buffer_size(other._buffer_size),
          _buffer(other.buffer()),
          _release_function(std::move(other._release_function)),
          _discard_function(std::move(other._discard_function)),
          _was_released(other._was_released) {
        other._fd = -1;
        other._buffer_size = 0;
        other._buffer = nullptr;
        other._release_function = nullptr;
        other._discard_function = nullptr;
        other._was_released = true;
    }
 
    // Move assignment operator
    ImageFD& operator=(ImageFD&& other) noexcept {
        if (this != &other) {
            // Release the current object first to avoid leaking the old fd/callback
            release();
            _header = std::move(other._header);
            _fd = other._fd;
            _buffer_size = other._buffer_size;
            _buffer = other._buffer;
            _release_function = std::move(other._release_function);
            _discard_function = std::move(other._discard_function);
            _was_released = other._was_released;
            other._fd = -1;
            other._buffer_size = 0;
            other._buffer = nullptr;
            other._release_function = nullptr;
            other._discard_function = nullptr;
            other._was_released = true;
        }
        return *this;
    }
 
    const ImageHeader& header() const { return _header; }
 
    int fd() const { return _fd; }
 
    uint64_t bufferSize() const { return _buffer_size; }
 
    uint8_t* buffer() const { return _buffer; }
 
    /*
     * Release the FD, signalling that the buffer is no longer in use.
     */
    void release() {
        if (_release_function) {
            _release_function();
            _release_function = nullptr;
        }
        _fd = -1;
        _buffer = nullptr;
        _was_released = true;
    }
 
    bool wasReleased() const { return _was_released; }
 
    /*
     * Mark a replacement buffer to be reclaimed without rendering (the compose never happened). The
     * buffer is still forwarded on release; the producer just discards it. No-op for
     * non-replacement images.
     */
    void discard() {
        if (_discard_function) {
            _discard_function();
            _discard_function = nullptr;
        }
    }
 
   private:
    ImageHeader _header{};
    int _fd{-1};
    uint64_t _buffer_size{0};  
    uint8_t* _buffer{nullptr};
 
    std::function<void()> _release_function{nullptr};
 
    std::function<void()> _discard_function{nullptr};
 
    bool _was_released{false};
};
 
struct CameraDescriptor {
    enum class SensorType {
        Unknown = 0,
        Monochrome = 1,
        RGB = 2,
        Depth = 3,
        RGBD = 4,
        IR = 5,
    };
    enum class MountOrientation {
        Unknown = 0,
        Dynamic = 1,
        Forward = 2,
        Backward = 3,
        Upward = 4,
        Downward = 5,
        Leftward = 6,
        Rightward = 7,
    };
    enum class MountOrientationDetail {
        Unknown = 0,
        Left = 1,
        Right = 2,
        Upper = 3,
        Lower = 4,
    };
    enum class PrimaryPurpose {
        Unknown = 0,
        Generic = 1,
        OpticalFlow = 2,
        VIO = 3,
        FPV = 4,
        Survey = 5,
        Tracking = 6,
    };
    enum class ImageRotation : uint8_t {
        None = 0,
        Rotation90Clockwise = 1,
        Rotation180 = 2,
        Rotation270Clockwise = 3,
    };
    struct ExtrinsicCalibration {
        Eigen::Quaterniond rotation{-0.5, 0.5, -0.5, 0.5};  // {q, x, y, z} default front facing
        Eigen::Vector3d translation{0.0, 0.0, 0.0};
    };
 
    CameraDescriptor() = default;
    explicit CameraDescriptor(const auterion_core_msgs::msg::CameraInfo& camera_info);
 
    operator auterion_core_msgs::msg::CameraInfo() const;
 
    std::string device_driver;
    std::string device_identifier;
 
    std::string unique_hash;
    std::string unique_name;
 
    std::string camera_model;
 
    SensorType sensor_type{SensorType::Unknown};
    MountOrientation mount_orientation{MountOrientation::Unknown};
    MountOrientationDetail mount_orientation_detail{MountOrientationDetail::Unknown};
    PrimaryPurpose primary_purpose{PrimaryPurpose::Unknown};
    ImageRotation image_rotation{ImageRotation::None};
    ExtrinsicCalibration extrinsic_calibration;
};
 
struct TrackingStatus {
    enum class Mode { None, Point, Rectangle };
 
    bool active{false};    
    bool coasting{false};  
    bool error{false};
    Mode mode{Mode::None};
    float point_x{0.F};  
    float point_y{0.F};
    float radius{0.F};
    float rectangle_top_x{0.F};  
    float rectangle_top_y{0.F};
    float rectangle_bottom_x{0.F};
    float rectangle_bottom_y{0.F};
    unsigned resolution_x{0};  
    unsigned resolution_y{0};
    unsigned hfov_deg{0};  
};
 
struct CameraServiceStatus {
    uint64_t session_id{0};  
};
 
class CameraMonitor {
   public:
    using NotificationCallback = std::function<void(const CameraDescriptor&)>;
 
    CameraMonitor(SDK& sdk, const NotificationCallback& on_camera_added,
                  const NotificationCallback& on_camera_removed = nullptr);
 
    ~CameraMonitor() = default;
 
   private:
    std::shared_ptr<CameraMonitorImpl> _impl;
};
 
class Camera {
   public:
    Camera(SDK& sdk, const CameraDescriptor& camera_descriptor);
    Camera(std::shared_ptr<CameraImpl> impl);
    ~Camera() = default;
 
    static Camera waitForCamera(SDK& sdk,
                                const std::function<bool(const CameraDescriptor&)>& find_callback);
 
    [[deprecated(
        "waitForCamera with 'camera_descriptor' not used anymore. This will be removed in some "
        "later release.")]] static Camera
    waitForCamera(SDK& sdk,
                  std::optional<auterion::CameraDescriptor> camera_descriptor = std::nullopt);
 
    static std::optional<Camera> open(SDK& sdk, const std::string& unique_name,
                                      const std::chrono::seconds& timeout = 30s);
    static std::optional<Camera> open(
        SDK& sdk, const std::function<bool(const CameraDescriptor&)>& find_callback,
        const std::chrono::seconds& timeout = 30s);
    static std::optional<Camera> openFirst(SDK& sdk, const std::chrono::seconds& timeout = 30s);
 
    using ImageCallback = std::function<void(const Image& image)>;
    using ImageFDCallback = std::function<void(const std::shared_ptr<ImageFD>& image_fd)>;
    using CameraInfoAfterSwitchCallback =
        std::function<void(const sensor_msgs::msg::CameraInfo::SharedPtr& camera_info)>;
    using TrackingStatusCallback = std::function<void(const TrackingStatus& status)>;
    using ServiceStatusCallback = std::function<void(const CameraServiceStatus& status)>;
 
    void subscribeImage(const ImageCallback& callback);
 
    void subscribeImageFD(const ImageFDCallback& callback, float max_rate_hz = 0.0F);
 
    void setImagesPaused(bool paused);
 
    struct ImageReplacementOptions {
        enum class PixelFormat {
            SameAsImage,
            NV12,
            RGBA,
            YUYV,  
        };
 
        ImageReplacementOptions() {}
 
        std::chrono::milliseconds timeout =
            300ms;  
        PixelFormat pixel_format{PixelFormat::SameAsImage};  
        bool use_output_image_size{false};  
 
        ImageReplacementOptions& withTimeout(std::chrono::milliseconds timeout_val) {
            timeout = timeout_val;
            return *this;
        }
 
        ImageReplacementOptions& withPixelFormat(PixelFormat format) {
            pixel_format = format;
            return *this;
        }
 
        ImageReplacementOptions& withUseOutputImageSize(bool use) {
            use_output_image_size = use;
            return *this;
        }
    };
 
    void activateImageReplacement(
        const ImageReplacementOptions& options = ImageReplacementOptions());
    void deactivateImageReplacement();
    std::optional<ImageFD> getReplacementImage(const ImageFD& image);
 
    std::optional<ImageFD> getReplacementImage(uint32_t original_frame_id);
 
    std::chrono::steady_clock::time_point lastReplacementImageOffer() const;
 
    [[deprecated(
        "subscribeCameraInfo() is deprecated and will be removed in a future release. The "
        "CameraInfo is instead included in the image header obtained with subscribeImage(), and "
        "can also be queried with getCameraInfo().")]] void
    subscribeCameraInfo(
        std::function<void(const sensor_msgs::msg::CameraInfo::SharedPtr)> callback);
 
    void subscribeCameraInfoAfterSwitch(const CameraInfoAfterSwitchCallback& callback);
 
    void subscribeTrackingStatus(TrackingStatusCallback callback);
 
    void subscribeServiceStatus(ServiceStatusCallback callback);
 
    bool cameraInfoValid() const;
 
    std::optional<cv::Size> getLastImageSize() const;
 
    Eigen::Vector3d unprojectImagePoint(const cv::Point& image_point) const;
 
    bool getPoseBodyCamera(Eigen::Quaterniond& q_body_camera, Eigen::Vector3d& t_body_camera) const;
 
    bool getPoseWorldCamera(Eigen::Quaterniond& q_enu_camera, Eigen::Vector3d& t_enu_camera,
                            const tf2::TimePoint& time = tf2::TimePointZero) const;
 
    bool getDeltaRotationWorldCamera(Eigen::Quaterniond& q_delta_world_camera,
                                     tf2::TimePoint& time_start, tf2::TimePoint& time_end);
 
    bool getCameraInfo(sensor_msgs::msg::CameraInfo& camera_info) const;
 
    bool getCameraInfoDirect(sensor_msgs::msg::CameraInfo& camera_info) const;
 
    bool switchTo(const std::function<bool(const CameraDescriptor&)>& find_callback);
 
    const CameraDescriptor& descriptor() const;
 
    CameraControl& control();
 
    bool usesImageSession() const;
 
   private:
    std::shared_ptr<CameraImpl> _impl;
};
 
}  // namespace auterion