ROS2による画像処理

画像処理プログラミングの方法について学びます．
Tags:
Categories:

目的

ロボットの自律移動に必要な外界の情報を獲得するために様々なセンサが搭載されています． 多くのロボットに搭載されるセンサの1つとしてカメラがあります． 講義の冒頭ではスライドを用いてディジタル画像処理について学び， 後半では画像処理プログラミングの方法について学びます．

インストール手順

ROS2にてカメラを使うためにパッケージをインストールします． ノートPCに搭載されたカメラやUSBカメラを使うようにするためのパッケージは幾つかありますが， 今回はusb_camパッケージを利用します．

1

$ sudo apt install ros-humble-v4l2-camera

起動と画像の表示

インストールしたusb_camパッケージを実行してみましょう．

1

$ ros2 run v4l2_camera v4l2_camera_node

これでROS2で画像トピックを扱えるようになります． コマンドラインツールでどんなトピックが配信されているか確認しましょう．

$ ros2 topic list 
/camera_info
/image_raw
/image_raw/compressed
/image_raw/compressedDepth
/image_raw/theora
/parameter_events
/rosout

複数のトピックが配信されていることがわかりますが，画像トピックは/image_rawです． 画像トピックのメッセージ型を調べてみましょう．

$ ros2 topic type /image_raw
sensor_msgs/msg/Image

トピック/image_rawのメッセージ型はsensor_msgs/msg/Imageであることが確認できました． メッセージ型sensor_msgs/msg/Imageでどんなデータが配信されているか調べます．

$ ros2 interface show sensor_msgs/msg/Image
# This message contains an uncompressed image
# (0, 0) is at top-left corner of image

std_msgs/Header header # Header timestamp should be acquisition time of image
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id
                             # Header frame_id should be optical frame of camera
                             # origin of frame should be optical center of cameara
                             # +x should point to the right in the image
                             # +y should point down in the image
                             # +z should point into to plane of the image
                             # If the frame_id here and the frame_id of the CameraInfo
                             # message associated with the image conflict
                             # the behavior is undefined

uint32 height                # image height, that is, number of rows
uint32 width                 # image width, that is, number of columns

# The legal values for encoding are in file src/image_encodings.cpp
# If you want to standardize a new string format, join
# ros-users@lists.ros.org and send an email proposing a new encoding.

string encoding       # Encoding of pixels -- channel meaning, ordering, size
                      # taken from the list of strings in include/sensor_msgs/image_encodings.hpp

uint8 is_bigendian    # is this data bigendian?
uint32 step           # Full row length in bytes
uint8[] data          # actual matrix data, size is (step * rows)

出力された情報が多いので分かりにくいですが，画像の縦方向の大きさがheight， 横方向の大きさがwidth，画像フォーマットはencoding，画像データはwidthにて配信されます． 最後にtopic echoコマンドで画像データを購読します．

eader:
  stamp:
    sec: 1710914261
    nanosec: 466276747
  frame_id: camera
height: 480
width: 640
encoding: rgb8
is_bigendian: 0
step: 1920
data:
- 105
- 106
- 101
- 105
- 106

表示される情報が多いので，上記は出力の大半を省略しています． 上記からカメラから得られた画像の大きさは，480x640画素，画像フォーマットはrgb8であることがわかります． rgb8とは，画像の1画像を赤，緑，青，各色8ビット(2の8乗=256色)で表現していることを表します． 各画素の色情報をdataで表現していますが，画素が多いためターミナルで表示するには限界があります． そこで，ビューアーを利用して画像を表示しましょう． 画像を表示する方法は幾つかありますが，今回はrqtを用いた画像ビューアーを用いて確認します．

1

$ ros2 run rqt_image_view rqt_image_view

画面左上に画像トピックを選ぶことができますので，image_rawを選択して画像が表示できるか確認してください．

ここまでの内容で画像がトピック通信により配信されていることが確認できました． プログラムにより画像トピックを購読し，その情報を元にプログラミングすれば，画像処理を用いた走行が可能となります． ただし，購読したトピックはメッセージ型のフォーマットとなるため， 一般的には画像処理ライブラリOpenCVで扱いやすいCV:Mat型に変換します． 変換時に使用されるのがcv_bridgeと呼ばれるライブラリです．

画像処理プログラム

配信されている画像トピックをプログラムで購読し，簡単な画像処理をしてみましょう． 今回は取得した画像の色を情報を表示するプログラムを作成します．

これまでに学んだ方法にてワークスペースとパッケージを作成し， 下記のサンプルプログラムを入力してください．

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

import rclpy
import cv2
from rclpy.node import Node
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

class ColorChecker(Node):
    WIDTH = 50
    HEIGHT = 50
    
    def __init__(self):
        super().__init__('color_checker_node')
        self.image_sub = self.create_subscription(Image, "/image_raw", self.image_callback, 10)
        self.bridge = CvBridge()

    def image_callback(self, data):
        try:
            color_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            self.get_logger().info(e)

        h, w, c = color_image.shape
        x1 = int(w / 2) - self.WIDTH
        x2 = int(w / 2) + self.WIDTH
        y1 = int(h / 2) - self.HEIGHT
        y2 = int(h / 2) + self.HEIGHT

        r = g = b = 0
        for i in range(y1, y2):
            for j in range(x1, x2):
                b = b + color_image[i, j, 0]
                g = g + color_image[i, j, 1]
                r = r + color_image[i, j, 2]

        ave_r = r / ((self.WIDTH * 2) * (self.HEIGHT * 2))
        ave_g = g / ((self.WIDTH * 2) * (self.HEIGHT * 2))
        ave_b = b / ((self.WIDTH * 2) * (self.HEIGHT * 2))

        cv2.rectangle(color_image, (x1, y1), (x2, y2), (255, 255, 255))
        self.get_logger().info(f'r:{ave_r}, g:{ave_g}, b:{ave_b}')
        cv2.imshow("Origin Image", color_image)
        cv2.waitKey(10)

