ROS2プログラミング基礎_3-3: TurtleBot3のUSBカメラ画像から顔検出（カスケード分類器）

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

https://www.kspub.co.jp/book/detail/5289563.html

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

### 1. 必要なライブラリ等のインストール
#### 1.1 OpenCV関連
・ROS2プログラミング基礎_3-1 を参照（実施済みなら必要なし）
https://qiita.com/pez/items/00b2ca3d67ed87e7b0fc

#### 1.2 カスケード分類器
・下記 github にある「haarcascade_frontalface_default.xml」と
「haarcascade_eye.xml」を使わせてもらうのでダウンロードして保存します

https://github.com/opencv/opencv/tree/master/data/haarcascades

#### <補足> カスケード分類器についての補足情報
https://www.pc-koubou.jp/magazine/21280



## 2. ワークスペース作成
今回も前回作成した image_ws を再利用することにします．
```ruby:
$ cd ~/image_ws/src/
```


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


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

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

#### 4.3 ソースコード作成
今回もパッケージ作成時に自動的に作られている my_image_face_node.py を編集することにします．
```ruby:
$ cd ~/image_ws/src/my_image_face/my_image_face
$ emacs my_image_face_node.py
```
下記のように記述してみましょう．
カスケード分類器で顔と目を検出するものになっています．
```ruby:my_image_face_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

face_cascade = cv2.CascadeClassifier(
  cv2.data.haarcascades + 'haarcascade_frontalface_default.xml')
eye_cascade = cv2.CascadeClassifier(
  cv2.data.haarcascades + 'haarcascade_eye.xml')


class FaceDetection(Node):

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

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

    self.br = CvBridge()
   
  def image_callback(self, data):
    frame = self.br.imgmsg_to_cv2(data, "bgr8")
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    
    faces = face_cascade.detectMultiScale(gray, 1.3, 5)
    
    for (x,y,w,h) in faces:
        cv2.rectangle(frame,(x,y),(x+w,y+h),(255,0,0),2)
        roi_gray = gray[y:y+h, x:x+w]
        roi_color = frame[y:y+h, x:x+w]
        eyes = eye_cascade.detectMultiScale(roi_gray, 1.1, 9)
        for (ex,ey,ew,eh) in eyes:
            cv2.rectangle(roi_color,(ex,ey),(ex+ew,ey+eh),(0,255,0),2)
    
    face_detection_result = self.br.cv2_to_imgmsg(frame, "bgr8")
    self.publisher.publish(face_detection_result)

    cv2.imshow("Camera", frame)
    
    cv2.waitKey(1)


def main():
    rclpy.init()
    face_detection = FaceDetection()
    try:
        rclpy.spin(face_detection)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()
```
## 5. カスケード分類器の準備
手順の「1.2」でダウンロードした「haarcascade_frontalface_default.xml」と
「haarcascade_eye.xml」を，先ほど作成した「my_image_face_node.py」と同じディレクトリに移動またはコピーする必要があります．コマンド操作でもGUIを使ってもいいわけですがコマンド操作の場合は下記のようになります．
```ruby:コピーしたいファイルがあるディレクトリに移動
$ cd ~/ダウンロード
[注] 自分がファイルを保存した場所です（各々違います）
```
```ruby:コピー
$ cp haarcascade_frontalface_default.xml ~/image_ws/src/my_image_face/my_image_face/haarcascade_frontalface_default.xml
$ cp haarcascade_eye.xml ~/image_ws/src/my_image_face/my_image_face/haarcascade_eye.xml
```


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

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

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

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

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

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

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

TurtleBot3に接続したUSBカメラの画像が表示され，顔を映した場合に四角い枠で検出できれば成功です．

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

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


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




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