はじめに

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

このページではTurtlebot3に接続したカメラの画像からカスケード分類器を使った顔の検出を行い，検出されたらロボットを前進させるプログラムを自作します．「ROS2プログラミング基礎3-3」に少しだけ手を加えたものです．

環境

・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」と
使わせてもらうのでダウンロードして保存します

2. ワークスペース作成

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

$ cd ~/image_ws/src/

3. パッケージ作成

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

$ cd ~/image_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_facecontrol_node my_facecontrol

4. プログラミング

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

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

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

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

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

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

entry_points={
    'console_scripts': [
        'my_facecontrol_node = my_facecontrol.my_facecontrol_node:main' 
    ],
},

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

4.3 ソースコード作成

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

$ cd ~/image_ws/src/my_facecontrol/my_facecontrol
$ emacs my_facecontrol_node.py

下記のように記述してみましょう．
カメラ画像から何も検出されなければ停止，顔を検出したら前進するというプログラムになっています．

my_facecontrol_node.py

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

import cv2
from cv_bridge import CvBridge

face_cascade = cv2.CascadeClassifier(
  cv2.data.haarcascades + 'haarcascade_frontalface_default.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.publisher2 = self.create_publisher(Twist, 'cmd_vel', 10)
    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)
    
    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)

    self.set_vel(0.0, 0.0)
    detect = False
    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]
        detect = True

    if detect == True:
      self.set_vel(0.2, 0.0)
    else:
      self.set_vel(0.0, 0.0)
    
    face_detection_result = self.br.cv2_to_imgmsg(frame, "bgr8")
    self.publisher.publish(face_detection_result)
    self.publisher2.publish(self.vel)

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


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

    

def main():
    rclpy.init()
    face_detection = FaceDetection()
    try:
        rclpy.spin(face_detection)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()

5. カスケード分類器の準備

手順の「1.2」でダウンロードした「haarcascade_frontalface_default.xml」を，先ほど作成した「my_facecontrol_node.py」と同じディレクトリに移動またはコピーする必要があります．コマンド操作でもGUIを使ってもいいわけですがコマンド操作の場合は下記のようになります．

コピーしたいファイルがあるディレクトリに移動

$ cd ~/ダウンロード
[注] 自分がファイルを保存した場所です（各々違います）

コピー

$ cp haarcascade_frontalface_default.xml ~/image_ws/src/my_facecontrol/my_facecontrol/haarcascade_frontalface_default.xml

6. ビルド

ワークスペース名直下のディレクトリ以外はビルドできません
初めてビルドしたとき build, install, log ディレクトリが作成されます

$ cd ~/image_ws
$ colcon build

成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます

7. 設定ファイルの反映

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

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
※こちらも既に記述済みであれば必要ありません

.bashrcの編集

$ emacs ~/.bashrc

source ~/image_ws/install/setup.bash
を追加してから反映させる

$ source ~/.bashrc

8. ノードの実行（実機）

今回はロボットを動かすプログラムなのでロボットを起動します．
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_facecontrol my_facecontrol_node

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

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

MEMO

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