####################################
##FILL ME IN
####################################
## title = 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.
## level= IntermediateCategory
## keywords = CompressedImage, OpenCV, Publisher, Subscriber
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TOC(4)>>

##startpage 
== Python Code ==
{{{#!python
#!/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_raw/compressed",
            CompressedImage)
        # self.bridge = CvBridge()

        # subscribed Topic
        self.subscriber = rospy.Subscriber("/camera/image/compressed",
            CompressedImage, self.callback,  queue_size = 1)
        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...

== Shebang ==
{{{#!python
#!/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 ==

{{{#!python
# 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, 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.

{{{#!python
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 ==

{{{#!python
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 ==
{{{#!python
def __init__(self):
    '''Initialize ros publisher, ros subscriber'''
    # topic where we publish
    self.image_pub = rospy.Publisher("/output/image_raw/compressed",
        CompressedImage)
    # self.bridge = CvBridge()

    # subscribed Topic
    self.subscriber = rospy.Subscriber("/camera/image/compressed",
        CompressedImage, self.callback,  queue_size = 1)
    if VERBOSE :
        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 ===
{{{#!python
#### 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). 

=== Select and create a feature detector ===
{{{#!python
#### 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.

{{{#!python
# 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)
}}}
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.

{{{#!python
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 a compressed image to publish ===
{{{#!python
#### 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.

{{{#!python
# Publish new image
self.image_pub.publish(msg)
}}}
To publish use the method publish from the rospy.Publisher.


##endpage