ROS2 Camera Stream Project
Previously we got some basic insight on how to program ROS2 nodes and services. Now let us take a step further in ROS2 programming.
We will create a publisher node to capture a video stream from a camera and publish it to a topic.
We will also create a subscriber node who will subscribe to the camera stream topic and display it in an OpenCV window.
Install the OpenCV dependencies.
$ pip install opencv-python
Create a new ROS2 package with dependencies.
$ cd ~/ros2_ws/src
$ ros2 pkg create --build-type ament_python camera_stream --dependencies rclpy image_transport cv_bridge sensor_msgs std_msgs opencv-python
Create the camera_stream_publisher.py file and populate it with the following code:
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
class CameraStreamPub(Node):
"""
Create a CameraStreamPub class, which is a subclass of the Node class.
"""
def __init__(self):
"""
Class constructor to set up the node.
"""
# Initiate the Node class's constructor and give it a name.
super().__init__('camera_stream_pub')
# Create the publisher. This publisher will publish an Image
# to the video_frame_data topic. The queue size is 10 messages.
self.publisher_ = self.create_publisher(Image, 'video_frame_data', 10)
# We will publish a message every 0.1 seconds.
timer_period = 0.1 # seconds
# Create the timer.
self.timer = self.create_timer(timer_period, self.timer_callback)
# Create a VideoCapture object.
# The argument '0' gets the default webcam.
self.cap = cv2.VideoCapture(0)
# Used to convert between ROS and OpenCV images.
self.br = CvBridge()
def timer_callback(self):
"""
Callback function.
This function gets called every 0.1 seconds.
"""
# Capture frame-by-frame.
# This method returns True/False as well
# as the video frame.
ret, frame = self.cap.read()
if ret == True:
# Publish the image.
# The 'cv2_to_imgmsg' method converts an OpenCV
# image to a ROS 2 image message.
self.publisher_.publish(self.br.cv2_to_imgmsg(frame))
def main(args=None):
"""
Main function to initialize the node and start the camera stream publisher.
"""
# Initialize the rclpy library.
rclpy.init(args=args)
# Create the node.
camera_stream_publisher_node = CameraStreamPub()
# Spin the node so the callback function is called.
rclpy.spin(camera_stream_publisher_node)
# Destroy the node explicitly.
camera_stream_publisher_node.destroy_node()
# Shutdown the ROS client library for Python.
rclpy.shutdown()
if __name__ == '__main__':
main()
Create the camera_stream_subscriber.py file and populate it with the following code:
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
class CameraStreamSub(Node):
"""
Create a CameraStreamSub class, which is a subclass of the Node class.
"""
def __init__(self):
"""
Class constructor to set up the node.
"""
# Initiate the Node class's constructor and give it a name.
super().__init__('camera_stream_sub')
# Create the subscriber. This subscriber will receive an Image
# from the video_frames topic. The queue size is 10 messages.
self.subscription = self.create_subscription(
Image,
'video_frame_data',
self.listener_callback,
10)
self.subscription # prevent unused variable warning
# Used to convert between ROS and OpenCV images.
self.br = CvBridge()
self.running = True
def listener_callback(self, data):
"""
Callback function to receive and display images.
"""
if self.running:
# Convert ROS Image message to OpenCV image.
current_frame = self.br.imgmsg_to_cv2(data)
# Display image.
cv2.imshow("camera_stream", current_frame)
# Raise SystemExit exception to quit if ESC key is pressed.
if cv2.waitKey(1) == 27:
self.running = False
cv2.destroyAllWindows()
raise SystemExit
def main(args=None):
"""
Main function to initialize the node and start the camera stream subscriber.
"""
# Initialize the rclpy library.
rclpy.init(args=args)
# Create the node.
camera_stream_subscriber_node = CameraStreamSub()
# Spin the node so the callback function is called.
try:
rclpy.spin(camera_stream_subscriber_node)
# Exit if SystemExit exception is raised.
except SystemExit:
print("Camera stream output stopped")
# Destroy the node explicitly.
camera_stream_subscriber_node.destroy_node()
# Shutdown the ROS client library for Python.
rclpy.shutdown()
if __name__ == '__main__':
main()
Add the entry points to setup.py inside console_scripts section:
'camera_stream_pub = camera_stream.camera_stream_publisher:main',
'camera_stream_sub = camera_stream.camera_stream_subscriber:main',
Since we already specified our dependencies during the package creation itself, we do not need to edit the package.xml file.
Build the package.
$ cd ~/ros2_ws
$ colcon build --packages-select camera_stream
Source the new package.
$ source install/setup.bash
Run the camera stream publisher node to start publishing the camera frames.
$ ros2 run camera_stream camera_stream_pub
In another terminal, run the camera stream subscriber node.
$ cd ~/ros2_ws
$ source install/setup.bash$ ros2 run camera_stream camera_stream_sub
The camera frames will be displayed in an OpenCV window.
You can press ESC on the active OpenCV window to exit.
