#!/usr/bin/env python3 import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy from sensor_msgs.msg import Image, CompressedImage from geometry_msgs.msg import PoseStamped, TransformStamped import tf2_ros import numpy as np import cv2 import cv2.aruco as aruco import math class ArUcoDetectorNode(Node): def __init__(self): super().__init__('aruco_detector_node') # --- [Parameters] --- self.declare_parameter('image_topic', '/camera/image') self.declare_parameter('pose_topic', '/aruco_detector/pose') self.declare_parameter('marker_size', 0.15) # 150mm self.declare_parameter('camera_frame', 'camera_optical_link') self.declare_parameter('show_image', False) # Default to false since Web UI displays it self.image_topic = self.get_parameter('image_topic').value self.pose_topic = self.get_parameter('pose_topic').value self.marker_size = self.get_parameter('marker_size').value self.camera_frame = self.get_parameter('camera_frame').value self.show_image = self.get_parameter('show_image').value # Calibration parameters (from hikrobot camera calibration) self.fx = 1203.078148 self.fy = 1206.096396 self.cx = 699.186863 self.cy = 565.715472 self.mtx = np.array([[self.fx, 0, self.cx], [0, self.fy, self.cy], [0, 0, 1]]) self.dist = np.array([[-0.102740, 0.093985, -0.000759, -0.001804, 0.0]]) # ArUco Setup self.dictionary = aruco.getPredefinedDictionary(aruco.DICT_7X7_50) self.parameters = aruco.DetectorParameters() # Publishers self.pose_pub = self.create_publisher(PoseStamped, self.pose_topic, 10) self.comp_image_pub = self.create_publisher(CompressedImage, '/aruco_detector/image/compressed', 10) self.image_pub = self.create_publisher(Image, '/aruco_detector/image', 10) # TF Broadcaster self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) # Throttling time tracker self.last_process_time = self.get_clock().now() # Subscriber with QoS Profile (depth=1, reliable) to avoid queue backlog qos_profile = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=1 ) self.create_subscription(Image, self.image_topic, self.image_callback, qos_profile) self.get_logger().info('==========================================') self.get_logger().info(' ROS2 ARUCO DETECTOR HIKROBOT ') self.get_logger().info(f' Subscribing to: {self.image_topic}') self.get_logger().info(f' Publishing to: {self.pose_topic}') self.get_logger().info('==========================================') def rotation_matrix_to_quaternion(self, R): tr = R[0, 0] + R[1, 1] + R[2, 2] if tr > 0: S = math.sqrt(tr + 1.0) * 2 qw = 0.25 * S qx = (R[2, 1] - R[1, 2]) / S qy = (R[0, 2] - R[2, 0]) / S qz = (R[1, 0] - R[0, 1]) / S elif (R[0, 0] > R[1, 1]) and (R[0, 0] > R[2, 2]): S = math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2 qw = (R[2, 1] - R[1, 2]) / S qx = 0.25 * S qy = (R[0, 1] + R[1, 0]) / S qz = (R[0, 2] + R[2, 0]) / S elif R[1, 1] > R[2, 2]: S = math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2 qw = (R[0, 2] - R[2, 0]) / S qx = (R[0, 1] + R[1, 0]) / S qy = 0.25 * S qz = (R[1, 2] + R[2, 1]) / S else: S = math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2 qw = (R[1, 0] - R[0, 1]) / S qx = (R[0, 2] + R[2, 0]) / S qy = (R[1, 2] + R[2, 1]) / S qz = 0.25 * S return qx, qy, qz, qw def image_callback(self, msg): # Throttling to ~7Hz to prevent CPU starvation and queue backlog current_time = self.get_clock().now() dt = (current_time - self.last_process_time).nanoseconds / 1e9 if dt < 0.14: return self.last_process_time = current_time try: # Direct raw conversion to avoid cv_bridge dependencies if msg.encoding == 'rgb8': img_rgb = np.frombuffer(msg.data, dtype=np.uint8).reshape((msg.height, msg.width, 3)) frame = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR) elif msg.encoding == 'bgr8': frame = np.frombuffer(msg.data, dtype=np.uint8).reshape((msg.height, msg.width, 3)) else: self.get_logger().error(f'Unsupported image encoding: {msg.encoding}') return except Exception as e: self.get_logger().error(f'Image conversion failed: {e}') return # Check if resize is needed to speed up ArUco detection (1440x1080 -> 360x270) h, w = frame.shape[:2] scale = 1.0 if w > 800: scale = 0.25 frame_resized = cv2.resize(frame, (int(w * scale), int(h * scale))) else: frame_resized = frame # ArUco detection on resized frame for speed corners, ids, _ = aruco.detectMarkers(frame_resized, self.dictionary, parameters=self.parameters) if ids is not None: # Rescale corners back to original resolution for accurate pose estimation and drawing if scale != 1.0: corners = [c / scale for c in corners] gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001) for corner in corners: cv2.cornerSubPix(gray, corner, (5, 5), (-1, -1), criteria) # Draw detected markers on the frame aruco.drawDetectedMarkers(frame, corners) for i in range(len(ids)): marker_id = ids[i][0] # Estimate pose rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners[i], self.marker_size, self.mtx, self.dist) rmat, _ = cv2.Rodrigues(rvec) # Flip to match OpenCV camera frame convention R_flip = np.array([ [1, 0, 0], [0, -1, 0], [0, 0, -1] ], dtype=np.float32) rmat_corrected = np.dot(rmat, R_flip) qx, qy, qz, qw = self.rotation_matrix_to_quaternion(rmat_corrected) tvec_center = tvec[0][0] x, y, z = tvec_center[0], tvec_center[1], tvec_center[2] # Publish PoseStamped (embed marker_id in frame_id for web client) pose_msg = PoseStamped() pose_msg.header.stamp = msg.header.stamp pose_msg.header.frame_id = f'aruco_marker_{marker_id}' pose_msg.pose.position.x = x pose_msg.pose.position.y = y pose_msg.pose.position.z = z pose_msg.pose.orientation.x = qx pose_msg.pose.orientation.y = qy pose_msg.pose.orientation.z = qz pose_msg.pose.orientation.w = qw self.pose_pub.publish(pose_msg) # Broadcast TF Transform t = TransformStamped() t.header.stamp = msg.header.stamp t.header.frame_id = self.camera_frame t.child_frame_id = f'aruco_marker_{marker_id}' t.transform.translation.x = x t.transform.translation.y = y t.transform.translation.z = z t.transform.rotation.x = qx t.transform.rotation.y = qy t.transform.rotation.z = qz t.transform.rotation.w = qw self.tf_broadcaster.sendTransform(t) # Calculate Yaw angle in degrees from quaternion siny_cosp = 2.0 * (qw * qz + qx * qy) cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) yaw_deg = math.degrees(math.atan2(siny_cosp, cosy_cosp)) # Draw 3D axis and ID for Web UI view rvec_corrected, _ = cv2.Rodrigues(rmat_corrected) cv2.drawFrameAxes(frame, self.mtx, self.dist, rvec_corrected, tvec_center, 0.05) text_pos1 = (int(corners[i][0][0][0]), int(corners[i][0][0][1]) - 40) text_pos2 = (int(corners[i][0][0][0]), int(corners[i][0][0][1]) - 15) text_pos3 = (int(corners[i][0][0][0]), int(corners[i][0][0][1]) + 10) cv2.putText(frame, f"ID: {marker_id}", text_pos1, cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2) cv2.putText(frame, f"X:{x:.2f} Y:{y:.2f} Z:{z:.2f}", text_pos2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2) cv2.putText(frame, f"Yaw: {yaw_deg:.1f}deg", text_pos3, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 100, 255), 2) # Scale down output image to 1/4th res (1440x1080 -> 360x270) to drastically reduce network bandwidth & browser UI load h_out, w_out = frame.shape[:2] frame_out = cv2.resize(frame, (int(w_out * 0.25), int(h_out * 0.25))) # Publish compressed image for Web UI Dashboard try: _, jpeg_data = cv2.imencode('.jpg', frame_out) comp_msg = CompressedImage() comp_msg.header = msg.header comp_msg.format = 'jpeg' comp_msg.data = jpeg_data.tobytes() self.comp_image_pub.publish(comp_msg) except Exception as e: self.get_logger().error(f'Failed to publish compressed image: {e}') # Publish raw image for web_video_server topic discovery try: raw_msg = Image() raw_msg.header = msg.header raw_msg.height = frame_out.shape[0] raw_msg.width = frame_out.shape[1] raw_msg.encoding = 'bgr8' raw_msg.is_bigendian = 0 raw_msg.step = frame_out.shape[1] * 3 raw_msg.data = frame_out.tobytes() self.image_pub.publish(raw_msg) except Exception as e: self.get_logger().error(f'Failed to publish raw image: {e}') if self.show_image: cv2.imshow("ROS2 ArUco Detector", frame_out) cv2.waitKey(1) def main(args=None): rclpy.init(args=args) node = ArUcoDetectorNode() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: cv2.destroyAllWindows() node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()