RasPi5上のROS2でRealSense D435を使いつつ、YOLOv8で画像処理をしよう!

どうもどうも。文京区でPayPay使ってお買い物をすると20%ポイント還元、都内でpaypay等のバーコード決済でお買い物をすると10%ポイント還元、2つのキャンペーンを併用し文京区でポケカを買うと30%オフで買える魔法に気づいたTanaportです。またしてもアドカレに登場です(n回目)。

昨日の記事はほんだ先輩の「自家製アルマイトの方法」でした。いや~真似してみたいですが自宅ではできないですね~。

この記事を書き始めたのが12/10の22時で、どうしても埋まらなければ記事を5回に分けて細かく書きます。現役の代で技術記事書ける人間もう少しはいるだろ。(無事に埋まりました)というか、そもそも内容がクソほど重たい(口悪い)ので分割したほうが良さそうかもです。マイクロマウスには全く関係ないですが、まぁこれはこれでいいよね。

さて、それでは本題に入っていきます。ROS2をちゃんと学び始めて2週間でききかじった知識なので不正確な情報があるかもしれませんがお許しください。

ROS2って何やねん

ROS2(Robot Operating System 2)は、ロボットのソフト開発を簡素化するためのフレームワークです。OSという大層な名前を冠していますが、実際は既存のWindowsやLinuxといったOSの上で動作するソフトウェアフレームワークです。ノードと呼ばれる機能別のモジュール同士でトピック通信と呼ばれる通信を相互に行ってシステムを構築します。下の写真はROS2のチュートリアルからそのまま引用させていただきました。ノードの作成に使用する言語は、C++でもPythonでも構いません。異なる言語同士のノードでも、同じプロトコルの通信を用いるので通信が可能なのです。

例えば、マイクロマウスをコーディングするときは、IMUから角度情報を取得するモジュール、モータの回転速度を計測するモジュール…とセンサの値を取得するモジュール群と、それらから得た値から計算をしてモータの出力に変換するモジュールがあり、変換された値からモータを回転させるモジュールがあります。

ROS2ではこれらのモジュールは、「ノード」と呼ばれていて、センサ値を取得する「ノード」からセンサ値「パブリッシュ」してモータの出力に変換する「ノード」が「サブスクライブ」することでセンサ値を取得するのです。「パブリッシュ」された値は全ノードが参照でき、例えば、モータの出力に変換するノードのために「パブリッシュ」したセンサ値を、別のフェールセーフ用のノードが「サブスクライブ」して異常を検知するといったことができます。

ROS2には他にもサービス通信というものが存在し、こちらはノードが別のノードに通信を要求することで成立する通信です(雑)。

勉強の仕方

正直、自分も手探りなので「このやり方がベスト!」という方法を編み出してはいません。が、まずはPythonやC++のルールをある程度知らないとどうしようもないので、コーディングのチュートリアルを行ってルールを学びましょう。

C++を学習するのにおすすめのサイト
Pythonを学習するのにおすすめのサイト

なんかアフィリエイトとかで広告収入狙ってるクソサイトみたいな紹介になってしまいましたが、上に挙げたもので自分は学びました。

そして本題のROS2の学び方です。正直、世の中にいろいろな本が出版されていますが、本よりネットの大海原から情報をかき集めた方が有益だと思います。そして、中でも公式のチュートリアルが一番わかり易いです(あたりまえ)。英語かつ日本語訳をすると一部バグるのがネック。長めのチュートリアルなので、東海オンエアと乃木坂工事中を横目に観ながら作業してました。

チュートリアルはこちらから↓
(OSのバージョンごとにROS2のバージョンも異なるので、自分の環境に合わせたバージョンを読みましょう。下のリンクはUbuntu 24.04環境用のROS2 Jazzyのドキュメントです)
https://docs.ros.org/en/jazzy/Tutorials.html

あとは実践しながら学ぶものだと思うので早速実践に入ります。realsenseを使うので、持ってない方はごめんなさい…。あるものとして進めてください。

