Let's look at how we can interface and work with the Kinect sensor in ROS. ROS is bundled with OpenNI driver, which can fetch RGB and the depth image of Kinect. This package can be used for Microsoft Kinect, PrimeSense Carmine, Asus Xtion Pro, and Pro Live.
This driver mainly publishes raw depth, RGB, and IR image streams. The openni_launch package will install packages such as openni_camera and openni_launch. The openni_camera package is the Kinect driver that publishes raw data and sensor information, whereas the openni_launch package contains ROS launch files. It's basically an XML file that launches multiple nodes at a time and publishes data such as point clouds.
The following command will open the OpenNI device and load all nodelets to convert raw depth/RGB/IR streams to depth images, disparity images, and point clouds. The ROS nodelet package is designed to provide a way to run multiple algorithms in the same process with zero copy transport between algorithms.
$ roslaunch openni_launch openni.launch
You can view the RGB image using a ROS tool called image_view
$ rosrun image_view image_view image:=/camera/rgb/image_color
In the next section, we will see how to interface these images to OpenCV for image processing.
ROS is integrated into many libraries. OpenCV is also integrated into ROS mainly for image processing. The vision_opencv ROS stack includes the complete OpenCV library and interface to ROS.
The vision_opencv provides several packages:
cv_bridge: This contains theCvBridgeclass; this class converts from ROS image messages to OpenCV image data type and vice versaimage_geometry: This contains a collection of methods to handle image and pixel geometry
The following diagram shows how OpenCV is interfaced to ROS:
OpenCV-ROS interfacing
The image data type of OpenCV are IplImage and Mat. If we want to work with OpenCV in ROS, we have to convert IplImage or Mat to ROS Image messages. The ROS package vision_opencv has the CvBridge class; this class can convert IplImage to ROS image and vice versa.
The following section shows how to create a ROS package; this package contains node to subscribe RGB, depth image, process the RGB image to detect edges, and display all images after converting to an image type equivalent to OpenCV.
We can create a package called sample_opencv_pkg with the following dependencies, that is, sensor_msgs, cv_bridge, rospy, and std_msgs. The sensor_msgs dependency defines messages for commonly used sensors, including cameras and scanning laser rangefinders; cv_bridge is the OpenCV interface of ROS.
The following command will create the ROS package with the preceding dependencies:
$ catkin-create-pkg sample_opencv_pkg sensor_msgs cv_bridge rospy std_msgs
After creating the package, create a scripts folder inside the package and save the code in the mentioned in the next section.
The first section of the Python code is given below. It mainly includes importing of rospy, sys, cv2, sensor_msgs, cv_bridge, and the numpy module. The sensor_msgs dependency imports the ROS data type of Image and
CameraInfo. The cv_bridge module imports the CvBridge class for converting ROS image data type to the OpenCV data type and vice versa:
import rospy import sys import cv2 import cv2.cv as cv from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge, CvBridgeError import numpy as np
The following section of code is a class definition in Python to demonstrate CvBridge functions. The class is named as cvBridgeDemo:
class cvBridgeDemo():
def __init__(self):
self.node_name = "cv_bridge_demo"
#Initialize the ros node
rospy.init_node(self.node_name)
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.node_name
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
# And one for the depth image
cv.NamedWindow("Depth Image", cv.CV_WINDOW_NORMAL)
cv.MoveWindow("Depth Image", 25, 350)
# Create the cv_bridge object
self.bridge = CvBridge()
# Subscribe to the camera image and depth topics and set
# the appropriate callbacks
self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image, self.image_callback)
self.depth_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback)
rospy.loginfo("Waiting for image topics...")The following code gives a callback function of the color image from Kinect. When a color image comes on the /camera/rgb/image_color topic, it will call this function. This function will process the color frame for edge detection and show the edge detected and raw color image:
def image_callback(self, ros_image):
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
frame = self.bridge.imgmsg_to_cv(ros_image, "bgr8")
except CvBridgeError, e:
print e
# Convert the image to a Numpy array since most cv2 functions
# require Numpy arrays.
frame = np.array(frame, dtype=np.uint8)
# Process the frame using the process_image() function
display_image = self.process_image(frame)
# Display the image.
cv2.imshow(self.node_name, display_image)
# Process any keyboard commands
self.keystroke = cv.WaitKey(5)
if 32 <= self.keystroke and self.keystroke < 128:
cc = chr(self.keystroke).lower()
if cc == 'q':
# The user has press the q key, so exit
rospy.signal_shutdown("User hit q key to quit.")The following code gives a callback function of the depth image from Kinect. When a depth image comes on the /camera/depth/raw_image topic, it will call this function. This function will show the raw depth image:
def depth_callback(self, ros_image):
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
# The depth image is a single-channel float32 image
depth_image = self.bridge.imgmsg_to_cv(ros_image, "32FC1")
except CvBridgeError, e:
print e
# Convert the depth image to a Numpy array since most cv2 functions
# require Numpy arrays.
depth_array = np.array(depth_image, dtype=np.float32)
# Normalize the depth image to fall between 0 (black) and 1 (white)
cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
# Process the depth image
depth_display_image = self.process_depth_image(depth_array)
# Display the result
cv2.imshow("Depth Image", depth_display_image)The following function is called process_image(), which will convert the color image to grayscale, then blur the image, and find the edges using the canny edge filter:
def process_image(self, frame):
# Convert to grayscale
grey = cv2.cvtColor(frame, cv.CV_BGR2GRAY)
# Blur the image
grey = cv2.blur(grey, (7, 7))
# Compute edges using the Canny edge filter
edges = cv2.Canny(grey, 15.0, 30.0)
return edgesThe following function is called process_depth_image(). It simply returns the depth frame:
def process_depth_image(self, frame):
# Just return the raw image for this demo
return frameThis function will close the image window when the node shuts down:
def cleanup(self):
print "Shutting down vision node."
cv2.destroyAllWindows()The following code is the main() function. It will initialize the cvBridgeDemo() class and call the ros spin() function:
def main(args):
try:
cvBridgeDemo()
rospy.spin()
except KeyboardInterrupt:
print "Shutting down vision node."
cv.DestroyAllWindows()
if __name__ == '__main__':
main(sys.argv)Save the preceding code to cv_bridge_demo.py and change the permission of the node using the following command. The node is only visible to the rosrun command if we give it executable permission.
$ chmod +X cv_bridge_demo.py
The following are the commands to start the driver and node. Start the Kinect driver using the following command:
$ roslaunch openni_launch openni.launch
Run the node using the following command:
$ rosrun sample_opencv_pkg cv_bridge_demo.py
The following is the screenshot of the output:
RGB, depth, and edge images