Comment utiliser OpenCV avec ROS2 en C++ et Python – French ROS Tutorial

This tutorial is created by Robotics Ambassador 023 Enzo

Rosbotics Ambassador Program https://www.theconstruct.ai/rosbotics-ambassador/)
Cours: ROS2 Basics in 5 Days C++: https://app.theconstructsim.com/courses/133
Cours: ROS2 Basics in 5 Days Python: https://app.theconstruct.ai/courses/ros2-basics-in-5-days-humble-python-132

Introduction

Le but de ce tutoriel est d’apprendre à utiliser la library OpenCV dans vos nodes C++ et Python. OpenCV (Open Source Computer Vision Library) est une bibliothèque de traitement d’images open source. Elle offre des fonctionnalités avancées pour la manipulation, l’analyse et la reconnaissance d’images, y compris la détection d’objets, le suivi, la segmentation, et bien plus encore. Cette library est très répendu et peut s’averer utile pour de nombreuses applications en robotique. Nous allons donc voir ensemble comment utiliser OpenCV à l’aide de la library cv_bridge dans vos nodes ROS2 C++ et Python.

Prérequis: Utiliser une installation ROS2 avec Gazebo et OpenCV¶

Installation ROS2

Vous pouvez vous connecter au site TheConstruct pour accéder à des machines virtuelles préconfigurées avec une installation ROS. Pour lancer et tester le code, vous devez lancer la simulation du turtlebot3 sur gazebo. Pour faire cela, entez les commandes suivantes dans le terminal:

# Declare le model de turtlebot3 à simuler
export TURTLEBOT3_MODEL=waffle_pi
# Lance la simulation du turtlebot dans gazebo
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

Une fois la simulation du turtlebot3 lancée, celle-ci va publier sur le topic /camera/image_raw que nous allons utiliser dans les codes d’exemples.

Après avoir exécuté ces commandes, appuyez sur le bouton de l’interface gazebo et selectionnez Open Gazebo.

Vous devriez obtenir le résultat suivant:

Si vous voulez effectuer le tutoriel sur votre installation local, vous devez avoir ROS2 installé (https://docs.ros.org/en/humble/Installation.html), avoir configuré un workspace (https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html) et avoir installé le turtlebot3.

# pour installer tous les packages du turtlebot3
# remplacez humble par votre distro ROS2
sudo apt-get install ros-humble-turtlebot3*
# n'oubliez pas d'exporter le modèle de Turtlebot3 que vous souhaitez utiliser
export TURTLEBOT3_MODEL=waffle_pi

Installer OpenCV

Par défault, toutes les installations de ROS incluent OpenCV mais on peut quand même verifier:

# Pour OpenCV C++ et Python
sudo apt install libopencv-dev python3-opencv

Partie 1: Créer un node C++ utilisant OpenCV

Nous allons commencer par créer un package ROS2 pour compiler et executer les nodes Python et C++ du tutorial.

cd ros2_ws/src/
# Créer le package
ros2 pkg create cv_bridge_tutorial

Nous allons maintenant créer le ficher C++ qui va contenir notre node:

cd ~/ros2_ws/src/cv_bridge_tutorial/src
touch cvbridge_example_cpp.cpp

Une fois le fichier créer, ouvrez le dans votre éditeur de code et copiez y le code suivant:

// cvbridge_example_cpp.cpp

// La library ROS2 C++

#include "rclcpp/rclcpp.hpp"

// Le message ROS image

#include "sensor_msgs/msg/image.hpp"

// Cette bibliothèque fournit le bridge permettant de faire la convertion ROS image <-> OpenCV

#include "cv_bridge/cv_bridge.h"

// La bibliothèque utilisée pour le traitement d'image

#include "opencv2/opencv.hpp"

​

class ImageProcessor : public rclcpp::Node {

public:

    ImageProcessor() : Node("image_processor") {

        // Subscribe au topic /camera/image_raw du turtlebot3

        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(

            "/camera/image_raw", 10, std::bind(&ImageProcessor::imageCallback, this, std::placeholders::_1));

​

        // Créer un publisher pour republier les images après traitement

        publisher_ = this->create_publisher<sensor_msgs::msg::Image>("processed_image_topic_cpp", 10);

    }

​

private:

    void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) {

        try {

            // Créer un cv_brigde pointer

            cv_bridge::CvImagePtr cv_ptr;

            // Convertir l'image message ROS reçue en image OpenCV en utilisant toCvCopy

            // L'image OpenCV est accessible via cv_ptr->image

            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);

            /*

            Une fois que votre image est convertie au format OpenCV, vous pouvez utiliser toutes

            les methodes fournies par la library C++ opencv2/opencv.

            */

            // Créer un objec cv::Mat (OpenCV Object) qui va contenir l'image

            cv::Mat grayscale_image;

​

            // Convertit l'image du bridge en greyscale et l'assigne dans grayscale_image

            cv::cvtColor(cv_ptr->image, grayscale_image, cv::COLOR_BGR2GRAY);

​

            // Créer un autre objet cv::Mat pour recevoir une nouvelle image

            cv::Mat thresholded_image;

​

            // Effectue un thresholdeding sur l'image greyscale et assigne l'image

            // de sortie dans thresholded_image

            cv::threshold(grayscale_image, thresholded_image, 128, 255, cv::THRESH_BINARY);

​

            // Convertit l'image OpenCV en image message ROS 

            sensor_msgs::msg::Image::SharedPtr processed_msg = cv_bridge::CvImage(

                std_msgs::msg::Header(), sensor_msgs::image_encodings::MONO8, thresholded_image).toImageMsg();

​

            // Publie l'image obtenue sur le topic processed_image_topic

            publisher_->publish(*processed_msg);

​

        } catch (cv_bridge::Exception& e) {

            RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());

            return;

        }

    }

