ロボットプログラミングⅡ-2021：ROS2演習11 – OpenCVとPythonプログラムで画像処理をしよう！

この記事は金沢工業大学ロボティクス学科で2021年後学期開講中のロボットプログラミングⅡ用です．

今回は、コンピュータビジョンライブラリの定番OpenCVをROSで使うためにcv_bridgeを用いたPythonプログラムを勉強します。サンプルのPythonプログラムではカメラから取得した画像のエッジを検出したり、指定した色領域を抽出しています。

なお、この記事は、以下のサンプルプログラムはROSチュートリアルと「ROSで始めるロボットプログラミング、著者：小倉崇、出版社：工学社」を参考にしました。

Converting between ROS images and OpenCV images (Python)

サンプルプログラム（説明付き）

このサンプルプログラムの概要を説明します。まず、カメラから取得したデータが /camera/image_rawトピックにあるので、それをサブスクライブします。次に、callback関数では、トピックの画像の生データdataはsensor_msgs/Image型なので、それをOpenCVで処理できるようにcv:Mat型に変換します。OpenCVでの処理は、指定した色領領域を抽出するためにマスク画像を生成し、それと画像のビット毎の論理積を計算したり、画像のエッジを検出するために、カラー画像からグレースケール画像に変換し、その結果をウインドウで表示しています。詳細については、以下のソースコードをご覧ください。

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rclpy
import cv2
import numpy as np
from rclpy.node import Node
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class ImageConverter(Node):
    def __init__(self):
        super().__init__('image_converter')
        self.image_pub = self.create_publisher(Image, "/image_topic", 10)
        self.image_sub = self.create_subscription(Image, "/camera/image_raw",self.callback,10)
        self.bridge = CvBridge()

    def callback(self,data):
        try:
            # ROS2のsensor_msgs/Image型からOpenCVで処理適量にcv::Mat型へ変換する。
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        # RGB表色系からHSV表色系に変換
        hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)

        # しきい値の設定（ここでは赤を抽出）
        color_min = np.array([150,100, 50])
        color_max = np.array([180,255,255])

        # マスク画像を生成
        color_mask = cv2.inRange(hsv_image, color_min, color_max);
        # 画像配列のビット毎の倫理積席。マスク画像だけが抽出される。
        cv_image2  = cv2.bitwise_and(cv_image, cv_image, mask = color_mask)

        # 重心を求める。moments関数を使うためグレースケール画像へ変換。
        gray_image2 = cv2.cvtColor(cv_image2, cv2.COLOR_BGR2GRAY)
        mu = cv2.moments(gray_image2, True)
        if mu["m00"] == 0: # マスクをかけた画像がない場合(ここでは赤）の処理
            x, y = -999, -999
        else:
            x,y= int(mu["m10"]/mu["m00"]) , int(mu["m01"]/mu["m00"])

        # 重心を中心として半径20ピックセルの円を描画
        color  = (0, 255, 0)
        center = (x, y)
        radius = 20
        cv2.circle(cv_image2, center, radius, color)

        # エッジを検出する。Canny関数を使うためRGBからグレースケールへ変換
        gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        cv_image3  = cv2.Canny(gray_image, 15.0, 30.0);


        # ウインドウのサイズを変更
        cv_half_image = cv2.resize(cv_image,   (0,0),fx=0.5, fy=0.5)
        cv_half_image2 = cv2.resize(cv_image2, (0,0),fx=0.5,fy=0.5);
        cv_half_image3 = cv2.resize(cv_image3, (0,0),fx=0.5,fy=0.5);

        # ウインドウ表示
        cv2.imshow("Origin Image", cv_half_image)
        cv2.imshow("Result Image", cv_half_image2)
        cv2.imshow("Edge Image",   cv_half_image3)
        cv2.waitKey(3)

        try:
            # パブリッシュするためにOpenCVのcv::MatからROSのsensor_msgs/Image型へ変換
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image2, "bgr8"))
        except CvBridgeError as e:
            print(e)


