Webでカメラ表示::Raspberry Pi4 + Ubuntu22.04 + ROS2 Humble でロボット作り（その６）

# Webでカメラ表示

前回ROS2とWebの連携ができたので、今回はラズパイのカメラ映像をWebで見れるようにします。

## PiCamera V2使用

USBカメラではなく、PiCameraのV2使います。（MIPIカメラって言うのかな）

raspi-configでカメラを有効にしていない場合は以下のコマンドで有効にします。

```bash
$ sudo raspi-config
```
[3 Interface Options] を選択して [I1 Legacy Camera] を Enableにして再起動します。

導入方法は以下のサイトを参考にしました。
[ROS2 galacticでMIPIカメラから画像を取る](https://zenn.dev/katsuitoh/articles/3585925a819b7d)

以下のコマンドで v4l2-ctlが使えるようにします。
```bash
$ sudo apt update
$ sudo apt install v4l-utils
```
インストールができたら、以下のコマンドで /dev/video0の状態を確認します。
```bash
$ v4l2-ctl --list-devices
bcm2835-codec-decode (platform:bcm2835-codec):
	/dev/video10
	/dev/video11
	/dev/video12
	/dev/video18
	/dev/video31
	/dev/media1

bcm2835-isp (platform:bcm2835-isp):
	/dev/video13
	/dev/video14
	/dev/video15
	/dev/video16
	/dev/video20
	/dev/video21
	/dev/video22
	/dev/video23
	/dev/media0
	/dev/media2

mmal service 16.1 (platform:bcm2835-v4l2-0):
	/dev/video0
```

/dev/video0が表示されていて、mmal service と出ていればOKです。
※有効になっていない場合、/dev/video0は表示されても上記のように出ていない場合があります。

次のコマンドでconfig.txtを編集します。
```bash
$ sudo nano /boot/firmware/config.txt
```
[pi4]や[all]の場所にいかの記述がなければ追記します。
```
start_x=1
gpu_mem=128
gpu_mem_1024=256
```
:::note info
Raspberry Pi 4B 4Gや8Gの場合は gpu_mem_1024=512でも良さそうです。
そのあたりのGPUのメモリ設定は以下リンクを参照ください。
https://www.raspberrypi.com/documentation/computers/config_txt.html
:::

修正したら、再起動してください。

## v4l2-cameraパッケージの追加
参考にしたサイトと導入方法は一緒です。
ROS2の種類の部分だけhumbleに変えてインストールします。

```bash
$ sudo apt install ros-humble-v4l2-camera
```
インストールができたら、表示されるか確認します。

```bash
$ ros2 run v4l2_camera v4l2_camera_node
```
別のターミナルから
```bash
$ ros2 run rqt_image_view rqt_image_view
```
を実行します。

:::note warn
rqtで画像を確認するのでリモートデスクトップ環境やモニタを繋いだ状態で確認します。
:::

rqtが起動したら、左上のプルダウンで /image_raw を選択すると映像が表示されます。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51900/5a3f4871-dfa0-11fb-bfac-427be2a3cdfa.png)
確認できたら、一旦Ctrl+Cで終了します。

## image-transportパッケージの追加
こちらも参考サイトの導入と同じです。

```bash
$ sudo apt install ros-humble-image-transport
```
上記コマンドでインストールしたら、以下のコマンドで圧縮画像を表示させます。
```bash
$ ros2 run image_transport republish raw --ros-args --remap in:=/image_raw --remap out:=/image_raw/compressed
```
別のターミナルで再度rqtを起動します。
```bash
ros2 run rqt_image_view rqt_image_view
```

左上のプルダウンで /image_raw/compressed が選択できるようになっておれば、圧縮画像の送信もできています。

では、一旦Ctrl+Cで終了してください。

## Web用の画像配信ノードの作成
上記で画像を表示できるようになりましたが、Webで画像を表示させるためにはもう一苦労必要になります。

simplejpeg を使用するので、インストールします。
※pip3 を使ってインストールするため、pip3もインストールします。

```bash
$ sudo apt install -y python3-pip
$ pip3 install simplejpeg
```

以下のコマンドでワークスペースにパッケージを作成します。
```bash
$ cd ~/[ワークスペース]/src
$ ros2 pkg create --build-type ament_python --node-name web_image_node web_image
```
パッケージの生成ができたら、Visual Studio Codeなどで web_image/web_image/web_image_node.py を編集します。

