ROS2でOAK-D lite depth AI Cameraを動作させる 1

## はじめに
[OAK D lite depth AI camera](https://www.switch-science.com/products/7651?srsltid=AfmBOoq2mCRcP3Foi21C4FXdiNMODuIuVVrUOyBAuUdzPYo7hPoj3vNy)をROS2で動作させる方法を解説します．このカメラは，内部にチップを含んでおり，カメラ本体側でニューラルネットワークを動作させて推論をさせることができます．[^1]物体認識やトラッキングなどをカメラ側で完結させることができます．ROSとはロボットオペレーティングシステムのことで，ロボットや自動走行車の開発に用いられています．[^2]
　本記事はこのカメラをROS2で動作させるための方法を簡単なサンプルコードを交えて解説します．
 


[^1]:https://www.switch-science.com/products/7651?srsltid=AfmBOor688KlMrQs7V7G3NElCKl2oA0EjPCGBbAbwHP48pJ5zsXRzVqx
[^2]:https://www.itaccess.co.jp/service/adv/column/ros%E3%81%A8%E3%81%AF/


| **動作環境** | 
| --- | 
| Raspberry Pi 4 (8G)　| 
| Ubuntu Mate 22.04| 
| ROS hunble| 

<!---
### 目次
1
2　DepthAI　概要
3  ←実装だけ知りたい人向け
--->
## 1．導入
DepthAIのインストールはこのページの手順に従って行います。

https://docs.luxonis.com/software/depthai/manual-install/

1. Installing dependencies
2. Installing DepthAI

ページで示されている上記ステップで完了です。
Macを使っている方はこちらの方の記事[^3]がわかりやすいかもしれません。
[^3]:https://zenn.dev/karaage0703/scraps/79299ef4063626


## 2．DepthAI概要
<!--
必要な説明：


    このカメラの仕組みと構成について．　
    depth ai apiのこと
    説明した上で実際にインストールして動作させる．ここはでもまで
-->
　DepthAI カメラをROSで使うには，**DepthAI**についてある程度知っておく必要があります．
　DepthAIとは，OAKカメラを制御したり，AI推論やコンピュータービジョンを実装するためのSDKです．このSDKにはPythonやC++向けのAPIが用意されており，我々はそれを使ってカメラ側のVPUにオリジナルの処理を記述できます．
 
![スクリーンショット 2024-09-15 21.21.39.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/93423ef3-89b6-f03c-9c72-6b80837eb9c9.png)
出典：[Luxonis Docs | Software](https://docs.luxonis.com/software/)

このAPIでカメラ側の処理は，DepthAI Componentsという概念に沿って実装します．用語はROSによく似ています．

- ノード (Nodes)
ノードは、DepthAIデバイス内での個々の独立した処理を指します（上図の黄色部分）．ノードは独立した処理単位であり、複数のノードを組み合わせて複雑な処理を構築します．

- パイプライン (Pipeline)
パイプラインは、複数のノードを連結して処理のフローを作成する構造です．DepthAIでは、パイプラインがデバイス上にアップロード・デプロイされることでカメラデバイス側での処理が決定します．

- メッセージ (Messages)
メッセージは、ノード間でのデータのやり取りです．例えば　``camera rgb ---message---> [Neural Network] ---message---> 推論結果``など．

- XLinkIn/XLinkOutノード
***XLink***という、DepthAIデバイス（カメラ）とホストコンピュータ間の通信プロトコルが用意されています。デバイスにアップロードするパイプラインでは、XLinkIn/Outノードがその役割を果たします。XLinkOutノードはデバイスからホストへデータ送信、XLinkInノードがホストからデバイス側へのデータ送信を行います。やり取りするデータはホスト側で管理され、FIFO形式で動作します。


![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/7c870a9f-7b3f-3985-894f-bac34b4d63ca.png)
出典：[Luxonis Docs | DepthAI Components/Device/Device queues](https://docs.luxonis.com/software/depthai-components/device/)

詳細は[公式ドキュメント](https://docs.luxonis.com/software/)を参考にしてください．（特にDepthAI Components, Tutorialあたり）


## 3.ROS2でカメラ画像をトピック通信してみる
DepthAIに関しての理解をもとに考えると、実装するROS上のNodeは以下のようなものであればよいはずです。

**①カメラ内での処理をノード単位で記述し、②パイプラインとしてデバイスにデプロイし、③XLinkIn/Outノードを有してカメラの制御とデータのやり取りを行う。**

まずはカメラ画像をそのままパブリッシュするノードを実装し，動かしてみます．

 
<details><summary>サンプルコード</summary>

```python: image_publish_test.py
'''chat GPTで機械的にコメントを追加してあります'''
import rclpy  # ROS2ライブラリ
from rclpy.node import Node  # ノードクラス
from sensor_msgs.msg import Image  # ROSの画像メッセージ型
import depthai as dai  # DepthAIライブラリ
import cv2  # OpenCVライブラリ
import numpy as np  # Numpyライブラリ

class OakDCameraNode(Node):
    def __init__(self):
        super().__init__('image_topic_test')  # ROS2ノード名を設定

        # DepthAIパイプラインの作成
        pipeline = dai.Pipeline()

        # カラーカメラノードの作成と設定
        cam_rgb = pipeline.createColorCamera()
        cam_rgb.setPreviewSize(320, 240)  # 解像度を設定
        cam_rgb.setInterleaved(False)
        cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB)  # RGBカメラを使用
        cam_rgb.setFps(30)  # フレームレートを30FPSに設定

        # XLinkOutノードの作成（ホストへのデータ出力用）
        xout_rgb = pipeline.createXLinkOut()
        xout_rgb.setStreamName("rgb")  # ストリーム名を設定
        cam_rgb.preview.link(xout_rgb.input)  # カメラ出力をXLinkOutにリンク

        # デバイス初期化とパイプラインの開始
        self.device = dai.Device(pipeline)

        # ROS2のImageメッセージを送信するパブリッシャーの作成
        self.image_pub = self.create_publisher(Image, 'camera/image_raw', 3)

        # 30FPSでタイマーを設定し、コールバックを実行
        self.timer = self.create_timer(0.03, self.timer_callback)

    def timer_callback(self):
        # デバイスからフレームを取得
        in_rgb = self.device.getOutputQueue(name="rgb", maxSize=4, blocking=False).get()

        # OpenCV形式のフレームに変換
        frame = in_rgb.getCvFrame()

        # ROS2 Imageメッセージの作成と設定
        msg = Image()
        msg.header.stamp = self.get_clock().now().to_msg()  # 現在時刻をタイムスタンプに設定
        msg.height = frame.shape[0]  # 画像の高さ
        msg.width = frame.shape[1]  # 画像の幅
        msg.encoding = 'bgr8'  # 画像のエンコーディング
        msg.is_bigendian = False
        msg.step = frame.shape[1] * 3  # 1行あたりのバイト数（RGBの3チャネル）
        msg.data = np.array(frame).tobytes()  # OpenCVフレームをバイト列に変換

        # トピックにメッセージを配信
        self.image_pub.publish(msg)

def main(args=None):
    rclpy.init(args=args)  # ROS2初期化
    oakd_camera_node = OakDCameraNode()  # ノードのインスタンス化

    try:
        rclpy.spin(oakd_camera_node)  # ノードをスピン（実行）
    except KeyboardInterrupt:
        pass

    oakd_camera_node.destroy_node()  # ノードの破棄
    rclpy.shutdown()  # ROS2終了

if __name__ == '__main__':
    main()  # メイン関数の実行

```

```python:image_subscriber.cpp

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <string>

class MinimalImageSubscriber : public rclcpp::Node
{
public:
  MinimalImageSubscriber(const std::string& topic_name, int width, int height) 
  : Node("opencv_image_subscriber"), target_width_(width), target_height_(height)
  {simplescreenrecordersimplescreenrecorder
    // Initialize subscriber with the given topic name
    subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
      topic_name, 2, std::bind(&MinimalImageSubscriber::image_callback, this, std::placeholders::_1));

    RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", topic_name.c_str());
  }

  ~MinimalImageSubscriber()
  {
    // Close all OpenCV windows when the node is being destroyed
    cv::destroyAllWindows();
  }

private:
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    try {
      // Convert ROS image message to OpenCV image
      cv::Mat cv_image = cv_bridge::toCvCopy(msg, "bgr8")->image;

      // Resize the image if target dimensions are specified
      if (target_width_ > 0 && target_height_ > 0) {
        cv::resize(cv_image, cv_image, cv::Size(target_width_, target_height_));
      }

      // Display image
      cv::imshow("Subscribed Image", cv_image);
      cv::waitKey(1); // Wait for a key press for 1 millisecond to allow UI events to process
    } catch (cv_bridge::Exception& e) {
      RCLCPP_ERROR(this->get_logger(), "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
  }

  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscriber_;
  int target_width_;
  int target_height_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);

  // Default values
  std::string topic_name = "/image_raw";
  int width = 0;  // Default: don't resize
  int height = 0; // Default: don't resize

  // Check if a topic name is provided via command line
  if (argc > 1) {
    topic_name = argv[1];
  }

  // Check if width and height are provided via command line
  if (argc > 3) {
    width = std::stoi(argv[2]);
    height = std::stoi(argv[3]);
  }

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting node with topic: %s", topic_name.c_str());
  if (width > 0 && height > 0) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Resizing image to: %d x %d", width, height);
  }

  // Create the subscriber node with the specific topic name and image size
  auto node = std::make_shared<MinimalImageSubscriber>(topic_name, width, height);

  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
```

　

</details>

サンプルコードの1つ目がカメラ側のパイプラインを設定し、カメラを起動して動画をPublishするノード、2つ目がそれをSubscribeして画面表示するノードです。それぞれ起動します。
```terminal    
$ ros2 run arcanain_imageprocessing_tutorial image_subscriber    
```
    
```terminal
$  ros2 run ROS2_OAKDlite_test image_topic_test
```

<iframe width="560" height="315" src="https://www.youtube.com/embed/NX6P6PCXp_Y?si=2_n_evs7QzM8rgdn" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>

このようにカメラ画像をpub/subすることができました。
次回はDepthAI cameraの特徴である，デバイス側でNeural Networkを動かし，推論結果をpub/subするノードの実装についてまとめます．




## 参考

https://github.com/Arcanain/ROS2_OAKDlite_test


https://github.com/Arcanain/arcanain_imageprocessing_tutorial



## 謝辞
この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.

https://www.gxp.co.jp/

https://qiita.com/organizations/gxp