Synchronize amcl_pose with 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!