Webでカメラ表示

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

PiCamera V2使用

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

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

$ sudo raspi-config

[3 Interface Options] を選択して [I1 Legacy Camera] を Enableにして再起動します。

導入方法は以下のサイトを参考にしました。
ROS2 galacticでMIPIカメラから画像を取る

以下のコマンドで v4l2-ctlが使えるようにします。

$ sudo apt update
$ sudo apt install v4l-utils

インストールができたら、以下のコマンドで /dev/video0の状態を確認します。

$ 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を編集します。

$ sudo nano /boot/firmware/config.txt

[pi4]や[all]の場所にいかの記述がなければ追記します。

start_x=1
gpu_mem=128
gpu_mem_1024=256

Raspberry Pi 4B 4Gや8Gの場合は gpu_mem_1024=512でも良さそうです。
そのあたりのGPUのメモリ設定は以下リンクを参照ください。
https://www.raspberrypi.com/documentation/computers/config_txt.html

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

v4l2-cameraパッケージの追加

参考にしたサイトと導入方法は一緒です。
ROS2の種類の部分だけhumbleに変えてインストールします。

$ sudo apt install ros-humble-v4l2-camera

インストールができたら、表示されるか確認します。

$ ros2 run v4l2_camera v4l2_camera_node

別のターミナルから

$ ros2 run rqt_image_view rqt_image_view

を実行します。

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

rqtが起動したら、左上のプルダウンで /image_raw を選択すると映像が表示されます。

確認できたら、一旦Ctrl+Cで終了します。

image-transportパッケージの追加

こちらも参考サイトの導入と同じです。

$ sudo apt install ros-humble-image-transport

上記コマンドでインストールしたら、以下のコマンドで圧縮画像を表示させます。

$ ros2 run image_transport republish raw --ros-args --remap in:=/image_raw --remap out:=/image_raw/compressed

別のターミナルで再度rqtを起動します。

ros2 run rqt_image_view rqt_image_view

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

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

Web用の画像配信ノードの作成

上記で画像を表示できるようになりましたが、Webで画像を表示させるためにはもう一苦労必要になります。

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

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

以下のコマンドでワークスペースにパッケージを作成します。

$ 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

※ 上記コードはROSboardのコードを参考にしています（cv_bridge.pyなんてまるまるそのまんま使ってます）

作成できたら、ビルドします。

$ cd ~/[ワークスペース]
$ colcon build --packages-select web_image
$ source install/setup.bash

ついでに、.bashrcに以下のコマンドも追加しておきます。

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();
    });
});

以下の部分のホスト名を修正お忘れなく

url : 'ws://[ホスト名].local:9090'

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

$ cd ~/[ワークスペース]
$ ros2 launch camera.launch.py

別のターミナルで

$ ros2 launch rosbridge_server rosbridge_websocket_launch.xml

ローンチファイルが実行されたら、ブラウザからhttp://ホスト名.local/index.html を入力して左上にある[start]ボタンを押すと画像が表示されればOKです。

蛇足

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

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

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

目次