rclpyでROS2のsensor/Imageトピックから画像を取得し透かしを入れて別トピックとして配信する - Bye Bye Moore
と
ROS2 hubmle環境にArUcoマーカーの認識をつける - Bye Bye Moore
の内容を統合し、特定のIDをもつArUcoの姿勢情報を再配信画像の右上に表示してみます
実際のところ
import rclpy from rclpy.node import Node from sensor_msgs.msg import Image from geometry_msgs.msg import PoseArray from cv_bridge import CvBridge import cv2 import numpy as np class ImagePoseOverlayNode(Node): def __init__(self): # ノードの初期化 super().__init__('image_pose_overlay_node') # cv_bridgeを初期化（ROSのイメージとOpenCVのイメージを変換するためのブリッジ） self.bridge = CvBridge() # ポーズと画像のトピックを購読 self.pose_sub = self.create_subscription(PoseArray, '/aruco_poses', self.pose_callback, 10) self.image_sub = self.create_subscription(Image, '/camera/image', self.image_callback, 10) # 重畳されたイメージをパブリッシュするためのパブリッシャーを作成 self.image_pub = self.create_publisher(Image, '/overlay_image', 10) # 最新のポーズを保存するための変数 self.latest_pose = None def pose_callback(self, msg: PoseArray): # ArUcoマーカーの姿勢を取得 for pose in msg.poses: # IDが42のマーカーの姿勢をチェック（ここは実際の実装に応じて変更が必要） if pose.orientation.w == 42: self.latest_pose = pose break def image_callback(self, msg: Image): # 画像を取得し、最新のポーズを使用してテキストをオーバーレイ if self.latest_pose: cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") text = f"x: {self.latest_pose.position.x}, y: {self.latest_pose.position.y}, z: {self.latest_pose.position.z}" # テキストを画像に描画するための設定 font = cv2.FONT_HERSHEY_SIMPLEX (text_width, text_height), _ = cv2.getTextSize(text, font, 0.5, 1) # テキストの背景となる黒い矩形を描画 cv2.rectangle(cv_image, (5, 5), (5 + text_width, 5 + text_height + 10), (0, 0, 0), -1) # テキストを画像に描画 cv2.putText(cv_image, text, (10, 20), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) # 重畳された画像をパブリッシュ overlayed_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") self.image_pub.publish(overlayed_msg) def main(args=None): rclpy.init(args=args) node = ImagePoseOverlayNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()
