realsense D435iを用いたArUcoTagの認識と/cmd_velのpublish

自分のメモ帳です
# 事前準備
realsense camera用のSDK、パッケージをインストール
https://qiita.com/porizou1/items/9d44053dbce648f1470d
# ソースコード
```python:my_apriltag_package/setup.py
from setuptools import setup

package_name = 'my_apriltag_package'

setup(
    name=package_name,
    version='0.1.0',
    packages=[package_name],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user@example.com',
    description='AprilTagを使ったロボット制御パッケージ',
    license='Apache License 2.0',
    entry_points={
        'console_scripts': [
            'apriltag_control = my_apriltag_package.apriltag_control:main',
        ],
    },
)

```

```xml:my_apriltag_package/package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_apriltag_package</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="muneishi.yoshimasa.p5@dc.tohoku.ac.jp">yoshimasa</maintainer>
  <license>Apache License 2.0</license>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <depend>rclpy</depend>
  <depend>geometry_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>cv_bridge</depend>
  <depend>realsense2_camera_msgs</depend>
  <depend>pupil_apriltags</depend>
  <depend>opencv-python</depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

```
```python:my_apriltag_package/launch/detect.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='my_apriltag_package',
            executable='apriltag_control',
            name='apriltag_control',
            output='screen'
        ),
    ])
```

```python:my_apriltag_package/my_apriltag_package/apriltag_control.py
import cv2
import numpy as np
import rclpy
from rclpy.node import Node
from cv_bridge import CvBridge
from realsense2_camera_msgs.msg import RGBD
from geometry_msgs.msg import Twist
import copy
import time
from pupil_apriltags import Detector

class RsSub(Node):
    def __init__(self):
        super().__init__('minimal_subscriber')

        self.bridge = CvBridge()
        self.subscription = self.create_subscription(RGBD, '/camera/camera/rgbd', self.listener_callback, 10)
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        self.debug_image = None
        self.tags = []
        self.elapsed_time = 0
        self.lastest_position = []
        self.lastest_center = []
        self.at_detector = Detector(families='tag36h11')

        self.target_distance = 500  # 目標距離 (mm)
        self.linear_speed = 0.2
        self.angular_speed = 0.5

    def listener_callback(self, msg):
        start_time = time.time()

        # カラー画像を取得
        self.debug_image = copy.deepcopy(self.bridge.imgmsg_to_cv2(msg.rgb, "bgr8"))

        # グレースケール画像を取得してAprilTag検出
        image = cv2.cvtColor(self.bridge.imgmsg_to_cv2(msg.rgb, "bgr8"), cv2.COLOR_BGR2GRAY)
        self.tags = self.at_detector.detect(image)

        if len(self.tags) > 0:
            tag = self.tags[0]  # 最も近いタグを選択
            self.get_logger().info(f"AprilTag {tag.tag_id} を検出しました。")
            self.control_robot(msg, tag)

        # AprilTag を描画して GUI に表示
        self.display_image(msg, self.debug_image, self.tags)

        self.elapsed_time = time.time() - start_time

    def control_robot(self, msg, tag):
        """AprilTag の位置を元に /cmd_vel を送信"""
        cmd = Twist()
        depth_image = self.bridge.imgmsg_to_cv2(msg.depth, "passthrough")
        center = tag.center

        try:
            depth_data = depth_image[int(center[1]), int(center[0])] # mmに変換
            # self.get_logger().info(f"取得した depth_data: {depth_data} mm ")
            error_distance = depth_data - self.target_distance
            error_x = center[0] - 640  # 画像の中心との差分 (1280x720 の場合)

            # 前後移動
            if abs(error_distance) > 50:
                cmd.linear.x = max(-self.linear_speed, min(self.linear_speed, -error_distance * 0.5 / 1000))

            # 回転
            if abs(error_x) > 50:
                cmd.angular.z = max(-self.angular_speed, min(self.angular_speed, -error_x * 0.002))

            self.cmd_vel_pub.publish(cmd)

        except IndexError:
            self.get_logger().error("深度データの取得に失敗しました。")

    def display_image(self, msg, image, tags):
        """cv2.imshow() で GUI に表示"""
        depth_image = self.bridge.imgmsg_to_cv2(msg.depth, "passthrough")

        for tag in tags:
            tag_id = tag.tag_id
            center = (int(tag.center[0]), int(tag.center[1]))
            corners = tag.corners

            # タグの深度を取得 (mm単位)
            depth_data = depth_image[int(tag.center[1]), int(tag.center[0])]
            # self.get_logger().info(f"取得した depth_data: {depth_data} mm ")

            # AprilTagの中心と角を描画
            cv2.circle(image, center, 5, (0, 0, 255), 2)
            cv2.line(image, (int(corners[0][0]), int(corners[0][1])),
                     (int(corners[1][0]), int(corners[1][1])), (255, 0, 0), 2)
            cv2.line(image, (int(corners[1][0]), int(corners[1][1])),
                     (int(corners[2][0]), int(corners[2][1])), (255, 0, 0), 2)
            cv2.line(image, (int(corners[2][0]), int(corners[2][1])),
                     (int(corners[3][0]), int(corners[3][1])), (0, 255, 0), 2)
            cv2.line(image, (int(corners[3][0]), int(corners[3][1])),
                     (int(corners[0][0]), int(corners[0][1])), (0, 255, 0), 2)

            # タグIDと深度を表示
            cv2.putText(image, f"ID: {tag_id}", (center[0] - 10, center[1] - 10),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2, cv2.LINE_AA)
            cv2.putText(image, f"Depth: {int(depth_data)} mm", (center[0] - 40, center[1] - 40),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2, cv2.LINE_AA)

        cv2.imshow("AprilTag Detection", image)
        cv2.waitKey(1)  # 画面を更新し続ける

def main(args=None):
    rclpy.init(args=args)
    node = RsSub()

    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()
        cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

```

# 実行
Realsense cameraを起動（Topic:/camera/camera/rgbdを起動するためにenable_rgbd:=が必要）
```
#Terminal 1
ros2 launch realsense2_camera rs_launch.py enable_rgbd:=true enable_sync:=true align_depth.enable:=true enable_color:=true enable_depth:=true 
```
ArUcoTag認識用のパッケージを実行
```
#Terminal 2
ros2 run my_apriltag_package apriltag_control
```
```
#Terminal 3
ros2 run feetech_controller mecanum_feetech_node
```
 
# メモ
/camera/camera/rgbdの深度情報の単位はmm

# 参考
感謝です
https://qiita.com/basalte/items/07c0e234b43eb25f2694