OpenCVとROS 2で学ぶ画像処理入門

# はじめに
こんにちは！オガネソンです！今回は [OpenCV Advent calender 2024](https://qiita.com/advent-calendar/2024/opencv) に参加させていただきました！ROS 2とOpenCVを使ってお手軽に画像処理に入門しましょう！

筆者のプログラムの実行環境は以下の通りです。
| 項目 | 環境 |
|:-:|:-:|
| OS  | Ubuntu22.04 LTS  |
| ROS 2  | humble  |
| OpenCV  | 3.8  |

:::note warn
WindowsやMacOSでもROS 2のインストールはできますが、画像の表示などのGUI操作のできない可能性があります！
ですので、Linuxをお使いのOSとデュアルブートするか、Linuxをインストール済みのPCを借りてください。
:::

# 何をやる記事か
この記事では、ROS 2の<font color=red>トピック通信</font>によって画像を送受信しつつ、受け取った画像をOpenCVで処理したいと思います。

# ROS 2とは？
 **ROS（Robot Operating System）** とは、ロボットアプリケーションを作るためのオープンソースライブラリとツールの集まったものです。つまり、ROSはロボット開発用のミドルウェアであり、ロボットアプリケーションの開発を容易にしてくれます。ROSはロボット開発用ミドルウェアの中でもシェアが最も多く、ロボット開発のデファクトスタンダードになっています。ROS 2はROSのリアルタイム制御に対応していなかったり、Windowsで使えなかったりといった問題を解決するために開発されました。現在では2022年からHumble Hawksbillが長期サポートのバージョンでリリースされ、2024年からはJazzyがリリースされています。

 また、ROS 2ではPythonやC++などの言語をサポートしており、それらの言語でROS 2のAPI（ロボットアプリケーション）を作ることができます。本記事でも、PythonによってROS 2の機能を使って画像を送受信します。

## ROS 2のインストール方法
<details><summary> ROS 2のインストール </summary>

以下のコマンドをターミナルに貼り付けて実行しましょう。
```bash
$ sudo apt update \
  && sudo apt install -y --no-install-recommends \
     locales \
     software-properties-common tzdata \
  && locale-gen ja_JP ja_JP.UTF-8  \
  && update-locale LC_ALL=ja_JP.UTF-8 LANG=ja_JP.UTF-8 \
  && add-apt-repository universe
$ sudo apt update && sudo apt install -y curl &&\
curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg &&\
    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && sudo apt update && apt upgrade -y && sudo apt install -y ros-humble-desktop
```
そして、以下のコマンドでsourceを通してエラーがでなければ成功です。
```bash
source /opt/ros/humble/setup.bash
```
</details>

## ROS 2のトピック通信
ROS 2の通信は、ROS 2の<font color=green>ノード</font>間で<font color=red>トピック</font>というデータで通信します。<font color=green>ノード</font>とは実行中のプログラムのことです。トピックにはいくつもの「情報の型」があり、そのデータ型のことを<font color=blue>メッセージ</font>といいます。トピックの種類としては、速度に関するトピックや、画像に関するトピック、点群データに関するトピックなど、数えきれません。その中でも今回は "画像のトピック" を使って画像を送受信したいと思います。

トピック通信をしている図はこちらになります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3573929/b9f99ffb-0630-8391-ea5e-652871bd427c.png)


# トピックの送受信のやり方
トピックを送信する側のノードを<font color=red>パブリッシャー</font>といい、受信する側のノードを<font color=blue>サブスクライバー</font>といいます。まず、パブリッシャーの作り方について説明します。以下にPythonで作成した画像トピックのパブリッシュのプログラムを示します。今回送信する画像はイラストやの画像を使います。

![face_smile_man4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3573929/ac04f654-52c4-6990-f4cc-4a9a2b999566.png)

## パブリッシャーの書き方
では、こちらの`sample.png`を送るプログラムを示します。`imagepub.py`という名前で作りましょう。