今回やりたいこと

私はゼミの時間に、テニスボールを検知して回収するロボットを自分の学習時間込みで3ヶ月で作ろうという無謀なことをしています。マウスのノウハウをそのまま流用したいので、差動二輪のロボットです。正直、テニスボールに向かって位置を制御したいので非ホロノミック系(つい最近系の名前を知ったから言いたいだけ)は避けたほうが良い気はしますがまあ考えないようにする。今回はテニスボールを検知するところまでを記事にしたいと思います。

画像処理はPythonで記述したほうがドキュメントが豊富で、簡潔にコーディングができるので今回はPythonをメインにノードを作成します。

開発環境の紹介

開発環境はだいたい最新を追いました。ドキュメントが少なめで、すこし苦労しました….。

ハード

  • Raspberry Pi 5 8GB
    Pi4と違って応答は遅いけど直接IOピンが出ているのが良いよね。ただし、5V5Aが電源の要求スペックなので組み込み用途には電源を取るのが難しい。ゼミでロボット作る用に、6セルLiPoから5V5Aを供給する電源基板を作りましたが安定動作するかは検証しないとわかりません。電源基板の検証結果はまた別の記事で。
  • Raspberry Pi AI HAT+ 13TOPS
    ラズパイはバイト先の関係で安く買えるので採用したものの、ドキュメントが少なくてキツイ…
    今回はこの子で画像処理は行いません。次回紹介します。
  • Realsense D435
    研に転がってたやつ

ソフト

