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!
↧