Tutorials/CloudSim/notebook camcorder


 * 1) ROS Camcorder

This tutorial shows how to write a python program that subscribes to camera images and saves a snapshot to disk. This small program can be used on a variety of robots (PR2, tutlebot, Atlas). In ROS, camera images are sent as messages in streams that nodes can subscribe to. In this tutorial, we create a Python node that subscribes to jpeg images and shows them.


 * 1) Prerequisites

This tutorial assumes you have completed the introduction to iPython Notebook.

Before creating new tasks make sure you have stopped any previously running tasks.


 * 1) Create the task

First create a task using CloudSim. Any world from the VRC tasks is fine; for this tutorial we'll use VRC task 3 and grab images from Atlas' multisense camera. Go to the CloudSim console, create a task with these parameters:


 * Task title : `Grab Snapshot`
 * ROS package : `drcsim_gazebo` previous drcsim version used "atlas_utils"
 * Launch file : `vrc_task_3.launch`



Start the simulation once the task is created.


 * 1) The World

Start the `WebGL interface` and open the `3D View` in a new tab. You see the the world as shown below:




 * 1) Subscribing to image topics

Start `Python Notebook` in a new tab, select `New Notebook` and get ready to code!

First, initialize ROS:

import os
 * 1) python stuff

from sensor_msgs.msg import CompressedImage import rospy
 * 1) ROS imports

rospy.init_node('camcorder')

print "ROS node initialized"

We'll then create a ROS topic subscriber which will subscribe ROS topics. The Subscriber object takes callback function which will be executed once a message is received.

This class below contains a subscriber that subscribes a topic of type `CompressedImage`. Once a message it received, it then unsubscribes from the topic and saves the image data to disk.

class SingleSubscription(object): """   Subscribe to a single ROS camera message,    saves it to disk and dissapears    """ def snapshot(self, rospath, dest_path): """       Subscribe to the ROS topic and prepare to        save the incomming image to disk in the dest_dir directory        """ # flag to indicate when the image has been saved self.done = False self.path = os.path.abspath(dest_path) # subscribe to ROS topic self.subscriber = rospy.Subscriber(rospath, CompressedImage, self.callback)

def callback(self, ros_msg): """        This method is invoked each time a new ROS message is generated.        the message is of type CompressedImage, with data and format        """ print 'received image of type: "%s"' % ros_msg.format # we don't need to be called again self.subscriber.unregister print "writing %s" % self.path # create directories if necessary dest_dir = os.path.split(self.path)[0] if not os.path.exists(dest_dir): os.makedirs(dest_dir) # write image to disk with open(self.path, 'w') as f:           f.write(ros_msg.data) self.done = True Create a `snapshot` function that makes use of the `SingleSubscription` class above. The function will wait until a message has been received or when timed out. def snapshot(rospath, dst_path='./snapshot.jpg'): """   Subscribes to the rospath ROS topic and saves an image in     the dst_path directory    """

print "subscribing to %s" % rospath sub = SingleSubscription sub.snapshot(rospath, dst_path) print "waiting" for i in range(10): if sub.done: print "snapshot complete" return sub.path break rospy.rostime.wallsleep(0.5) print "retry %s/10" % (i+1) print "Timeout!"

The iPython Notebook can display images using the matplotlib module. This allows us to inspect the saved image on the web.

import matplotlib import matplotlib.pyplot import matplotlib.image import numpy as np
 * 1) use matplotlib to display images in the notebook

def show(img_path): """   Displays a png or jpeg file from disk    """ img = matplotlib.image.imread(img_path) # check for grayscale images cmap_ = cm.get_cmap if len(img.shape) == 2: cmap_ = cm.Greys_r # by default, the image will appear upside down. # flipup and setting the origin will make it look right np.flipud(img) matplotlib.pyplot.imshow(img, origin='lower', cmap=cmap_)

Ok, we're almost ready take a snapshot, but first we need to know what image topics are being published. Similar to `rostopic list`, the rospy module contains methods to enumerate all topics and their respective type. We can then look up topics of specific types that we're interested in, in this case, we will subscribe to topics with the `CompressedImage` message type.

def get_typed_topics(topic_type = 'sensor_msgs/CompressedImage'): """   Lists all topics of a given types    """ # get a list of tuples that contain the topic name and its type topics = [x[0] for x in rospy.get_published_topics if x[1] == topic_type] # filter out the depth messages topics = [x for x in topics if x.find('compressedDepth') == -1] return topics

Call it to get a list of compressed image topics.

get_typed_topics

An example output is:

 ['/sandia_hands/l_hand/camera/right/image_color/compressed', '/sandia_hands/r_hand/camera/left/image_rect_color/compressed', '/multisense_sl/camera/left/image_rect/compressed', '/sandia_hands/r_hand/camera/right/image_mono/compressed', '/multisense_sl/camera/left/image_color/compressed', '/sandia_hands/l_hand/camera/left/image_mono/compressed', '/sandia_hands/l_hand/camera/right/image_rect_color/compressed', '/sandia_hands/r_hand/camera/left/image_color/compressed', '/multisense_sl/camera/right/image_raw/compressed', '/sandia_hands/r_hand/camera/right/image_rect/compressed', '/multisense_sl/camera/right/image_rect_color/compressed', '/sandia_hands/r_hand/camera/right/image_color/compressed', '/multisense_sl/camera/right/image_rect/compressed', '/sandia_hands/r_hand/camera/right/image_rect_color/compressed', '/sandia_hands/l_hand/camera/right/image_rect/compressed', '/sandia_hands/r_hand/camera/left/image_mono/compressed', '/sandia_hands/l_hand/camera/left/image_rect_color/compressed', '/multisense_sl/camera/left/image_raw/compressed', '/sandia_hands/l_hand/camera/right/image_raw/compressed', '/sandia_hands/r_hand/camera/left/image_raw/compressed', '/sandia_hands/r_hand/camera/left/image_rect/compressed', '/sandia_hands/l_hand/camera/left/image_rect/compressed', '/sandia_hands/r_hand/camera/right/image_raw/compressed', '/sandia_hands/l_hand/camera/left/image_color/compressed', '/multisense_sl/camera/left/image_mono/compressed', '/multisense_sl/camera/left/image_rect_color/compressed', '/multisense_sl/camera/right/image_mono/compressed', '/sandia_hands/l_hand/camera/left/image_raw/compressed', '/multisense_sl/camera/right/image_color/compressed', '/sandia_hands/l_hand/camera/right/image_mono/compressed']

Let's select a compressed image from the robot's camera and take a snapshot of it!

Make sure the topic exists for your version of the drcsim package path = snapshot('/multisense_sl/camera/left/image_raw/compressed', 'snapshot.jpg') show(path)
 * 1) do it!

You should see an image similar to the screenshot below:




 * 1) Troubleshooting

If you're not receiving any images from the image topic and keep seeing retry failures, try restarting the ipython notebook kernel. Then, execute the code again in the correct order.


 * 1) What's next

Using the techniques discussed here, it is also possible to process images and republish results to a new ROS topic. This is a common ROS pattern that can help you create a robust perception pipeline.

Notebook: Color object detection.
 * 1) Next ##