ROS2,wifi経由でカメラ画像をトピック通信（WSL2使用）

## はじめに
ROS2を使用していて，ロボットに装着したUSBカメラの画像をwifi経由で通信しようとした際，遅延が激しくてまともに使えないということがありました．色々設定を調べて改善したので忘備録として残しています．apt install で入る usb-cam ノードは使用せず，自作します．なお画像処理・表示にはOpenCVを使用します．

### 動作確認した環境
##### PC
・OS: Windows11 Pro (64bit)
・ROS: ROS2 humble (WSL2のUbuntu22.04に)

##### ロボット
・TurtleBot3 burger (Raspberry Pi 3B+)
・ROS: ROS2 foxy

 ※このページの範疇ではロボットの種類は関係なく，ROS2 humble か ROS2 foxy が導入されていて正常に稼働していればハードは何でも（普通のPCでもエッジコンピュータでも）そのまま適用できるはずです．ですが，このページでは便宜上「ロボット」と「PC」と呼び分けています．
 

##### ここまでの環境構築については下記参照
・WSL2にROS2を入れてgazeboでシミュレーションするまでの手順
https://qiita.com/pez/items/16011df0e663ceac694a

・TurtleBot3実機を動かすまでの手順
https://qiita.com/pez/items/bb18797ce516468c7875

## 1. ロボットの方の設定とプログラム
ロボットにUSBカメラ（ここでは logicool C270n HD WEBCAM) を接続する．また，必要なライブラリをインストールする．
```ruby:OpenCV
$ pip3 install opencv-python
$ pip3 install opencv-contrib-python
```
※pip3未導入の場合はsudo apt install python3-pip


ROS2とOpenCVのインターフェースとなるパッケージや通信関係のパッケージなどをインストールする．
```ruby:foxyの場合
$ sudo apt -y install ros-foxy-vision-opencv
$ sudo apt -y install ros-foxy-v4l2-camera
$ sudo apt -y install ros-foxy-cv-bridge ros-foxy-image-transport
$ sudo apt -y install ros-foxy-rmw-cyclonedds-cpp
```

```ruby:humbleの場合
$ sudo apt -y install ros-humble-vision-opencv
$ sudo apt -y install ros-humble-v4l2-camera
$ sudo apt -y install ros-humble-cv-bridge ros-humble-image-transport
$ sudo apt -y install ros-humble-rmw-cyclonedds-cpp
```

カメラ画像を publish するノードを自作するので下記実施．
ここではワークスペースを「cam_ws」，ノード名を「my_cam_node」，パッケージ名を「my_cam」としています．
```ruby:ワークスペース作成
$ mkdir -p ~/cam_ws/src/
```
```ruby:パッケージ作成
$ cd ~/cam_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_cam_node my_cam
```
ソースコードとしては自動的に作られている「my_cam_node.py」を編集することにします．
```ruby:移動して開く（エディタは好みで）
$ cd ~/cam_ws/src/my_cam/my_cam
$ emacs my_cam_node.py
```

最終的に次のようなプログラムになりました．画素数やフレームレート，JPEGの画質を非常に低く設定しています．これほど低くしなくても遅延しない場合は「create_timer」，「fps」，画素数，「IMWRITE_JPEG_QUALITY」などの設定を変更して様子をみましょう．

```ruby:my_cam_node.py
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from rclpy.qos import qos_profile_sensor_data
import cv2
import numpy as np

class CompressedCamNode(Node):
    def __init__(self):
        super().__init__('compressed_cam_node')
        self.publisher_ = self.create_publisher(CompressedImage, 'image_raw/compressed', qos_profile_sensor_data)
        self.timer = self.create_timer(0.2, self.timer_callback)  # 5Hz
        self.cap = cv2.VideoCapture(0)  # USBカメラ (ID:0)
        if not self.cap.isOpened():
            self.get_logger().error("カメラを開けませんでした")
            exit(1)
        self.fps = 10 #フレームレート
        self.get_logger().info("USBカメラを起動しました")
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)

    def timer_callback(self):
        ret, frame = self.cap.read()
        if not ret:
            self.get_logger().warning("カメラ画像が取得できません")
            return

        # JPEGに圧縮
        encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 20]
        ret, buffer = cv2.imencode('.jpg', frame, encode_param)
        if not ret:
            self.get_logger().warning("JPEG圧縮に失敗しました")
            return

        msg = CompressedImage()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.format = "jpeg"
        msg.data = np.array(buffer).tobytes()

        self.publisher_.publish(msg)
        self.get_logger().info("Compressed image published")

    def destroy_node(self):
        self.cap.release()
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    node = CompressedCamNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("終了します")
    finally:
        node.destroy_node()
        rclpy.shutdown()
```
#### ビルド
```ruby:
$ cd ~/my_cam
$ colcon build
```

