ROS2プログラミング演習_08: カメラ画像を処理した結果によってロボットの動作を制御

## はじめに

TurtleBot3をROS2（WSL2使用）で制御します．
本ページでは実機(TurtleBot3)にUSBカメラを接続し，その画像を処理した結果をもとにロボットを制御するプログラムを自作します．


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

##### ロボット
・TurtleBot3 waffle pi (Raspberry Pi 3B+)
　または
・TurtleBot3 burger (Raspberry Pi 3B+)


##### ここまでの環境構築については下記参照
・WSL2にROS2を導入
https://qiita.com/pez/items/16011df0e663ceac694a

・TurtleBot3実機を動かすまでの手順
https://qiita.com/pez/items/bb18797ce516468c7875

・TurtleBob3にUSBカメラを接続して画像データをトピック通信
https://qiita.com/pez/items/5ba36ba1cba134946f10

##### MEMO
2025.06.23 動作確認


## 1. 必要な設定について
#### TurtleBot3にはカメラ画像をpublishする自作ノード「my_cam_node」が導入済み, リモートPCには必要なライブラリ（OpenCV関連など）が導入済みとします．
https://qiita.com/pez/items/5ba36ba1cba134946f10



## 2. ワークスペース作成
ワークスペース名は image_ws としています．
```ruby:
$ cd ~/image_ws/src/
```


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


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

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

#### 4.3 ソースコード作成
今回もパッケージ作成時に自動的に作られている my_facecontrol_node.py を編集することにします．
```ruby:
$ cd ~/image_ws/src/my_facecontrol/my_facecontrol
$ emacs my_facecontrol_node.py
```
下記のように記述してみましょう．
カメラ画像から何も検出されなければ停止，顔を検出したら前進するというプログラムになっています．

```ruby:my_facecontrol_node.py

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import CompressedImage

from geometry_msgs.msg import Twist, Pose, Point
from nav_msgs.msg import Odometry

import cv2
import numpy as np

cascade_path = cv2.data.haarcascades +"haarcascade_frontalface_alt2.xml"
cascade = cv2.CascadeClassifier(cascade_path)
if cascade.empty():
  print("Cascade file read error")
  exit()


class FaceDetection(Node):

    def __init__(self):
        super().__init__('my_facecontrol_node')

        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        self.subscription = self.create_subscription(
            CompressedImage,
            '/image_raw/compressed',
            self.image_callback,
            qos
        )

        self.pub = self.create_publisher(Twist, 'cmd_vel', 10)

        self.frame_count = 0
        self.get_logger().info('Subscribed to /image_raw/compressed')


        self.x, self.y, self.yaw = 0.0, 0.0, 0.0
        self.x0, self.y0, self.yaw0 = 0.0, 0.0, 0.0
        self.vel =Twist()
        self.set_vel(0.0, 0.0)

        
    def image_callback(self, msg):
        self.frame_count += 1

        # フレームスキップ（例: 2フレームに1回だけ処理）
        if self.frame_count % 2 != 0:
            return

        # CompressedImageからOpenCV画像へデコード
        np_arr = np.frombuffer(msg.data, np.uint8)
        source = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

        # リサイズ
        resized = cv2.resize(source, (640, 480))

        # 処理（グレースケール）
        gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
        #_, result = cv2.threshold(gray, 128, 255, cv2.THRESH_BINARY)

        faces = cascade.detectMultiScale(
          gray, scaleFactor=1.11, minNeighbors=3, flags=0,
          minSize=(100,100), maxSize=(300,300)
        )

        detect = False

        for(x,y,w,h) in faces:
          cv2.rectangle(resized, (x,y), (x+w, y+h),(0,255,255),2,cv2.LINE_AA)
          detect = True

          
        if detect == True:
          self.set_vel(0.2, 0.0)
        else:
          self.set_vel(0.0, 0.0)

        self.pub.publish(self.vel)

        
        # 表示
        cv2.imshow("Color Image", resized)
        #cv2.imshow("Processed Image", result)
        cv2.waitKey(1)

        self.get_logger().info(f'Frame {self.frame_count}: image processed')


    def set_vel(self, linear, angular):
      self.vel.linear.x = linear
      self.vel.angular.z = angular



def main():
    rclpy.init()
    node = FaceDetection()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        cv2.destroyAllWindows()
        rclpy.shutdown()

```



## 5. ビルド
ワークスペース名直下のディレクトリ以外はビルドできません
初めてビルドしたとき build, install, log ディレクトリが作成されます
```ruby:
$ cd ~/image_ws
$ colcon build
```
成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます

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

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
※こちらも既に記述済みであれば必要ありません
```ruby:.bashrcの編集
$ emacs ~/.bashrc
```
source ~/image_ws/install/setup.bash
を追加してから反映させる
```ruby:
$ source ~/.bashrc
```

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

※下記２つはロボット側でコマンド入力
```ruby:TurtleBot3にて(SSH接続など)
$ ros2 launch turtlebot3_bringup robot.launch.py
```

```ruby:TurtleBot3にて(さらに別のターミナルからSSH接続)
$ ros2 run my_cam my_cam_node
```

※下記２つはリモートPC側でコマンド入力
```ruby:自作のノードを実行
$ ros2 run my_facecontrol my_facecontrol_node
```
もし「そんなパッケージはない」というエラーが出る場合は改めて
「source ~/.bashrc」をしてからそのターミナルで再度実行してみましょう

TurtleBot3に接続したUSBカメラの画像が表示され，顔を映した場合に四角い枠で検出されると同時にロボットが前進，検出されていない時は停止という動作をすれば成功です．



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