ROS2プログラミング基礎_3-4: 深層学習による物体検出（YOLOv5）

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

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

このページではTurtlebot3に接続したカメラの画像からYOLOv5を使って物体検出を行うプログラムを自作します．

## 環境
・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. 必要なライブラリ等のインストール

・ROS2プログラミング基礎_3-1 を参照（実施済みなら必要なし）
https://qiita.com/pez/items/00b2ca3d67ed87e7b0fc

YOLOv5 はホームディレクトリの turtlebot3_ws 内にインストールされているものとします．（違う場所にある場合は下記プログラムの「detector.py」の一部に修正が必要です）


## 2. ワークスペース作成
今回は yolo_ws にすることにします．
```ruby:
$ mkdir -p ~/yolo_ws/src/
```


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


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

#### 4.2 setup.py ファイルについて（※今回編集必要）
```ruby:
$ cd ~/yolo_ws/src/my_yolo
$ emacs setup.py
```
23行目あたり
今回は python のコードを２つ作成しますので下記のように追記して下さい．
```ruby:
entry_points={
    'console_scripts': [
        'detector = my_yolo.detector:main',     #←これを追記！
        'my_yolo_node = my_yolo.my_yolo_node:main' 
    ],
},
```
つまり my_yolo パッケージ内に「detector.py」と「my_yolo_node.py」の２つのコードを作成します．

#### 4.3 ソースコード作成
今回作成する２つのコードのうち１つはパッケージ作成時に自動的に作られている my_yolo_node.py を編集して作ります．
```ruby:
$ cd ~/yolo_ws/src/my_yolo/my_yolo
$ emacs my_yolo_node.py
```

下記のように記述して保存します．

```ruby:my_yolo_node.py
import sys
import cv2

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from rclpy.utilities import remove_ros_args

from my_yolo.detector import Detector, parse_opt


class ObjectDetection(Node):

    def __init__(self, **args):
        super().__init__('object_detection')

        self.detector = Detector(**args)

        self.bridge = CvBridge()

        self.subscription = self.create_subscription(
            Image,
            'image_raw',
            self.image_callback,
            qos_profile_sensor_data)

    def image_callback(self, msg):
        try:
            img0 = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except CvBridgeError as e:
            self.get_logger().warn(str(e))
            return

        img0, result = self.detector.detect(img0)

        cv2.imshow('result', img0)
        cv2.waitKey(1)
        for i, r in enumerate(result):
            self.get_logger().info(
                f'{i}: ({r.u1}, {r.v1}) ({r.u2}, {r.v2})' +
                f' {r.name}, {r.conf:.3f}')


def main():
    rclpy.init()
    opt = parse_opt(remove_ros_args(args=sys.argv))
    node = ObjectDetection(**vars(opt))
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()
```

もう１つは「detector.py」という名前で新規に作成します．

```ruby:
$ emacs detector.py
```
下記のように記述して保存します（長いのでコピーして下さい）
ホームディレクトリ/yolo_ws/src/my_yolo/my_yolo に detector.py があり，
ホームディレクトリ/turtlebot_ws の中に yolov5 がある状態であれば下記のままで
大丈夫のはずです．

