259 lines
11 KiB
Python
Executable File
259 lines
11 KiB
Python
Executable File
#!/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()
|