How to synchronize image and marker display between two ROS nodes
I have two ROS nodes, a "display node" and a "detector node", that both use cv_bridge and subscribe to the same image topic. The detector node publishes an array of ROI boxes around the objects it is designed to detect (e.g. faces). While I could draw these boxes on an OpenCV image window using the same node, I would like the boxes display along with potentially other markers from other detector nodes.
So I have a display node that listens on both the raw image topic and the detector topics and draws the appropriate boxes on a marker image that it then merges with the raw image topic and updates an OpenCV image display window.
The problem I am having is that the markers (e.g. boxes) significantly lag behind the raw image. For instance, if my detector node uses the Haar face detector, and I move my head, the face boxes trail my head movement by a very noticeable amount. (Maybe half of a second?)
I am guessing this is because the display node can update the raw image display much faster than the detector node can update the face detections. But I'm not sure what I can do about it or if this approach is even a good idea.
UPDATE: Well, I've run into a snag. I have no trouble with the CameraInfo Python example found here. However, I cannot get synchronization to work when trying to synchronize /camera/rgb/image_color (I'm using a Kinect and the openni_camera node) and a test publisher I wrote.
Here are the two test scripts I am using. The first script simply publishes a DetectionArray message about 30 times per second with nothing but the header.stamp field set to rospy.Time.now(). The second script subscribes to this topic as well as /camera/rgb/image_color. While the second script successfully prints out the value of header.stamp using the regular rospy.Subscriber callback, I never see the message "Synchronized" from the TimeSynchronizer callback. Looking at the timestamps manually using rostopic echo on the /camera/rgb/image_color and /detections topics shows that they are very close to being synchronized already so I thought this should work.
I must be missing something simple I can't see so any tips would be appreciated!
Here is the test_pub.py publisher script:
#!/usr/bin/env python
""" test_pub.py - Version 0.1 2011-06-10
"""
import roslib
roslib.load_manifest('pi_rein')
import rospy
import sys
from std_msgs.msg import String
from pi_rein.msg import *
from pi_rein.srv import *
import message_filters
class TestSync:
def __init__(self):
rospy.init_node("test_pub")
self.test_pub = rospy.Publisher("detections", DetectionArray)
while not rospy.is_shutdown():
msg = DetectionArray()
msg.header.stamp = rospy.Time.now()
self.test_pub.publish(msg)
rospy.sleep(0.033)
def main(args):
try:
TestSync()
except KeyboardInterrupt:
print "Shutting down test pub node."
if __name__ == '__main__':
main(sys.argv)
Here is the test_sync.py script:
#!/usr/bin/env python
""" test_sync.py - Version 0.1 2011-06-10
"""
import roslib
roslib.load_manifest('pi_rein')
import ...