#!/usr/bin/env python

import rospy
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, NavSatFix
import os

class ImageGPSReader:
    def __init__(self):
        # Initialize ROS node
        rospy.init_node('image_gps_publisher', anonymous=True)
        
        # Initialize publishers
        self.image_pub = rospy.Publisher('/camera/image_raw', Image, queue_size=10)
        self.gps_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10)
        
        # Initialize CvBridge
        self.bridge = CvBridge()

        # Load GPS and image data
        self.gps_data = self.load_gps_data("/home/dongdong/2project/0data/NWPU/config/gps.txt")
        self.image_files = self.load_image_filenames("/home/dongdong/2project/0data/NWPU/img")

    def load_gps_data(self, gps_file):
        gps_data = {}
        with open(gps_file, 'r') as f:
            for line in f:
                parts = line.strip().split()
                timestamp = parts[0]
                lat, lon, alt = map(float, parts[1:])
                gps_data[timestamp] = (lat, lon, alt)
        return gps_data

    def load_image_filenames(self, img_folder):
        image_files = [f for f in os.listdir(img_folder) if f.endswith('.jpg')]
        image_files.sort()  # Ensure consistent ordering
        return image_files

    def publish_data(self):
        rate = rospy.Rate(10)  # 10 Hz
        
        for img_file in self.image_files:
            timestamp = img_file.split('.')[0]+ "."+ img_file.split('.')[1]  # Assuming timestamp is in the filename
            print("timestamp ",timestamp)

            if timestamp in self.gps_data:
                img_path = os.path.join("/home/dongdong/2project/0data/NWPU/img", img_file)
                cv_image = cv2.imread(img_path)

                if cv_image is not None:
                    # Publish image
                    img_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
                    img_msg.header.stamp = rospy.Time.from_sec(float(timestamp))
                    self.image_pub.publish(img_msg)

                    # Publish GPS data
                    lat, lon, alt = self.gps_data[timestamp]
                    gps_msg = NavSatFix()
                    gps_msg.header.stamp = rospy.Time.from_sec(float(timestamp))
                    gps_msg.latitude = lat
                    gps_msg.longitude = lon
                    gps_msg.altitude = alt
                    self.gps_pub.publish(gps_msg)

                    rospy.loginfo(f"Published image: {img_file} and GPS data")

            rate.sleep()  # Sleep to maintain the loop rate


            

if __name__ == '__main__':
    try:
        reader = ImageGPSReader()
        reader.publish_data()
    except rospy.ROSInterruptException:
        pass

#!/usr/bin/env python

import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image, NavSatFix
from collections import deque
import threading
import queue
import time



# Initialize queues and locks
image_queue = queue.Queue()  # Thread-safe queue for images
gps_queue = deque()  # Queue for GPS messages
gps_queue_lock = threading.Lock()  # Lock for GPS queue

# Initialize CvBridge
bridge = CvBridge()

def gps_callback(gps_msg):
    """Callback function for GPS messages."""
    global gps_queue
    with gps_queue_lock:
        gps_queue.append(gps_msg)  # Add GPS message to the right end of the queue

def image_callback(image_msg):
    """Callback function for image messages."""
    global image_queue, gps_queue

    # Convert ROS image message to OpenCV image
    try:
        cv_image = bridge.imgmsg_to_cv2(image_msg, "bgr8")
    except CvBridgeError as e:
        rospy.logerr("CvBridge Error: %s", e)
        return

    # Get the timestamp of the image
    image_timestamp = image_msg.header.stamp.to_sec()

    # Store image and timestamp in the queue
    image_queue.put((cv_image, image_timestamp))

    with gps_queue_lock:
        while len(gps_queue) != 0:
            gps_msg = gps_queue.popleft()  # Remove GPS message from the left end
            gps_timestamp = gps_msg.header.stamp.to_sec()

            time_difference = image_timestamp - gps_timestamp
            rospy.loginfo(f"Image timestamp: {image_timestamp}, GPS timestamp: {gps_timestamp}, Time difference: {time_difference}")

            if abs(time_difference) <= 0.01:  # Match if time difference is within 10ms
                latitude = gps_msg.latitude
                longitude = gps_msg.longitude
                altitude = gps_msg.altitude

                rospy.loginfo(f"Match successful. Latitude: {latitude}, Longitude: {longitude}, Altitude: {altitude}")

                # Data is used, discard this GPS message
                break
            elif gps_timestamp < image_timestamp - 0.01:
                # Data too old, discard this GPS message
                rospy.loginfo("GPS data too old, discarding.")
                pass
            elif gps_timestamp > image_timestamp + 0.01:
                # Data too new, put it back for later use
                gps_queue.appendleft(gps_msg)  # Add to the left end for later processing
                rospy.loginfo("GPS data too new, putting back for later use.")
                break

def display_images():


    # Create a window to show matched images
    cv2.namedWindow("Matched Image", cv2.WINDOW_AUTOSIZE)

    """Thread function to display images."""
    while not rospy.is_shutdown():
        if not image_queue.empty():
            cv_image, image_timestamp = image_queue.get()
            if cv_image is None:
                #rospy.logwarn("Received empty image.")
                continue
            #rospy.loginfo("Displaying image...")
            cv2.imshow("Matched Image", cv_image)
            cv2.waitKey(1)  # Refresh the window
        else: 
            cv2.waitKey(1) # 不能空转 会出问题


def main():
    """Main function to initialize ROS node and subscribers."""
    rospy.init_node('image_gps_matcher', anonymous=True)

    # Subscribe to image and GPS topics
    rospy.Subscriber('/camera/image_raw', Image, image_callback)
    rospy.Subscriber('/gps/fix', NavSatFix, gps_callback)


    # Start the display thread
    display_thread = threading.Thread(target=display_images)
    display_thread.daemon = True  # Daemonize thread
    display_thread.start()

    try:
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
    finally:
        cv2.destroyAllWindows()

if __name__ == '__main__':
    main()