```python web_image_node.py
import base64
import simplejpeg
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import String
from rclpy.qos import qos_profile_sensor_data
from web_image.cv_bridge import imgmsg_to_cv2

class WebImageNode(Node):
    def __init__(self):
        super().__init__('web_image_node')
        self.subscription = self.create_subscription(
            Image,
            #'image_raw',
            'image_raw/compressed',
            self.image_callback,
            qos_profile_sensor_data
        )
        self.subscription
        self.pub = self.create_publisher(String, 'web_image', 10)

    def image_callback(self, msg):
        cv2_img = imgmsg_to_cv2(msg, flip_channels = True)    
        # if image has alpha channel, cut it out since we will ultimately compress as jpeg
        if len(cv2_img.shape) == 3 and cv2_img.shape[2] == 4:
            cv2_img = cv2_img[:,:,0:3]

        # if image has only 2 channels, expand it to 3 channels for visualization
        # channel 0 -> R, channel 1 -> G, zeros -> B
        if len(cv2_img.shape) == 3 and cv2_img.shape[2] == 2:
            cv2_img = np.stack((cv2_img[:,:,0], cv2_img[:,:,1], np.zeros(cv2_img[:,:,0].shape)), axis = -1)

        # if image format isn't already uint8, make it uint8 for visualization purposes
        if cv2_img.dtype != np.uint8:
            if cv2_img.dtype == np.uint64:
                # keep only the most significant 8 bits (0 to 255)
                cv2_img = (cv2_img >> 56).astype(np.uint8)
            elif cv2_img.dtype == np.uint32:
                # keep only the most significant 8 bits (0 to 255)
                cv2_img = (cv2_img >> 24).astype(np.uint8)
            elif cv2_img.dtype == np.uint16:
                # keep only the most significant 8 bits (0 to 255)
                cv2_img = (cv2_img >> 8).astype(np.uint8)
            elif cv2_img.dtype == np.float16 or cv2_img.dtype == np.float32 or cv2_img.dtype == np.float64:
                # map the float range (0 to 1) to uint8 range (0 to 255)
                cv2_img = np.clip(cv2_img * 255, 0, 255).astype(np.uint8)

        try:
            if len(cv2_img.shape) == 2:
                cv2_img = np.expand_dims(cv2_img, axis=2)
                if not cv2_img.flags['C_CONTIGUOUS']:
                    cv2_img = cv2_img.copy(order='C')
                img_jpeg = simplejpeg.encode_jpeg(cv2_img, colorspace = "GRAY", quality = 50)
            elif len(cv2_img.shape) == 3:
                if not cv2_img.flags['C_CONTIGUOUS']:
                    cv2_img = cv2_img.copy(order='C')
                if cv2_img.shape[2] == 1:
                    img_jpeg = simplejpeg.encode_jpeg(cv2_img, colorspace = "GRAY", quality = 50)
                elif cv2_img.shape[2] == 4:
                    img_jpeg = simplejpeg.encode_jpeg(cv2_img, colorspace = "RGBA", quality = 50)
                elif cv2_img.shape[2] == 3:
                    img_jpeg = simplejpeg.encode_jpeg(cv2_img, colorspace = "RGB", quality = 50)
            else:
                img_jpeg = b''

        except OSError as e:
            print(e)    
        else:
            pub_msg = String()
            pub_msg.data = base64.b64encode(img_jpeg).decode()
            self.pub.publish(pub_msg)
            #self.get_logger().info(f'publish')

def main():
    try:
        print('Hi from web_image.')
        rclpy.init()
        node = WebImageNode()
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('Ctrl+C received - exiting...')
    finally:
        node.get_logger().info('ROS node shutdown')
        node.destroy_node()
        rclpy.shutdown()
        print('program close')

if __name__ == '__main__':
    main()
```
次に、同じディレクトリに cv_bridge.py を作ります。
```python cv_bridge.py
#!/usr/bin/env python3

"""
Mimics the functionality of cv_bridge but in pure python, without OpenCV dependency.

An additional optional flip_channels argument is provided in the imgmsg_to_cv2() function
for convenience (e.g. if resulting numpy array is going to be fed directly into a neural network
that expects channels in RGB order instead of OpenCV-ish BGR).

Author: dheera@dheera.net
"""

import numpy

BPP = {
  'bgr8': 3,
  'rgb8': 3,
  '16UC1': 2,
  '8UC1': 1,
  'mono16': 2,
  'mono8': 1
}

def imgmsg_to_cv2(data, desired_encoding="passthrough", flip_channels=False):
    """
    Converts a ROS image to an OpenCV image without using the cv_bridge package,
    for compatibility purposes.
    """

    if desired_encoding == "passthrough":
        encoding = data.encoding
    else:
        encoding = desired_encoding

    if (encoding == 'bgr8' and not flip_channels) or (encoding=='rgb8' and flip_channels):
        return numpy.frombuffer(data.data, numpy.uint8).reshape((data.height, data.width, 3))
    elif (encoding == 'rgb8' and not flip_channels) or (encoding=='bgr8' and flip_channels):
        return numpy.frombuffer(data.data, numpy.uint8).reshape((data.height, data.width, 3))[:, :, ::-1]
    elif (encoding == 'bgra8' and not flip_channels) or (encoding == 'rgba8' and flip_channels):
        return numpy.frombuffer(data.data, numpy.uint8).reshape((data.height, data.width, 4))[:,:,0:3]
    elif (encoding == 'rgba8' and not flip_channels) or (encoding == 'bgra8' and flip_channels):
        return numpy.frombuffer(data.data, numpy.uint8).reshape((data.height, data.width, 4))[:, :, ::-1][:,:,1:]
    if (encoding == 'bgr16' and not flip_channels) or (encoding=='rgb16' and flip_channels):
        return numpy.frombuffer(data.data, numpy.uint16).reshape((data.height, data.width, 3))
    elif (encoding == 'rgb16' and not flip_channels) or (encoding=='bgr16' and flip_channels):
        return numpy.frombuffer(data.data, numpy.uint16).reshape((data.height, data.width, 3))[:, :, ::-1]
    elif (encoding == 'bgra16' and not flip_channels) or (encoding == 'rgba16' and flip_channels):
        return numpy.frombuffer(data.data, numpy.uint16).reshape((data.height, data.width, 4))[:,:,0:3]
    elif (encoding == 'rgba16' and not flip_channels) or (encoding == 'bgra16' and flip_channels):
        return numpy.frombuffer(data.data, numpy.uint16).reshape((data.height, data.width, 4))[:, :, ::-1][:,:,1:]
    # 8-bit unsigned int types
    elif encoding == 'mono8' or encoding == '8UC1':
        return numpy.frombuffer(data.data, numpy.uint8).reshape((data.height, data.width))
    elif encoding == '8UC2':
        return numpy.frombuffer(data.data, numpy.uint8).reshape((data.height, data.width, 2))
    elif encoding == '8UC3':
        return numpy.frombuffer(data.data, numpy.uint8).reshape((data.height, data.width, 3))
    elif encoding == '8UC4':
        return numpy.frombuffer(data.data, numpy.uint8).reshape((data.height, data.width, 4))
    # 16-bit unsigned int types
    elif encoding == 'mono16' or encoding == '16UC1':
        return numpy.frombuffer(data.data, numpy.uint16).reshape((data.height, data.width))
    elif encoding == '16UC2':
        return numpy.frombuffer(data.data, numpy.uint16).reshape((data.height, data.width, 2))
    elif encoding == '16UC3':
        return numpy.frombuffer(data.data, numpy.uint16).reshape((data.height, data.width, 3))
    elif encoding == '16UC4':
        return numpy.frombuffer(data.data, numpy.uint16).reshape((data.height, data.width, 4))
    # 8-bit signed int types
    elif encoding == '8SC1':
        return numpy.frombuffer(data.data, numpy.int8).reshape((data.height, data.width))
    elif encoding == '8SC2':
        return numpy.frombuffer(data.data, numpy.int8).reshape((data.height, data.width, 2))
    elif encoding == '8SC3':
        return numpy.frombuffer(data.data, numpy.int8).reshape((data.height, data.width, 3))
    elif encoding == '8SC4':
        return numpy.frombuffer(data.data, numpy.int8).reshape((data.height, data.width, 4))
    # 16-bit signed int types
    elif encoding == '16SC1':
        return numpy.frombuffer(data.data, numpy.int16).reshape((data.height, data.width))
    elif encoding == '16SC2':
        return numpy.frombuffer(data.data, numpy.int16).reshape((data.height, data.width, 2))
    elif encoding == '16SC3':
        return numpy.frombuffer(data.data, numpy.int16).reshape((data.height, data.width, 3))
    elif encoding == '16SC4':
        return numpy.frombuffer(data.data, numpy.int16).reshape((data.height, data.width, 4))
    # 32-bit signed int types
    elif encoding == '32SC1':
        return numpy.frombuffer(data.data, numpy.int32).reshape((data.height, data.width))
    elif encoding == '32SC2':
        return numpy.frombuffer(data.data, numpy.int32).reshape((data.height, data.width, 2))
    elif encoding == '32SC3':
        return numpy.frombuffer(data.data, numpy.int32).reshape((data.height, data.width, 3))
    elif encoding == '32SC4':
        return numpy.frombuffer(data.data, numpy.int32).reshape((data.height, data.width, 4))
    # 32-bit float types
    elif encoding == '32FC1':
        return numpy.frombuffer(data.data, numpy.float32).reshape((data.height, data.width))
    elif encoding == '32FC2':
        return numpy.frombuffer(data.data, numpy.float32).reshape((data.height, data.width, 2))
    elif encoding == '32FC3':
        return numpy.frombuffer(data.data, numpy.float32).reshape((data.height, data.width, 3))
    elif encoding == '32FC4':
        return numpy.frombuffer(data.data, numpy.float32).reshape((data.height, data.width, 4))
    # 64-bit float types
    elif encoding == '64FC1':
        return numpy.frombuffer(data.data, numpy.float64).reshape((data.height, data.width))
    elif encoding == '64FC2':
        return numpy.frombuffer(data.data, numpy.float64).reshape((data.height, data.width, 2))
    elif encoding == '64FC3':
        return numpy.frombuffer(data.data, numpy.float64).reshape((data.height, data.width, 3))
    elif encoding == '64FC4':
        return numpy.frombuffer(data.data, numpy.float64).reshape((data.height, data.width, 4))
    else:
        print("Unsupported encoding %s" % encoding)
        return None

def cv2_to_imgmsg(cv2img, encoding='bgr8'):
    """
    Converts an OpenCV image to a ROS image without using the cv_bridge package,
    for compatibility purposes.
    """

    from sensor_msgs.msg import Image
    
    msg = Image()
    msg.width = cv2img.shape[1]
    msg.height = cv2img.shape[0]
    msg.encoding = encoding
    msg.step = BPP[encoding]*cv2img.shape[1]
    msg.data = numpy.ascontiguousarray(cv2img).tobytes()

    return msg
```
:::note info
※ 上記コードは[ROSboard](https://github.com/dheera/rosboard)のコードを参考にしています（cv_bridge.pyなんてまるまるそのまんま使ってます）
:::

作成できたら、ビルドします。
```bash
$ cd ~/[ワークスペース]
$ colcon build --packages-select web_image
$ source install/setup.bash
```

ついでに、.bashrcに以下のコマンドも追加しておきます。
```bash
source ${HOME}/[ワークスペース]/install/setup.bash
```

## カメラ起動用のローンチファイルを作成

ここまで出来たら、ワークスペース直下にカメラ起動用のローンチファイルを作成します。
```python camera.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    v4l2_node = Node(
        package='v4l2_camera',
        executable='v4l2_camera_node',
        output='screen',
        parameters=[{'image_size': [640, 480]}]
    )

    image_transport = Node(
        package='image_transport',
        executable='republish',
        arguments=["raw"],
        remappings=[('in', '/image_raw'),
                    ('out', '/image_raw/compressed')
                   ]
    )

    web_image = Node(
        package='web_image',
        executable='web_image_node'
    )

    return LaunchDescription([
        v4l2_node,
        image_transport,
        web_image
    ])
```

## Web側の準備
前回作成したファイルを修正して画像を表示できるようにします。

/home/[ユーザー]/www/html 以下に、index.htmlを作成します。
```html index.html
<!DOCTYPE html>
<html lang="ja">
<head>
    <meta charset="UTF-8">
    <meta name="viewport" content="width=device-width">
    <title>Web Image</title>
    <link rel="stylesheet" href="css/index.css">
    <script src="https://cdnjs.cloudflare.com/ajax/libs/jquery/3.6.1/jquery.min.js"></script>
    <script src="http://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
    <script src="http://static.robotwebtools.org/roslibjs/current/roslib.min.js"></script>
    <script src="js/index.js"></script>
</head>
<body>
    <div id="main-contents">
        <img id="camera_view" src="">
    </div>
    <div id="top-left">
        <button id="start">start</button>
        <button id="stop">stop</button>
    </div>
</body>
</html>
```

次にCSSを/home/[ユーザー]/www/html/css 以下に作成します。

```css index.css
body{
    background: gray;
    color: white;
}

#main-contents {
    margin: 0;
    padding: 0;
    position: relative;
    top: 0;
    left: 0;
    text-align: center;
}

#camera_view {
    width: auto;
    height: 100vh;
}

#top-left {
    position: absolute;
    top: 0;
    left: 0;
}
```

次にJavascriptを/home/[ユーザー]/www/html/js 以下に作成します。

```javascript index.js
$(function() {
    let ros = new ROSLIB.Ros({
        url : 'ws://[ホスト名].local:9090'
    });

    ros.on('connection', function() {
        console.log('Connected to websocket server.');
    });
    ros.on('error', function(error) {
        console.log('Error connecting to websocket server: ', error);
    });
    ros.on('close', function() {
         console.log('Connection to websocket server closed.');
    });

    // Camera
    let camera_listener = new ROSLIB.Topic({
        ros : ros,
        name : '/web_image',
        messageType : 'std_msgs/msg/String'
    });

    function ros_camera_topic_subscribe() {
        camera_listener.subscribe(function(message) {
            const imagedata = "data:image/jpeg;base64," + message.data;
            setBlob(imagedata);
        });
    }

    let prevSrc = '';

    async function setBlob(data_uri){
        const blob = await (await fetch(data_uri)).blob();
        let nowSrc = window.URL.createObjectURL(blob);
        document.getElementById('camera_view').src = nowSrc;
        if (prevSrc !== '') window.URL.revokeObjectURL(prevSrc);
        prevSrc = nowSrc;
    }

    $('#start').on('click', function() {
        ros_camera_topic_subscribe();
    });

    $('#stop').on('click', function() {
        camera_listener.unsubscribe();
    });
});
```

:::note warn
以下の部分のホスト名を修正お忘れなく
```
url : 'ws://[ホスト名].local:9090'
```
:::

これで準備は出来ましたので、表示してみましょう。

```bash
$ cd ~/[ワークスペース]
$ ros2 launch camera.launch.py
```
別のターミナルで
```bash
$ ros2 launch rosbridge_server rosbridge_websocket_launch.xml
```

ローンチファイルが実行されたら、ブラウザからhttp://ホスト名.local/index.html を入力して左上にある[start]ボタンを押すと画像が表示されればOKです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51900/c72ff2dd-6199-32aa-295e-3afd9677f092.png)

## 蛇足
ROS2で生成される画像は、そのままだとWeb側で表示できませんでした。
なので、一度 web_imageノードで受け取ったあと、DataURI形式に変換して、std_msgs/msg/String形式でデータを送っています。

また、Web側もDataURIをそのまま画像として表示させるのは上限があり小さな画像でないと表示できないため、一旦BLOB形式に変換して表示するようにしています。（1280x960までは確認できました）
ただ、DataURIからBLOBに変換するとメモリを食うので、一度表示した画像のBLOBは破棄する処理を書いています。
なのですが、長時間表示させるとブラウザが固まるのでうまく破棄できていないのかもです。

次回は、IMUを実装してWeb連携を書く予定です。

## 目次

* [[←前回]ROS2とWebの連携::Raspberry Pi4 + Ubuntu22.04 + ROS2 Humble でロボット作り（その５）](https://qiita.com/shoichi4411/items/fcbe4b0fc2d98c72b55e)
* [[次回→]WebでIMU取得::Raspberry Pi4 + Ubuntu22.04 + ROS2 Humble でロボット作り（その７）](https://qiita.com/shoichi4411/items/bb9e792a0dfb600c90ad)