HARD2020：ロボット視覚の作り方（Cameraと OpenCV）

この記事はHARD2020（Home AI Robot Development)ワークショップ用です。今回はcv_bridgeを使い、ROSでコンピュータビジョンライブラリOpenCVを使いカメラから取得した画像を処理します。この記事は、以下のROSチュートリアルと「ROSで始めるロボットプログラミング、小倉著」を参考にしています。

Converting between ROS images and OpenCV images (Python)

ソース

ROSのチュートリアルと「ROSで始めるロボットプログラミング」を参考に改変しています。

#!/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 image_converter:
    def __init__(self):
        self.image_pub = rospy.Publisher("image_topic", Image, queue_size=1)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/create1/camera/image_raw",Image,self.callback)

    def callback(self,data):
        try:
            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)
        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:
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image2, "bgr8"))
        except CvBridgeError as e:
            print(e)

def main():
    ic = image_converter()
    rospy.init_node('camera', anonymous=True)
    try:
        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_camera.tarファイルをクリックして、Cドライブ直下に保存する。~/catkin_ws/src/create_autonomy/ca_description/urdfの下にダウンロードする。
  - urdf_camera.tar
- Ubuntu端末を開いて、次のコマンドでurdf_camera.tarを~/catkin_ws/src/create_autonomy/ca_description/urdfの下にコピーする。UbuntuからWindowsのCドライブのパスは/mnt/cになる。
  - cp /mnt/c/urdf_camera.tar ~/catkin_ws/src/create_autonomy/ca_description/urdf

端末で以下のコマンドを実行して追加済みファイルへ変更する。
- $ roscd ca_description/urdf
- $ tar xvf urdf_camera.tar
- $ mv create_base.xacro create_base.xacro.org
- $ mv create_base_gazebo.xacro create_base_gazebo.xacro.org
- $ cp ./urdf_camera/create_base.xacro create_base.xacro
- $ cp ./urdf_camera/create_base_gazebo.xacro create_base_gazebo.xacro

準備

$ cd ~/catkin_ws/src
$ mkdir hard2020
$ cd ~/catkin_ws/src/hard2020
$ catkin_create_pkg camera sensor_msgs opencv2 cv_bridge rospy std_msgs
$ cd camera/src
エディタを使い上のプログラムをcamera.pyという名前をつけてsrc下に保存する。以下はgeditを使う場合。
- $ gedit camera.py
Pythonはビルドする必要はないが、ROSで使用する場合は必要になる場合があるので、以下のコマンドを実行する。
- $ cd ~/catkin_ws
- $ catkin build camera
コマンドで実行できるように実行権を与える。
- $ chmod u+x camera.py

実行

端末を３つ開き、以下のコマンドを実行する。
シミュレータの起動
- $ roslaunch ca_gazebo create_empty_world.launch
cameraノードの起動
- $ rosrun camera camera.py
遠隔操作
- $ roslaunch ca_tools keyboard_teleop.launch

ハンズオン

サンプルプログラムの実行
- シミュレータにコーラ缶(Coke Can)をロボットの前に挿入する。
  - Insert タブ→http://gazbosim.org/models/→Coke Can
- ロボットをキーボードで操作してコーラ缶の方向に進んだり、その場回転して画像処理が行われていることを確認する。

OpenCV
- 勾配検出フィルタ
  - サンプルプログラムではエッジ検出にcanny関数を使用した。その他にも勾配検出フィルタとして、Sobel関数、Laplacian関数などがある。参考リンクのOpenCV-Pythonチュートリアルで調べて、サンプルプログラムを改変して違いを見てみよう。
- 物体の輪郭抽出
  - サンプルプログラムではコーラ缶の位置を推定するのに赤領域の重心を使った。より正確にするには物体の輪郭を検出するfindContours関数がある。参考リンクで調べて輪郭を使って物体の位置を推定してみよう。
応用
- コーラ缶の手前で停止するプログラムを作ろう。なお、カメラでは物体の距離が直接わからないので、コーラ缶の輪郭を検出し、その面積や外接矩形のサイズ[pixel]から推定する必要がある。コーラ缶のサイズはgazeboのGUIで簡単に調べることができる。
- コーラ缶を任意の位置において、それをロボットが探して近づき、手前で停止するプログラムを作ろう。なお、カメラの位置が比較的高いので近づいてコーラ缶を見失う場合には、urdfファイルを変更して高さを変えてもよい。

参考リンク

OpenCV-Pythonチュートリアル

終わり