ROS2プログラミング演習_04: TurtleBot3のカメラ画像＋OpenCV [Gazebo]

## はじめに
Gazeboシミュレータでturtlebot3を動かします．
このページではTurtleBot3のカメラの映像を取得・表示するプログラムを自作します．

#### 環境
・OS: Windows11 Pro (64bit)
・ROS: ROS2 humble (WSL2のUbuntu22.04に)

ここまでの環境構築については下記参照
・WSL2にROS2を入れてgazeboでシミュレーションするまでの手順
https://qiita.com/pez/items/16011df0e663ceac694a


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

入っていなければ入れておく
```ruby:OpenCV
pip3 install opencv-contrib-python
```
※pip3未導入の場合はsudo apt install python3-pip
```ruby:ROS2とOpenCVのインターフェースとなるパッケージ
sudo apt install ros-humble-vision-opencv
```

下記２つは後で使うものですが今のうちにインストールしておきましょう
```ruby:USBカメラ用ノードのパッケージ
sudo apt install ros-humble-usb-cam
```

```ruby:YOLO
pip3 install ultralytics
```



## 2. ワークスペース作成
今回は image_ws としておきます
```ruby:
mkdir -p ~/image_ws/src/
```


## 3. パッケージ作成
今回ノード名は my_image_node
パッケージ名は my_image
にすることにします
```ruby:
cd ~/image_ws/src
ros2 pkg create --build-type ament_python --node-name my_image_node my_image
```


## 4. プログラミング
#### 4.1 package.xml ファイルについて
```ruby:
cd ~/image_ws/src/my_image
emacs package.xml
```
自作パッケージを一般公開するときには変更する必要がありますが，今回も特になにもしません．

#### 4.2 setup.py ファイルについて
```ruby:
cd ~/image_ws/src/my_image
emacs setup.py
```
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要があります．今回もパッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっているはずです．
```ruby:
entry_points={
    'console_scripts': [
        'my_image_node = my_image.my_image_node:main' 
    ],
},
```
※もし１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を変更する必要があります．

#### 4.3 ソースコード作成
今回もパッケージ作成時に自動的に作られている my_image_node.py を編集することにします．
```ruby:
cd ~/image_ws/src/my_image/my_image
emacs my_image_node.py
```
下記のように記述してみましょう．
OpenCVを使った２値化処理を行うプログラムになっています．
```ruby: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,
            '/camera/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 ディレクトリが作成されます
```ruby:
cd ~/image_ws
colcon build
```
成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます

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

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
```ruby:.bashrcの編集
emacs ~/.bashrc
```
source ~/image_ws/install/setup.bash
を追加してから反映させる
```ruby:
source ~/.bashrc
```

## 7. ノードの実行（gazebo）

※家環境
```ruby:
export TURTLEBOT3_MODEL=waffle
ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py
```


### 自作ノードの実行
```ruby:自作のノードを実行
ros2 run my_image my_image_node
```
```ruby:結果表示／また別のターミナルで
rqt
```

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

TurtleBot3(waffle)のカメラの画像が表示できれば成功です．
通常の画像と２値化処理された画像の２つが表示されます．

## 8. 補足
#### numpyのバージョン
numpyのバージョンが新しすぎるというエラーが出ることがあるのでその際はバージョンを下げてしまいましょう
```ruby:numpyのバージョンを下げる
pip3 install "numpy<2"
```
#### TurtleBot3のモデル
export TURTLEBOT3_MODEL=waffle を入力するのが面倒な場合は「.bashrc」に書き込むと多少は楽になります．「.bashrc」の中に「export TURTLEBOT3_MODEL=waffle」を追記すれば「source ~/.bashrc」の際に一緒に実施されます．

#### ロボットの動作

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

#### gazeboの表示がおかしくなったとき
gazeboが立ち上がった時にロボットが表示されなかったり，位置や姿勢がおかしい場合はすべて立ち上げなおすことをお勧めします．ubuntuではなくpowershell上で下記コマンドを入力．
```ruby:シャットダウン（ubuntu内ではなくpowershell上で）
wsl --shutdown
```

## 9. 演習課題
#### ①エッジ検出を行う
以前の課題にあったエッジ検出(cv2.Canny)のプログラムを応用して，「result」の方の画像をエッジ検出画像に変更して下さい

#### ②物体検出を行う
以前の課題であった物体検出(yolov11n)のプログラムを応用して，を使って「result」の方の画像を物体検出画像に変更して下さい




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

下記
今回使いませんがメモとして残しておきます
```ruby:カメラキャリブレーション用のパッケージ
$ sudo apt -y install ros-humble-camera-calibration-parsers
$ sudo apt -y install ros-humble-camera-info-manager
$ sudo apt -y install ros-humble-launch-testing-ament-cmake
$ sudo apt -y install ros-humble-image-pipeline
```