|Note: This tutorial assumes that you have completed the previous tutorials: building a ROS package.|
|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.|
Understanding the Example SolutionDescription: This tutorial walks through the code of the servicesim example solution detailing the ROS interfaces used and the state machine of the competition.
Tutorial Level: BEGINNER
This node is an example of how to perform image processing with OpenCV in a ROS node
The code is available on the servicesim bitbucket repository. Here is the full content of the file, we will go through it line by line below.
1 #! /usr/bin/env python 2 3 # Copyright (C) 2018 Open Source Robotics Foundation, Inc. 4 # 5 # Licensed under the Apache License, Version 2.0 (the "License"); 6 # you may not use this file except in compliance with the License. 7 # You may obtain a copy of the License at 8 # 9 # http://www.apache.org/licenses/LICENSE-2.0 10 # 11 # Unless required by applicable law or agreed to in writing, software 12 # distributed under the License is distributed on an "AS IS" BASIS, 13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 # See the License for the specific language governing permissions and 15 # limitations under the License. 16 17 import cv2 18 19 from cv_bridge import CvBridge, CvBridgeError 20 21 import numpy 22 23 import rospy 24 25 from sensor_msgs.msg import Image 26 27 28 class ColorSegmentator(object): 29 def __init__(self): 30 # create an instance of CVBridge, this will allow to convert ROS messages 31 # to OpenCV CVMat and vice versa 32 self.bridge = CvBridge() 33 # create a publisher to publish the modified image to 34 self.segmented_img_pub = rospy.Publisher( 35 '/servicebot/camera_front/segmented_image', Image, queue_size=1) 36 # create a subscriber to receive the robots front camera image 37 self.image_sub = rospy.Subscriber( 38 '/servicebot/camera_front/image_raw', Image, self.image_callback) 39 # define a range of color to detect guest shirts 40 self.boundaries = [ 41 # keep only dark red pixels 42 # ([50, 17, 10], [200, 50, 56]), 43 # keep only green pixels 44 # ([10, 40, 5], [200, 50, 30]), 45 # keep only blue pixels 46 ([5, 5, 50], [25, 25, 145]) 47 ] 48 49 def image_callback(self, msg): 50 # Function called each time an image is received from the robot 51 try: 52 # convert the image to a CVMat 53 cv_image = self.bridge.imgmsg_to_cv2(msg, 'rgb8') 54 except CvBridgeError as e: 55 rospy.logerr(e) 56 return 57 for (lower, upper) in self.boundaries: 58 # create NumPy arrays from the boundaries 59 lower = numpy.array(lower, dtype='uint8') 60 upper = numpy.array(upper, dtype='uint8') 61 62 # find the colors within the specified boundaries and create a mask out of it 63 mask = cv2.inRange(cv_image, lower, upper) 64 # apply the mask to the image 65 output = cv2.bitwise_and(cv_image, cv_image, mask=mask) 66 try: 67 # convert the resulting image to a ros Image message 68 masked_image_msg = self.bridge.cv2_to_imgmsg(output, 'rgb8') 69 except CvBridgeError as e: 70 rospy.logerr(e) 71 return 72 # publish the resulting image 73 self.segmented_img_pub.publish(masked_image_msg) 74 75 76 def blob_detector(): 77 node = ColorSegmentator() 78 rospy.init_node('blob_detector') 79 rospy.spin() 80 81 82 if __name__ == '__main__': 83 blob_detector()
The Code Explained
Now, let's break the code down.
Importing the python modules
17 import cv2
Import the openCV python module
Import cv_bridge, this module will allow us to convert ROS messages to OpenCV data structures easily We will use numpy to perform mathematical operations on our data structures
The competition example class
28 class ColorSegmentator(object):
Define the class where we will implement our ROS Node
Initialisation of the class
First we create a CVBridge instance that we will use for bridging our images between OpenCV and ROS.
Create a ROS Image publisher to publish the image modified after the processing.
Create a ROS Image subscriber to receive the raw Images from the robot's camera.
Here we define color ranges for the 3 shirt colors the competition guests wear. The left most list describes the minimal RGB values of a pixel to be kept in the image, the right list represent the highest ones
49 def image_callback(self, msg):
Define a callback function that will be called every time an image is received. While this function is going to perform very simple image processing operation, it can be modified to add arbitrarily complex image processing
53 cv_image = self.bridge.imgmsg_to_cv2(msg, 'rgb8')
First we convert the ROS Image message to an OpnCV Image
63 mask = cv2.inRange(cv_image, lower, upper)
Compute the pixels that are in the range of color defined in self.boundaries
65 output = cv2.bitwise_and(cv_image, cv_image, mask=mask)
Modify the image to keep only the pixels in range and make all the other pixels black
68 masked_image_msg = self.bridge.cv2_to_imgmsg(output, 'rgb8')
Convert the resulting image into a ROS Image message
Publish the image