自分のメモ帳です
事前準備
realsense camera用のSDK、パッケージをインストール
https://qiita.com/porizou1/items/9d44053dbce648f1470d
ソースコード
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',
],
},
)
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>
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'
),
])
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