はじめに

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

このページではTurtlebot3に接続したカメラの画像からCannyエッジ検出という手法を用いて物体の輪郭を検出するプログラムを自作します．

環境

・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のセットアップ
https://qiita.com/pez/items/1d3d15b3911d5dab1702
→ この状態からUSBカメラを接続（Logicool C270n HD WEBCAM など）

1. 必要なライブラリ等の確認

OpenCV関連

・ROS2プログラミング基礎_3-1 を参照（実施済みなら必要なし）
https://qiita.com/pez/items/00b2ca3d67ed87e7b0fc

2. ワークスペース作成

今回は前回作成した image_ws を再利用することにします．

$ cd ~/image_ws/src/

3. パッケージ作成

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

$ cd ~/image_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_image_canny_node my_image_canny

4. プログラミング

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

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

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

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

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

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

entry_points={
    'console_scripts': [
        'my_image_canny_node = my_image_canny.my_image_canny_node:main' 
    ],
},

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

4.3 ソースコード作成

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

$ cd ~/image_ws/src/my_image_canny/my_image_canny
$ emacs my_image_canny_node.py

下記のように記述してみましょう．

my_image_canny_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 CannyEdgeDetection(Node):

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

        self.publisher = self.create_publisher(
            Image,
            'edges_result', 10)

        self.br = CvBridge()

    def image_callback(self, data):
        frame = self.br.imgmsg_to_cv2(data, 'bgr8')
        frame_grayscale = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        edges = cv2.Canny(frame_grayscale, threshold1=100, threshold2=200)

        edges_result = self.br.cv2_to_imgmsg(edges, 'passthrough')
        self.publisher.publish(edges_result)

        cv2.imshow('Original Image', frame)
        cv2.imshow('Canny Edge Detection', edges)

        cv2.waitKey(1)


def main():
    rclpy.init()
    canny_edge_detection = CannyEdgeDetection()
    try:
        rclpy.spin(canny_edge_detection)
    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 に追記
※こちらも既に記述済みであれば必要ありませんが反映させること（source ~/.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_canny my_image_canny_node

もし「そんなパッケージはない」というエラーが出る場合は改めて
「source ~/.bashrc」をしてからそのターミナルで再度実行してみましょう

TurtleBot3に接続したUSBカメラの画像が２つ表示（１つは元画像，もう１つはエッジ検出結果）されれば成功です．

8. 補足：前回（3-1）と同様

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

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

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

MEMO

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