#=============文件结构
ros_cgg/
├── build
├── devel
└── src
    └── image_gps_package
        ├── CMakeLists.txt
        ├── package.xml
        └── src
            ├── image_gps_publisher.cpp
            └── image_gps_subscriber.cpp

#==============编译
cd ~/ros_cgg
catkin_make
source devel/setup.bash


#==============手动运行
#0开启source确保找到
cd ~/ros_cgg
source devel/setup.bash
# 手动开启三个窗口
#1启动ROS Master：
roscore
#2运行你的C++发布节点：
rosrun image_gps_package image_gps_publisher
#3运行c++接受节点
rosrun image_gps_package image_gps_subscriber


#============ 脚本运行
sudo chmod +x run_ros_nodes.sh

#!/bin/bash

#外部给与执行权限
#sudo chmod +x run_ros_nodes.sh

# 定义 ROS 安装路径  #安装时候添加到系统路径了 不需要每次都source
ROS_SETUP="/opt/ros/noetic/setup.bash" 
# 定义工作目录路径  自己的工程没有加到系统路径，每次需要source
WORKSPACE_DIR="/home/dongdong/2project/1salm/2Mycode/ros/ros_cgg"


#指定目录

# 启动 ROS Master
echo "Starting ROS 总结点..."
gnome-terminal -- bash -c "\
cd $WORKSPACE_DIR; source devel/setup.bash; \
roscore; \
exec bash"

# 等待 ROS Master 启动
sleep 5

# 运行 C++ 发布节点
echo "Running C++ 发布节点..."
gnome-terminal -- bash -c "\
cd $WORKSPACE_DIR; source devel/setup.bash; \
rosrun image_gps_package image_gps_publisher; \
exec bash"

# 运行 C++ 接受节点
echo "Running C++ 订阅节点..."
gnome-terminal -- bash -c "\
cd $WORKSPACE_DIR; source devel/setup.bash; \
rosrun image_gps_package image_gps_subscriber; \
exec bash"

cmake_minimum_required(VERSION 3.0.2)
project(image_gps_package)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  cv_bridge
)

find_package(OpenCV REQUIRED)
find_package(Boost REQUIRED COMPONENTS filesystem)

catkin_package(
  CATKIN_DEPENDS roscpp sensor_msgs cv_bridge
  DEPENDS Boost
)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
)

# 编译发布节点
add_executable(image_gps_publisher src/image_gps_publisher.cpp)
target_link_libraries(image_gps_publisher
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
  ${Boost_LIBRARIES}
)

# 编译接受节点
add_executable(image_gps_subscriber src/image_gps_subscriber.cpp)
target_link_libraries(image_gps_subscriber
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
  ${Boost_LIBRARIES}
)

<?xml version="1.0"?>
<package format="2">
  <name>image_gps_package</name>
  <version>0.0.1</version>
  <description>
    A package to publish and subscribe to images and GPS data using ROS.
  </description>

  <!-- Maintainer of the package -->
  <maintainer email="your_email@example.com">Your Name</maintainer>

  <!-- License of the package -->
  <license>MIT</license>

  <!-- Build tool required to build this package -->
  <buildtool_depend>catkin</buildtool_depend>

  <!-- Dependencies of the package during build and runtime -->
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>cv_bridge</exec_depend>

  <!-- Declare additional dependencies required for building this package -->
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>cv_bridge</build_export_depend>

  <!-- Export information, can be used by other packages -->
  <export>
    <!-- Export any specific information here -->
  </export>
</package>

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/filesystem.hpp>
#include <fstream>
#include <sstream>
#include <iostream>

class ImageGPS_Reader {


public:

    std::vector<boost::filesystem::path> image_files_;
    struct GPSData {
        std::string timestamp;
        double lat, lon, alt;
    };
    std::map<std::string, GPSData> gps_data_;


