/*
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

#include <chrono>
#include <csignal>
#include <cstddef>
#include <cstdint>
#include <iostream>
#include <mutex>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <cstring>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <thread>
#include <Eigen/Dense>
#include <atomic>
#include <unordered_map>
#include "data_logger.h"
#include "lidar_api.h"
#include "lidar_api_type.h"
#include "rawCloudRender.h"
#include <deque> 
#include <mutex>  
#include <vector> 
#include <queue>
#include <unistd.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <filesystem>

#include <yaml-cpp/yaml.h>
#include "polynomial_camera.hpp"
#include "camera_pose_visualization.h"

struct CameraParams {
    int width, height;
    double fx, fy, cx, cy, skew;
    double k2, k3, k4, k5, k6, k7;
    double p1, p2;
};

enum class OdometryType {
    STANDARD = LIDAR_DT_SLAM_ODOMETRY,
    HIGHFREQ = LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
    TRANSFORM = LIDAR_DT_SLAM_ODOMETRY_TF
};

#define LOG_LEVEL_NONE 0
#define LOG_LEVEL_ERROR 1
#define LOG_LEVEL_WARN 2
#define LOG_LEVEL_INFO 3
#define LOG_LEVEL_DEBUG 4


extern int g_log_level;
extern int g_sendcloudrender;
extern int g_use_host_ros_time;
double get_ptp_smoothed_delay();
double get_ptp_smoothed_offset();
#ifdef ROS2

    #include "rclcpp/rclcpp.hpp"
    #include "std_msgs/msg/string.hpp"
    #include <std_msgs/msg/header.hpp>
    #include <visualization_msgs/msg/marker_array.hpp>
    #include "sensor_msgs/msg/image.hpp"
    #include "sensor_msgs/msg/imu.hpp"
    #include "sensor_msgs/msg/point_cloud2.hpp"
    #include "sensor_msgs/point_cloud2_iterator.hpp"
    #include <sensor_msgs/msg/compressed_image.hpp>
    #include <builtin_interfaces/msg/time.hpp>
    #include <nav_msgs/msg/odometry.hpp>
    #include <nav_msgs/msg/path.hpp>
    #include <sensor_msgs/msg/point_field.hpp>
    #include "tf2/LinearMath/Quaternion.h"
    #include "tf2_ros/transform_broadcaster.h"
    namespace ros {
        using namespace rclcpp;
        using namespace std_msgs::msg;
        using namespace sensor_msgs::msg;
        using namespace nav_msgs::msg;
        using namespace visualization_msgs::msg;
        using Time = builtin_interfaces::msg::Time;
    }
    

    #define LOG_ERROR(...)
    #define LOG_WARN(...)
    #define LOG_INFO(...)
    #define LOG_DEBUG(...)
#else

    #include <ros/ros.h>
    #include <ros/package.h>
    #include <sensor_msgs/Image.h>
    #include <std_msgs/Header.h>
    #include <sensor_msgs/Imu.h>
    #include <sensor_msgs/PointCloud2.h>
    #include <sensor_msgs/point_cloud2_iterator.h>
    #include <sensor_msgs/CompressedImage.h>
    #include <nav_msgs/Odometry.h>
    #include <nav_msgs/Path.h>
    #include <sensor_msgs/Image.h>
    #include <tf2_ros/transform_broadcaster.h>
    #include <tf2/LinearMath/Quaternion.h>
    #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
    namespace ros {
        using namespace ::ros;
        using namespace sensor_msgs;
        using namespace nav_msgs;
    }
    

    #define LOG_ERROR(...) \
        if (g_log_level >= LOG_LEVEL_ERROR) { \
            ROS_ERROR(__VA_ARGS__); \
        }
    #define LOG_WARN(...) \
        if (g_log_level >= LOG_LEVEL_WARN) { \
            ROS_WARN(__VA_ARGS__); \
        }
    #define LOG_INFO(...) \
        if (g_log_level >= LOG_LEVEL_INFO) { \
            ROS_INFO(__VA_ARGS__); \
        }
    #define LOG_DEBUG(...) \
        if (g_log_level >= LOG_LEVEL_DEBUG) { \
            ROS_DEBUG(__VA_ARGS__); \
        }
#endif


#ifdef ROS2
    namespace sensor_msgs {
        using PointField = msg::PointField;
    }
#else
    namespace sensor_msgs {
        using PointField = ::sensor_msgs::PointField;
    }
#endif

// Common definitions
#define PAI 3.14159265358979323846
#define DTOF_NUM_ROW_PER_GROUP 6
// Common functions
inline ros::Time ns_to_ros_time(uint64_t timestamp_ns) {
    ros::Time t;
    #ifdef ROS2
        t.sec = static_cast<int32_t>(timestamp_ns / 1000000000);
        t.nanosec = static_cast<uint32_t>(timestamp_ns % 1000000000);
    #else
        t.sec = static_cast<uint32_t>(timestamp_ns / 1000000000);
        t.nsec = static_cast<uint32_t>(timestamp_ns % 1000000000);
    #endif
    return t;
}

inline uint64_t ros_time_to_ns(const ros::Time &t) {
    #ifdef ROS2
        return static_cast<uint64_t>(t.sec) * 1000000000ULL + t.nanosec;
    #else
        return static_cast<uint64_t>(t.sec) * 1000000000ULL + t.nsec;
    #endif
}

inline ros::Time make_aligned_stamp(uint64_t sensor_timestamp_ns
#ifdef ROS2
                                    , const rclcpp::Node::SharedPtr& node
#endif
                                    ) {
    if (g_use_host_ros_time == 1) {
        #ifdef ROS2
            return node->now();
        #else
            return ros::Time::now();
        #endif
    }

    uint64_t ts_ns = sensor_timestamp_ns;
    if (g_use_host_ros_time == 2) {
        const double offset_s = get_ptp_smoothed_offset();
        const int64_t offset_ns = static_cast<int64_t>(offset_s * 1e9);
        const int64_t base_ns = static_cast<int64_t>(sensor_timestamp_ns);
        const int64_t aligned_ns = base_ns - offset_ns;
        ts_ns = (aligned_ns < 0) ? 0ULL : static_cast<uint64_t>(aligned_ns);
    }

    return ns_to_ros_time(ts_ns);
}

class RosNodeControlInterface {
    public:
        virtual ~RosNodeControlInterface() = default;
        virtual void setDtofSubframeODR(int odr) = 0;
        virtual int getDtofSubframeODR() const = 0;
        virtual void setSendOdomBaseLinkTF(bool send_odom_baselink_tf) = 0;
        virtual bool sendOdomBaseLinkTF() const = 0;
        virtual void setCloudRawConfidenceThreshold(int threshold) = 0;
        virtual int cloudRawConfidenceThreshold() const = 0;
    };
    
RosNodeControlInterface* getRosNodeControl();

// Multi-sensor publisher class
class MultiSensorPublisher {
public:
    #ifdef ROS2
        MultiSensorPublisher(rclcpp::Node::SharedPtr node)
            : node_(node),cameraposevisual_ {1.0f, 0.0f, 0.0f, 1.0f} {
            initialize_publishers();
            // initialize_data_logger();
        }
    #else
        MultiSensorPublisher(ros::NodeHandle& nh)
            : cameraposevisual_(1.0f, 0.0f, 0.0f, 1.0f) {
            initialize_publishers(nh);
            // initialize_data_logger();
        }
    #endif
    
    std::filesystem::path get_root_dir() const { return root_dir_; }

    void set_log_level(int level) {
        g_log_level = level;
    }
    // Optional external logger setter
    void set_data_logger(std::shared_ptr<BinaryDataLogger> logger) {
        data_logger_ = std::move(logger);
    }

    int get_pose_index() {
        return pose_index_.load();
    }

    int get_cloud_index() {
        return cloud_index_.load();
    }

    int get_image_index() {
        return image_index_.load();
    }

    int get_wcwi_index() {
        return wcwi_index_.load();
    }
 
    rawCloudRender render_;
    void publishImu(imu_convert_data_t *stream) {
        #ifdef ROS2
            sensor_msgs::msg::Imu imu_msg;
        #else
            ros::Imu imu_msg;
        #endif
        
        #ifdef ROS2
            imu_msg.header.stamp = make_aligned_stamp(stream->stamp, node_);
        #else
            imu_msg.header.stamp = make_aligned_stamp(stream->stamp);
        #endif
        imu_msg.header.frame_id = "imu_link";

        imu_msg.linear_acceleration.y = -1 * stream->accel_x;
        imu_msg.linear_acceleration.x = stream->accel_y;
        imu_msg.linear_acceleration.z = stream->accel_z;

        imu_msg.angular_velocity.y = -1 * stream->gyro_x;
        imu_msg.angular_velocity.x = stream->gyro_y;
        imu_msg.angular_velocity.z = stream->gyro_z;

        imu_msg.orientation.x = 0.0;
        imu_msg.orientation.y = 0.0;
        imu_msg.orientation.z = 0.0;
        imu_msg.orientation.w = 1.0;
        
        #ifdef ROS2
            imu_pub_->publish(std::move(imu_msg));
        #else
            imu_pub_.publish(imu_msg);
        #endif

        if(data_logger_) {
            const double ts_sec = static_cast<double>(stream->stamp) / 1e9;
            float ax = imu_msg.linear_acceleration.x;
            float ay = imu_msg.linear_acceleration.y;
            float az = imu_msg.linear_acceleration.z;
            float wx = imu_msg.angular_velocity.x;
            float wy = imu_msg.angular_velocity.y;
            float wz = imu_msg.angular_velocity.z;

            std::vector<uint8_t> blob;
            blob.reserve(sizeof(double) + sizeof(float) * 6);
            auto append_pod = [&](const auto& v) {
                const uint8_t* p = reinterpret_cast<const uint8_t*>(&v);
                blob.insert(blob.end(), p, p + sizeof(v));
            };
            append_pod(ts_sec);
            append_pod(ax);
            append_pod(ay);
            append_pod(az);
            append_pod(wx);
            append_pod(wy);
            append_pod(wz);
            data_logger_->enqueueIMUFrame(std::move(blob));
        }
    }
#ifdef ROS2
    using ImageMsg = sensor_msgs::msg::Image;
    using PointCloud2Msg = sensor_msgs::msg::PointCloud2;
    using ImageConstPtr = ImageMsg::ConstSharedPtr;
    using PointCloud2ConstPtr = PointCloud2Msg::ConstSharedPtr;
#else
    using ImageMsg = sensor_msgs::Image;
    using PointCloud2Msg = sensor_msgs::PointCloud2;
    using ImageConstPtr = sensor_msgs::ImageConstPtr;
    using PointCloud2ConstPtr = sensor_msgs::PointCloud2ConstPtr;
#endif
void try_process_pair() {
    // Record queue status
    size_t rgb_size, pcd_size;
    {
        std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
        std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
        rgb_size = rgb_image_queue_.size();
        pcd_size = pcd_queue_.size();
    }
    
    while (true) {
        ImageConstPtr rgb_msg = nullptr;
        PointCloud2ConstPtr pcd_msg = nullptr;
        
        // Get a pair of data from queues (with lock protection)
        {
            std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
            std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
            
            if (!rgb_image_queue_.empty() && !pcd_queue_.empty()) {
                rgb_msg = rgb_image_queue_.front();
                pcd_msg = pcd_queue_.front();
            }
        }
        
        if (!rgb_msg || !pcd_msg) {
            break;
        }
        
        // timestamp
        uint64_t rgb_stamp = ros_time_to_ns(rgb_msg->header.stamp);
        uint64_t pcd_stamp = ros_time_to_ns(pcd_msg->header.stamp);
        int64_t time_diff = static_cast<int64_t>(rgb_stamp) - static_cast<int64_t>(pcd_stamp);
        int64_t abs_time_diff = std::abs(time_diff);
        
        // Check if time difference is within allowed range (50ms)
        const int64_t MAX_TIME_DIFF = 50000000; // 50ms in nanoseconds
        if (abs_time_diff > MAX_TIME_DIFF) {
            // Remove older timestamped message
            {
                std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
                std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
                
                if (time_diff > 0) {
                    // RGB timestamp is newer, remove PCD
                    pcd_queue_.pop_front();
                } else {
                    // PCD timestamp is newer, remove RGB
                    rgb_image_queue_.pop_front();
                }
            }
            
            // Try next pair
            continue;
        }
        
        // Time difference within allowed range, process data pair
        {
            std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
            std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
            
            // Remove messages from queues
            rgb_image_queue_.pop_front();
            pcd_queue_.pop_front();
        }
        
        // Process data pair
        process_pair(rgb_msg, pcd_msg);
    }
}
bool validate_render_parameters(std::vector<std::vector<float>>& rgb_image, 
                               capture_Image_List_t* cloud_stream, 
                               int pcd_idx) 
{
    // 1. Check RGB image validity
    if (rgb_image.empty()) {
        #ifndef ROS2
            ROS_ERROR("Invalid RGB image: empty vector");
        #endif
        return false;
    }
    
    // Check RGB image dimension consistency
    const size_t height = rgb_image.size();
    const size_t width = (height > 0) ? rgb_image[0].size() : 0;
    
    if (height == 0 || width == 0) {
        #ifndef ROS2
            ROS_ERROR("Invalid RGB image dimensions: %zux%zu", 
                     height, width);
        #endif
        return false;
    }
    
    // 2. Check point cloud stream pointer validity
    if (!cloud_stream) {
        #ifndef ROS2
            ROS_ERROR("Invalid cloud stream: null pointer");
        #endif
        return false;
    }
    
    // 3. Check point cloud index validity
    if (pcd_idx < 0 || pcd_idx >= 10) {
        #ifndef ROS2
            ROS_ERROR("Invalid pcd index: %d (must be 0-9)", pcd_idx);
        #endif
        return false;
    }
    
    // 4. Check point cloud data validity
    buffer_List_t& cloud = cloud_stream->imageList[pcd_idx];
    if (!cloud.pAddr) {
        #ifndef ROS2
            ROS_ERROR("Invalid cloud data: null pointer");
        #endif
        return false;
    }
    
    if (cloud.width <= 0 || cloud.height <= 0) {
        #ifndef ROS2
            ROS_ERROR("Invalid cloud dimensions: %dx%d", 
                     cloud.width, cloud.height);
        #endif
        return false;
    }
    
    return true;
}

void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_msg)
{
    auto start_time = std::chrono::steady_clock::now();

    const int input_image_width = rgb_msg->width;
    const int input_image_height = rgb_msg->height;

    // Verify input image format
    if (rgb_msg->encoding != "bgr8") {
        #ifndef ROS2
            ROS_ERROR("Unsupported image format: %s. Only bgr8 is supported.", 
                      rgb_msg->encoding.c_str());
        #endif
        return;
    }

    // Prepare point cloud data stream
    capture_Image_List_t cloud_stream;
    const int pcd_idx = 2;
    cloud_stream.imageList[pcd_idx].height = pcd_msg->height;
    cloud_stream.imageList[pcd_idx].width = pcd_msg->width;

    const auto total_point_num = pcd_msg->width * pcd_msg->height;
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*pcd_msg, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*pcd_msg, "y");
    sensor_msgs::PointCloud2ConstIterator<float> iter_z(*pcd_msg, "z");
    std::vector<float> cloud_flat;
    cloud_flat.reserve(total_point_num * 4);

    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
        cloud_flat.push_back(*iter_y * -1000.0f);
        cloud_flat.push_back(*iter_z * 1000.0f);
        cloud_flat.push_back(*iter_x * 1000.0f);
        cloud_flat.push_back(0.0f);
    }
    cloud_stream.imageList[pcd_idx].pAddr = cloud_flat.data();

    
    if (input_image_height > 0 && input_image_width > 0) {
        // Use BGR8 image data directly
        std::vector<std::vector<float>> rgb_image(input_image_height, std::vector<float>(input_image_width));
        
        // Convert BGR8 data to required format for rendering
        const uint8_t* bgr_data = rgb_msg->data.data();
        for (int y = 0; y < input_image_height; ++y) {
            for (int x = 0; x < input_image_width; ++x) {
                // Start position of each pixel (BGR format)
                int idx = y * rgb_msg->step + x * 3;
                
                // Extract RGB values and combine into 32-bit integer
                uint8_t b = bgr_data[idx];
                uint8_t g = bgr_data[idx + 1];
                uint8_t r = bgr_data[idx + 2];
                uint32_t rgb_int = (r << 16) | (g << 8) | b;
                
                // Convert to float
                float rgb_float;
                std::memcpy(&rgb_float, &rgb_int, sizeof(float));
                rgb_image[y][x] = rgb_float;
            }
        }
        // Render colored point cloud
        std::vector<float> rgbCloud_flat;
        render_.render(rgb_image, &cloud_stream, pcd_idx, rgbCloud_flat);
        const int valid_point_num = rgbCloud_flat.size() / 4;
         
        // Create and publish RGB point cloud
        PointCloud2Msg output_msg;
        output_msg.header.frame_id = "odin1_base_link";
        output_msg.header.stamp = rgb_msg->header.stamp; // Use original image timestamp
        output_msg.height = 1;
        output_msg.width = valid_point_num;

        sensor_msgs::PointCloud2Modifier modifier(output_msg);
        modifier.setPointCloud2Fields(
            4,
            "x", 1, sensor_msgs::PointField::FLOAT32,
            "y", 1, sensor_msgs::PointField::FLOAT32,
            "z", 1, sensor_msgs::PointField::FLOAT32,
            "rgb", 1, sensor_msgs::PointField::FLOAT32
        );
        modifier.resize(valid_point_num);

        sensor_msgs::PointCloud2Iterator<float> iter_res_x(output_msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_res_y(output_msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_res_z(output_msg, "z");
        sensor_msgs::PointCloud2Iterator<float> iter_res_rgb(output_msg, "rgb");

        for (int i = 0; i < valid_point_num; i++) {
            *iter_res_x = rgbCloud_flat[4*i]; ++iter_res_x;
            *iter_res_y = rgbCloud_flat[4*i+1]; ++iter_res_y;
            *iter_res_z = rgbCloud_flat[4*i+2]; ++iter_res_z;
            *iter_res_rgb = rgbCloud_flat[4*i+3]; ++iter_res_rgb;
        }

        #ifdef ROS2
            rgbcloud_pub_->publish(output_msg);
        #else
            rgbcloud_pub_.publish(output_msg);
        #endif
    } 
}

void publishIntensityCloud(capture_Image_List_t* stream, int idx)
{
    // Check index validity
    if (idx < 0 || idx >= 10) {
        #ifndef ROS2
            ROS_ERROR("Invalid index %d for intensity cloud", idx);
        #endif
        return;
    }

    // Check point cloud data validity
    buffer_List_t &cloud = stream->imageList[idx];
    if (!cloud.pAddr) {
        #ifndef ROS2
            ROS_ERROR("Invalid point cloud: null data pointer at index %d", idx);
        #endif
        return;
    }
 
    if (cloud.width <= 0 || cloud.height <= 0) {
        #ifndef ROS2
            ROS_ERROR("Invalid point cloud dimensions: %dx%d at index %d", 
                     cloud.width, cloud.height, idx);
        #endif
        return;
    }

    #ifdef ROS2
        auto msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
    #else
        auto msg = boost::make_shared<sensor_msgs::PointCloud2>();
    #endif

    // Set message header
    msg->header.frame_id = "odin1_base_link";
    #ifdef ROS2
        msg->header.stamp = make_aligned_stamp(cloud.timestamp, node_);
    #else
        msg->header.stamp = make_aligned_stamp(cloud.timestamp);
    #endif

    msg->height = cloud.height;
    msg->width = cloud.width;
    msg->is_dense = false;
    msg->is_bigendian = false;

    // Set point cloud fields
    sensor_msgs::PointCloud2Modifier modifier(*msg);
    modifier.setPointCloud2Fields(
        6,
        "x", 1, sensor_msgs::PointField::FLOAT32,
        "y", 1, sensor_msgs::PointField::FLOAT32,
        "z", 1, sensor_msgs::PointField::FLOAT32,
        "intensity", 1, sensor_msgs::PointField::UINT8,
        "confidence", 1, sensor_msgs::PointField::UINT16,
        "offset_time", 1, sensor_msgs::PointField::FLOAT32
    );
    modifier.resize(msg->height * msg->width);


    sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");
    sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*msg, "intensity");
    sensor_msgs::PointCloud2Iterator<uint16_t> iter_confidence(*msg, "confidence");
    sensor_msgs::PointCloud2Iterator<float> iter_offsettime(*msg, "offset_time");

    float* xyz_data_f = static_cast<float*>(cloud.pAddr);
    int total_points = cloud.height * cloud.width;
	//std::cout << stream->imageCount << std::endl;
    float dtof_subframe_odr = getRosNodeControl()->getDtofSubframeODR() / 1000.0f;
    // printf("dtof_subframe_odr: %f\n", dtof_subframe_odr);

    if (stream->imageCount == 4) {

        uint8_t* intensity_data = static_cast<uint8_t*>(stream->imageList[2].pAddr);
        uint16_t* confidence_data = static_cast<uint16_t*>(stream->imageList[3].pAddr);
        int confidence_threshold = getRosNodeControl()->cloudRawConfidenceThreshold();
        for (int i = 0; i < total_points; ++i) {
            if (confidence_data[i] < confidence_threshold) {
                *iter_x = 0.0f; ++iter_x;
                *iter_y = 0.0f; ++iter_y;
                *iter_z = 0.0f; ++iter_z;
                *iter_intensity = 0; ++iter_intensity;
                *iter_confidence = 0; ++iter_confidence;
                *iter_offsettime = 0.0f; ++iter_offsettime;
            } else {
                // XYZ point
                *iter_x = xyz_data_f[i * 3 + 2] / 1000.0f; ++iter_x;
                *iter_y = -xyz_data_f[i * 3 + 0] / 1000.0f; ++iter_y;
                *iter_z = xyz_data_f[i * 3 + 1] / 1000.0f; ++iter_z;
                
                *iter_intensity = intensity_data[i]; ++iter_intensity;
                *iter_confidence = confidence_data[i]; ++iter_confidence;
                
                if (dtof_subframe_odr > 0.0) {
                    int line_num = i / 256;
                    int group = line_num / DTOF_NUM_ROW_PER_GROUP;
                    float timestamp_offset = group * 1.0 / dtof_subframe_odr;

                    *iter_offsettime = timestamp_offset;
                    ++iter_offsettime;
                }
            }
    	}
    } else {
        uint16_t* intensity_data = static_cast<uint16_t*>(stream->imageList[2].pAddr);
        
        for (int i = 0; i < total_points; ++i) {
            *iter_x = xyz_data_f[i * 4 + 2] / 1000.0f; ++iter_x;
            *iter_y = -xyz_data_f[i * 4 + 0] / 1000.0f; ++iter_y;
            *iter_z = xyz_data_f[i * 4 + 1] / 1000.0f; ++iter_z;
            
            float intensity = (intensity_data[i] - 10) * 255.0f / (12500 - 10);
            if (intensity > 255) {
                *iter_intensity = 255;
            } else if (intensity < 0) {
                *iter_intensity = 0;
            } else {
                *iter_intensity = static_cast<uint8_t>(intensity);
            }
            ++iter_intensity;
            
            *iter_confidence = 0;
            ++iter_confidence;
        }
    }

    // Only cache point cloud if cloud_render is enabled
    if (g_sendcloudrender) {
        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
        
        // Create deep copy of point cloud
        #ifdef ROS2
            auto msg_copy = std::make_shared<sensor_msgs::msg::PointCloud2>(*msg);
        #else
            auto msg_copy = boost::make_shared<sensor_msgs::PointCloud2>();
            *msg_copy = *msg;  // Deep copy
        #endif
        
        // Queue management
        if (pcd_queue_.size() >= 10) {
            pcd_queue_.pop_front();
        }
        
        // Add to queue (using copy)
        pcd_queue_.push_back(msg_copy);
    }

    // Publish point cloud
    #ifdef ROS2
        cloud_pub_->publish(*msg);
    #else
        cloud_pub_.publish(msg);
    #endif
}

void publishGrayUInt8(capture_Image_List_t *stream, int idx) {
    ImageMsg msg;
    #ifdef ROS2
        msg.header.stamp = make_aligned_stamp(stream->imageList[idx].timestamp, node_);
    #else
        msg.header.stamp = make_aligned_stamp(stream->imageList[idx].timestamp);
    #endif
    msg.header.frame_id = "map";

    int width = stream->imageList[idx].width;
    int height = stream->imageList[idx].height;

    msg.height = height;
    msg.width = width;
    msg.encoding = "mono8";
    msg.is_bigendian = false;
    msg.step = width * sizeof(uint8_t);

    size_t image_size = msg.step * height;

    msg.data.resize(image_size);

    memcpy(msg.data.data(), stream->imageList[idx].pAddr, image_size);

    #ifdef ROS2
        intensity_gray_pub_->publish(msg);
    #else
        intensity_gray_pub_.publish(msg);
    #endif
}

void publishRgb(capture_Image_List_t *stream) {
    buffer_List_t &image = stream->imageList[0];

    // old version yuv data
    if (image.length == image.width * image.height * 3 / 2) {
        #ifdef ROS2
            RCLCPP_INFO(rclcpp::get_logger("publishRgb"), "old format rgb data, please upgrade device firmware");
        #else
            ROS_INFO("old format rgb data, please upgrade device firmware");
        #endif
    } else {// new version jpeg data

        std::vector<uint8_t> jpeg_data(static_cast<uint8_t*>(image.pAddr),
                                        static_cast<uint8_t*>(image.pAddr) + image.length);

        // convert back to bgr8                                                
        cv::Mat decoded_image = cv::imdecode(jpeg_data, cv::IMREAD_COLOR);

        cv_bridge::CvImage cv_image;
        #ifdef ROS2
            cv_image.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp, node_);
        #else
            cv_image.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp);
        #endif
        cv_image.encoding = "bgr8";
        cv_image.image = decoded_image;

        if (g_sendcloudrender) {
            std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
            if (rgb_image_queue_.size() >= 10) {
                rgb_image_queue_.pop_front();
            }
            rgb_image_queue_.push_back(cv_image.toImageMsg());
            }        

        // Enqueue binary logging for image
        if (data_logger_) {
            const uint32_t idx_now = image_index_.fetch_add(1, std::memory_order_relaxed);
            const double ts_sec = static_cast<double>(stream->imageList[0].timestamp) / 1e9;
            const uint32_t jpeg_size = static_cast<uint32_t>(jpeg_data.size());
            std::vector<uint8_t> blob;
            blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t) + jpeg_size);
            auto append_pod = [&](const auto& v) {
                const uint8_t* p = reinterpret_cast<const uint8_t*>(&v);
                blob.insert(blob.end(), p, p + sizeof(v));
            };
            append_pod(idx_now);
            append_pod(ts_sec);
            append_pod(jpeg_size);
            blob.insert(blob.end(), jpeg_data.begin(), jpeg_data.end());
            data_logger_->enqueueImageFrame(std::move(blob));
        }

        // undistort image
        cv::Mat undistorted_image = cv::Mat::zeros(decoded_image.size(), decoded_image.type());
        cv_bridge::CvImage cv_undistorted_image;

        if (m_undistort_map_init_success) {
            cv::remap(decoded_image, undistorted_image, m_undistort_map_x, m_undistort_map_y, cv::INTER_LINEAR);
            #ifdef ROS2
                cv_undistorted_image.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp, node_);
            #else
                cv_undistorted_image.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp);
            #endif
            cv_undistorted_image.encoding = "bgr8";
            cv_undistorted_image.image = undistorted_image;
        }

        #ifdef ROS2
        {
            rgb_pub_->publish(*cv_image.toImageMsg());
            if (m_undistort_map_init_success) {
                undistort_rgb_pub_->publish(*cv_undistorted_image.toImageMsg());
            }

            // original jpeg - always publish as it's small
            sensor_msgs::msg::CompressedImage jpeg_msg;
            jpeg_msg.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp, node_);
            jpeg_msg.format = "jpeg";
            jpeg_msg.data = jpeg_data;

            compressed_rgb_pub_->publish(jpeg_msg);
        }
        #else
        {
            rgb_pub_.publish(cv_image.toImageMsg());
            if (m_undistort_map_init_success) {
                undistort_rgb_pub_.publish(cv_undistorted_image.toImageMsg());
            }

            // original jpeg
            sensor_msgs::CompressedImagePtr jpeg_msg(new sensor_msgs::CompressedImage());
            jpeg_msg->header.stamp = make_aligned_stamp(stream->imageList[0].timestamp);
            jpeg_msg->format = "jpeg";
            jpeg_msg->data = jpeg_data;

            compressed_rgb_pub_.publish(jpeg_msg);
        }
        #endif
    }

}


    void publishPC2XYZRGBA(capture_Image_List_t* stream, int idx) 
    {
        #ifdef ROS2
                sensor_msgs::msg::PointCloud2 msg;
                msg.header.frame_id = "odom";
                msg.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp, node_);

                //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Point cloudrgba %ld",stream->imageList[0].timestamp);

                size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4;
                uint32_t points = stream->imageList[idx].length / pt_size;

                msg.height = 1;
                msg.width = points;
                msg.is_dense = false;

                sensor_msgs::PointCloud2Modifier modifier(msg);
                modifier.setPointCloud2Fields(
                    4,
                    "x", 1, sensor_msgs::msg::PointField::FLOAT32,
                    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
                    "z", 1, sensor_msgs::msg::PointField::FLOAT32,
                    "rgb", 1, sensor_msgs::msg::PointField::FLOAT32
                );
                modifier.resize(msg.width * msg.height);

                sensor_msgs::PointCloud2Iterator<float> iter_x(msg, "x");
                sensor_msgs::PointCloud2Iterator<float> iter_y(msg, "y");
                sensor_msgs::PointCloud2Iterator<float> iter_z(msg, "z");
                sensor_msgs::PointCloud2Iterator<float> iter_rgb(msg, "rgb");
        #else
            sensor_msgs::PointCloud2 msg;
            msg.header.frame_id = "odom";
            msg.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp);
            
            size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4;
            uint32_t points = stream->imageList[idx].length / pt_size;
            
            msg.height = 1;
            msg.width = points;
            msg.is_dense = false;
            
            sensor_msgs::PointCloud2Modifier modifier(msg);
            modifier.setPointCloud2Fields(
                4,
                "x", 1, sensor_msgs::PointField::FLOAT32,
                "y", 1, sensor_msgs::PointField::FLOAT32,
                "z", 1, sensor_msgs::PointField::FLOAT32,
                "rgb", 1, sensor_msgs::PointField::FLOAT32
            );
            modifier.resize(msg.width * msg.height);
            
            sensor_msgs::PointCloud2Iterator<float> iter_x(msg, "x");
            sensor_msgs::PointCloud2Iterator<float> iter_y(msg, "y");
            sensor_msgs::PointCloud2Iterator<float> iter_z(msg, "z");
            sensor_msgs::PointCloud2Iterator<float> iter_rgb(msg, "rgb");
        #endif
        
        // Shared data processing logic
        int32_t* xyz_data = static_cast<int32_t*>(stream->imageList[idx].pAddr);
        
        for(uint32_t i = 0; i < points; i++) {
            int32_t* ptr = xyz_data + 7*i;
            
#ifdef ROS2
                *iter_x = static_cast<float>(ptr[0]) / 10000.0f; ++iter_x;
                *iter_y = static_cast<float>(ptr[1]) / 10000.0f; ++iter_y;
                *iter_z = static_cast<float>(ptr[2]) / 10000.0f; ++iter_z;
#else
                *iter_x = (1.0 * ptr[0]) / 1e4; ++iter_x;
                *iter_y = (1.0 * ptr[1]) / 1e4; ++iter_y;
                *iter_z = (1.0 * ptr[2]) / 1e4; ++iter_z;
#endif
            
            uint8_t r = ptr[3] & 0xff;
            uint8_t g = ptr[4] & 0xff;
            uint8_t b = ptr[5] & 0xff;  
            uint8_t a = ptr[6] & 0xff;
            
            uint32_t packed_rgb = (static_cast<uint32_t>(r) << 16) | 
                                (static_cast<uint32_t>(g) << 8)  | 
                                static_cast<uint32_t>(b);
            
            float rgb_float;
            std::memcpy(&rgb_float, &packed_rgb, sizeof(float));
            
            *iter_rgb = rgb_float; ++iter_rgb;
        }

        // Enqueue binary logging for point cloud (XYZRGB per point)
        if (data_logger_ && points > 0) {
            const double ts_sec = static_cast<double>(stream->imageList[0].timestamp) / 1e9;
            const uint32_t idx_now = cloud_index_.fetch_add(1, std::memory_order_relaxed);
            // Compute total blob size: header + per-point payload
            const size_t header_size = sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t);
            const size_t point_size = sizeof(float) * 3 + sizeof(uint8_t) * 4;
            std::vector<uint8_t> blob;
            blob.reserve(header_size + static_cast<size_t>(points) * point_size);
            auto append_pod = [&](const auto& v) {
                const uint8_t* p = reinterpret_cast<const uint8_t*>(&v);
                blob.insert(blob.end(), p, p + sizeof(v));
            };
            append_pod(idx_now);
            append_pod(ts_sec);
            append_pod(points);

            for (uint32_t i = 0; i < points; ++i) {
                int32_t* ptr = xyz_data + 7 * i;
                float fx = static_cast<float>(ptr[0]) / 10000.0f;
                float fy = static_cast<float>(ptr[1]) / 10000.0f;
                float fz = static_cast<float>(ptr[2]) / 10000.0f;
                uint8_t r = static_cast<uint8_t>(ptr[3] & 0xff);
                uint8_t g = static_cast<uint8_t>(ptr[4] & 0xff);
                uint8_t b = static_cast<uint8_t>(ptr[5] & 0xff);
                uint8_t a = static_cast<uint8_t>(ptr[6] & 0xff);
                append_pod(fx);
                append_pod(fy);
                append_pod(fz);
                blob.push_back(r);
                blob.push_back(g);
                blob.push_back(b);
                blob.push_back(a);
            }
            data_logger_->enqueuePointCloudFrame(std::move(blob));
        }
        
#ifdef ROS2
            xyzrgbacloud_pub_->publish(std::move(msg));
#else
            xyzrgbacloud_pub_.publish(msg);
#endif
    }

    void recordrotate(capture_Image_List_t* stream) { 
        if(data_logger_) {
            uint32_t data_len = stream->imageList[0].length;
            if (data_len == sizeof(ros_odom_convert_complete_t)) {
                ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr;
                const uint32_t idx_now = wcwi_index_.fetch_add(1, std::memory_order_relaxed);
                const double ts_sec = static_cast<double>(odom_data->timestamp_ns) / 1e9;
                float pose_arr[4];
                pose_arr[0] = static_cast<float>((odom_data->orient[0]) / 1e6);
                pose_arr[1] = static_cast<float>((odom_data->orient[1]) / 1e6);
                pose_arr[2] = static_cast<float>((odom_data->orient[2]) / 1e6);
                pose_arr[3] = static_cast<float>((odom_data->orient[3]) / 1e6);

                // Save only rotate (pose_arr) to bin file
                std::vector<uint8_t> blob;
                blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(float) * 4);
                auto append_pod = [&](const auto& v) {
                    const uint8_t* p = reinterpret_cast<const uint8_t*>(&v);
                    blob.insert(blob.end(), p, p + sizeof(v));
                };
                append_pod(idx_now);
                append_pod(ts_sec);
                for (int i = 0; i < 4; ++i) append_pod(pose_arr[i]);
                data_logger_->enqueueRotateFrame(std::move(blob));

                // Build TCL matrix (4x4) from pose_cov
                Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
                for (int idx = 0; idx < 16; ++idx) {
                    T_CL(idx / 4, idx % 4) = static_cast<double>(odom_data->pose_cov[idx]);
                }
                // Force last row to be [0, 0, 0, 1] for valid transformation matrix
                T_CL(3, 0) = 0.0; T_CL(3, 1) = 0.0; T_CL(3, 2) = 0.0; T_CL(3, 3) = 1.0;
                
                // Build TIL matrix (4x4) from twist_cov
                Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
                for (int idx = 0; idx < 16; ++idx) {
                    T_IL(idx / 4, idx % 4) = static_cast<double>(odom_data->twist_cov[idx]);
                }
                // Force last row to be [0, 0, 0, 1] for valid transformation matrix
                T_IL(3, 0) = 0.0; T_IL(3, 1) = 0.0; T_IL(3, 2) = 0.0; T_IL(3, 3) = 1.0;
                
                // Debug print to compare with cloud_reprojection values
                // static int host_print_count = 0;
                // if (host_print_count++) {
                //     std::cout << "=== host_sdk_sample T_CL from odom_data->pose_cov ===" << std::endl;
                //     std::cout << T_CL << std::endl;
                //     std::cout << "=== host_sdk_sample T_IL from odom_data->twist_cov ===" << std::endl;
                //     std::cout << T_IL << std::endl;
                // }
                
                // Extract rotation and translation from T_CL
                Eigen::Matrix3d RCL = T_CL.block<3, 3>(0, 0);
                Eigen::Vector3d TCL = T_CL.block<3, 1>(0, 3);
                
                // Extract rotation and translation from T_IL
                Eigen::Matrix3d RIL = T_IL.block<3, 3>(0, 0);
                Eigen::Vector3d TIL = T_IL.block<3, 1>(0, 3);

                // Debug print extracted RCL and TCL
                // static int rcl_print_count = 0;
                // if (rcl_print_count++ ) {
                //     std::cout << "=== host_sdk_sample RCL (3x3 rotation from T_CL) ===" << std::endl;
                //     std::cout << RCL << std::endl;
                //     std::cout << "=== host_sdk_sample TCL (3x1 translation from T_CL) ===" << std::endl;
                //     std::cout << TCL.transpose() << std::endl;
                //     std::cout << "=== host_sdk_sample RIL (3x3 rotation from T_IL) ===" << std::endl;
                //     std::cout << RIL << std::endl;
                //     std::cout << "=== host_sdk_sample TIL (3x1 translation from T_IL) ===" << std::endl;
                //     std::cout << TIL.transpose() << std::endl;
                // }
                
                // Save RIL, TIL, RCL, TCL to YAML file
                static int save_count = 0;
                static int index_count = 0;
                save_count++;
                
                bool should_save = true; // Always save, or modify this condition as needed
                if (should_save || save_count % 1 == 0 || index_count == 0) {
                    std::string OUTPUT_PATH = root_dir_.string();
                    std::ofstream yaml_file;
                    if(index_count == 0)
                        yaml_file.open(OUTPUT_PATH + "/calib_online.yaml");
                    else
                        yaml_file.open(OUTPUT_PATH + "/calib_online.yaml", std::ios::app);
                    
                    if (yaml_file.is_open()) {
                        yaml_file << "frame:\n";
                        yaml_file << "    index: " << index_count << "\n";
                        yaml_file << "    timestamp: " << std::fixed << std::setprecision(10) << ts_sec << "\n";
                        yaml_file << "    cam_num: 1\n";
                        
                        // Save TCL (camera-lidar translation) in the same format as Tcl_0
                        yaml_file << "    Tcl_0: [\n";
                        Eigen::Matrix4d T_CL_output = Eigen::Matrix4d::Identity();
                        T_CL_output.block<3, 3>(0, 0) = RCL;
                        T_CL_output.block<3, 1>(0, 3) = TCL;
                        for (int i = 0; i < 4; i++) {
                            yaml_file << "        ";
                            for (int j = 0; j < 4; j++) {
                                yaml_file << std::fixed << std::setprecision(10) << T_CL_output(i, j);
                                if (i != 3 || j != 3) yaml_file << ", ";
                            }
                            if (i != 3) yaml_file << "\n";
                        }
                        yaml_file << "\n    ]\n\n";
                        
                        // Save TIL (IMU-lidar transformation) as body_T_lidar
                        yaml_file << "    body_T_lidar: !!opencv-matrix\n";
                        yaml_file << "       rows: 4\n";
                        yaml_file << "       cols: 4\n";
                        yaml_file << "       dt: d\n";
                        yaml_file << "       data: [ ";
                        Eigen::Matrix4d T_IL_output = Eigen::Matrix4d::Identity();
                        T_IL_output.block<3, 3>(0, 0) = RIL;
                        T_IL_output.block<3, 1>(0, 3) = TIL;
                        for (int i = 0; i < 4; i++) {
                            for (int j = 0; j < 4; j++) {
                                yaml_file << std::fixed << std::setprecision(10) << T_IL_output(i, j);
                                if (i != 3 || j != 3) yaml_file << ",";
                            }
                            if (i != 3) yaml_file << "\n              ";
                        }
                        yaml_file << "]\n\n";
                        
                        index_count++;
                        yaml_file.close();
                    }
                }
            }
        }

    }

    // Publish WIWC data (T_CL and T_IL extrinsics) as a separate topic
    void publishWiwc(capture_Image_List_t* stream) {
        uint32_t data_len = stream->imageList[0].length;
        if (data_len != sizeof(ros_odom_convert_complete_t)) {
            return;
        }
        
        ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr;
        
#ifdef ROS2
        auto msg = nav_msgs::msg::Odometry();
        msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns, node_);
        msg.header.frame_id = "odom";
#else
        nav_msgs::Odometry msg;
        msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns);
        msg.header.frame_id = "odom";
#endif
        
        // Store T_CL in pose.covariance (first 16 elements)
        // Force last row to be [0, 0, 0, 1] for valid transformation matrix
        for (int i = 0; i < 16; ++i) {
            msg.pose.covariance[i] = odom_data->pose_cov[i];
        }
        msg.pose.covariance[12] = 0.0;
        msg.pose.covariance[13] = 0.0;
        msg.pose.covariance[14] = 0.0;
        msg.pose.covariance[15] = 1.0;
        // Fill remaining with zeros
        for (int i = 16; i < 36; ++i) {
            msg.pose.covariance[i] = 0.0;
        }
        
        // Store T_IL in twist.covariance (first 16 elements)
        // Force last row to be [0, 0, 0, 1] for valid transformation matrix
        for (int i = 0; i < 16; ++i) {
            msg.twist.covariance[i] = odom_data->twist_cov[i];
        }
        msg.twist.covariance[12] = 0.0;
        msg.twist.covariance[13] = 0.0;
        msg.twist.covariance[14] = 0.0;
        msg.twist.covariance[15] = 1.0;
        // Fill remaining with zeros
        for (int i = 16; i < 36; ++i) {
            msg.twist.covariance[i] = 0.0;
        }
        
#ifdef ROS2
        wiwc_publisher_->publish(msg);
#else
        wiwc_publisher_.publish(msg);
#endif
    }

    void publishOdometry(capture_Image_List_t* stream, OdometryType odom_type, bool show_path, bool show_camerapose) {
        
#ifdef ROS2
            auto msg = nav_msgs::msg::Odometry();
#else
            ros::Odometry msg;
#endif
        
            msg.header.frame_id = "odom";
            msg.child_frame_id = "odin1_base_link";

            //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "odom %ld",odom_data->timestamp_ns);

            uint32_t data_len = stream->imageList[0].length;
            if (data_len == sizeof(ros_odom_convert_complete_t)) {

                ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr;
                #ifdef ROS2
                    msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns, node_);
                #else
                    msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns);
                #endif

                msg.pose.pose.position.x = static_cast<double>(odom_data->pos[0]) / 1e6;
                msg.pose.pose.position.y = static_cast<double>(odom_data->pos[1]) / 1e6;
                msg.pose.pose.position.z = static_cast<double>(odom_data->pos[2]) / 1e6;

                msg.pose.pose.orientation.x = static_cast<double>(odom_data->orient[0]) / 1e6;
                msg.pose.pose.orientation.y = static_cast<double>(odom_data->orient[1]) / 1e6;
                msg.pose.pose.orientation.z = static_cast<double>(odom_data->orient[2]) / 1e6;
                msg.pose.pose.orientation.w = static_cast<double>(odom_data->orient[3]) / 1e6;

                // Enqueue binary logging for pose
                if ((odom_type == OdometryType::STANDARD) && data_logger_) {
                    const uint32_t idx_now = pose_index_.fetch_add(1, std::memory_order_relaxed);
                    const double ts_sec = static_cast<double>(odom_data->timestamp_ns) / 1e9;
                    float pose_arr[7];
                    pose_arr[0] = static_cast<float>(msg.pose.pose.position.x);
                    pose_arr[1] = static_cast<float>(msg.pose.pose.position.y);
                    pose_arr[2] = static_cast<float>(msg.pose.pose.position.z);
                    pose_arr[3] = static_cast<float>(msg.pose.pose.orientation.x);
                    pose_arr[4] = static_cast<float>(msg.pose.pose.orientation.y);
                    pose_arr[5] = static_cast<float>(msg.pose.pose.orientation.z);
                    pose_arr[6] = static_cast<float>(msg.pose.pose.orientation.w);
                    std::vector<uint8_t> blob;
                    blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(float) * 7);
                    auto append_pod = [&](const auto& v) {
                        const uint8_t* p = reinterpret_cast<const uint8_t*>(&v);
                        blob.insert(blob.end(), p, p + sizeof(v));
                    };
                    append_pod(idx_now);
                    append_pod(ts_sec);
                    for (int i = 0; i < 7; ++i) append_pod(pose_arr[i]);
                    data_logger_->enqueuePoseFrame(std::move(blob));
                }
        
                msg.twist.twist.linear.x = static_cast<double>(odom_data->linear_velocity[0]) / 1e6;
                msg.twist.twist.linear.y = static_cast<double>(odom_data->linear_velocity[1]) / 1e6;
                msg.twist.twist.linear.z = static_cast<double>(odom_data->linear_velocity[2]) / 1e6;

                msg.twist.twist.angular.x = static_cast<double>(odom_data->angular_velocity[0]) / 1e6;
                msg.twist.twist.angular.y = static_cast<double>(odom_data->angular_velocity[1]) / 1e6;
                msg.twist.twist.angular.z = static_cast<double>(odom_data->angular_velocity[2]) / 1e6;

                // Copy original covariance data
                for (int i = 0; i < 36; ++i) {
                    msg.pose.covariance[i] = odom_data->pose_cov[i];
                }

                for (int i = 0; i < 36; ++i) {
                    msg.twist.covariance[i] = odom_data->twist_cov[i];
                }
            
            } else if (data_len == sizeof(ros2_odom_convert_t)) {

                ros2_odom_convert_t* odom_data = (ros2_odom_convert_t*)stream->imageList[0].pAddr;
                #ifdef ROS2
                    msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns, node_);
                #else
                    msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns);
                #endif

                msg.pose.pose.position.x = static_cast<double>(odom_data->pos[0]) / 1e6;
                msg.pose.pose.position.y = static_cast<double>(odom_data->pos[1]) / 1e6;
                msg.pose.pose.position.z = static_cast<double>(odom_data->pos[2]) / 1e6;

                msg.pose.pose.orientation.x = static_cast<double>(odom_data->orient[0]) / 1e6;
                msg.pose.pose.orientation.y = static_cast<double>(odom_data->orient[1]) / 1e6;
                msg.pose.pose.orientation.z = static_cast<double>(odom_data->orient[2]) / 1e6;
                msg.pose.pose.orientation.w = static_cast<double>(odom_data->orient[3]) / 1e6;
            }

#ifdef ROS2
            switch(odom_type) {
                case OdometryType::STANDARD:
                    {
                    if (getRosNodeControl()->sendOdomBaseLinkTF()) {
                        geometry_msgs::msg::TransformStamped transformStamped;
                        transformStamped.header.stamp = msg.header.stamp;
                        transformStamped.header.frame_id = "odom";
                        transformStamped.child_frame_id = "odin1_base_link";
                        transformStamped.transform.translation.x = msg.pose.pose.position.x;
                        transformStamped.transform.translation.y = msg.pose.pose.position.y;
                        transformStamped.transform.translation.z = msg.pose.pose.position.z;
                        transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
                        transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
                        transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
                        transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
                        tf_broadcaster->sendTransform(transformStamped);
                    }
                    odom_publisher_->publish(msg);

                    // Publish odom trajectory as visualization markers (green lines connecting adjacent points)
                    static visualization_msgs::msg::Marker marker;
                    static std::vector<geometry_msgs::msg::Point> path_points;
                    
                    if (show_path) {
                        marker.header = msg.header;
                        marker.ns = "odom_trajectory";
                        marker.id = 0;
                        marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
                        marker.action = visualization_msgs::msg::Marker::ADD;
                        marker.pose.orientation.w = 1.0;
                        marker.scale.x = 0.02;  // Line width
                        marker.color.r = 0.0;
                        marker.color.g = 1.0;
                        marker.color.b = 0.0;
                        marker.color.a = 1.0;
        
                        geometry_msgs::msg::Point pt;
                        pt.x = msg.pose.pose.position.x;
                        pt.y = msg.pose.pose.position.y;
                        pt.z = msg.pose.pose.position.z;
                        path_points.push_back(pt);
                        
                        // Keep only recent points to avoid memory issues (e.g., last 1000 points)
                        if (path_points.size() > 30000) {
                            path_points.erase(path_points.begin());
                        }
                        
                        marker.points = path_points;
        
                        // Publish marker array
                        static visualization_msgs::msg::MarkerArray marker_array;
                        marker_array.markers.clear();  // Clear previous markers
                        marker_array.markers.push_back(marker);
                        path_publisher_->publish(marker_array);
                    }

                    if (show_camerapose) {
                        // camera pose visualization (ROS2)
                        Eigen::Vector3d P(msg.pose.pose.position.x,
                                        msg.pose.pose.position.y,
                                        msg.pose.pose.position.z);
                        Eigen::Quaterniond R(msg.pose.pose.orientation.w,
                                            msg.pose.pose.orientation.x,
                                            msg.pose.pose.orientation.y,
                                            msg.pose.pose.orientation.z);
                        if (extrinsic_ok_) {
                            P = P + R * t_ic_;
                            R = R * R_ic_;
                        }
                        cameraposevisual_.reset();
                        cameraposevisual_.add_pose(P, R);
                        cameraposevisual_.publish_by(*pub_camera_pose_visual_, msg.header);
                    }
                    }
                    break;
                case OdometryType::HIGHFREQ:
                    odom_highfreq_publisher_->publish(std::move(msg));
                    break;
                case OdometryType::TRANSFORM:
                    {
                    geometry_msgs::msg::TransformStamped transformStamped;
                    transformStamped.header.stamp = msg.header.stamp;
                    transformStamped.header.frame_id = "odom";
                    transformStamped.child_frame_id = "map";
                    transformStamped.transform.translation.x = msg.pose.pose.position.x;
                    transformStamped.transform.translation.y = msg.pose.pose.position.y;
                    transformStamped.transform.translation.z = msg.pose.pose.position.z;
                    transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
                    transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
                    transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
                    transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
                    tf_broadcaster->sendTransform(transformStamped);
                    }
                    break;
            }
#else
            switch(odom_type) {
                case OdometryType::STANDARD:
                    {
                    if (getRosNodeControl()->sendOdomBaseLinkTF()) {
                        geometry_msgs::TransformStamped transformStamped;
                        transformStamped.header.stamp = msg.header.stamp;
                        transformStamped.header.frame_id = "odom";
                        transformStamped.child_frame_id = "odin1_base_link";
                        transformStamped.transform.translation.x = msg.pose.pose.position.x;
                        transformStamped.transform.translation.y = msg.pose.pose.position.y;
                        transformStamped.transform.translation.z = msg.pose.pose.position.z;
                        transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
                        transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
                        transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
                        transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
                        tf_broadcaster->sendTransform(transformStamped);
                    }
                    odom_publisher_.publish(msg);

                    if (show_path) {
                        // Publish odom trajectory as visualization markers (green lines connecting adjacent points)
                        static visualization_msgs::Marker marker;
                        static std::vector<geometry_msgs::Point> path_points;
                        
                        marker.header = msg.header;
                        marker.ns = "odom_trajectory";
                        marker.id = 0;
                        marker.type = visualization_msgs::Marker::LINE_STRIP;
                        marker.action = visualization_msgs::Marker::ADD;
                        marker.pose.orientation.w = 1.0;
                        marker.scale.x = 0.02;  // Line width
                        marker.color.r = 0.0;
                        marker.color.g = 1.0;
                        marker.color.b = 0.0;
                        marker.color.a = 1.0;

                        geometry_msgs::Point pt;
                        pt.x = msg.pose.pose.position.x;
                        pt.y = msg.pose.pose.position.y;
                        pt.z = msg.pose.pose.position.z;
                        path_points.push_back(pt);
                        
                        // Keep only recent points to avoid memory issues (e.g., last 1000 points)
                        if (path_points.size() > 30000) {
                            path_points.erase(path_points.begin());
                        }
                        
                        marker.points = path_points;

                        // Publish marker array
                        static visualization_msgs::MarkerArray marker_array;
                        marker_array.markers.clear();  // Clear previous markers
                        marker_array.markers.push_back(marker);
                        path_publisher_.publish(marker_array);
                    }

                    if (show_camerapose) {
                        // camera pose visualization (ROS1)
                        Eigen::Vector3d P(msg.pose.pose.position.x,
                                        msg.pose.pose.position.y,
                                        msg.pose.pose.position.z);
                        Eigen::Quaterniond R(msg.pose.pose.orientation.w,
                                            msg.pose.pose.orientation.x,
                                            msg.pose.pose.orientation.y,
                                            msg.pose.pose.orientation.z);
                            
                        if (extrinsic_ok_) {
                            P = P + R * t_ic_;
                            R = R * R_ic_;
                        }
                        cameraposevisual_.reset();
                        cameraposevisual_.add_pose(P, R);
                        cameraposevisual_.publish_by(pub_camera_pose_visual_, msg.header);
                    }
                    }
                    break;
                case OdometryType::HIGHFREQ:
                    odom_highfreq_publisher_.publish(msg);
                    break;
                case OdometryType::TRANSFORM:
                    {
                    geometry_msgs::TransformStamped transformStamped;
                    transformStamped.header.stamp = msg.header.stamp;
                    transformStamped.header.frame_id = "odom";
                    transformStamped.child_frame_id = "map";
                    transformStamped.transform.translation.x = msg.pose.pose.position.x;
                    transformStamped.transform.translation.y = msg.pose.pose.position.y;
                    transformStamped.transform.translation.z = msg.pose.pose.position.z;
                    transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
                    transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
                    transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
                    transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
                    tf_broadcaster->sendTransform(transformStamped);
                    }
                    break;
            }
#endif
    }

    void initialize_data_logger(std::string data_dir = "") {

        try {
            BinaryDataLogger::Options opts;
            opts.batch_size = 100;
            opts.base_dir = data_dir;
            data_logger_ = std::make_shared<BinaryDataLogger>(opts);
            root_dir_ = data_logger_->root_dir();
            #ifdef ROS2
                RCLCPP_INFO(node_->get_logger(), "Data logger initialized at %s", root_dir_.c_str());
            #endif
        } catch (...) {
            // Swallow logger initialization failures to avoid affecting runtime
            #ifdef ROS2
                RCLCPP_INFO(node_->get_logger(), "Failed to initialize data logger");
            #endif
            data_logger_.reset();
        }
    }

    int loadCameraParams(const std::string& yaml_file) {
        try {
            YAML::Node config = YAML::LoadFile(yaml_file);
    
            YAML::Node cam_node = config["cam_0"];
    
            m_camera_params.width = cam_node["image_width"].as<int>();
            m_camera_params.height = cam_node["image_height"].as<int>();
    
            double A11 = cam_node["A11"].as<double>();
            double A12 = cam_node["A12"].as<double>();
            double A22 = cam_node["A22"].as<double>();
            double u0 = cam_node["u0"].as<double>();
            double v0 = cam_node["v0"].as<double>();
    
            m_camera_params.fx = A11;
            m_camera_params.fy = A22;
            m_camera_params.cx = u0;
            m_camera_params.cy = v0;
            m_camera_params.skew = A12;
    
            m_camera_params.k2 = cam_node["k2"].as<double>();
            m_camera_params.k3 = cam_node["k3"].as<double>();
            m_camera_params.k4 = cam_node["k4"].as<double>();
            m_camera_params.k5 = cam_node["k5"].as<double>();
            m_camera_params.k6 = cam_node["k6"].as<double>();
            m_camera_params.k7 = cam_node["k7"].as<double>();
            m_camera_params.p1 = cam_node["p1"].as<double>();
            m_camera_params.p2 = cam_node["p2"].as<double>();
#if 0 
            std::cout << "成功读取相机参数:" << std::endl;
            std::cout << "图像尺寸: " << m_camera_params.width << "x" << m_camera_params.height << std::endl;
            std::cout << "焦距: fx=" << m_camera_params.fx << ", fy=" << m_camera_params.fy << std::endl;
            std::cout << "主点: cx=" << m_camera_params.cx << ", cy=" << m_camera_params.cy << std::endl;
            std::cout << "倾斜: " << m_camera_params.skew << std::endl;
            std::cout << "畸变系数: k2=" << m_camera_params.k2 << ", k3=" << m_camera_params.k3 
                      << ", k4=" << m_camera_params.k4 << ", k5=" << m_camera_params.k5 
                      << ", k6=" << m_camera_params.k6 << ", k7=" << m_camera_params.k7 << std::endl;
#endif            
            m_cam = std::make_unique<mini_vikit::PolynomialCamera>(m_camera_params.width, m_camera_params.height,
                m_camera_params.fx, m_camera_params.fy, m_camera_params.cx, m_camera_params.cy, m_camera_params.skew,
                m_camera_params.k2, m_camera_params.k3, m_camera_params.k4, m_camera_params.k5, m_camera_params.k6, m_camera_params.k7);

            // Load extrinsic Tcl_0 (camera to lidar)
            extrinsic_ok_ = false;
            if (config["Tcl_0"]) {
                auto T = config["Tcl_0"];
                if (T.IsSequence() && T.size() == 16) {
                    Eigen::Matrix4d Tcl;
                    for (int i = 0; i < 16; ++i) Tcl(i/4, i%4) = T[i].as<double>();
                    T_cl_ = Tcl;
                }
                std::cout << "Tcl: " << T_cl_ << std::endl;

                Eigen::Matrix4d Tic = T_il_ * T_cl_.inverse();
                Eigen::Matrix3d Ric = Tic.block<3,3>(0,0);
                Eigen::Vector3d tic = Tic.block<3,1>(0,3);
                R_ic_ = Eigen::Quaterniond(Ric);
                t_ic_ = tic;
                extrinsic_ok_ = true;
            }

            m_cam_init_success = true;
            return 0;
        } catch (const std::exception& e) {
            std::cerr << "读取YAML文件失败: " << e.what() << std::endl;
            m_cam_init_success = false;
            return -1;
        }
    }

    void buildUndistortMap()
    {
        m_undistort_map_x.create(m_camera_params.height, m_camera_params.width, CV_32F);
        m_undistort_map_y.create(m_camera_params.height, m_camera_params.width, CV_32F);

        for (int v_out = 0; v_out < m_camera_params.height; ++v_out) {
            for (int u_out = 0; u_out < m_camera_params.width; ++u_out) {
                double x_norm = (u_out - m_cam->cx()) / m_cam->fx();
                double y_norm = (v_out - m_cam->cy()) / m_cam->fy();

                // remove skew
                x_norm = x_norm - y_norm * m_cam->skew() / m_cam->fx();

                Eigen::Vector2d uv(x_norm, y_norm);
                Eigen::Vector2d distorted_pixel = m_cam->world2cam(uv);

                m_undistort_map_x.at<float>(v_out, u_out) = static_cast<float>(distorted_pixel[0]);
                m_undistort_map_y.at<float>(v_out, u_out) = static_cast<float>(distorted_pixel[1]);
            }
        }
        m_undistort_map_init_success = true;
    }

private:
    // Add the following member variables
    std::mutex rgb_queue_mutex_;
    std::deque<ImageConstPtr> rgb_image_queue_;
    const size_t max_rgb_queue_size_ = 10; // Cache up to 10 image frames
    
    std::mutex pcd_queue_mutex_;
    std::deque<PointCloud2ConstPtr> pcd_queue_;
    const size_t max_pcd_queue_size_ = 10; // Maximum cache frames

    // Binary logger and frame indices
    std::shared_ptr<BinaryDataLogger> data_logger_;
    std::atomic<uint32_t> pose_index_{0};
    std::atomic<uint32_t> cloud_index_{0};
    std::atomic<uint32_t> image_index_{0};
    std::atomic<uint32_t> wcwi_index_{0};

    std::filesystem::path root_dir_;

    // Updated helper functions
    ImageConstPtr getLatestRgbImage() {
        std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
        return (!rgb_image_queue_.empty()) ? rgb_image_queue_.back() : nullptr;
    }

    PointCloud2ConstPtr getLatestIntensityCloud() {
        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
        return (!pcd_queue_.empty()) ? pcd_queue_.back() : nullptr;
    }
    std::vector<cv::Mat> getRgbImageQueueSnapshot() {
        std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
        std::vector<cv::Mat> images;
        for (const auto& msg : rgb_image_queue_) {
            try {
                cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(*msg, "bgr8");
                images.push_back(cv_ptr->image.clone());
            } catch (cv_bridge::Exception& e) {
                #ifndef ROS2
                    ROS_ERROR("cv_bridge exception: %s", e.what());
                #endif
            }
        }
        return images;
    }

    CameraParams m_camera_params;
    std::unique_ptr<mini_vikit::PolynomialCamera> m_cam;
    bool m_cam_init_success;
    bool m_undistort_map_init_success = false;
    cv::Mat m_undistort_map_x;
    cv::Mat m_undistort_map_y;
    Eigen::Quaterniond R_ic_ {Eigen::Quaterniond::Identity()};
    Eigen::Vector3d t_ic_ {Eigen::Vector3d::Zero()};
    bool extrinsic_ok_ {false};
    Eigen::Matrix4d T_il_ = (Eigen::Matrix4d() <<
        1.0, 0.0, 0.0, 0.00347,
        0.0, 1.0, 0.0, 0.03447,
        0.0, 0.0, 1.0, 0.02174,
        0.0, 0.0, 0.0, 1.0).finished();
    Eigen::Matrix4d T_cl_ = Eigen::Matrix4d::Identity(); // Camera->Lidar from YAML
#ifdef ROS2
    std::vector<sensor_msgs::msg::PointCloud2> getIntensityCloudQueueSnapshot() {
        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
        std::vector<sensor_msgs::msg::PointCloud2> clouds;
        
        for (const auto& msg_ptr : pcd_queue_) {
            clouds.push_back(*msg_ptr);
        }
        
        return clouds;
    }
#else
    std::vector<sensor_msgs::PointCloud2> getIntensityCloudQueueSnapshot() {
        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
        std::vector<sensor_msgs::PointCloud2> clouds;
        
        for (const auto& msg_ptr : pcd_queue_) {
            clouds.push_back(*msg_ptr);
        }
        
        return clouds;
    }
#endif

    void initialize_publishers() {
        #ifdef ROS2
            // Small data with queue depth 1
            auto qos_small = rclcpp::QoS(1)
                                    .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
                                    .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);

            // Large sensor data with larger queue to avoid blocking
            auto qos_sensor = rclcpp::QoS(10)
                                    .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
                                    .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);

            imu_pub_ = node_->create_publisher<ros::Imu>("odin1/imu", qos_small);
            rgb_pub_ = node_->create_publisher<ros::Image>("odin1/image", qos_sensor);
            cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", qos_sensor);
            xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", qos_sensor);
            odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", qos_small);
            odom_highfreq_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_highfreq", qos_small);
            path_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/path", qos_sensor);
            pub_camera_pose_visual_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/camera_pose_visual", qos_sensor);
            rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", qos_sensor);
            compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", qos_small);
            undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", qos_sensor);
            intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", qos_sensor);
            wiwc_publisher_ = node_->create_publisher<ros::Odometry>("odin1/wiwc", qos_small);
            tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
        #endif
    }
    #ifdef ROS1
        void initialize_publishers(ros::NodeHandle& nh) {
            imu_pub_ = nh.advertise<ros::Imu>("odin1/imu", 4000);
            rgb_pub_ = nh.advertise<ros::Image>("odin1/image", 100);
            cloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_raw", 100);
            xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 100);
            odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry", 100);
            odom_highfreq_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry_highfreq", 4000);
            path_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/path", 100);
            pub_camera_pose_visual_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/camera_pose_visual", 100);
            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 100);
            compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 100);
            undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 100);
            intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 100);
            wiwc_publisher_ = nh.advertise<ros::Odometry>("odin1/wiwc", 100);
            tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>();
        }
    #endif

    #ifdef ROS2
        rclcpp::Node::SharedPtr node_;
        rclcpp::Publisher<ros::Imu>::SharedPtr imu_pub_;
        rclcpp::Publisher<ros::Image>::SharedPtr rgb_pub_;
        rclcpp::Publisher<ros::PointCloud2>::SharedPtr cloud_pub_;
        rclcpp::Publisher<ros::PointCloud2>::SharedPtr xyzrgbacloud_pub_;
        rclcpp::Publisher<ros::Odometry>::SharedPtr odom_publisher_;
        rclcpp::Publisher<ros::Odometry>::SharedPtr odom_highfreq_publisher_;
        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr path_publisher_;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr rendered_cloud_pub_;
        rclcpp::Publisher<PointCloud2Msg>::SharedPtr rgbcloud_pub_;
        rclcpp::Publisher<ImageMsg>::SharedPtr rgbFromnv12_pub_;
        rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_rgb_pub_; // New compressed image publisher
        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr undistort_rgb_pub_;
        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr intensity_gray_pub_;
        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_camera_pose_visual_;
        rclcpp::Publisher<ros::Odometry>::SharedPtr wiwc_publisher_;
        camera_pose_visualization cameraposevisual_;
        std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
    #else
        ros::Publisher imu_pub_;
        ros::Publisher rgb_pub_;
        ros::Publisher cloud_pub_;
        ros::Publisher xyzrgbacloud_pub_;
        ros::Publisher odom_publisher_;
        ros::Publisher odom_highfreq_publisher_;
        ros::Publisher path_publisher_;
        ros::Publisher pub_camera_pose_visual_;
        camera_pose_visualization cameraposevisual_;
        ros::Publisher rendered_cloud_pub_;
        ros::Publisher rgbcloud_pub_;
        ros::Publisher rgbFromnv12_pub_;
        ros::Publisher compressed_rgb_pub_; // New compressed image publisher
        ros::Publisher undistort_rgb_pub_;
        ros::Publisher intensity_gray_pub_;
        ros::Publisher wiwc_publisher_;
        std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
    #endif
};

class CommandLineControl {
public:
    using Callback = std::function<void(const std::string&, int)>;


    void register_key(const std::string& key, int default_value = 0) {
        std::lock_guard<std::mutex> lock(mtx);
        kv_map[key] = default_value;
    }

    void set(const std::string& key, int value) {
        Callback cb_to_invoke = nullptr;
        {
            std::lock_guard<std::mutex> lock(mtx);
            auto it = kv_map.find(key);
            if (it != kv_map.end()) {
                if (it->second != value) {
                    it->second = value;
                    cb_to_invoke = callback;
                } 
            } else {
                return;
            }
        }

        if (cb_to_invoke) {
            cb_to_invoke(key, value);
        }
    }

    int get(const std::string& key) const {
        std::lock_guard<std::mutex> lock(mtx);
        auto it = kv_map.find(key);
        return (it != kv_map.end()) ? it->second : -1;
    }


    void register_callback(Callback cb) {
        std::lock_guard<std::mutex> lock(mtx);
        callback = cb;
    }

private:
    std::unordered_map<std::string, int> kv_map;
    std::thread input_thread;
    std::atomic<bool> running;
    mutable std::mutex mtx;
    Callback callback;

};

#define STREAMCTRL "streamctrl"        /* start/stop all streams */
#define SENDRGB    "sendrgb"           /* send RGB data */
#define SENDIMU    "sendimu"           /* send IMU data */
#define SENDODOM   "sendodom"          /* send odometry data */
#define SENDDTOF   "senddtof"          /* send raw cloud data */
#define SENDCLOUDSLAM  "sendcloudslam"      /* send rgb cloud data */
#define EXIT       "q"                 /* exit sample */