概要

• 前回の記事では､ロボットアームで何かを掴む風の動作を実装しました

  • ただし､画像センサを付けていないので､目隠しでUFOキャッチャー遊びをするような状況です

  • USBカメラで物体を認識しつつ､掴むプログラムを作りたいです

• 今回はROS2で画像を処理する方法を学びます

例のごとく､こちらの書籍・レポジトリを参考にコードを書いていきます｡

USBカメラの認識

カメラの動作確認

cv2ライブラリを使った､usbカメラとの接続(基礎の基礎)を実装します｡
注意点として､dockerを立ち上げる前に､usbカメラを接続しておく必要があります｡

import cv2
import time
cap=cv2.VideoCapture(0)

while True:
    _,source=cap.read()
    cv2.imshow("source",source)
    gray=cv2.cvtColor(source,cv2.COLOR_RGB2GRAY)
    _,result=cv2.threshold(gray,128,255,cv2.THRESH_BINARY)
    cv2.imshow("result",result)
    time.sleep(0.1)
    cv2.waitKey(1)

ROS2でのusbカメラモジュールの立ち上げと監視

ROS2には様々なパッケージがあり､その中に､usbカメラの画像を取得するモジュールが含まれているようです｡

使い方は簡単で､以下のコマンドを実行するだけです｡

ros2 run usb_cam usb_cam_node_exe 

このプログラムは､観測した画像を"image_raw"として配信(publish)する機能があるようです｡
配信されている画像を確認するには､rqtというモニタリングソフトなどを使うようです｡
別のターミナルでrqtを立ち上げます｡

rqt

以下のようにクリックしていくことで､publishされている画像をモニタリングできます｡

ROS2での画像データの受取

配信されている画像を受け取って処理するプログラムを作ります｡
新たに､img_receiver.pyを以下のフォルダに作っておきます｡

img_receiver.pyに中身を加えていきます｡

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image


class ImgReceiver(Node):

    def __init__(self):
        super().__init__('img_receiver')
        self.subscription = self.create_subscription(
            Image,
            'image_raw',
            self.image_callback,
            qos_profile_sensor_data)

    def image_callback(self, data):
        self.get_logger().info('got image!')
        pass


def main():
    rclpy.init()
    img_receiver = ImgReceiver()
    try:
        rclpy.spin(img_receiver)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()

コードの重要ポイントをgptに解説してもらいました｡
create_subscription メソッド:

• このメソッドは、特定のトピック（ここでは image_raw）に対するサブスクリプションを作成します。サブスクリプションは、そのトピックにメッセージが公開されるたびに通知を受け取るためのものです。

1. Image:

   • これはサブスクリプションが期待するメッセージの型を示しています。Image は、画像データを扱うための一般的なメッセージ型です。

2. 'image_raw':

   • これはサブスクリプションが監視するトピックの名前です。このトピックに新しいメッセージが公開されるたびに、self.image_callback 関数が呼び出されます。

3. self.image_callback:

   • これはトピックに新しいメッセージが公開されたときに呼び出されるコールバック関数を指定します。この関数は、新しい Image メッセージを受け取って何らかの処理を行います。

4. qos_profile_sensor_data:

   • これはQuality of Service (QoS) プロファイルを示しています。QoSプロファイルは、メッセージ通信の品質や信頼性に関する設定を提供します。qos_profile_sensor_data は、センサーデータの伝送のための一般的なQoS設定を持つ、ROS 2で予め定義されているプロファイルの一つです

setup.pyにモジュールを追加します｡

あとはbuildするだけです｡

cd ~/airobot_ws/ 
colcon build --symlink-install

実行

ros2 run opencv_ros2 img_receiver 

実行すると､
[INFO] [1696527528.446362671] [img_receiver]: got image!
[INFO] [1696527528.545350359] [img_receiver]: got image!
[INFO] [1696527528.646337785] [img_receiver]: got image!
[INFO] [1696527528.745250521] [img_receiver]: got image!
[INFO] [1696527528.836038943] [img_receiver]: got image!
的なメッセージが連投されるはずです｡

raw_imageをsubscriptionとして受け取って､image_callbackが呼び出されるようです｡

受け取ったデータの処理

image_callbackの中にデータ処理コードを書き加えていきます｡
書籍では画像の2値化がなされていました｡コピペでは芸がないので､輪郭を強調するコードにしてみます｡
処理した画像はpublisherとして発信します｡

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


class ImgReceiver(Node):

    def __init__(self):
        super().__init__('img_receiver')

        self.br = CvBridge()
        self.subscription = self.create_subscription(
            Image,
            'image_raw',
            self.image_callback,
            qos_profile_sensor_data)

        self.publisher = self.create_publisher(
            Image,
            'processed', 10)

    def image_callback(self, data):
        self.get_logger().info('got image!')

        source = self.br.imgmsg_to_cv2(data, 'bgr8')
        edges = cv2.Canny(source, 50, 150)  # 50と150は下限と上限の閾値
        result_msg = self.br.cv2_to_imgmsg(edges, 'passthrough')
        self.publisher.publish(result_msg)
        self.get_logger().info('publish image!')

        pass


def main():
    rclpy.init()
    img_receiver = ImgReceiver()
    try:
        rclpy.spin(img_receiver)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()

実行結果は以下の通り｡
無事に輪郭抽出できました｡

深度カメラに対応したros2のモジュールもあるようです｡
D435は以下のコマンドで動きました｡

ただし筆者の環境では遅延が酷く､使い物にならない感じでした｡何らかのトラブルが起きている印象です｡
また､近距離用のD405ではエラーが出たので､他のプログラムが必要なようです｡

ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true

Arucoマーカーの認識

詳細は次回以降に行う予定です｡
配布されているdocker imageでエラーが出たので､修正しておきます｡
(opencv, opencv-contribの競合が原因?)

pip uninstall opencv-python
pip uninstall opencv-contrib-python
pip install opencv-contrib-python==4.5.5.64

opencvのバージョン指定も重要で､aruco周りはバージョンが0.1ずれるだけで､使えない関数が続出します｡

以下のコマンドを実行すると､aruco マーカーを認識します｡

ros2 run opencv_ros2 aruco_node_tf 

ここで取得した座標をアーム座標系とうまく接続すれば､カメラ認識による物体の把持が可能になるというストーリーのはずです｡