Introduction to Image Processing in ROS2

RGB Image

This document explains how to process images obtained from robot cameras in a ROS2 environment. Specifically, we will learn how to implement two basic image processing techniques: grayscale conversion and edge detection.

Learning Objectives

By the end of this lesson, you will be able to:

  • Understand how to work with image data in ROS2
  • Convert between ROS2 image messages and OpenCV format
  • Implement basic image processing operations (grayscale conversion, edge detection)
  • Process camera images in real-time using ROS2 nodes

Image Processing Overview

This lesson covers:

  • Processing images obtained from robot cameras in real-time
  • Implementation of basic image processing techniques using the OpenCV library
  • Image data transmission and reception using ROS2 topic communication
  • Converting between different image formats and representations

1. What is Image Processing

Image processing is a technology that improves image quality or extracts information from images by applying operations to digital images. In robotics, image processing is used for the following purposes:

  • Environment Recognition: Understand the surrounding environment
  • Object Detection: Identify specific objects
  • Position Estimation: Estimate robot position and orientation
  • Obstacle Avoidance: Detect and avoid obstacles

Basic Image Processing Techniques

  • Grayscale Conversion: Convert color images to black and white
  • Edge Detection: Detect contours of objects in images
  • Noise Removal: Remove noise from images
  • Binarization: Convert images to two colors: black and white

2. Basics of Image Processing in ROS2

In ROS2, image data is handled as sensor_msgs/Image type messages. To perform image processing, the following steps are required:

  1. Subscribe to images from the camera
  2. Convert ROS2 image messages to OpenCV format
  3. Perform image processing using OpenCV
  4. Display or publish processing results as needed

Required Libraries

  • rclpy: ROS2 Python client library
  • cv_bridge: Library for converting between ROS2 image messages and OpenCV format images
  • OpenCV: Library for image processing

3. Implementation of Grayscale Conversion

Grayscale Image

Grayscale conversion is a process that converts color images to black and white. It converts the color information (RGB) of each pixel into a single value representing brightness.

Implementation Steps

  1. Create a ROS2 node
  2. Subscribe to images from the camera
  3. Convert images to grayscale
  4. Display conversion results

Code Example

image_gray_processor.py

#!/usr/bin/env python3
# Import ROS 2 libraries
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image  # ROS 2 image message type
# Import cv_bridge for converting between ROS and OpenCV image formats
from cv_bridge import CvBridge
import cv2  # OpenCV library for image processing
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy

class ImageGrayProcessor(Node):
    """
    A ROS 2 node that processes camera images and converts them to grayscale.
    """
    def __init__(self):
        # Initialize the ROS 2 node
        super().__init__('image_gray_processor')

        # Set QoS profile for image data
        # Image data is large and frequent, so we use BEST_EFFORT to prioritize
        # real-time performance over guaranteed delivery
        qos_profile = QoSProfile(
            depth=5,  # Keep last 5 images in queue
            reliability=QoSReliabilityPolicy.BEST_EFFORT,  # Allow dropped frames
            durability=QoSDurabilityPolicy.VOLATILE  # Don't store messages for late subscribers
        ) 
               
        # Subscribe to camera image topic
        # When a new image arrives, image_callback will be called
        self.subscription = self.create_subscription(
            Image,  # Message type
            '/kachaka/front_camera/image_raw',  # Topic name
            self.image_callback,  # Callback function
            qos_profile  # Quality of Service settings
        )
        
        # Create a CvBridge instance for converting ROS images to OpenCV format
        # CvBridge handles the conversion between ROS Image messages and OpenCV's numpy arrays
        self.bridge = CvBridge()
        
        self.get_logger().info("ImageGrayProcessor has started.")

    def image_callback(self, msg):
        """
        Callback function called whenever a new image is received.
        
        This function:
        1. Converts the ROS Image message to OpenCV format
        2. Converts the color image to grayscale
        3. Displays the result in a window
        
        Parameters:
            msg: sensor_msgs/Image message from the camera
        """
        try:
            # Convert ROS Image message to OpenCV image format
            # imgmsg_to_cv2 converts ROS Image to OpenCV's numpy array format
            # desired_encoding='bgr8' specifies we want BGR (Blue-Green-Red) format
            # which is OpenCV's default color format (note: different from RGB)
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

            # Convert color image to grayscale
            # cvtColor converts between color spaces
            # COLOR_BGR2GRAY converts from BGR color to grayscale (single channel)
            # Grayscale images have one value per pixel (brightness) instead of three (BGR)
            gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)

            # Display the grayscale image in a window
            # imshow creates/updates a window with the image
            # waitKey(1) processes window events and waits 1ms
            # This is necessary for the window to update and be responsive
            cv2.imshow("Grayscale Image", gray_image)
            cv2.waitKey(1)
        except Exception as e:
            # If conversion fails (e.g., wrong image format), log the error
            # This prevents the node from crashing on bad image data
            self.get_logger().error(f"Error processing image: {str(e)}")

