HARD2021：OpenCVとPythonプログラムで画像処理をしよう！

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

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

Converting between ROS images and OpenCV images (Python)

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

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

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

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

class ImageConverter:
    def __init__(self, node_name):
        self.node_name = node_name
        self.image_pub = rospy.Publisher(node_name, Image, queue_size=1)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/create1/camera/image_raw",Image,self.callback)

    def callback(self,data):
        try:
            # ROSの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:
        node_name = 'camera' 
        rospy.init_node(node_name, anonymous=True)
        ImageConverter(node_name)
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
        cv2.destroyAllWindows()

カメラの追加

現在のロボットモデルにはカメラが搭載されいないので追加する。ロボットモデルについては~/catkin_ws/src/create_autonomy/ca_description/urdfの中でいろいろ記述されている。カメラを追加するために以下の２つのファイルに追記する。なお、既に追加済みのファイルがあるので、次のファイル編集作業をやる必要はありません。

ファイル編集
- create_base.xacro：110行目 <!– Simulation sensors –>の上に以下を挿入する。

- create_base_gazebo.xacro：下から３行目に以下を挿入する。

追加済みファイルへの変更

以下のurdf_camera2021.tarファイルをクリックしてダウンロードする。~/catkin_ws/src/create_autonomy/ca_description/下に移動する。
- urdf_camera2021.tar
端末を開いて、ディレクトリへ移動してurdf_camera2021.tarを展開する。
- $ cd ~/catkin_ws/src/create_autonomy/ca_description
- $ tar xvf urdf_camera2021.tar
追加済みファイルを上書きする前にバックアップを取る
- $ cd ~/catkin_ws/src/create_autonomy/ca_description/urdf
- $ cp create_base.xacro create_base.xacro.org
- $ cp create_base_gazebo.xacro create_base_gazebo.xacro.org
追加済みファイルを上書きする。
- $ cp ../urdf_camera/create_base_camera.xacro create_base.xacro
- $ cp ../urdf_camera/create_base_gazebo_camera.xacro create_base_gazebo.xacro

パッケージの作成

次のコマンドでcameraパッケージを作りましょう！
- $ cd ~/catkin_ws/src/hard2021
- $ catkin_create_pkg camera sensor_msgs cv_bridge rospy std_msgs

ソースコードの作成

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

ビルド

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

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

実　行

端末を３つ開き、以下のコマンドを実行する。
１番目の端末：シミュレータの起動
- rvizを使わないので次のコマンドでcreate_autonomyパッケージ用の環境変数RVIZをfalse(無効)にする。
  - $ export RVIZ=false
- Gazeboの起動
  - $ roslaunch ca_gazebo create_empty_world.launch
- 起動したGazeboにコーラ缶(Coke Can)をロボットの前に挿入する。
  - GazeboのInsert タブ→http://gazbosim.org/models/→Coke Can
２番目の端末。cameraノードの起動
- $ rosrun camera camera.py
３番目の端末。遠隔操作
- $ roslaunch ca_tools keyboard_teleop.launch

ハンズオン

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