    ImageGPS_Reader() {
        std::string data_dir="/home/dongdong/2project/0data/NWPU/";
        std::string gps_dir=data_dir+"/config/gps.txt";
        std::string img_dir=data_dir+"img";
        // Load GPS data
        loadGPSData(gps_dir);

        // Load image filenames
        loadImageFilenames(img_dir);
    }

    void loadGPSData(const std::string& gps_file) {
        std::ifstream file(gps_file);
        if (!file.is_open()) {
            ROS_ERROR("Failed to open GPS file: %s", gps_file.c_str());
            return;
        }

        std::string line;
        while (std::getline(file, line)) {
            std::istringstream iss(line);
            std::string timestamp;
            double lat, lon, alt;
            if (!(iss >> timestamp >> lat >> lon >> alt)) { break; }
            gps_data_[timestamp] = {timestamp,lat, lon, alt};
        }
        file.close();
    }

    void loadImageFilenames(const std::string& img_folder) {
        namespace fs = boost::filesystem;
        fs::directory_iterator end_itr;
        for (fs::directory_iterator itr(img_folder); itr != end_itr; ++itr) {
            if (fs::is_regular_file(itr->status()) && itr->path().extension() == ".jpg") {
                image_files_.push_back(itr->path());
            }
        }
    }



};

int main(int argc, char** argv) {
    ros::init(argc, argv, "image_gps_publisher");

    ImageGPS_Reader reader;
    
    // Initialize ROS node handle
    ros::NodeHandle nh;
    // Create publishers
    ros::Publisher image_pub_ = nh.advertise<sensor_msgs::Image>("/camera/image_raw", 10);
    ros::Publisher gps_pub_ = nh.advertise<sensor_msgs::NavSatFix>("/gps/fix", 10);


    ros::Rate rate(10);  // 10 Hz

    while (ros::ok()) {
        if (!reader.image_files_.empty() && !reader.gps_data_.empty()) {
            for (const auto& img_file : reader.image_files_) {
                std::string timestamp = img_file.stem().string();  // Extract timestamp from filename
                
                if (reader.gps_data_.find(timestamp) != reader.gps_data_.end()) {// 确保GNSS对应图像存在
                    std::string img_path = img_file.string();
                    cv::Mat cv_image = cv::imread(img_path, cv::IMREAD_COLOR);
                    if (!cv_image.empty()) {
                        // 发布图像

                        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", cv_image).toImageMsg();
                        msg->header.stamp = ros::Time(std::stod(timestamp));                
                        image_pub_.publish(msg);

                        // 发布GNSS信息
                        auto gps = reader.gps_data_[timestamp];
                        sensor_msgs::NavSatFix gps_msg;
                        gps_msg.header.stamp = ros::Time(std::stod(timestamp));                      
                        gps_msg.latitude = gps.lat;
                        gps_msg.longitude = gps.lon;
                        gps_msg.altitude = gps.alt;
                        gps_pub_.publish(gps_msg);

                        ROS_INFO("Published image: %s and GPS data", img_file.filename().string().c_str());
                        rate.sleep(); // 按照设定的频率休眠
                    }
                }
            }
        }
        //处理一次所有的回调函数。这个调用会处理所有的回调并清空消息队列。对于消息处理来说，它确保了消息的及时处理。
        ros::spinOnce();
    }//while

    return 0;
}

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/filesystem.hpp>
#include <fstream>
#include <sstream>
#include <iostream>
#include <thread>
#include <mutex>

class ImageGPS_Reader {
public:
    std::vector<boost::filesystem::path> image_files_;
    struct GPSData {
        std::string timestamp;
        double lat, lon, alt;
    };
    std::map<std::string, GPSData> gps_data_;

    ImageGPS_Reader() {
        std::string data_dir = "/home/dongdong/2project/0data/NWPU/";
        std::string gps_dir = data_dir + "/config/gps.txt";
        std::string img_dir = data_dir + "img";
        // Load GPS data
        loadGPSData(gps_dir);

        // Load image filenames
        loadImageFilenames(img_dir);
    }

