LoginSignup
6
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@pez(Ryo ITO)

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

Last updated at Posted at 2022-11-18

はじめに

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

このページでは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 にすることにします．

$ mkdir -p ~/yolo_ws/src/

3. パッケージ作成

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

$ cd ~/yolo_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_yolo_node my_yolo

4. プログラミング

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

$ cd ~/yolo_ws/src/my_yolo
$ emacs package.xml

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

4.2 setup.py ファイルについて（※今回編集必要）

$ cd ~/yolo_ws/src/my_yolo
$ emacs setup.py

23行目あたり
今回は python のコードを２つ作成しますので下記のように追記して下さい．

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 を編集して作ります．

$ cd ~/yolo_ws/src/my_yolo/my_yolo
$ emacs my_yolo_node.py

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

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」という名前で新規に作成します．

$ emacs detector.py

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

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 ディレクトリが作成されます

$ cd ~/yolo_ws
$ colcon build

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

6. 設定ファイルの反映

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

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記

.bashrcの編集
$ emacs ~/.bashrc

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

$ source ~/.bashrc

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

今回はロボットを動かすプログラムなのでロボットを起動します．
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_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.ノードの実行
　→ 動作を確認

6
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
6
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?