def main(args=None):
    rclpy.init(args=args)
    node = ColorChecker()
    rclpy.spin(node)

    cv2.destroyAllWindows()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

ビルドして実行すると下記のようなウィンドウが表示されます． 画像の中央に白色の正方形が表示され，この正方形内の画素値の平均をターミナルで出力しています． 1枚目の画像は赤色に印刷さた用紙に白色の矩形が表示され， ターミナルの出力は，おおよそr(赤)が185，g(緑)が82，b(青)が101が出力されています． 赤色の成分が強く出力されているのが確認できました． 2枚目の画像は紫色に印刷された領域に白色の矩形が表示され， ターミナルの出力からは先ほどの結果よりrが小さくなり，bが強くなっています．

実行結果1 実行結果1

プログラムの内容

プログラムの内容を確認しましょう．

1
2
3
4
5

import rclpy
import cv2
from rclpy.node import Node
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

必要なモジュールを読み込んでいます． OpenCVを使用する場合にはcv2の他に，ROSのデータからOpenCVで扱えるデータ形式に変換する必要があるためCvBridgeを読み込む必要があります．

7
8
9

class ColorChecker(Node):
    WIDTH = 50
    HEIGHT = 50

ColorCheckerというクラスを定義しています． また，クラス変数としてWIDTHとHEIGHTを宣言し，それぞれに値を代入しています． これら2つの変数は注目する領域の大きさを表しています．実際には，(WIDTHx2)x(HEIGHTx2)の画素数となるので注意してください．

11
12
13
14

    def __init__(self):
        super().__init__('color_checker_node')
        self.image_sub = self.create_subscription(Image, "/image_raw", self.image_callback, 10)
        self.bridge = CvBridge()

__init__関数内でトピック/image_rawを購読した際のコールバック関数を定義しています． また，メッセージ型として購読される画像をCV:Mat型に変換するためにCvBridgeを使用する準備をしています．

16
17
18
19
20

    def image_callback(self, data):
        try:
            color_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            self.get_logger().info(e)

画像を購読するとimage_callback関数が実行されます． メッセージ型として購読される画像(data)をCvBridgeのimgmsg_to_cv2によりCV:Mat型に変換してcolor_imageに代入しています． imgmsg_to_cv2の第2引数で変換する画像フォーマットを指定できます． 今回はCV:Mat型に対応するために"bgr8"にしています． dataでは色の並び順がRGB(赤緑青)ですが，BGR(青緑赤)の順番になることに注意してください． rgb8も用意されていますが，後述する画像の表示にはopencvで用意されている関数を使用しており， 色がBGRの順に並んでいることを想定しているため，画像を表示すると赤と青が入れ替わってしまいます． それを防ぐためにも"bgr8"を指定してプログラムで並び順に対応します．

22
23
24
25
26

        h, w, c = color_image.shape
        x1 = int(w / 2) - self.WIDTH
        x2 = int(w / 2) + self.WIDTH
        y1 = int(h / 2) - self.HEIGHT
        y2 = int(h / 2) + self.HEIGHT

画像データから画像の縦方向の大きさ，横方向の大きさ，チャンネル数(RGBの3色)を読み取り変数に代入しています． また，注目領域の左上の座標(x1,y1)と右下の座標(x2,y2)を計算しています．

28
29
30
31
32
33

        r = g = b = 0
        for i in range(y1, y2):
            for j in range(x1, x2):
                b = b + color_image[i, j, 0]
                g = g + color_image[i, j, 1]
                r = r + color_image[i, j, 2]

3次元配列で表現されている画像データcolor_imageの注目領域の画素にアクセスし，それぞれの画素値をr，g，bに足し込んでいます． 先述した通り，画像の色がBGRの順番に並んでいることに注意してください．

35
36
37

        ave_r = r / ((self.WIDTH * 2) * (self.HEIGHT * 2))
        ave_g = g / ((self.WIDTH * 2) * (self.HEIGHT * 2))
        ave_b = b / ((self.WIDTH * 2) * (self.HEIGHT * 2))

各色の平均値を計算するために，注目領域の画素数(矩形の面積)で割っています．

39
40
41
42

        cv2.rectangle(color_image, (x1, y1), (x2, y2), (255, 255, 255))
        self.get_logger().info(f'r:{ave_r}, g:{ave_g}, b:{ave_b}')
        cv2.imshow("Origin Image", color_image)
        cv2.waitKey(10)

rectangle関数により注目領域を矩形で描画しています． 第4引数の数値を変えることで色を操作できます． 計算した各色の平均値をターミナルに出力しています． 41行目ではimshow関数を用いて画像を表示しています． 42行目のwaitkey関数は，画像を表示するウィンドウからキーボード入力を受け付ける関数です． 引数は，受け付けの待ち時間[msec]です． 指定した時間だけウィンドウに画像を表示することができます．

44
45
46
47
48
49
50
51
52
53
54

def main(args=None):
    rclpy.init(args=args)
    node = ColorChecker()
    rclpy.spin(node)

    cv2.destroyAllWindows()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

main関数は以前と同じ内容ですので省略します．