拙著「ROS2とPythonで作って学ぶAIロボット入門」（講談社）を授業で使用する場合の講義資料のたたき台です． 今回は、5章ビジョンの補足です。コンピュータビジョンライブラリの定番OpenCVをROSで使うためにcv_bridgeを用いたPythonプログラムを勉強します。サンプルのPythonプログラムではカメラから取得した画像のエッジを検出したり、指定した色領域を抽出しています。 なお、この記事のサンプルプログラムはROSチュートリアルと「ROSで始めるロボットプログラミング、著者：小倉崇、出版社：工学社」を参考に，出村が作成しました。AIロボット入門本にはないサンプルです。

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

  • このサンプルプログラムの概要を説明します。まず、カメラから取得したデータが /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)


def main():   
    try:
        rclpy.init()
        image_converter = ImageConverter()
        rclpy.spin(image_converter)
        image_converter.destroy_node
        rclpy.shutdown()
    except KeyboardInterrupt:
        print("Shutting down")
        cv2.destroyAllWindows()

依存関係のあるパッケージのインストール（教科書のDockerイメージを使っている場合はこの作業は不要です．）

  • $ 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/chapter5
    • $ ros2 pkg create --build-type ament_python --node-name camera camera

ソースコードの作成

  • エディタVSCodiumを開き、上のプログラムをコピペしてcamera.pyというファイル名で~/catkin_ws/src/camera/cameraディレクトリの中に保存する。
    • $ cd ~/airobot_ws 
    • $ codium . 
    • $ cd ~/airobot_ws/src/chapter5/camera/camera

ビルド

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

実　行

  • 端末を開き３分割し，以下のコマンドを実行する。
  •  上段の端末：シミュレータの起動
    • カメラの高さを低くしたいのでロボットモデルをhappy_miniではなくwaffle_piにする。
      • $ export TURTLEBOT3_MODEL=waffle_pi
    • Gazeboの起動
      • $ ros2 launch turtlebot3_gazebo empty_world.launch.py
    • 起動したGazeboにコーラ缶(Coke Can)をロボットの前に挿入する。
      • GazeboのInsert タブ→http://models.gazebosim.org/→Coke Can
  • 中段の端末。cameraノードの起動
    • $ ros2 run camera  camera

  • 下段の端末。遠隔操作
    • $ ros2 run turtlebot3_teleop teleop_keyboard

ハンズオン

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

参考神リンク

終わり