    void loadGPSData(const std::string& gps_file) {
        std::ifstream file(gps_file);
        if (!file.is_open()) {
            ROS_ERROR("Failed to open GPS file: %s", gps_file.c_str());
            return;
        }

        std::string line;
        while (std::getline(file, line)) {
            std::istringstream iss(line);
            std::string timestamp;
            double lat, lon, alt;
            if (!(iss >> timestamp >> lat >> lon >> alt)) { break; }
            gps_data_[timestamp] = {timestamp, lat, lon, alt};
        }
        file.close();
    }

    void loadImageFilenames(const std::string& img_folder) {
        namespace fs = boost::filesystem;
        fs::directory_iterator end_itr;
        for (fs::directory_iterator itr(img_folder); itr != end_itr; ++itr) {
            if (fs::is_regular_file(itr->status()) && itr->path().extension() == ".jpg") {
                image_files_.push_back(itr->path());
            }
        }
    }
};

// Global variables for shared state and mutex
ImageGPS_Reader reader;
ros::Publisher image_pub_;
ros::Publisher gps_pub_;
std::mutex mtx;

void publishImages() {
    
    std::vector<boost::filesystem::path> image_files_copy;
    {
        std::lock_guard<std::mutex> lock(mtx);
        image_files_copy = reader.image_files_;
    }

    ros::Rate rate(10);  // 10 Hz
    while (ros::ok()) {
        for (const auto& img_file : image_files_copy) {
            std::string timestamp = img_file.stem().string();  // Extract timestamp from filename
            
            cv::Mat cv_image = cv::imread(img_file.string(), cv::IMREAD_COLOR);
            if (!cv_image.empty()) {
                // 发布图像
                sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", cv_image).toImageMsg();
                msg->header.stamp = ros::Time(std::stod(timestamp));                
                image_pub_.publish(msg);
                ROS_INFO("Published image: %s", img_file.filename().string().c_str());
            }
            rate.sleep();
        }
    }
}

void publishGPSData() {

    std::map<std::string, ImageGPS_Reader::GPSData> gps_data_copy;
    {
        std::lock_guard<std::mutex> lock(mtx);
        gps_data_copy = reader.gps_data_;
    }

    ros::Rate rate(10);  // 10 Hz
    while (ros::ok()) {

        for (const auto& entry : gps_data_copy) {
            const auto& timestamp = entry.first;
            const auto& gps = entry.second;

            // 发布GNSS信息
            sensor_msgs::NavSatFix gps_msg;
            gps_msg.header.stamp = ros::Time(std::stod(timestamp));                      
            gps_msg.latitude = gps.lat;
            gps_msg.longitude = gps.lon;
            gps_msg.altitude = gps.alt;
            gps_pub_.publish(gps_msg);
            ROS_INFO("Published GPS data for timestamp: %s", timestamp.c_str());
            rate.sleep();
        }
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "image_gps_publisher");
    ros::NodeHandle nh;

    // Create publishers
    image_pub_ = nh.advertise<sensor_msgs::Image>("/camera/image_raw", 10);
    gps_pub_ = nh.advertise<sensor_msgs::NavSatFix>("/gps/fix", 10);

    // Start threads for publishing images and GPS data
    std::thread image_thread(publishImages);
    std::thread gps_thread(publishGPSData);

    // Wait for threads to finish (they run indefinitely in this example)
    image_thread.join();
    gps_thread.join();

    return 0;
}

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <deque>
#include <utility>

std::deque<std::pair<cv::Mat, ros::Time>> image_queue;
std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue;
std::mutex gpsQueue_Lock;// 队列锁

void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg) {
    gpsQueue_Lock.lock();
    gpsQueue.push(gps_msg);
    gpsQueue_Lock.unlock();
}


