Example 7
In this example, we will review the image_view ROS package and a Python script that captures an image from the RealSense camera.
BBegin by checking out the feature/upright_camera_view branch in the stretch_ros repository. The configuration of the camera results in the images being displayed sideways. Thus, this branch publishes a new topic that rotates the raw image upright.
cd ~/catkin_ws/src/stretch_ros/stretch_core
git checkout feature/upright_camera_view
# Terminal 1
roslaunch stretch_core stretch_driver.launch
# Terminal 2
roslaunch stretch_core d435i_low_resolution.launch
# Terminal 3
rosrun rviz rviz -d /home/hello-robot/catkin_ws/src/stretch_tutorials/rviz/perception_example.rviz
Capture Image with image_view
There are a couple of methods to save an image using the image_view package.
OPTION 1: Use the image_view
node to open a simple image viewer for ROS sensor_msgs/image topics.
# Terminal 4
rosrun image_view image_view image:=/camera/color/image_raw_upright_view
OPTION 2: Use the image_saver
node to save an image to the terminals current work directory.
# Terminal 4
rosrun image_view image_saver image:=/camera/color/image_raw_upright_view
Capture Image with Python Script
In this section, you can use a Python node to capture an image from the RealSense camera. Execute the capture_image.py node to save a .jpeg image of the image topic /camera/color/image_raw_upright_view.
# Terminal 4
cd ~/catkin_ws/src/stretch_tutorials/src
python capture_image.py
The Code
#!/usr/bin/env python
import rospy
import sys
import os
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
class CaptureImage:
"""
A class that converts a subscribed ROS image to a OpenCV image and saves
the captured image to a predefined directory.
"""
def __init__(self):
"""
A function that initializes a CvBridge class, subscriber, and save path.
:param self: The self reference.
"""
self.bridge = CvBridge()
self.sub = rospy.Subscriber('/camera/color/image_raw', Image, self.callback, queue_size=1)
self.save_path = '/home/hello-robot/catkin_ws/src/stretch_tutorials/stored_data'
def callback(self, msg):
"""
A callback function that converts the ROS image to a CV2 image and stores the
image.
:param self: The self reference.
:param msg: The ROS image message type.
"""
try:
image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
except CvBridgeError, e:
rospy.logwarn('CV Bridge error: {0}'.format(e))
file_name = 'camera_image.jpeg'
completeName = os.path.join(self.save_path, file_name)
cv2.imwrite(completeName, image)
rospy.signal_shutdown("done")
sys.exit(0)
if __name__ == '__main__':
rospy.init_node('capture_image', argv=sys.argv)
CaptureImage()
rospy.spin()
The Code Explained
Now let's break the code down.
#!/usr/bin/env python
import rospy
import sys
import os
import cv2
rospy
if you are writing a ROS Node. There are functions from sys, os, and cv2 that are required within this code. cv2 is a library of Python functions that implements computer vision algorithms. Further information about cv2 can be found here: OpenCV Python.
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
sensor_msgs.msg
is imported so that we can subscribe to ROS Image
messages. Import CvBridge to convert between ROS Image
messages and OpenCV images.
def __init__(self):
"""
A function that initializes a CvBridge class, subscriber, and save path.
:param self: The self reference.
"""
self.bridge = CvBridge()
self.sub = rospy.Subscriber('/camera/color/image_raw', Image, self.callback, queue_size=1)
self.save_path = '/home/hello-robot/catkin_ws/src/stretch_tutorials/stored_data'
def callback(self, msg):
"""
A callback function that converts the ROS image to a cv2 image and stores the
image.
:param self: The self reference.
:param msg: The ROS image message type.
"""
try:
image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
except CvBridgeError as e:
rospy.logwarn('CV Bridge error: {0}'.format(e))
imgmsg_to_cv2()
function.
file_name = 'camera_image.jpeg'
completeName = os.path.join(self.save_path, file_name)
cv2.imwrite(completeName, image)
path.join()
function. Then use the imwrite()
function to save the image.
rospy.signal_shutdown("done")
sys.exit(0)
rospy.init_node('capture_image', argv=sys.argv)
CaptureImage()
rospy.init_node(NAME, ...)
, is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Instantiate the CaptureImage()
class.
rospy.spin()
Edge Detection
In this section, we highlight a node that utilizes the Canny Edge filter algorithm to detect the edges from an image and convert it back as a ROS image to be visualized in RViz. Begin by running the following commands.
# Terminal 4
cd ~/catkin_ws/src/stretch_tutorials/src
python edge_detection.py
The Code
#!/usr/bin/env python
import rospy
import sys
import os
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
class EdgeDetection:
"""
A class that converts a subscribed ROS image to a OpenCV image and saves
the captured image to a predefined directory.
"""
def __init__(self):
"""
A function that initializes a CvBridge class, subscriber, and other
parameter values.
:param self: The self reference.
"""
self.bridge = CvBridge()
self.sub = rospy.Subscriber('/camera/color/image_raw', Image, self.callback, queue_size=1)
self.pub = rospy.Publisher('/image_edge_detection', Image, queue_size=1)
self.save_path = '/home/hello-robot/catkin_ws/src/stretch_tutorials/stored_data'
self.lower_thres = 100
self.upper_thres = 200
rospy.loginfo("Publishing the CV2 Image. Use RViz to visualize.")
def callback(self, msg):
"""
A callback function that converts the ROS image to a CV2 image and goes
through the Canny Edge filter in OpenCV for edge detection. Then publishes
that filtered image to be visualized in RViz.
:param self: The self reference.
:param msg: The ROS image message type.
"""
try:
image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
except CvBridgeError as e:
rospy.logwarn('CV Bridge error: {0}'.format(e))
image = cv2.Canny(image, self.lower_thres, self.upper_thres)
image_msg = self.bridge.cv2_to_imgmsg(image, 'passthrough')
image_msg.header = msg.header
self.pub.publish(image_msg)
if __name__ == '__main__':
rospy.init_node('edge_detection', argv=sys.argv)
EdgeDetection()
rospy.spin()
The Code Explained
Since that there are similarities in the capture image node, we will only breakdown the different components of the edge detection node.
self.lower_thres = 100
self.upper_thres = 200
image = cv2.Canny(image, self.lower_thres, self.upper_thres)
image_msg = self.bridge.cv2_to_imgmsg(image, 'passthrough')
image_msg.header = msg.header
self.pub.publish(image_msg)