​

    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;

    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;

};

​

int main(int argc, char** argv) {

    rclcpp::init(argc, argv);

    auto node = std::make_shared<ImageProcessor>();

    rclcpp::spin(node);

    rclcpp::shutdown();

}

Dans ce node exemple, on souscrit à l’image publié par le turtlebot3 sur le topic /camera/image_raw, on utilise ensuite un cv_bridge pour la convertir dans un format attendu par les méthodes de la library OpenCV. On utilise la methode cv::threshold qui applique un thresholding à l’image (https://docs.opencv.org/4.x/db/d8e/tutorial_threshold.html). On reconvertit à l’aide du cv_bridge l’image dans le format Image message de ROS et on republit cette image sur le topic /processed_image_topic_cpp.

Pour l’encodage, si vous indiquez “passthrough” dans le parametre encoding du CV_bridge, l’image de sortie aura le même encodage que l’image d’entrée. Une grande partie des fonctions d’OpenCV attendent en entrée des images possèdant un de ces deux encodages: mono8 and bgr8.

On doit ensuite éditer le CMakeList.txt pour pouvoir indiquer les dépendences necessaire à la compilation du node:

cmake_minimum_required(VERSION 3.8)

project(cv_bridge_tutorial)

​

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

  add_compile_options(-Wall -Wextra -Wpedantic)

endif()

​

# Toutes les dépendances requises

find_package(ament_cmake REQUIRED)

find_package(rclcpp REQUIRED)

find_package(std_msgs REQUIRED)

find_package(sensor_msgs REQUIRED)

find_package(cv_bridge REQUIRED)

find_package(OpenCV REQUIRED)

​

# uncomment the following section in order to fill in

# further dependencies manually.

# find_package(<dependency> REQUIRED)

​

add_executable(cvbridge_example_cpp src/cvbridge_example_cpp.cpp)

ament_target_dependencies(cvbridge_example_cpp rclcpp std_msgs sensor_msgs cv_bridge OpenCV)

​

install(PROGRAMS

  DESTINATION lib/${PROJECT_NAME}

)

install(TARGETS

  cvbridge_example_cpp

  DESTINATION lib/${PROJECT_NAME}

)

ament_package()

Vous pouvez build votre workspace pour verifier si la configuration du CmakeList.txt fonctionne:

cd ~/ros2_ws
colcon build
source install/setup.bash

Partie 2: Créer un node Python utilisant OpenCV

Nous allons utiliser le package crée dans la partie précedante. On va d’abord créer un dossier script et y ajouter le fichier cvbridge_example_py.py qui va contenir notre node:

cd ~/ros2_ws/src/cv_bridge_tutorial/
mkdir scripts
cd scripts
touch cvbridge_example_py.py

Une fois le fichier créer, ouvrez le dans votre éditeur de code et copiez y le code suivant:

#!/usr/bin/python3

​

# La library ROS2 Python

import rclpy

# La classe modele de node Python

from rclpy.node import Node

# Le message ROS image

from sensor_msgs.msg import Image

# Cette bibliothèque fournit le bridge permettant de faire la convertion ROS image <-> OpenCV

from cv_bridge import CvBridge

# La bibliothèque utilisée pour le traitement d'image

import cv2

​

class ImageProcessorNode(Node):

    def __init__(self):

        super().__init__('image_processor_node')

        # Subscribe au topic /camera/image_raw du turtlebot3

        self.subscription = self.create_subscription(

            Image,

            '/camera/image_raw',

            self.image_callback,

            10)

        self.subscription  # prevent unused variable warning

​

        # Créer un publisher pour republier les images après traitement

        self.publisher = self.create_publisher(

            Image,

            '/processed_image_topic_python',

            10)

        # Créer un attribut de class CVBridge

        self.cv_bridge = CvBridge()

​

    def image_callback(self, msg):

        try:

            # Utilise la methode imgmsg_to_cv2 du cv_bridge pour convertir l'image du format ROS image

            # à une image OpenCV

            cv_image = self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

        except Exception as e:

            self.get_logger().error(f"Error converting image: {e}")

            return

​

        """

        Une fois que votre image est convertie au format OpenCV, vous pouvez utiliser toutes

        les methodes fournies par la library python cv2.

        """

        # Convertit l'image OpenCV en grayscale

        gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)

