Robotics StackExchange | Archived questions

Issue with message_filters and PointCloud2 in python

I want to subscribe to pointcloud data and image data and sync them and then publish them again. I have written a short and simple script for this:

import rospy
from sensor_msgs.msg import Image as SesnorImage
from sensor_msgs.msg import CameraInfo
# from sensor_msgs.msg import PointCloud2
import message_filters


class synchronizer:
    def __init__(self):
        self.pub_Image = rospy.Publisher('image_raw_sync', SesnorImage, queue_size=1)
        self.pub_Cam_Info = rospy.Publisher('camera_info_sync', CameraInfo, queue_size=1)
        # self.pub_Lidar = rospy.Publisher('rslidar_points_sync', PointCloud2, queue_size=1)

        self.imageInput = message_filters.Subscriber('/image_raw', SesnorImage)
        self.cameraInfo = message_filters.Subscriber('/camera_info', CameraInfo)
        # self.lidar = message_filters.Subscriber('/rslidar_points', PointCloud2)

        self.ts = message_filters.TimeSynchronizer([self.imageInput
                                                    ,self.cameraInfo
                                                    # , self.lidar
                                                    ], 10)
        self.ts.registerCallback(self.general_callback)

        self._image_raw = SesnorImage()
        self._camera_info = CameraInfo()
        # self._rslidar_points = PointCloud2()

    def general_callback(self
                         , image_raw
                         , camera_info
                         # , rslidar_points
                         ):
        self._image_raw = image_raw
        self._camera_info = camera_info
        # self._rslidar_points = rslidar_points

    def publisher(self):
        while True:
            self.pub_Image.publish(self._image_raw)
            self.pub_Cam_Info.publish(self._camera_info)
            # self.pub_Lidar.publish(self._rslidar_points)


if __name__ == '__main__':
    rospy.init_node('synchronizer')
    synchronizer = synchronizer()
    synchronizer.publisher()
    rospy.spin()

But when I uncomment the commented lines, there would be no data in the published topics! so when I add the PointCloud2 sensor messages, everything gows bad and I dont know why this is happening. Why is this happening?

Asked by pedram.ghz on 2019-02-07 14:58:21 UTC

Comments

Answers

I suspect you want to be using the approximate time synchronizer, since the image messages and point cloud messages will be unlikely to have identical time stamps.

If the point clouds are being published with slightly different time stamps to the image messages then you would expect the callback to never be called, since there are never three messages with identical time straps.

Hope this helps.

Asked by PeteBlackerThe3rd on 2019-02-07 17:29:57 UTC

Comments

Thanks for your answer, actually I tried that one as well but there is no difference! I also tried commenting the lines related to camera_info and image_raw and uncomment the pointcloud lines but still, it cannot subscribe to that only topic(rslidar_points).

Asked by pedram.ghz on 2019-02-08 04:25:08 UTC

I mean even I cannot subscribe to just one topic of pointloud data(rslidar_points) and the process does not enter the subscriber's callback function at all.

Asked by pedram.ghz on 2019-02-08 04:27:38 UTC

Are you sure PointClouds are being published? Is the topic correct?

Asked by gvdhoorn on 2019-02-08 04:43:36 UTC

yeap, everything seems to be fine :) :( :)

Asked by pedram.ghz on 2019-02-08 05:34:22 UTC

Just for a sanity check do you want to setup 3 normal subscribers to each topic and print the topic name and timestamp each time one is received.

Asked by PeteBlackerThe3rd on 2019-02-08 05:42:08 UTC

Thanks to all of you! as Pete suggested, I checked the topics with normal subscribers and figured out the problem was with the time stamps. Now the problem is solved :)

Asked by pedram.ghz on 2019-02-08 06:59:55 UTC

I checked the topics with normal subscribers and figured out the problem was with the time stamps

so what was the problem with the timestamps?

Future readers of this Q&A will be very grateful if you could provide a little more information on what exactly was wrong with them.

Asked by gvdhoorn on 2019-02-08 07:04:43 UTC

I am facing the same problem, can you explain me how the problem is solved by checking the time stamps?. I am stuck in the same thing.

Asked by gautamjain on 2019-10-23 09:30:59 UTC

@gvdhoorn is right... So, in my case, the problem was that one of the topics was publishing messages with time stamp 0 all the time.

Asked by pedram.ghz on 2019-02-10 05:50:43 UTC

Comments