```python:imagepub.py
#!/usr/bin/env python3
import rclpy
import cv2
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class ImagePub(Node): #rclpyのNodeクラスを継承
    def __init__(self): #コンストラクタ（インスタンス化した時に呼び出される関数）
        self.bridge = CvBridge() #cvbridgeをインスタンス化
        super().__init__("new_image_node") #Nodeクラスのコンストラクタを使用
        self.img_pub = self.create_publisher(Image, "/image", 10) #パブリッシャーの定義
        self.imagesend()

    #画像のパブリッシュをする関数
    def imagesend(self)
        img = cv2.imread("/home/ubuntu/image/sample.jpeg") #画像をopencvで読み込み
        cv_image = self.bridge.cv2_to_imgmsg(img, encoding="bgr8") #ROSで画像を使うためにデータを変換

        self.img_pub.publish(cv_image) #画像の送信
        
        cv2.imshow("mme",img) #送信する画像の表示
        cv2.waitKey(0) #表示する時間
        cv2.destroyAllWindows() #画像のタブの表示終了

def main():
    try:
        rclpy.init() #ROS2の初期化
        node = ImagePub() #クラスを初期化（インスタンス化）
        rclpy.spin(node) #ずっと起動し続ける
        rclpy.shutdown() #ROS2が止まったらノードもシャットダウン
    except KeyboardInterrupt: #ctrl+Cが押されたら
        pass #プログラム終了

if __name__ == "__main__":
    main()
```

プログラムの主要な部分の解説をします。

ROS 2のパブリッシャーはプログラムではこの部分になります。
```python
self.img_pub = self.create_publisher(Image, "/image", 10)
```
ROS 2ではトピックのパブリッシュは`create_publisher`と書きます。また、この関数の引数（代入している数）の部分を見てみると、`(Image, "/image", 10)`となっています。最初の引数はトピックの型です。この場合はプログラムの５行目でインポートしている`sensor_msgs.msg.Image`というメッセージ型（データ型）です。そして、２つ目の引数の`"/image"`は、トピックの名前です。最後の３つ目の引数の`10`はパブリッシュしたトピックデータを保持する数を指定しています。ちなみに`self`を変数の前に付けているのはクラス内のプライベートな変数だからです。

そして、パブリッシャーを定義したところで`imagesend()`関数を見てみます。
```python
img = cv2.imread("/home/ubuntu/image/sample.jpeg")
```
ここで`sample.jpeg`を読み込んでいます。

```python
cv_image = self.bridge.cv2_to_imgmsg(img, encoding="bgr8")
```
続いてOpenCVで読み込んだ画像はC++っぽい書き方をすると`CV::Mat`型になっていて、パブリッシャーで指定されていた`sensor_msgs.msg.Image`型のデータになっていません。なので、`CvBridge`でデータを変換します。

```python
self.img_pub.publish(cv_image)
```
そしたら`sensor_msgs.msg.Image`型の画像トピックをパブリッシュします。

```python
cv2.imshow("mme",img)
```
パブリッシュされた画像はこの関数で見ることができます。画像のタブが表示されます。最初の引数はタブの名前、２番目の引数は表示させたい画像の入った変数です。

続いてメイン関数を見てみます。Pythonではメイン関数を作らなくても動くのですが、筆者はこの方が見やすいのでメイン関数でメインの処理をしています。

```python
rclpy.init()
```
ここでROS 2の初期化を行なっています。rclpyモジュールが初期化されることでこれ以降のノードの宣言などができるようになります。
また、メイン関数を作っただけでは動かないので、以下のコードを一番下に記述します。
```python
if __name__ == "__main__":
    main()
```
これでプログラムの主な部分の解説は終わりです。

## プログラムの実行
作ったプログラムを実行してみましょう！以下のコードをプログラムを作ったディレクトリで実行しましょう！
```bash
python3 imagepub.py
```
すると、画像のタブが表示されると思います。別のターミナルを開いて
```bash
ros2 topic list
```
と打って実行しましょう。すると
```bash
/parameter_events
/rosout
/image
```
一番下に作ったトピックがあれば成功です！

## サブスクライバーの書き方
トピックを送信する側だけでなく、受信する側も書いてみましょう。