void imageCallback(const sensor_msgs::ImageConstPtr& image_msg) {
    // Convert ROS image message to OpenCV image
    cv_bridge::CvImagePtr cv_image;
    try {
        cv_image = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    
    // Get the timestamp of the image
    double image_timestamp = image_msg->header.stamp.toSec();

    // Store image and timestamp in the queue
    image_queue.emplace_back(cv_image->image, image_timestamp);

    
    gpsQueue_Lock.lock();

    while(!gpsQueue.empty()){

        sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
        double gps_timestamp = GPS_msg->header.stamp.toSec();
        double time_diffrence = image_timestamp - gps_timestamp;
        printf("img t: %f, gps t: %f , time_diffrence: %f \n", image_timestamp, gps_timestamp,time_diffrence);
       

        if(abs(image_timestamp - gps_timestamp)<=0.01){// 图像和gps时间差在0.01s=10ms内的算匹配成功 vins-fusion参考值
            // 在线阶段，假设图像20帧/S 相邻帧间隔50ms 中间值间隔25ms算阈值考虑。
            // 离线阶段，一般会提前手动处理图像和GPS时间戳对齐，图像名字以时间戳保存，GNSS的第一列也是一样的时间戳。

            // 经纬度、海拔
            double latitude = GPS_msg->latitude;
            double longitude = GPS_msg->longitude;
            double altitude = GPS_msg->altitude;
            gpsQueue.pop();// 使用完毕 丢弃
            
            std::cout << std::fixed << std::setprecision(9)
            << "图像和GNSS匹配成功" 
            <<" 经度: "<<latitude
            <<" 纬度: "<<longitude
            <<" 高度: "<<altitude
            <<std::endl;

            // 送入 位姿估计线程

            cv::imshow("Matched Image", cv_image->image);
            cv::waitKey(1);
            break;

          
        }
        else if(gps_timestamp < image_timestamp - 0.01)// GPS先开始采集，图像滞后，比图像早期的GPS丢弃
        {
             gpsQueue.pop();
             //break;
        }
        else if(gps_timestamp > image_timestamp + 0.01)// GPS采集快，图像处理慢，比图像快的GPS保留，等待后续图像来匹配
        {
            break;
        }

    }


    gpsQueue_Lock.unlock();// gps
}



int main(int argc, char** argv) {
    // Initialize ROS
    ros::init(argc, argv, "image_gps_matcher");
    ros::NodeHandle nh;

    // Subscribe to image and GPS topics
    ros::Subscriber image_sub = nh.subscribe("/camera/image_raw", 10, imageCallback);
    ros::Subscriber gps_sub = nh.subscribe("/gps/fix", 10, gpsCallback);
    cv::namedWindow("Matched Image", cv::WINDOW_AUTOSIZE);

    // 使用 MultiThreadedSpinner，指定线程数为 4
    //ros::MultiThreadedSpinner spinner(4);
    // 调用 spinner 来处理回调函数
    //spinner.spin();


    /*
        阻塞调用，处理回调函数
        ros::spin() 使节点进入一个循环，不断处理所有回调函数，直到节点被关闭或中断。
        适用于那些只依赖于回调函数的节点，例如只处理订阅消息和服务请求的节点。
        它会阻塞当前线程，使得节点在处理回调函数时不会执行其他代码。
        如果你的节点没有其他需要执行的任务，使用 ros::spin() 可以保持节点活跃，确保所有回调函数得到处理。
    */
    ros::spin();
     /*
        非阻塞调用 
        处理所有到达的回调函数一次，然后返回。
        它是非阻塞的，允许主循环继续执行其他代码。
        当你需要在节点中执行除回调处理之外的其他逻辑时，例如定时任务、计算或其他非 ROS 相关的操作。
        你可以在主循环中调用 ros::spinOnce()，使其在处理回调函数的同时执行其他任务。
     */
    
    //ros::spinOnce();

    return 0;
}