About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

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

Python Code

#!/usr/bin/env python
"""OpenCV feature detectors with ros CompressedImage Topics in python.

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.
"""
__author__ =  'Simon Haller <simon.haller at uibk.ac.at>'
__version__=  '0.1'
__license__ = 'BSD'
# Python libs
import sys, time

# numpy and scipy
import numpy as np
from scipy.ndimage import filters

# OpenCV
import cv2

# Ros libraries
import roslib
import rospy

# Ros Messages
from sensor_msgs.msg import CompressedImage
# We do not use cv_bridge it does not support CompressedImage in python
# from cv_bridge import CvBridge, CvBridgeError

VERBOSE=False

class image_feature:

    def __init__(self):
        '''Initialize ros publisher, ros subscriber'''
        # topic where we publish
        self.image_pub = rospy.Publisher("/output/image/compressed",CompressedImage)
        # self.bridge = CvBridge()

        # subscribed Topic
        self.subscriber = rospy.Subscriber("/camera/image/compressed", CompressedImage, self.callback)
        if VERBOSE :
            print "subscribed to /camera/image/compressed"


    def callback(self, ros_data):
        '''Callback function of subscribed topic. 
        Here images get converted and features detected'''
        if VERBOSE :
            print 'received image of type: "%s"' % ros_data.format

        #### direct conversion to CV2 ####
        np_arr = np.fromstring(ros_data.data, np.uint8)
        image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
        
        #### Feature detectors using CV2 #### 
        # "","Grid","Pyramid" + 
        # "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF"
        method = "GridFAST"
        feat_det = cv2.FeatureDetector_create(method)
        time1 = time.time()

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

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

        #### Create CompressedIamge ####
        msg = CompressedImage()
        msg.header.stamp = rospy.Time.now()
        msg.format = "jpeg"
        msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
        # Publish new image
        self.image_pub.publish(msg)
        
        #self.subscriber.unregister()

def main(args):
    '''Initializes and cleanup ros node'''
    ic = image_feature()
    rospy.init_node('image_feature', anonymous=True)
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down ROS Image feature detector module"
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

The Code Explained

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

Include Lines

# Python libs
import sys, time
# numpy and scipy
import numpy as np
from scipy.ndimage import filters

# OpenCV
import cv2

# Ros libraries
import roslib
import rospy

# Ros Messages
from sensor_msgs.msg import CompressedImage

Time is included to measure the time for feature detection. Numpy, 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.

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

class image_feature:

    def __init__(self):
    ...
    
    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

    def __init__(self):
        '''Initialize ros publisher, ros subscriber'''
        # topic where we publish
        self.image_pub = rospy.Publisher("/output/image/compressed",CompressedImage)
        # self.bridge = CvBridge()

        # subscribed Topic
        self.subscriber = rospy.Subscriber("/camera/image/compressed", CompressedImage, self.callback)
        if VERBOSE :
            print "subscribed to /camera/image/compressed"

During initialisation a publisher gets created and 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:

#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
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).

#### Feature detectors using CV2 #### 
# "","Grid","Pyramid" + 
# "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF"
method = "GridFAST"
feat_det = cv2.FeatureDetector_create(method)
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.

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

cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY) - converts the image into a grayscale image - the feature detection algorithms do not take color images. The detection geths started with the grayscale image. In VERBOSE mode the time for detection and the amount of feat points get printed to the commandline.

for featpoint in featPoints:
    x,y = featpoint.pt
    cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1)
        
cv2.imshow('cv_img', image_np)
cv2.waitKey(2)
}}
Lets draw a circle around every detected point on the color image and show the image.

{{{
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
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.

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

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