odin_ros_driver1/include/image_overlay_node.hpp

/*
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
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

#ifdef ROS2
    #include <rclcpp/rclcpp.hpp>
    #include <sensor_msgs/msg/image.hpp>
    #include <sensor_msgs/msg/compressed_image.hpp>
    #include <cv_bridge/cv_bridge.h>
    #include <mutex>
#else
    #include <ros/ros.h>
    #include <sensor_msgs/Image.h>
    #include <sensor_msgs/CompressedImage.h>
    #include <cv_bridge/cv_bridge.h>
#endif

#include <opencv2/opencv.hpp>
#include <string>
#include <memory>

#ifdef ROS2
class ImageOverlayNode : public rclcpp::Node
{
public:
    ImageOverlayNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

private:
    using Image = sensor_msgs::msg::Image;
    using CompressedImage = sensor_msgs::msg::CompressedImage;

    std::string reprojected_topic_;
    std::string camera_topic_;
    std::string output_topic_;
    int jpeg_quality_;

    rclcpp::Subscription<Image>::SharedPtr reproj_sub_;
    rclcpp::Subscription<Image>::SharedPtr camera_sub_;
    rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_;

    cv::Mat latest_reproj_img_;
    cv::Mat latest_camera_img_;
    std_msgs::msg::Header latest_reproj_header_;
    std_msgs::msg::Header latest_camera_header_;
    std::mutex mutex_;

    void reprojCallback(const Image::ConstSharedPtr& msg);
    void cameraCallback(const Image::ConstSharedPtr& msg);
    void publishHcatCompressed();
};
#else
#include <mutex>
class ImageOverlayNode
{
public:
    ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);

private:
    ros::NodeHandle nh_, pnh_;

    std::string reprojected_topic_;
    std::string camera_topic_;
    std::string output_topic_;
    int jpeg_quality_;

    ros::Subscriber reproj_sub_;
    ros::Subscriber camera_sub_;
    ros::Publisher combined_pub_;

    cv::Mat latest_reproj_img_;
    cv::Mat latest_camera_img_;
    std_msgs::Header latest_reproj_header_;
    std_msgs::Header latest_camera_header_;
    std::mutex mutex_;

    void reprojCallback(const sensor_msgs::ImageConstPtr& msg);
    void cameraCallback(const sensor_msgs::ImageConstPtr& msg);
    void publishHcatCompressed();
};
#endif