Gazeboシミュレート上のカメラをOpenCVで取得する (ROS2-Foxy)

Python
2022.11.04
スポンサーリンク

スポンサーリンク

はじめに

前回は、LIDAR のトピックの取得方法と、その処理方法について説明しました。

Gazebo シミュレート上でLIDARをPythonで処理する(ROS2-Foxy)
Gazebo シミュレート上でLIDAR,をPythonで処理します(ROS2-Foxy) LIDARのデータの確認方法とデータの取り扱い方について解説していきます。
nutritionfoodtech.com
2022.10.31

今回は、Gazebo 上でシミュレートしているカメラの映像を取得して障害物を検知する方法を説明します。

前提条件

前提条件は、以下の通りです。

  • /image_raw トピックが存在している
  • /cmd_vel トピックが存在している

上記トピックがあれば、問題ありません。

Gazebo上のカメラの映像を取得する

早速、カメラの映像を取得する方法を説明していきます。

今回使用するカメラのトピック名は、/camera/image_raw となります。

前回までの記事を完了した方は以下のコマンドで Python ファイルを作成してください。

cd ~/ros_ws/src/ros2_first_test/ros2_first_test
touch get_camera.py

映像を取得する get_camera.py は、以下の通りです。

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

class getVideoFrame(Node):
    def __init__(self):
        # wake up node name
        super().__init__("video_subscriber")
        # declare subscriber
        self.subscription = self.create_subscription(Image, "/camera1/image_raw", self.get_camera_data, 10)
        # setting cv2
        self.bridge = CvBridge()

    def get_camera_data(self, data):
        # separate scan data
        frame = self.bridge.imgmsg_to_cv2(data)
        # show frame
        cv2.imshow("Gazebo-camera1", frame)
        cv2.waitKey(1)

def main(args=None):
    rclpy.init(args=args)
    gVF = getVideoFrame()
    rclpy.spin(gVF)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

コードの内容を説明します。ここでは、新たに登場した部分のみを扱います。

self.subscription = self.create_subscription(Image, "/camera1/image_raw", self.get_camera_data, 10)

sensor_msgs/msg/Image は、公式サイトに記載のある通り、ヘッダーや画像のBGR値で構成されています。create_subscription によって、これを取得します。

次は、ROS2 の映像と OpenCV をつなぐために必須の CVBridgeクラスを用意します。

self.bridge = CvBridge()

self.get_camera_data では、この CVBridge を用いて画像を取得します。

# separate scan data
frame = self.bridge.imgmsg_to_cv2(data)
# show frame
cv2.imshow("Gazebo-camera1", frame)
cv2.waitKey(1)

CVBridge.imgmsg_to_cv2(camera_topic) により、sensor_msgs/msg/Image 型をもつカメラトピックから、画像の部分のみを抽出し、OpenCV で扱えるように、numpy に変換します。

cv2.imshow(“frame-name”, frame) は、frame-name という Window 名で frame の画像を表示するウィジェットを立ち上げます。

cv2.waitKey(1) は、約0.001s 待機します。cv2.waitKey(1000) にすると、約1s 待機します。
cv2.waitKey(0) は、キー入力があるまで待機します。

setup.pyの編集

ここまで準備できたら、setup.py に先ほど作成した get_camera.py を登録します。

'console_scripts': [
    'translate_cmdvel=ros2_first_test.translate_cmdvel:main',
    'get_imu=ros2_first_test.get_imu:main',
    'get_lidar=ros2_first_test.get_lidar:main',
    'get_camera=ros2_first_test.get_camera:main',
],

あとは、ビルドして実行するだけです。
カメラトピックをパブリッシュするための Gazebo シミュレーション環境を起動してください。

cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.launch.py

Gazebo が立ち上がったら、Gazebo上に適当な障害物を設置してください。

別のターミナルを開いて、以下を実行します。

ros2 run ros2_first_test get_camera

以下のような画面になれば、成功です。

障害物の検知

次は、OpenCVを用いて画像から障害物を検知できるようにします。

def get_camera_data 内を変更していきます。

def get_camera_data(self, data):
    # separate scan data
    frame = self.bridge.imgmsg_to_cv2(data)
    image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    image = image[:600, :]
    image = cv2.inRange(image, 0, 100)
    # 輪郭抽出
    contours, _ = cv2.findContours(image,
                                   cv2.RETR_LIST,
                                   cv2.CHAIN_APPROX_NONE)
    if len(contours) > 0:
        contour = max(contours, key=lambda x: cv2.contourArea(x))
    else:
        contour = contours
    frame = cv2.drawContours(frame, contour, -1, (0, 255, 0), 5)
    # show frame
    cv2.imshow("Gazebo-camera1", frame)
    cv2.waitKey(1)

コードの解説をしていきます。

image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
image = image[:600, :]