#### 設定ファイルの編集
オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
```ruby:.bashrcの編集
$ emacs ~/.bashrc
```
**source ~/cam_ws/install/setup.bash**

を追加する．ついでに，効果はあまり感じられなかったのですが下記も追加

**export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp**

デフォルトの設定である「fastrtps」よりは高速になるとのこと．



#### 設定ファイルの反映

```ruby:
$ source ~/.bashrc
```


## 2. リモートPCの方の設定とプログラム
ロボットの方と同様に必要なライブラリをインストールする．
```ruby:OpenCV
$ pip3 install opencv-python
$ pip3 install opencv-contrib-python
```
※pip3未導入の場合はsudo apt install python3-pip


ROS2とOpenCVのインターフェースとなるパッケージや通信関係のパッケージなどをインストールする．
```ruby:foxyの場合
$ sudo apt -y install ros-foxy-vision-opencv
$ sudo apt -y install ros-foxy-v4l2-camera
$ sudo apt -y install ros-foxy-cv-bridge ros-foxy-image-transport
$ sudo apt -y install ros-foxy-rmw-cyclonedds-cpp
```

```ruby:humbleの場合
$ sudo apt -y install ros-humble-vision-opencv
$ sudo apt -y install ros-humble-v4l2-camera
$ sudo apt -y install ros-humble-cv-bridge ros-humble-image-transport
$ sudo apt -y install ros-humble-rmw-cyclonedds-cpp
```


カメラ画像を subscribe するノードを自作するので下記実施．
ここではワークスペースを「image_ws」，ノード名を「my_image_node」，パッケージ名を「my_image」としています．
```ruby:ワークスペース作成
$ mkdir -p ~/image_ws/src/
```

```ruby:パッケージ作成
$ cd ~/image_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_image_node my_image
```

ソースコードとしては自動的に作られている「my_image_node.py」を編集することにします．
```ruby:移動して開く（エディタは好みで）
$ cd ~/image_ws/src/my_image/my_image
$ emacs my_image_node.py
```

こちらはOpenCVを使った２値化処理を行って表示するプログラムになっています．画像が小さいのでリサイズの部分で適当に拡大するとよいと思います。

```ruby:my_image_node.py
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import CompressedImage

import cv2
import numpy as np

class CompressedImgProc(Node):

    def __init__(self):
        super().__init__('compressed_image_node')

        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        self.subscription = self.create_subscription(
            CompressedImage,
            '/image_raw/compressed',
            self.image_callback,
            qos
        )

        self.frame_count = 0
        self.get_logger().info('Subscribed to /image_raw/compressed')

    def image_callback(self, msg):
        self.frame_count += 1

        # フレームスキップ（例: 2フレームに1回だけ処理）
        if self.frame_count % 2 != 0:
            return

        # CompressedImageからOpenCV画像へデコード
        np_arr = np.frombuffer(msg.data, np.uint8)
        source = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

        # リサイズ
        resized = cv2.resize(source, (320, 240))

        # 処理（グレースケール＋二値化）
        gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
        _, result = cv2.threshold(gray, 128, 255, cv2.THRESH_BINARY)

        # 表示
        cv2.imshow("Color Image", resized)
        cv2.imshow("Processed Image", result)
        cv2.waitKey(1)

        self.get_logger().info(f'Frame {self.frame_count}: image processed')


def main():
    rclpy.init()
    node = CompressedImgProc()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        cv2.destroyAllWindows()
        rclpy.shutdown()


```

#### ビルド
```ruby:
$ cd ~/my_image
$ colcon build
```

#### 設定ファイルの編集
オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
```ruby:.bashrcの編集
$ emacs ~/.bashrc
```
**source ~/image_ws/install/setup.bash**

を追加する．ついでにこちらでも下記を追加

**export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp**



#### 設定ファイルの反映

```ruby:
$ source ~/.bashrc
```



## 3. ノードの実行（実機）
※カメラノードの動作確認だけであれば bringup コマンドは不要

```ruby:ロボット（TurtleBot3）にて
$ ros2 launch turtlebot3_bringup robot.launch.py
```

```ruby:ロボット（TurtleBot3）にて(別のターミナルで)
$ ros2 run my_cam my_cam_node
```

※下記２つはリモートPC側でコマンド入力
```ruby:リモートPCにて
$ ros2 run my_image my_image_node
```

TurtleBot3に接続したUSBカメラの画像が表示できれば成功です．
通常の画像と２値化処理された画像の２つが表示されます．