```python:imagesub.py
#!/usr/bin/env python3
import rclpy
import cv2
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class ImageSub(Node): #rclpyのNodeクラスを継承
    def __init__(self): #コンストラクタ（インスタンス化した時に呼び出される関数）
        self.bridge = CvBridge() #cvbridgeをインスタンス化
        super().__init__("new_image_sub") #Nodeクラスのコンストラクタを使用
        self.img_sub = self.create_subscription(Image, "/image",image_callback, 10) #サブスクライバーの定義

    #受け取った画像の処理をする関数
    def image_callback(self, img):
        # ROSの画像メッセージをOpenCVで使える形式に変換する
        image = self.bridge.imgmsg_to_cv2(img, 'bgr8')

        # 画像の幅と高さを取得
        height, width = image.shape[:2] #xは縦、yは横

        # 16:9のアスペクト比を計算
        new_width = int((16 / 9) * height)

        # 中央から切り抜く範囲を計算
        x1 = 0
        x2 = width
        y1 = 100
        y2 = int(x2*9/16) + y1

        # 切り抜き
        cropped_image = image[y1:y2, x1:x2]

        # 画像の表示
        cv2.imshow("new_image", cropped_image)

        cv2.waitKey(0) # 表示する時間
        cv2.destroyAllWindows() #画像のタブの表示終了

def main():
    try:
        rclpy.init() #ROS2の初期化
        node = ImageSub() #クラスを初期化（インスタンス化）
        rclpy.spin(node) #ずっと起動し続ける
        rclpy.shutdown() #ROS2が止まったらノードもシャットダウン
    except KeyboardInterrupt: #ctrl+Cが押されたら
        pass #プログラム終了

if __name__ == "__main__":
    main()
```

プログラムの解説をします。サブスクライバーは受け取った後の処理をコールバック関数（image_callback()）に書くのが特徴です。

## プログラムの実行
パブリッシャーのプログラムを実行したまま、別のターミナルで以下のコマンドを実行しましょう。
```bash
python3 imagesub.py
```
すると、切り取られた画像のタブが新たに表示されるはずです。

# 画像トピックの送受信プログラム

上記のプログラムでは、画像の送信、受信のプログラムは分けて作っていましたが、ここでは画像の送受信するプログラムを一つにまとめて作りました。

```python
#!/usr/bin/env python3
import cv2
import rclpy
import time
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class face_cascade(Node): #Nodeクラスから継承
    def __init__(self):
    	super().__init__("sample_image_node") #ノードをNodeクラスのコンストラクタで初期化
    	self.img_pub = self.create_publisher(Image, "sample_image", 10) #画像トピックを出力するパブリッシャー
    	self.img_sub = self.create_subscription(Image, "sample_image", self.callback, 10) #画像のトピックを受け取るサブスクライバー
    	self.cv_bridge = CvBridge() #画像形式をopencvとパブリッシュされた画像とで変換するクラス
    	self.image_send()

    def image_send(self): #パブリッシャーのためのメソッド
	    
	    image = cv2.cvtColor(cv2.imread("/home/ubuntu/image/sample.jpeg"),cv2.COLOR_BGR2RGB) #1079,736,3
	    self.img_pub.publish(self.cv_bridge.cv2_to_imgmsg(image, encoding="bgr8"))

    def callback(self, img): #受け取った画像の処理
        # ROSの画像メッセージをOpenCVで使える形式に変換する
        image = self.cv_bridge.imgmsg_to_cv2(img, 'bgr8')

        # 画像の幅と高さを取得
        height, width = image.shape[:2] #xは縦、yは横

        # 16:9のアスペクト比を計算
        new_width = int((16 / 9) * height)

        # 中央から切り抜く範囲を計算
        x1 = 0
        x2 = width
        y1 = 100
        y2 = int(x2*9/16) + y1

        # 切り抜き
        cropped_image = image[y1:y2, x1:x2]

        # 画像の表示
        cv2.imshow("new_image", cropped_image)

        cv2.waitKey(0) # 表示する時間
        cv2.destroyAllWindows() #画像のタブの表示終了

def main(): #メインの処理はここに書く
    try:
        rclpy.init() #rclpyの初期化
        node = face_cascade() #インスタンス化
        rclpy.spin(node) #ずっと起動し続ける
        rclpy.shutdown()
    except KeyboardInterrupt:
        pass

if __name__ == "__main__" :
    main()
```

プログラムの詳しい説明は後日追記します！クラスを使用しており、画像のパブリッシュ、サブスクライブの処理はすべてコンストラクタで行っています。サブスクライブした画像をコールバック関数（callback(self, img)）で処理し、最後に画像を表示しています。画像サイズが大きかったので切り取っているのと、画像の色が反転していたので直しているのがこのプログラムの特徴です。

# なんちゃって顔認識してみよう
基本的な画像のノード間の送受信を上記のプログラムで学んだので、次はいよいよ深層学習などによる学習アルゴリズムを使った認識ではありませんが、HaarCascadeを使った顔認識をしてみたいと思います！

