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 Publisher

Description: 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

Contents

1. Python Code
The Code Explained

Python Code

   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         
  59         #### Feature detectors using CV2 #### 
  60         # "","Grid","Pyramid" + 
  61         # "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF"
  62         method = "GridFAST"
  63         feat_det = cv2.FeatureDetector_create(method)
  64         time1 = time.time()
  65 
  66         # convert np image to grayscale
  67         featPoints = feat_det.detect(
  68             cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY))
  69         time2 = time.time()
  70         if VERBOSE :
  71             print '%s detector found: %s points in: %s sec.'%(method,
  72                 len(featPoints),time2-time1)
  73 
  74         for featpoint in featPoints:
  75             x,y = featpoint.pt
  76             cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1)
  77         
  78         cv2.imshow('cv_img', image_np)
  79         cv2.waitKey(2)
  80 
  81         #### Create CompressedIamge ####
  82         msg = CompressedImage()
  83         msg.header.stamp = rospy.Time.now()
  84         msg.format = "jpeg"
  85         msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
  86         # Publish new image
  87         self.image_pub.publish(msg)
  88         
  89         #self.subscriber.unregister()
  90 
  91 def main(args):
  92     '''Initializes and cleanup ros node'''
  93     ic = image_feature()
  94     rospy.init_node('image_feature', anonymous=True)
  95     try:
  96         rospy.spin()
  97     except KeyboardInterrupt:
  98         print "Shutting down ROS Image feature detector module"
  99     cv2.destroyAllWindows()
 100 
 101 if __name__ == '__main__':
 102     main(sys.argv)

The Code Explained

Now, let's break down the code...

Shebang

   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.

Include Lines

   1 # Python libs
   2 import sys, time
   3 # numpy and scipy
   4 import numpy as np
   5 from scipy.ndimage import filters
   6 
   7 # OpenCV
   8 import cv2
   9 
  10 # Ros libraries
  11 import roslib
  12 import rospy
  13 
  14 # Ros Messages
  15 from sensor_msgs.msg import CompressedImage

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)

Class definition

   1 class image_feature:
   2 
   3     def __init__(self):
   4     ...
   5     
   6     def callback(self, ros_data):

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

   1 #### direct conversion to CV2 ####
   2 np_arr = np.fromstring(ros_data.data, np.uint8)
   3 image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)

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

   1 #### Feature detectors using CV2 #### 
   2 # "","Grid","Pyramid" + 
   3 # "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF"
   4 method = "GridFAST"
   5 feat_det = cv2.FeatureDetector_create(method)
   6 time1 = time.time()

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.

   1 # convert np image to grayscale
   2 featPoints = feat_det.detect(cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY))
   3 time2 = time.time()
   4 if VERBOSE :
   5     print '%s detector found: %s featpoints in: %s sec.' %(method, 
   6         len(featPoints),time2-time1)

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.

   1 for featpoint in featPoints:
   2     x,y = featpoint.pt
   3     cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1)
   4         
   5 cv2.imshow('cv_img', image_np)
   6 cv2.waitKey(2)

Lets draw a circle around every detected point on the color image and show the image.

Create a compressed image to publish

   1 #### Create CompressedIamge ####
   2 msg = CompressedImage()
   3 msg.header.stamp = rospy.Time.now()
   4 msg.format = "jpeg"
   5 msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()

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.

   1 # Publish new image
   2 self.image_pub.publish(msg)

To publish use the method publish from the rospy.Publisher.

ROS 2 Documentation

Wiki

Page

User

Python CompressedImage Subscriber Publisher

Python Code

The Code Explained

Shebang

Include Lines

Class definition

The init method

The callback method

Converting the compressed image to cv2

Select and create a feature detector

Create a compressed image to publish

ROS 2 Documentation

Wiki

Page

User

Python CompressedImage Subscriber Publisher

Python Code

The Code Explained

Shebang

Include Lines

Class definition

The __init__ method

The callback method

Converting the compressed image to cv2

Select and create a feature detector

Create a compressed image to publish

The init method