```ruby:detector.py
import sys

import argparse
import os
import sys
from pathlib import Path

import torch
import torch.backends.cudnn as cudnn
import numpy as np

from ament_index_python.packages import get_package_prefix

# 親ディレクトリ名がパッケージ名と仮定
package = str(Path(__file__).resolve().parent.name)
print('package:', package)
# インストールディレクトリの親の親がワークスペースと仮定
workspace = Path(get_package_prefix(package)).parents[1]
# インストールディレクトリの親の親の親がホームディレクトリと仮定
workspace2 = Path(get_package_prefix(package)).parents[2]
print('workspace:', workspace)
print('workspace2:', workspace2)
#ホームディレクトリのturtlebot3_wsの中にyolov5があることを仮定
ROOT = workspace2 / 'turtlebot3_ws/yolov5'
print('ROOT:', ROOT)
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # add ROOT to PATH
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # relative


# yolov5で用意されたモジュールを利用する
from models.common import DetectMultiBackend
from utils.general import (check_img_size, check_imshow, check_requirements,
                           non_max_suppression, print_args, scale_boxes)
from utils.plots import Annotator, colors
from utils.torch_utils import select_device
from utils.augmentations import letterbox


class Result:
    def __init__(self, xyxy=(0.0, 0.0, 0.0, 0.0), name='', conf=0.0):
        self.u1 = float(xyxy[0])
        self.v1 = float(xyxy[1])
        self.u2 = float(xyxy[2])
        self.v2 = float(xyxy[3])
        self.name = name
        self.conf = float(conf)


class Detector:

    def __init__(
        self,
        weights=ROOT / 'yolov5s.pt',  # model.pt path(s)
        data=ROOT / 'data/coco128.yaml',  # dataset.yaml path
        imgsz=(640, 640),  # inference size (height, width)
        conf_thres=0.25,  # confidence threshold
        iou_thres=0.45,  # NMS IOU threshold
        max_det=1000,  # maximum detections per image
        device='',  # cuda device, i.e. 0 or 0,1,2,3 or cpu
        view_img=False,  # show results
        classes=None,  # filter by class: --class 0, or --class 0 2 3
        agnostic_nms=False,  # class-agnostic NMS
        augment=False,  # augmented inference
        visualize=False,  # visualize features
        line_thickness=3,  # bounding box thickness (pixels)
        hide_labels=False,  # hide labels
        hide_conf=False,  # hide confidences
        half=False,  # use FP16 half-precision inference
        dnn=False,  # use OpenCV DNN for ONNX inference    ):
    ):
        check_requirements(exclude=('tensorboard', 'thop'))
        # Load model
        self.device = select_device(device)
        self.model = DetectMultiBackend(
            weights, device=self.device, dnn=dnn, data=data)
        self.stride = self.model.stride
        self.names = self.model.names
        self.pt = self.model.pt
        self.jit = self.model.jit
        self.onnx = self.model.onnx
        self.engine = self.model.engine
        self.imgsz = check_img_size(imgsz, s=self.stride)  # check image size
        self.view_img = view_img
        self.augment = augment
        self.visualize = visualize
        self.conf_thres = conf_thres
        self.iou_thres = iou_thres
        self.classes = classes
        self.agnostic_nms = agnostic_nms
        self.max_det = max_det
        self.line_thickness = line_thickness
        self.hide_labels = hide_labels
        self.hide_conf = hide_conf
        self.half = half

        # Half
        # FP16 supported on limited backends with CUDA
        self.half &= ((self.pt or self.jit or self.onnx or self.engine)
                      and self.device.type != 'cpu')
        if self.pt or self.jit:
            self.model.model.half() if self.half else self.model.model.float()

        # Dataloader
        self.view_img = True  # check_imshow()
        # set True to speed up constant image size inference
        cudnn.benchmark = True
        self.model.warmup(imgsz=(1, 3, *imgsz))  # warmup

    @torch.no_grad()
    def detect(self, img0):
        img = letterbox(img0, self.imgsz, stride=self.stride)[0]

        img = img.transpose((2, 0, 1))[::-1]  # HWC to CHW, BGR to RGB
        img = np.ascontiguousarray(img)

        img = torch.from_numpy(img).to(self.device)
        img = img.half() if self.half else img.float()  # uint8 to fp16/32
        img /= 255  # 0 - 255 to 0.0 - 1.0
        img = img[None]  # expand for batch dim

        # Inference
        pred = self.model(img, augment=self.augment, visualize=self.visualize)

        # NMS
        pred = non_max_suppression(
            pred, self.conf_thres, self.iou_thres, self.classes,
            self.agnostic_nms, max_det=self.max_det)

        det = pred[0]
        s = '%gx%g ' % img.shape[2:]  # print string
        torch.tensor(img0.shape)[[1, 0, 1, 0]]  # normalization gain whwh
        annotator = Annotator(
            img0, line_width=self.line_thickness, example=str(self.names))

        if len(det):
            # Rescale boxes from img_size to im0 size
            det[:, :4] = scale_boxes(
                img.shape[2:], det[:, :4], img0.shape).round()

            # Print results
            for c in det[:, -1].unique():
                n = (det[:, -1] == c).sum()  # detections per class
                # add to string
                s += f"{n} {self.names[int(c)]}{'s' * (n > 1)}, "
            # Write results
            result = []
            for *xyxy, conf, cls in reversed(det):
                result.append(Result(xyxy, self.names[int(cls)], conf))
                if self.view_img:  # Add bbox to image
                    c = int(cls)  # integer class
                    label = None if self.hide_labels else (
                        self.names[c] if self.hide_conf else (
                            f'{self.names[c]} {conf:.2f}'))
                    annotator.box_label(xyxy, label, color=colors(c, True))

            return img0, result
        else:
            return img0, []


def parse_opt(args):
    sys.argv = args
    parser = argparse.ArgumentParser()
    parser.add_argument(
        '--weights', nargs='+', type=str, default=ROOT / 'yolov5s.pt',
        help='model path(s)')
    parser.add_argument(
        '--data', type=str, default=ROOT / 'data/coco128.yaml',
        help='(optional) dataset.yaml path')
    parser.add_argument(
        '--imgsz', '--img', '--img-size', nargs='+', type=int, default=[640],
        help='inference size h,w')
    parser.add_argument(
        '--conf-thres', type=float, default=0.25, help='confidence threshold')
    parser.add_argument(
        '--iou-thres', type=float, default=0.45, help='NMS IoU threshold')
    parser.add_argument(
        '--max-det', type=int, default=1000,
        help='maximum detections per image')
    parser.add_argument(
        '--device', default='', help='cuda device, i.e. 0 or 0,1,2,3 or cpu')
    parser.add_argument(
        '--view-img', action='store_true', help='show results')
    parser.add_argument(
        '--classes', nargs='+', type=int,
        help='filter by class: --classes 0, or --classes 0 2 3')
    parser.add_argument(
        '--agnostic-nms', action='store_true', help='class-agnostic NMS')
    parser.add_argument(
        '--augment', action='store_true', help='augmented inference')
    parser.add_argument(
        '--visualize', action='store_true', help='visualize features')
    parser.add_argument(
        '--line-thickness', default=3, type=int,
        help='bounding box thickness (pixels)')
    parser.add_argument(
        '--hide-labels', default=False, action='store_true',
        help='hide labels')
    parser.add_argument(
        '--hide-conf', default=False, action='store_true',
        help='hide confidences')
    parser.add_argument(
        '--half', action='store_true',
        help='use FP16 half-precision inference')
    parser.add_argument(
        '--dnn', action='store_true',
        help='use OpenCV DNN for ONNX inference')
    opt = parser.parse_args()
    opt.imgsz *= 2 if len(opt.imgsz) == 1 else 1  # expand
    print_args(vars(opt))
    return opt
```


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

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

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
```ruby:.bashrcの編集
$ emacs ~/.bashrc
```
source ~/yolo_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 usb_cam usb_cam_node_exe
```

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

TurtleBot3に接続したUSBカメラの画像が表示され，物体を検出した際は色付きの枠が描かれれば成功です．

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

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


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




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