・・・とは言ったものの、付け加えるのはたった６行程度です。何を付け加えるのかと言うと、haar cascadeの関数を使って処理する部分と、opencvの機能で認識した顔の部分を四角形で描画するだけです。

```python
#!/usr/bin/env python3
import cv2
import rclpy
import time
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class face_cascade(Node): #Nodeクラスから継承
    def __init__(self):
    	super().__init__("sample_image_node") #ノードをNodeクラスのコンストラクタで初期化
    	self.img_pub = self.create_publisher(Image, "sample_image", 10) #画像トピックを出力するパブリッシャー
    	self.img_sub = self.create_subscription(Image, "sample_image", self.callback, 10) #画像のトピックを受け取るサブスクライバー
    	self.cv_bridge = CvBridge() #画像形式をopencvとパブリッシュされた画像とで変換するクラス
    	self.image_send()

    def image_send(self): #パブリッシャーのためのメソッド
	    
	    image = cv2.cvtColor(cv2.imread("/home/ubuntu/image/sample.jpeg"),cv2.COLOR_BGR2RGB) #1079,736,3
	    self.img_pub.publish(self.cv_bridge.cv2_to_imgmsg(image, encoding="bgr8"))

    def callback(self, img): #受け取った画像の処理
        # ROSの画像メッセージをOpenCVで使える形式に変換する
        image = self.cv_bridge.imgmsg_to_cv2(img, 'bgr8')

        # 画像の幅と高さを取得
        height, width = image.shape[:2] #xは縦、yは横

        # 16:9のアスペクト比を計算
        new_width = int((16 / 9) * height)

        # 中央から切り抜く範囲を計算
        x1 = 0
        x2 = width
        y1 = 100
        y2 = int(x2*9/16) + y1

        # 切り抜き
        cropped_image = image[y1:y2, x1:x2]

        # Cascade分類器によって笑顔の検出
        face_cascade = cv2.CascadeClassifier("/home/ubuntu/opencv/data/haarcascades/haarcascade_smile.xml")

        # 検出する
        cr_img = cv2.cvtColor(cropped_image,cv2.COLOR_BGR2RGB)
        faces = face_cascade.detectMultiScale(cr_img, scaleFactor=1.25, minNeighbors=5, minSize=(30, 30))

        # 矩形を画像に描画する
        for x, y, w, h in faces:
            cv2.rectangle(cr_img, (x, y), (x + w, y + h), color=(0, 255, 0), thickness=2)

        # 画像の表示
        cv2.imshow("new_image", cr_img)

        # 画像の保存
        #cv2.imwrite("cropped.jpg",cv2.cvtColor(cropped_image,cv2.COLOR_BGR2RGB))

        cv2.waitKey(0) # 表示する時間
        cv2.destroyAllWindows() #画像のタブの表示終了

def main(): #メインの処理はここに書く
    try:
        rclpy.init() #rclpyの初期化
        node = face_cascade() #インスタンス化
        rclpy.spin(node) #ずっと起動し続ける
        rclpy.shutdown()
    except KeyboardInterrupt:
        pass

if __name__ == "__main__" :
    main()
```

このプログラムで付け加えた箇所はこの行です！
```python
 # Cascade分類器によって笑顔の検出
        face_cascade = cv2.CascadeClassifier("/home/ubuntu/opencv/data/haarcascades/haarcascade_smile.xml")

        # 検出する
        cr_img = cv2.cvtColor(cropped_image,cv2.COLOR_BGR2RGB)
        faces = face_cascade.detectMultiScale(cr_img, scaleFactor=1.25, minNeighbors=5, minSize=(30, 30))

        # 矩形を画像に描画する
        for x, y, w, h in faces:
            cv2.rectangle(cr_img, (x, y), (x + w, y + h), color=(0, 255, 0), thickness=2)
```

顔の検出のための教師データが`/home/ubuntu/opencv/data/haarcascades/haarcascade_smile.xml`というファイルに入っています。このファイルを使うには、OpenCV 3.9以下のバージョンをインストールする必要があります。

現在はOpenCV4.0以降のパッケージしかインストールできないので、「OpenCV 3.9　インストール」と検索してもらえば過去のバージョンのインストールページが見つかると思います。

# 終わりに
いかがでしたか？機械学習のような複雑なプログラムを書かなくても十分顔認識くらいはできそうな気がしてきましたか？
明日（12/5）のアドカレは@nonbiri15様による「視差を可視化するモジュールを作ってみた」という記事です！おたのしみに〜