def main(args=None):
    rclpy.init(args=args)
    node = ImageGrayProcessor()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Shutting down...")
    finally:
        cv2.destroyAllWindows()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Code Explanation

  • QoS Profile: Set QoS profile suitable for image data transmission and reception
  • Subscription: Subscribe to images from the camera
  • cv_bridge: Convert between ROS2 image messages and OpenCV format images
  • Grayscale Conversion: Convert from BGR format to grayscale using cv2.cvtColor function
  • Image Display: Display processing results using cv2.imshow function

4. Implementation of Edge Detection

Edge Image

Edge detection is a process that detects the contours of objects in images. In OpenCV, the Canny edge detection algorithm is commonly used.

Implementation Steps

  1. Create a ROS2 node
  2. Subscribe to images from the camera
  3. Convert images to grayscale
  4. Apply Canny edge detection
  5. Display processing results

Code Example

image_edge_detection.py

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy

class ImageEdgeDetectionNode(Node):
    def __init__(self):
        super().__init__('image_edge_detection_node')
        
        # Set QoS profile
        qos_profile = QoSProfile(
            depth=5,
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            durability=QoSDurabilityPolicy.VOLATILE
        ) 
               
        # Subscribe to camera image topic
        self.subscription = self.create_subscription(
            Image,
            '/kachaka/front_camera/image_raw',
            self.image_callback,
            qos_profile
        )
        self.bridge = CvBridge()
        self.get_logger().info("Edge Detection Node has started.")

    def image_callback(self, msg):
        try:
            # Convert ROS Image message to OpenCV image
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

            # Convert to grayscale
            gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)

            # Apply Canny edge detection algorithm
            # Canny edge detection finds edges (sharp changes in intensity) in the image
            # Parameters:
            #   - gray_image: Input grayscale image
            #   - 50: Low threshold - edges weaker than this are discarded
            #   - 150: High threshold - edges stronger than this are kept
            # Edges between these thresholds are kept if connected to strong edges
            # These thresholds control sensitivity: lower values detect more edges
            edges = cv2.Canny(gray_image, 50, 150)

            # Display original image, grayscale image, and edge detection result
            cv2.imshow("Original Image", cv_image)
            cv2.imshow("Grayscale Image", gray_image)
            cv2.imshow("Edge Detection", edges)
            cv2.waitKey(1)
        except Exception as e:
            self.get_logger().error(f"Error processing image: {str(e)}")

def main(args=None):
    rclpy.init(args=args)
    node = ImageEdgeDetectionNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Shutting down...")
    finally:
        cv2.destroyAllWindows()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Code Explanation

  • Grayscale Conversion: Convert images to grayscale before edge detection
  • Canny Edge Detection: Detect edges using cv2.Canny function
    • 2nd argument (50): Low threshold
    • 3rd argument (150): High threshold
  • Multiple Image Display: Display original image, grayscale image, and edge detection results simultaneously

5. Package Creation and Execution

Package Creation

# Create grayscale conversion package
ros2 pkg create --build-type ament_python image_gray_processor --dependencies rclpy sensor_msgs cv_bridge opencv-python --node-name image_gray_processor

# Create edge detection package
ros2 pkg create --build-type ament_python image_edge_detection --dependencies rclpy sensor_msgs cv_bridge opencv-python --node-name image_edge_detection

Package Building

cd ~/ros2_ws
colcon build --packages-select image_gray_processor image_edge_detection

Node Execution

# Run grayscale conversion node
ros2 run image_gray_processor image_gray_processor

# Run edge detection node
ros2 run image_edge_detection image_edge_detection

6. Applications of Image Processing

Practical Applications

  • Object Tracking: Track specific objects
  • Face Recognition: Detect and recognize human faces
  • Character Recognition (OCR): Recognize characters in images
  • Obstacle Detection: Detect obstacles ahead of the robot

Research and Development Applications

  • SLAM (Simultaneous Localization and Mapping): Estimate position while creating environment maps
  • Object Recognition: Identify various objects
  • Gesture Recognition: Recognize human gestures

Exercises

Basic Image Processing

  1. Run the grayscale conversion node and observe how camera images are converted to black and white
  2. Run the edge detection node and observe how object contours are detected

Parameter Adjustment

  1. Change the Canny edge detection thresholds and observe changes in edge detection results