ROS2プログラミング基礎_3-1: TurtleBot3のUSBカメラ画像 + OpenCV

はじめに

こちらを参考書としてROS2プログラミングの勉強をしていきます．

このページではTurtlebot3に接続したカメラの映像を取得・表示するプログラムを自作します．

環境

・PC: Panasonic Let's Note CF-FV
・OS: Windows11 Pro (64bit)
・VirtualBox: 6.1.38 (Ubuntu20.04.5, GUI環境)
・ROS: ROS2 Foxy

・ロボット：
　Turtlebot3 burger (Raspberry Pi 3B+)
　Turtlebot3 waffle pi (Raspberry Pi 3B+)
　Turtlebot3 waffle pi (Raspberry Pi 4B)
　※いずれも同様の操作で大丈夫なことを確認

ここまでの環境構築については下記参照
・Windows PCにVirtualBox＋Ubuntuを導入
https://qiita.com/pez/items/a3ef1855f7e1e0ed3dfd
・ROS(ROS2 Foxy)をインストール
https://qiita.com/pez/items/1df36628524ff40a3d93
・Turtlebot3 burger のセットアップ
https://qiita.com/pez/items/1d3d15b3911d5dab1702
→ この後USBカメラを接続

1. 必要なライブラリのインストール

ここでの操作はリモートPCとTurlteBot3の両方で実施しました．
（今後の操作で必要ないものもあるかと思いますが未確認です）

入っていなければ入れておく

OpenCV

$ pip3 install opencv-contrib-python

※pip3未導入の場合はsudo apt install python3-pip

ROS2とOpenCVのインターフェースとなるパッケージ

$ sudo apt install ros-foxy-vision-opencv

USBカメラ用ノードのパッケージ

$ sudo apt install ros-foxy-usb-cam

YOLOv5のインストール（ソースコードからビルド）

$ cd ~/turtlebot3_ws/
$ git clone https://github.com/ultralytics/yolov5
$ cd yolov5
$ pip3 install -r requirements.txt

2. ワークスペース作成

今回は image_ws としておきます

$ mkdir -p ~/image_ws/src/

3. パッケージ作成

今回ノード名は my_image_node
パッケージ名は my_image
にすることにします

$ cd ~/image_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_image_node my_image

4. プログラミング

4.1 package.xml ファイルについて

$ cd ~/image_ws/src/my_image
$ emacs package.xml

自作パッケージを一般公開するときには変更する必要がありますが，今回も特になにもしません．

4.2 setup.py ファイルについて

$ cd ~/image_ws/src/my_image
$ emacs setup.py

23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要があります．今回もパッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっているはずです．

entry_points={
    'console_scripts': [
        'my_image_node = my_image.my_image_node:main' 
    ],
},

※もし１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を変更する必要があります．

4.3 ソースコード作成

今回もパッケージ作成時に自動的に作られている my_image_node.py を編集することにします．

$ cd ~/image_ws/src/my_image/my_image
$ emacs my_image_node.py

下記のように記述してみましょう．
OpenCVを使った２値化処理を行うプログラムになっています．

my_image_node.py

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image

import cv2
from cv_bridge import CvBridge


class ImgProcOpenCVROS(Node):

    def __init__(self):
        super().__init__('my_image_node')
        self.subscription = self.create_subscription(
            Image,
            'image_raw',
            self.image_callback,
            qos_profile_sensor_data)

        self.publisher = self.create_publisher(
            Image,
            'result', 10)
        
        self.br = CvBridge()

    def image_callback(self, data):
        source = self.br.imgmsg_to_cv2(data, 'bgr8')
        gray = cv2.cvtColor(source, cv2.COLOR_BGR2GRAY)
        _, result = cv2.threshold(gray, 128, 255, cv2.THRESH_BINARY)
        result_msg = self.br.cv2_to_imgmsg(result, 'passthrough')
        self.publisher.publish(result_msg)
        self.get_logger().info('Publishing image')


def main():
    rclpy.init()
    my_image_node = ImgProcOpenCVROS()
    try:
        rclpy.spin(my_image_node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()

5. ビルド

ワークスペース名直下のディレクトリ以外はビルドできません
初めてビルドしたとき build, install, log ディレクトリが作成されます

$ cd ~/image_ws
$ colcon build

成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます

6. 設定ファイルの反映

アンダーレイ設定ファイル（ROS2システムの設定）は .bashrc に記述済みとします
※下記が.bashrcに既に書かれているという意味
source /opt/ros/foxy/setup.bash

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記

.bashrcの編集

$ emacs ~/.bashrc

source ~/image_ws/install/setup.bash
を追加してから反映させる

$ source ~/.bashrc

7. ノードの実行（実機）

今回はロボットを動かすプログラムなのでロボットを起動します．
TurtleBot3の設定については別記事参照．
https://qiita.com/pez/items/1d3d15b3911d5dab1702

※下記２つはロボット側でコマンド入力

TurtleBot3にて(SSH接続など)

$ ros2 launch turtlebot3_bringup robot.launch.py

TurtleBot3にて(さらに別のターミナルからSSH接続)

$ ros2 run usb_cam usb_cam_node_exe

※下記２つはリモートPC側でコマンド入力

自作のノードを実行

$ ros2 run my_image my_image_node

結果表示／また別のターミナルで

$ rqt

立ち上がったらメニューの「Plugins」→「Visualization」→「Image View」を２回選ぶ．
さらにそれぞれのプラグインのトピックを選択するメニューでそれぞれ「/image_raw」と「/result」を選ぶ．

TurtleBot3に接続したUSBカメラの画像が表示できれば成功です．
通常の画像と２値化処理された画像の２つが表示されます．

8. 補足

カメラ画像を表示したままロボットを動作させてみましょう．
また別のターミナルを立ち上げてコマンドを入力することになります．
今まで作った my_teleop や my_gamepad などでもよいと思います．

このとき「ROS_DOMAIN_ID」の番号が同じ人（PC）で作業を手分けすることができます．
（例えば）
・PC1でTeleop
・PC2で地図作成
・PC3で画像処理
など

・カメラ画像だけを見てロボット操作は可能でしょうか？
・遅延はどのくらいあるでしょうか？
　→ 体感してみましょう

MEMO

＜プログラムを更新したときは＞
　1.プログラムの中身を編集して保存
　2.ビルド
　3.設定ファイルの反映
　4.ノードの実行
　→ 動作を確認

下記
今回使いませんがメモとして残しておきます

カメラキャリブレーション用のパッケージ

$ sudo apt -y install ros-foxy-camera-calibration-parsers
$ sudo apt -y install ros-foxy-camera-info-manager
$ sudo apt -y install ros-foxy-launch-testing-ament-cmake
$ sudo apt -y install ros-foxy-image-pipeline