0
$\begingroup$

How to get a frame transformation using TimeSynchronizer?

I am developing some 3D recognition application for a mobile robot. The robot has an RGB camera, depth camera, and uses rtabmap for SLAM, so I have colored point cloud as a map.

My application takes an input data for each moment of time, processes it, and outputs a segmented/labelled point cloud. The input data is following:

  1. RGB image
  2. Depth map
  3. Cumulative colored point cloud corresponding to this point in time
  4. Corresponding position of the robot (actually, camera) in the point cloud (item 3). I need it to project the point cloud to an image and make some operations.

A screenshot from Rviz to illustrate the input data (except the position of the robot): rviz screenshot

To get all this data at once, I am trying to write a callback function using message_filters.ApproximateTimeSynchronizer. But I can't figure out how to get the position data. When I try to use tf.TransformListener() along with other subscribers and a synchronizer, I get the error: AttributeError: 'TransformListener' object has no attribute 'registerCallback'

My code (simplified):

class SaveSyncedAll:

    def __init__(self):
        self.bridge = CvBridge()
        self.sub_rgb = message_filters.Subscriber("/raw_frontal_camera/color/image_raw", Image)
        self.sub_d = message_filters.Subscriber("/raw_frontal_camera/depth/image_rect_raw", Image)
        self.sub_gsm = message_filters.Subscriber(
            "/cloud_map", PointCloud2, queue_size=1, buff_size=52428800
        )
        self.listener = tf.TransformListener(cache_time=rospy.Duration(0.01))
        # === THE METHOD BELOW LEADS TO AN ERROR ===
        ts = message_filters.ApproximateTimeSynchronizer(
            [self.sub_rgb, self.sub_d, self.sub_gsm, self.listener], queue_size=10, slop=0.01
        )
        ts.registerCallback(self.callback_all)
        # where to save data
        self.dataset_dir = "/home/3Drecognition/data_samples/All-1"
        # we need only each 10th frame
        self.frame_freq = 10
        self.frame_n = 0

    def callback_all(self, data_rgb, data_d, data_pc):
        # get rgb image
        try:
            cv_image_rgb = self.bridge.imgmsg_to_cv2(data_rgb, "bgr8")
        except CvBridgeError as e:
            print(e)
            return

        # get depth image
        try:
            cv_image_d = self.bridge.imgmsg_to_cv2(data_d, desired_encoding='passthrough')
        except CvBridgeError as e:
            print(e)
            return

        # get depth image
        try:
            (trans_d, rot_d) = self.listener.lookupTransform(
                '/map', '/frontal_camera_depth_optical_frame', rospy.Time(0)
            )
            (trans_rgb, rot_rgb) = self.listener.lookupTransform(
                '/map', '/frontal_camera_rgb_optical_frame', rospy.Time(0)
            )
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            return

        # get colored point cloud
        xyz, rgb = unpack_pc2_data_1D(data_pc)

        # save obtained data sample to try my algorithm on it later
        # <.. some code here ..>


if __name__ == '__main__':
    rospy.init_node('save_synced_all', anonymous=True)
    ip = SaveSyncedAll()
    rospy.spin()

I'm using ROS Noetic.

$\endgroup$

2 Answers 2

0
$\begingroup$

I have two workaround solutions that do not directly answer your question (sorry), but might be useful to you or to future readers.

1. Use the image timestamp to get the transform

You could define the TransformListener in __init__, but instead of trying to synchronize it with the other topics, you can call it from inside the subscriber using the timestamp of one of the messages (or maybe their average). Hope that this tutorial is clearer than me.

2. Create a dedicated node to listen to tf

A non-elegant workaround is to have a node with just the TransformListener continuously listening for the transform you need, and re-publishing it on a dedicated topic that you can then subscribe with a message_filters.Subscriber.

The introduced delay should not be a big issue as transforms should come at a higher rate than images, and the introduced overhead should not be too long.

The above, of course, only if you are down for a quick and dirty solution.

$\endgroup$
1
$\begingroup$

AttributeError: 'TransformListener' object has no attribute 'registerCallback' does not correlate with the line that you're calling out in the code:

ts = message_filters.ApproximateTimeSynchronizer(
        [self.sub_rgb, self.sub_d, self.sub_gsm, self.listener], queue_size=10, slop=0.01
    )

It looks like it's related to the next line ts.registerCallback(self.callback_all) and the object appears to be a TransformListener object not the expected synchronizer.

Please make sure that you're simplified version actually runs and gives the error you're seeing. Also make sure that you give the full traceback. Just one error doesn't give enough info to understand what's happening.

I also see a problem that you aren't saving your synchronizer, ts in the object (aka self.ts = ...) which means it will go out of scope at the end of the __init__ and never trigger the callback.

$\endgroup$
1
  • $\begingroup$ Thanks, you're right! I'll fix my simplified version and give the full traceback. Hm, you know, I agree with you that ts not being in the object isn't right, but it actually works, callback gets triggered.. :D $\endgroup$
    – orsveri
    Commented Aug 18, 2022 at 7:05

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.