まずは、処理を簡単にするために cv2.cvtColor を用いて画像をグレースケールに変更します。
続いて、そのままの画像では少しだけ自分の車体が映ってしまうので、image = image[:600, :] で、該当部分を削除します。

image = cv2.inRange(image, 0, 100)

inRange 関数でグレースケール画像の値が 0 ~ 100 の部分のみを抽出します。

contours, _ = cv2.findContours(image,
                                   cv2.RETR_LIST,
                                   cv2.CHAIN_APPROX_NONE)

cv2.findContours で、抽出した画像から閉じた輪郭を検索します。
輪郭の数だけ、contours に保存されます。

if len(contours) > 0:
    contour = max(contours, key=lambda x: cv2.contourArea(x))
else:
    contour = contours

contours が 0 以上の長さを持つリストの場合、max 関数で最も輪郭の大きい値を抽出します。

frame = cv2.drawContours(frame, contour, -1, (0, 255, 0), 5)

最も大きい輪郭を、drawContours 関数で、元の画像に描画します。

ここまでできたら、ビルドして実行してみましょう。

cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.launch.py

別のターミナルを開きます。

ros2 run ros2_first_test get_camera

以下のような映像が表示されれば、完成です。

障害物の回避

ここまできたら、あとは /cmd_vel に、回避指令を出すだけです。
ルールとしては単純に、画像内に輪郭のある物体がなくなるまで回転→移動ということにします。

get_camera.py のコードは以下のようになります。

import rclpy 
import cv2 
from rclpy.node import Node 
from cv_bridge import CvBridge 
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

class getVideoFrame(Node):
    def __init__(self):
        # wake up node name
        super().__init__("video_subscriber")
        # declare subscriber
        self.subscription = self.create_subscription(Image, "/camera1/image_raw", self.get_camera_data, 10)
        self.publisher = self.create_publisher(Twist, "/first_robot/cmd_vel", 10)
        # period publisher
        timer_period = 0.5
        self.timer = self.create_timer(timer_period, self.send_cmd_vel)
        # setting cv2
        self.bridge = CvBridge()
        self.velocity = Twist()

    def get_camera_data(self, data):
        # separate scan data
        frame = self.bridge.imgmsg_to_cv2(data)
        image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        image = image[:600, :]
        image = cv2.inRange(image, 0, 100)
        # 輪郭抽出
        contours, _ = cv2.findContours(image,
                                       cv2.RETR_LIST,
                                       cv2.CHAIN_APPROX_NONE)
        if len(contours) > 0:
            contour = max(contours, key=lambda x: cv2.contourArea(x))
            self.x, self.y, _, _ = cv2.boundingRect(contour)
        else:
            contour = contours
            self.x, self.y = -1, -1
        frame = cv2.drawContours(frame, contour, -1, (0, 255, 0), 5)
        # show frame
        cv2.imshow("Gazebo-camera1", frame)
        cv2.waitKey(1)

    def send_cmd_vel(self):
        if 400 >= self.x >= 0:
            self.velocity.angular.z = -1.0
        elif 800 >= self.x >= 400:
            self.velocity.angular.z = 1.0
        else:
            self.velocity.angular.z = 0.0

        self.publisher.publish(self.velocity)

def main(args=None):
    rclpy.init(args=args)
    gVF = getVideoFrame()
    rclpy.spin(gVF)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

def send_cmd_vel について説明します。

def send_cmd_vel(self):
    if 400 >= self.x >= 0:
        self.velocity.angular.z = -1.0
    elif 800 >= self.x >= 400:
        self.velocity.angular.z = 1.0
    else:
        self.velocity.angular.z = 0.0

self.x は、画像の障害物の中心x座標です。
障害物の中心が、画面半分より左側にある場合は、self.velocity.angular.z = -1.0 とします。

画像から障害物が消えた場合は、self.velocity.angular.z = 0.0 として、ロボットの回転を停止します。

早速、ビルドして実行していきます。

cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.launch.py

Gazebo を立ち上げた後に、画像処理プログラムを起動します。

ros2 run ros2_first_test get_camera

以下のようにロボットが動けば完了です。

おわりに

簡単な画像処理と /cmd_vel を組み合わせて、障害物回避のプログラムを作成しました。
LIDAR や IMU、カメラのデータを上手に処理することができれば、自分なりのロボットモーションを作成することが可能になります。

次回からは、SLAM (自己位置推定) のプログラムを作成していきます。
URDF と launch ファイルの改良もしていきます。

SLAM (自己位置推定) のための Localization launch ファイルの設定 (ROS2-Foxy)
SLAM (自己位置推定) のための Launch ファイルの設定について説明します。 今回は SLAM (Simultaneous Localization and Mapping) のうち、Loaclization について焦点を当てて説明します。
nutritionfoodtech.com
2022.11.11
スポンサーリンク

コメント

タイトルとURLをコピーしました