​

        # Effectue un thresholding sur l'image et assigne le résultat à thresholded_image

        _, thresholded_image = cv2.threshold(gray_image, 128, 255, cv2.THRESH_BINARY)

​

        # Convertit l'image OpenCV en image message ROS

        processed_msg = self.cv_bridge.cv2_to_imgmsg(thresholded_image, encoding='mono8')

​

        # Republie l'image sur ROS

        self.publisher.publish(processed_msg)

​

def main(args=None):

    rclpy.init(args=args)

​

    image_processor_node = ImageProcessorNode()

​

    rclpy.spin(image_processor_node)

​

    image_processor_node.destroy_node()

    rclpy.shutdown()

​

if __name__ == '__main__':

main()

Une fois le node python écrit, on doit modifier le CmakeLists.txt créer dans la partie précédente pour pouvoir l’executer:

cmake_minimum_required(VERSION 3.8)

project(cv_bridge_tutorial)

​

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

  add_compile_options(-Wall -Wextra -Wpedantic)

endif()

​

# find dependencies

find_package(ament_cmake REQUIRED)

find_package(rclcpp REQUIRED)

find_package(std_msgs REQUIRED)

find_package(sensor_msgs REQUIRED)

find_package(cv_bridge REQUIRED)

find_package(OpenCV REQUIRED)

​

# uncomment the following section in order to fill in

# further dependencies manually.

# find_package(<dependency> REQUIRED)

​

add_executable(cvbridge_example_cpp src/cvbridge_example_cpp.cpp)

ament_target_dependencies(cvbridge_example_cpp rclcpp std_msgs sensor_msgs cv_bridge OpenCV)

​

install(PROGRAMS

  # Ajouter le scripts aux programmes à installer

  scripts/cvbridge_example_py.py

  DESTINATION lib/${PROJECT_NAME}

)

​

install(TARGETS

  cvbridge_example_cpp

  DESTINATION lib/${PROJECT_NAME}

)

​

ament_package()

On peut maintenant compiler le package avec les commandes suivantes:

cd ~/ros2_ws
colcon build
source install/setup.bash

Partie 3: Tester les nodes écrits précedemment

Les deux nodes du tutorial souscrivent au topic /camera/image_raw publié par le turtlebot3. Vous pouvez verifier que la simulation lancée durant l’introduction publie toujours ce topic avec la commande ros2 topic echo ou en lançant rviz2. Un fichier de configuration rviz2 est enregistré à la racine du workspace si vous utilisez le Rosject du tutorial. Vous pouvez lancer rviz2 avec cette configuration en utilisant la commande suivante:

rviz2 -d ~/ros2_ws/src/tutorial_config.rviz

Rviz2 devrait s’ouvrir avec l’interface suivante:

Vous pouvez lancer les nodes avec les commandes suivantes:

source install/setup.bash
# Pour le node python
ros2 run cv_bridge_tutorial cvbridge_example_py.py

Dans un autre terminal:

source install/setup.bash
# Pour le node C++
ros2 run cv_bridge_tutorial cvbridge_example_cpp

Vous devriez pourvoir visualiser le résultat des thresholding publiés par les nodes d’exemples:

Video Tutorial