if __name__ == '__main__':   
    try:
        rclpy.init()
        image_converter = ImageConverter()
        rclpy.spin(image_converter)
        image_converter.destroy_node
        rclpy.shutdown()
    except KeyboardInterrupt:
        print("Shutting down")
        cv2.destroyAllWindows()

依存関係のあるパッケージのインストール

$ sudo apt -y install ros-foxy-vision-opencv
$ sudo apt -y install ros-foxy-v4l2-camera
$ pip3 install opencv-python

パッケージの作成

次のコマンドでcameraパッケージを作りましょう！
- $ cd ~/airobot_ws/src
- $ ros2 pkg create --build-type ament_python --node-name camera camera

ソースコードの作成

エディタを開き、上のプログラムをコピペしてcamera.pyというファイル名で~/catkin_ws/src/camera/cameraディレクトリの中に保存する。以下はgeditを使う場合。
- $ cd ~/catkin_ws/src/camera/camera
- $ gedit camera.py &

ビルド

ROSにパッケージとして認識されるために、以下のコマンドでビルドする。
- $ cd ~/airobot_ws
- $ colcon build --symlink-install

rosrunコマンドで実行できるように実行権を与える。
- $ source ~/.bashrc
- $ cd ~/arobot_ws/src/camera/camera
- $ chmod u+x camera.py

実　行

端末を３つ開き、以下のコマンドを実行する。
１番目の端末：シミュレータの起動
- カメラの高さを低くしたいのでロボットモデルをhappy_miniではなくwaffle_piにする。
  - $ export TURTLEBOT3_MODEL=waffle_pi
- Gazeboの起動
  - $ ros2 launch turtlebot3_gazebo empty_world.launch.py
- 起動したGazeboにコーラ缶(Coke Can)をロボットの前に挿入する。
  - GazeboのInsert タブ→http://gazbosim.org/models/→Coke Can
２番目の端末。cameraノードの起動
- $ ros2 run camera camera.py
  - 上のコマンドを実行して、”No executable found”と出て実行できない場合は次のコマンドを実行する。
    - $ python3 ~/airobot_ws/src/camera/camera/camera.py
３番目の端末。遠隔操作
- $ ros2 run turtlebot3_teleop teleop_keyboard

ハンズオン

上の説明を参考にサンプルプログラムを実行して動作を確認しよう！
- 「パッケージの作成」、「ソースコードの作成」、「ビルド」、「実行」を順番にする。
- 起動したGazeboにコーラ缶(Coke Can)をロボットの前に挿入する。
  - Insert タブ→http://gazbosim.org/models/→Coke Can
- ロボットをキーボードで操作してコーラ缶の方向に進んだり、その場回転して画像処理が行われていることを確認する。
コーラ缶の手前で停止するプログラムを作ろう。なお、カメラでは物体の距離が直接わからないので、コーラ缶の輪郭を検出し、その面積や外接矩形のサイズ[pixel]から推定する必要がある。コーラ缶のサイズはgazeboのGUIで簡単に調べることができる。
コーラ缶を任意の位置において、それをロボットが探して近づき、手前で停止するプログラムを作ろう。なお、カメラの位置が比較的高いので近づいてコーラ缶を見失う場合には、urdfファイルを変更して高さを変えるなど工夫しよう。
勾配検出フィルタ
- サンプルプログラムではエッジ検出にcanny関数を使用した。その他にも勾配検出フィルタとして、Sobel関数、Laplacian関数などがある。参考リンクのOpenCV-Pythonチュートリアルで調べて、サンプルプログラムを改変して違いを見てみよう。
物体の輪郭抽出
- サンプルプログラムではコーラ缶の位置を推定するのに赤領域の重心を使った。より正確にするには物体の輪郭を検出するfindContours関数がある。参考リンクで調べて輪郭を使って物体の位置を推定してみよう。