lag in image saving / subscribing
Hey guys. I am having troblues in saving the last image from bebeop 2 parrot drone on my laptop. I run ROS Kinetic, Ubuntu 16.04 OS and use bebeop_autonomy driver.
In the following code I wish to save an Image only after the camera completes the action of moving the camera down.
#!/usr/bin/env python
from __future__ import print_function
import sys
import rospy
import cv2
import numpy as np
import time
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from std_msgs.msg import Int16 # For error/angle plot publishing
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
class image_converter:
def __init__(self):
self.bridge = CvBridge()
self.pub_camdown = rospy.Publisher('/bebop/camera_control', Twist, queue_size=1)
self.image_sub = rospy.Subscriber('/bebop/image_raw', Image, self.callback)
def cam_down(self):
cam = Twist()
cam.angular.y = -50.0
self.pub_camdown.publish(cam)
def callback(self, msg):
rospy.sleep(1)
try:
# Convert your ROS Image message to OpenCV2
cv2_img = self.bridge.imgmsg_to_cv2(msg, "bgr8") # cv2_img = CvBridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError, e:
print(e)
else:
time = msg.header.stamp
saved_img = cv2.imwrite(''+str(time)+'.jpeg', cv2_img)
cv2.imshow("Image window", cv2_img)
cv2.waitKey(1000)
def main(args):
rospy.init_node('image_converter', anonymous=True)
ic = image_converter()
time.sleep(1)
ic.cam_down()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
The callback function runs 10 times before saving the required images (the image after moving down the camera) Any ideas that can fix it?
Changes I tried that did not work:
- setting
Queue_size=1
orQueue_size=10
to the line of subscribing the image, - using different times in
rospy.sleep(1)
in the callback function results in the same number of iterations before getting the wanted image. - using different times in
time.sleep(1)
in the main function results the same.
Just a quick comment: don't do any of this time-based. It's not robust and never will be.
I'd recommend to use the appropriate topics from here to make this state-based: send the command to move the camera and monitor the camera's position. Then based on that information, determine whether you can capture images.
And an observation (but not something you can do anything about): that driver appears to use
geometry_msgs/Twist
to encode absolute angles. That is really not what that message type was designed for. It's a nasty violation of the interface.