Ask Your Question
0

Synchronize amcl_pose with image?

asked 2018-01-22 20:11:24 -0600

aseembits93 gravatar image

Hi, So I am trying to collect synchronized pairs of /amcl_pose and /image_rect_color on the Fetch robot. Here's the code I'm running.

#!/usr/bin/env python2
# amcl_listener
from copy import deepcopy
import rospy

from math import sin, cos


from geometry_msgs.msg import PoseWithCovarianceStamped as Pose
from sensor_msgs.msg import Image
import cv2
import pdb as pdb


class amcl_listener(object):
    def __init__(self):
            # Create a node
        rospy.init_node("amcl_listener")
        self.amcl_list = []
        self.image_list = []
        # Make sure sim time is working
        while not rospy.Time.now():
            pass
        self.img_sub = rospy.Subscriber(
            '/head_camera/rgb/image_rect_color', Image, self.cb_image)
        self.amcl_sub = rospy.Subscriber(
            '/amcl_pose', Pose, self.cb_amcl)
        self.curimg = None
        self.amcl_pose = None
        while not rospy.is_shutdown():
            pass


    def cb_image(self, msg):
        self.curimg = msg
        print "got image at ", rospy.Time.now()
    def cb_amcl(self, msg):
        self.amcl_pose = msg
        print "got amcl at ", rospy.Time.now()

    if __name__ == "__main__":
        start = amcl_listener()

Here comes the problem, The difference between the time the first image message arrives and the first amcl pose message arrives is close to 2 seconds according to rospy.Time.now(). I tried to compare header stamps but they are also problematic. Any help would be appreciated!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-01-23 03:43:43 -0600

Have a look at the time synchronizer message filter, this is designed to solve this problem for you.

It can subscribe to several different topics and executes its callback with sets of messages which are as close together in time as possible. This will probably mean that some of the first image messages will be discarded until the amcl messages start arriving.

Hope this helps.

edit flag offensive delete link more

Comments

Hi @PeteBlackerThe3rd! I have tried this thing. It doesn't work in my case. I think it works for messages with a similar update frequency. like stereo images. Actually, amcl_pose arrives first! that is the surprising part.

aseembits93 gravatar imageaseembits93 ( 2018-01-23 19:56:46 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-01-22 20:11:24 -0600

Seen: 90 times

Last updated: Jan 23 '18