Find the closest messages to a PointCloud2 message.

edit

Hello everyone, please allow me to describe what I want to perform. I have a bag with a pointcloud2 and multiple Image and Camera Info topics. I want to find the closest Image message to the a pointcloud2 of a certain timestamp. Here is my code, so far I can edit the rgb colors of a point but I want to do it according to the closest image (project 3d to pixel from image_geometry library) but the thing is I cannot extract the closest Image messages. Any ideas?

My first thought was to read messages from the bag inside a time window but it seems that the operand + cannot be done in 2 rostime.Time objects, but the subtraction can... So it leads to nothing.

import rosbag
import sensor_msgs.point_cloud2
import rospy
import struct
from sensor_msgs.msg import PointCloud2, PointField
import std_msgs.msg
import time


rospy.init_node("create_cloud_xyzrgb")
pub = rospy.Publisher("/velodyne_red_points", PointCloud2, queue_size=2)


bag = rosbag.Bag('/media/antonis/Data/Coloring_Test/4cam_VLP_decompressed_reordered.orig.bag')

for topic, msg, t in bag.read_messages(topics=['/velodyne_points']):
    points = []

    #msg_test,t_test = bag.read_messages(topics=['/cam/0/image_raw'], start_time = t - rospy.Time.from_sec(1), end_time = t + rospy.Time.from_sec(1))
    #print(msg_test)
    for point in sensor_msgs.point_cloud2.read_points(msg, skip_nans=True):
    # https://answers.ros.org/question/230680/extracting-the-xyz-coordinates-from-pointcloud2-data-in-python/    
            x = point[0]
            y = point[1]
            z = point[2]
            r = int(255.0)
            g = int(0)
            b = int(0)
            a = 255

            rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]

            pt = [x, y, z, rgb]
            points.append(pt)
    fields = [PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        PointField('rgb', 12, PointField.UINT32, 1),
        #PointField('rgba', 12, PointField.UINT32, 1),
        ]

    header = std_msgs.msg.Header()
    header.frame_id = "velodyne"
    header.stamp = t
    print(header)
    pc2 = sensor_msgs.point_cloud2.create_cloud(header, fields, points)
    pub.publish(pc2)


bag.close()