image_gps_publisher.py
#!/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
image_listener.py
#!/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()