ROS2でOpenCVでの画面出力とPub/Subを両立させる（Python, スレッド）

OpenCVを使って、cameraからcv2.VideoCapture(0)を行い、imshow()でOpenCVに用意されているwindowを表示することはよくあるかと思います。

その上で、ROS2でPub/Subをしたい時に、そのままimshowのwhile()にrclpy.spinOnce()を呼んでもうまく動かなかった（画像表示が行われなくなる）ので備忘録です。

結論

対処方法を先に書いてしまうと、スレッドを分けてあげると良いです。

import threading

def capture_and_display():
    cap = cv2.VideoCapture(0)
    while not stop_thread:
            ret, frame = cap.read()
            cv2.imshow('Camera', frame)
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
    cap.release()
    cv2.destroyAllWindows()


def main():
    global stop_thread
    stop_thread = False
    rclpy.init()
    node = rclpy.create_node("node")
    capture_thread = threading.Thread(target=capture_and_display)
    capture_thread.start()
    try:
        rclpy.spin(node)
    # ctrl + cなどでスレッドも止めるため
    except KeyboardInterrupt:
        stop_thread = True
    capture_thread.join()
    node.destroy_node()
    rclpy.shutdown()

必要最低限のコードは以上の形で、画像表示用のスレッドと、ROS2でspinを回す方の処理をmainスレッドで行う形で問題なく実行することができます。

このコードであれば、例えばこのようにtopicのsubscribeを行った上で

    hoge_subsctiption = node.create_subscription(Int32, "/hoge", lambda msg: hoge_callback(msg), 10)

下のようにその値をcallbackで受け取ってglobal変数に入れてあげ

def hoge_callback(msg):
    global hoge
    hoge = msg.data

このように書いてあげると、画面上にテキストを表示することができます。

    mark = np.zeros_like(frame)    
    cv2.putText(mark, f'{hoge})', (100, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)

基本的に用途としてはROS2で受け取って、それをスクリーン上に表示したいということになるかなと思うので、あまり問題はないかもですが、表示側のスレッドで変数の内容を変更するような場合はロックにならないような仕組み（キューを使うなど）が必要かもです。

まとめ

最初はRosQtなどでUI作らないといけないかなと思ったのですが、OpenCVのウィンドウそのまま使えたので、ロボットの表示画面（操縦画面）みたいなものを作る時は、これが便利かもです。

topicとしてimageを送り、Qtで受け取るプログラムを作るのもいいのですが、ネットワーク帯域圧迫だったり、処理能力に余力がない時にも、この方法であれば問題がないのでオススメです。