| 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 Solution
Description: 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
Contents
Description
This node is an example of how to perform image processing with OpenCV in a ROS node
The Code
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
73 self.segmented_img_pub.publish(masked_image_msg)
Publish the image
