|Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.|
Python CompressedImage Subscriber PublisherDescription: This example subscribes to a ros topic containing sensor_msgs::CompressedImage. It converts the CompressedImage into a numpy.ndarray, then detects and marks features in that image. It finally displays and publishes the new image - again as CompressedImage topic.
Keywords: CompressedImage, OpenCV, Publisher, Subscriber
Tutorial Level: INTERMEDIATE
1 #!/usr/bin/env python 2 """OpenCV feature detectors with ros CompressedImage Topics in python. 3 4 This example subscribes to a ros topic containing sensor_msgs 5 CompressedImage. It converts the CompressedImage into a numpy.ndarray, 6 then detects and marks features in that image. It finally displays 7 and publishes the new image - again as CompressedImage topic. 8 """ 9 __author__ = 'Simon Haller <simon.haller at uibk.ac.at>' 10 __version__= '0.1' 11 __license__ = 'BSD' 12 # Python libs 13 import sys, time 14 15 # numpy and scipy 16 import numpy as np 17 from scipy.ndimage import filters 18 19 # OpenCV 20 import cv2 21 22 # Ros libraries 23 import roslib 24 import rospy 25 26 # Ros Messages 27 from sensor_msgs.msg import CompressedImage 28 # We do not use cv_bridge it does not support CompressedImage in python 29 # from cv_bridge import CvBridge, CvBridgeError 30 31 VERBOSE=False 32 33 class image_feature: 34 35 def __init__(self): 36 '''Initialize ros publisher, ros subscriber''' 37 # topic where we publish 38 self.image_pub = rospy.Publisher("/output/image_raw/compressed", 39 CompressedImage) 40 # self.bridge = CvBridge() 41 42 # subscribed Topic 43 self.subscriber = rospy.Subscriber("/camera/image/compressed", 44 CompressedImage, self.callback, queue_size = 1) 45 if VERBOSE : 46 print "subscribed to /camera/image/compressed" 47 48 49 def callback(self, ros_data): 50 '''Callback function of subscribed topic. 51 Here images get converted and features detected''' 52 if VERBOSE : 53 print 'received image of type: "%s"' % ros_data.format 54 55 #### direct conversion to CV2 #### 56 np_arr = np.fromstring(ros_data.data, np.uint8) 57 image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) 58 #image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0: 59 60 #### Feature detectors using CV2 #### 61 # "","Grid","Pyramid" + 62 # "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF" 63 method = "GridFAST" 64 feat_det = cv2.FeatureDetector_create(method) 65 time1 = time.time() 66 67 # convert np image to grayscale 68 featPoints = feat_det.detect( 69 cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY)) 70 time2 = time.time() 71 if VERBOSE : 72 print '%s detector found: %s points in: %s sec.'%(method, 73 len(featPoints),time2-time1) 74 75 for featpoint in featPoints: 76 x,y = featpoint.pt 77 cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1) 78 79 cv2.imshow('cv_img', image_np) 80 cv2.waitKey(2) 81 82 #### Create CompressedIamge #### 83 msg = CompressedImage() 84 msg.header.stamp = rospy.Time.now() 85 msg.format = "jpeg" 86 msg.data = np.array(cv2.imencode('.jpg', image_np)).tostring() 87 # Publish new image 88 self.image_pub.publish(msg) 89 90 #self.subscriber.unregister() 91 92 def main(args): 93 '''Initializes and cleanup ros node''' 94 ic = image_feature() 95 rospy.init_node('image_feature', anonymous=True) 96 try: 97 rospy.spin() 98 except KeyboardInterrupt: 99 print "Shutting down ROS Image feature detector module" 100 cv2.destroyAllWindows() 101 102 if __name__ == '__main__': 103 main(sys.argv)
The Code Explained
Now, let's break down the code...
1 #!/usr/bin/env python
The shebang (#!) should be used in every script (on Unix like machines). Use the full environment to look for the python interpreter.
Time is included to measure the time for feature detection. Numpy, scipy and cv2 are included to handle the conversions, the display and feature detection.
The ros libraries are standard for ros integration - additionally we need the CompressedImage from sensor_msgs.
1 VERBOSE = False
If you set this to True you will get some additional information printed to the commandline (feature detection method, number of points, time for detection)
Defines a class with two methods: The _init_ method defines the instantiation operation. It uses the "self" variable, which represents the instance of the object itself.
The callback method uses again "self" and a (compressed) image from the subscribed topic.
The __init__ method
1 def __init__(self): 2 '''Initialize ros publisher, ros subscriber''' 3 # topic where we publish 4 self.image_pub = rospy.Publisher("/output/image_raw/compressed", 5 CompressedImage) 6 # self.bridge = CvBridge() 7 8 # subscribed Topic 9 self.subscriber = rospy.Subscriber("/camera/image/compressed", 10 CompressedImage, self.callback, queue_size = 1) 11 if VERBOSE : 12 print "subscribed to /camera/image/compressed"
First the publisher gets created. The publishers topic should be of the form: image_raw/compressed - see http://wiki.ros.org/compressed_image_transport Section 4.
Further during initialisation the topic "/camera/image/compressed" gets subscribed (using the callback method of the newly created object).
The callback method
The first important lines in the callback method are:
Converting the compressed image to cv2
Here the compressedImage first gets converted into a numpy array. The second line decodes the image into a raw cv2 image (numpy.ndarray).
Select and create a feature detector
In the first line a feature detector is selected. The second line creates the detector with the selected method. Before the feature detection gets started remember the time.
The first line has two parts: cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY) - converts the image into a grayscale image - the feature detection algorithms do not take color images. The second part starts the detection with the fresh converted grayscale image.
In VERBOSE mode the time for detection and the amount of feat points get printed to the commandline.
Lets draw a circle around every detected point on the color image and show the image.
Create a compressed image to publish
First create a new compressedImage and set the three fields 'header', 'format' and 'data'. For data field encode the cv2 image to a jpg, generate an numpy array and convert it to a string.
To publish use the method publish from the rospy.Publisher.