A strange behavior of rospy.wait_for_message

asked 2017-07-03 12:34:04 -0500

wy3 gravatar image

I write a node which provide image saving service during object manipulation. Then I call the service in a loop and it saves image in each loop after some manipulation. The script is as follow.

import rospy
import numpy as np

from sensor_msgs.msg import Image, CompressedImage
from project_gazebo.srv import imageSave

from cv_bridge import CvBridge, CvBridgeError
import cv2

class project_image_saver(object):
    def __init__(self):
        self.bridge = CvBridge()
        self.s = rospy.Service('/model_image_saver/image_save',
                               imageSave,
                               self.handle_image_save)

def handle_image_save(self, req):
    try:
        image_msg = rospy.wait_for_message(
            '/my_gripper/camera1/rgb/image_raw/compressed',
            CompressedImage,
            0.075)

        cv2_img = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")

    except (rospy.ROSException, CvBridgeError), e:
        print e
        success = False

    else:
        cv2.imwrite(req.path+'img'+'%04d'%req.number+'.jpg', cv2_img)
        success = True
    finally:
        return {'success':success}

if __name__ == '__main__':
    rospy.init_node('model_image_saver')
    image_saver = poke_image_saver()
    rospy.spin()

The problem is that the first two messages (images) are always the same in the first two iterations even though the scene totally changes. Then all images saved are delayed by one step comparing to the actual loop step. I change the subscribed topic to depth image topic and do the exact same thing. All depth images look distinct and in order.

It would be great if someone have a better idea on this. Thank you!

edit retag flag offensive close merge delete