データセット

  • RoboFlow(https://universe.roboflow.com/)からデータセットをいただいて使用します。
    データセットの学習は自宅のゲーミング用パソコン(結局PCゲームはハマれず、地元の友人とHoi4という思想の強いゲームをしただけでした)で行いました。以下は学習に使用したPCのスペックです。
    ・CPU:Ryzen 3700X
    ・GPU:RTX3070Ti
    ・メモリ:64GB
    ・ストレージ:2TB
    学習方法は次の記事にでもまとめます。

ROS2のワークスペースを作ろう

まずは以下の内容をターミナルに順に入力し、realsenseの画像をパブリッシュするパブリッシャーと、画像を受け取ってyolov8でテニスボールを検知するサブスクライバーを作成しましょう。

mkdir -p tennis_ball_ws/src
cd ~/tennis_ball_ws
colcon build
ros2 pkg create tennis_robot --build-type ament_python
cd tennis_robot
touch intel_pub.py
touch yolov8_sub.py
chmod +x intel_pub.py
chmod +x yolov8_sub.py

次に、realsenseの画像をパブリッシュするパブリッシャーの内容を作成していきます。以下のようにコードを書いてください。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import Header
from cv_bridge import CvBridge
import cv2
import pyrealsense2 as rs
import numpy as np

class IntelPublisher(Node):
   def __init__(self):
       super().__init__("intel_publisher")
       self.intel_publisher_rgb = self.create_publisher(Image, "rgb_frame", 10)
       self.br_rgb = CvBridge()
       timer_period = 0.05

       try:
           self.pipe = rs.pipeline()
           self.cfg = rs.config()
           self.cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
           self.pipe.start(self.cfg)
           self.timer = self.create_timer(timer_period, self.timer_callback)
           self.get_logger().info("Intel RealSense pipeline started successfully")
       except Exception as e:
           self.get_logger().error(f"Intel RealSense initialization failed: {e}")
           self.destroy_node()
           raise

   def timer_callback(self):
       try:
           frames = self.pipe.wait_for_frames()
           color_frame = frames.get_color_frame()
           if not color_frame:
               self.get_logger().warning("No color frame available")
               return

           color_image = np.asanyarray(color_frame.get_data())
           header = Header()
           header.stamp = self.get_clock().now().to_msg()
           header.frame_id = "camera_frame"
           msg = self.br_rgb.cv2_to_imgmsg(color_image, encoding="bgr8")
           msg.header = header

           self.intel_publisher_rgb.publish(msg)
           self.get_logger().info("Publishing RGB frame")
       except Exception as e:
           self.get_logger().error(f"Failed to capture frames: {e}")

def main(args=None):
   rclpy.init(args=args)
   try:
       intel_publisher = IntelPublisher()
       rclpy.spin(intel_publisher)
   except Exception as e:
       print(f"Exception in main: {e}")
   finally:
       if 'intel_publisher' in locals():
           intel_publisher.destroy_node()
       rclpy.shutdown()

if __name__ == "__main__":
   main()

次に、realsenseの画像をサブスクライブして、テニスボールを検知するサブスクライバーを作成していきます。以下のようにコードを書いてください。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from ultralytics import YOLO

class Yolov8Subscriber(Node):
   def __init__(self):
       super().__init__("yolov8_subscriber")
       self.subscription = self.create_subscription(Image, "rgb_frame", self.listener_callback, 10)
       self.br = CvBridge()
       # best.ptは自分の学習データに置き換えてください
       self.model = YOLO("/home/usr/tennis_ball_ws/src/tennis_robot/tennis_robot/best.pt") 
       self.get_logger().info("YOLOv8 node initialized")


   def listener_callback(self, msg):
       try:
           cv_image = self.br.imgmsg_to_cv2(msg, desired_encoding="bgr8")
           results = self.model(cv_image)
           annotated_frame = results[0].plot()
           cv2.imshow("YOLOv8 Detection", annotated_frame)
           cv2.waitKey(1)
           self.get_logger().info("YOLOv8 processing complete")

       except Exception as e:
           self.get_logger().error(f"Error in YOLOv8 processing: {e}")


def main(args=None):
   rclpy.init(args=args)
   yolov8_subscriber = Yolov8Subscriber()
   try:
       rclpy.spin(yolov8_subscriber)
   except KeyboardInterrupt:
       pass
   finally:
       yolov8_subscriber.destroy_node()
       rclpy.shutdown()
       cv2.destroyAllWindows()

if __name__ == "__main__":
   main()

パブリッシャーとサブスクライバーが完成したところで、ROS2が作成されたファイルを認識できるようsetup.pyを編集します。intel_pub.pyとyolov8_sub.pyがある階層の一つ上にsetup.pyがあります。setup.pyには以下のように記述します。

entry_points={
     'console_scripts': [
         "intel_pub = tennis_robot.intel_pub:main",
         "yolov8_sub = tennis_robot.yolov8_sub:main",
     ],
},

以上でビルドするための準備ができました。ディレクトリをtennis_ball_wsまで戻して、次のコマンドを打ってビルドしましょう。

colcon build --packages-select tennis_robot --symlink-install

エラーが出た場合は、エラー内容に従って一つ一つ対処していきましょう。

起動してみよう!

いままで作業を進めていた1つ目のターミナルで以下のコマンドを打ってパブリッシャーを起動させましょう。

ros2 run tennis_robot intel_pub

別の2つ目のターミナルを起動させ以下のコマンドを打ちましょう。source install/setup.bashはターミナルにROS2のワークスペースのパスを通すために行っています。ターミナル初回起動時には毎回宣言しましょう。ちゃんと設定すれば宣言不要になるんだけどね。

source install/setup.bash
ros2 run tennis_robot yolov8_sub

起動すると、realsenseの画像データを元に以下の写真のようにテニスボールを検知してくれます。ちなみに、Raspberry Pi 5の素の能力では2FPS程度でした。もっと早く推論する方法があるのかもしれませんが、遅い…。

最後に

この記事を読んでやってみようと思える人が現れて、少しでも役に立っていれば嬉しいです。ほぼ自分が忘れないように書いている節があるので…。

次回はまたしてもTanaportの記事です。今回使えなかったRaspberry Pi AI Kitを使用した推論の実装方法と、環境構築で触れなかった内容を紹介できたらなと思います。マイクロマウスやらないとまずいよ。定性的なことしか書いていないので、そろそろ定量的な記事も書きたいね。

コメント

タイトルとURLをコピーしました