Rospy problem with time synchronizer message filter

edit

Hi all,

I'm writing a simple python node that has to listen to the RGB and DEPTH image topics to do its stuff. So, by following these tutorials:

http://wiki.ros.org/rospy_tutorials/T...

http://wiki.ros.org/cv_bridge/Tutoria...

http://wiki.ros.org/message_filters

I ended up with this script:

#!/usr/bin/env python

from __future__ import print_function

import roslib
roslib.load_manifest('benchmarking_simulator')
import sys
import rospy
import cv2

from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import message_filters
from sensor_msgs.msg import Image

class image_subscriber:

  def __init__(self):
    self.image_pub = rospy.Publisher("image_topic_2",Image,queue_size=10)

    self.bridge = CvBridge()

    self.depth_image_sub = message_filters.Subscriber('depth_image', Image)
    self.rgb_image_sub = message_filters.Subscriber('rgb_image', Image)

    self.ts = message_filters.ApproximateTimeSynchronizer([self.depth_image_sub,self.rgb_image_sub],10,0.3)
    self.ts.registerCallback(self.callback)

  def callback(self,depth_data,rgb_data):
    try:
      cv_image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
    except CvBridgeError as e:
      print(e)


    cv2.imshow("Image window", cv_image)
    cv2.waitKey(3)

    try:
      self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
    except CvBridgeError as e:
      print(e)

def main(args):
  im_sub = image_subscriber()
  rospy.init_node('image_subscriber', anonymous=True)
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")
  cv2.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

What happens is that, when I run the node a window is open showing an image (as it is supposed to do) but afterwards, instead of showing the next image, it gets freezed on the same image.

Please, can someone explain me what is happening?

Thanks,

Federico

EDIT:

After some random trials I found out that the call to waitKey was causing the freeze. By setting:

cv2.waitKey(50)

the problem is gone.

But if I comment that line, I get:

[ERROR] [1498050010.200561]: bad callback: <bound method Subscriber.callback of <message_filters.Subscriber object at 0x7fb75af795d0>>
Traceback (most recent call last):
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/__init__.py", line 75, in callback
    self.signalMessage(msg)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/__init__.py", line 57, in signalMessage
    cb(*(msg + args))
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/__init__.py", line 287, in add
    self.signalMessage(*msgs)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/__init__.py", line 57, in signalMessage
    cb(*(msg + args))
  File "/home/dede/workspaces/catkin/src/benchmarking_simulator/scripts/image_subscriber.py", line 42, in callback
    self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 878, in publish
    raise ROSException("ROS node has not been initialized yet. Please call init_node() first")
ROSException: ROS node has not been initialized yet. Please call init_node() first

Please, can someone explain me what is happening?

Thanks,

Federico