diff --git a/jetson/ros2_ws/src/saltybot_apriltag/package.xml b/jetson/ros2_ws/src/saltybot_apriltag/package.xml new file mode 100644 index 0000000..25ef44e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag/package.xml @@ -0,0 +1,28 @@ + + + + saltybot_apriltag + 0.1.0 + + AprilTag (DICT_APRILTAG_36h11) landmark detector for SaltyBot CSI cameras. + Detects tags in incoming frames and publishes DetectedTagArray at 10 Hz. + + SaltyLab + MIT + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + saltybot_apriltag_msgs + + python3-numpy + python3-opencv + + pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_apriltag/resource/saltybot_apriltag b/jetson/ros2_ws/src/saltybot_apriltag/resource/saltybot_apriltag new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/__init__.py b/jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/_aruco_utils.py b/jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/_aruco_utils.py new file mode 100644 index 0000000..76814cd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/_aruco_utils.py @@ -0,0 +1,85 @@ +""" +_aruco_utils.py — Pure-Python helpers for AprilTag detection (no ROS2 deps). + +Importable without rclpy for unit testing. +""" + +from __future__ import annotations + +import math +import numpy as np +import cv2 + + +_FAMILY = 'DICT_APRILTAG_36h11' + + +def rvec_tvec_to_pose(rvec: np.ndarray, tvec: np.ndarray): + """ + Convert OpenCV rotation + translation vectors to geometry_msgs/Pose. + Returns a plain namespace object to avoid importing geometry_msgs here. + """ + R, _ = cv2.Rodrigues(rvec) + m = R + + t = m[0, 0] + m[1, 1] + m[2, 2] + if t > 0: + s = 0.5 / math.sqrt(t + 1.0) + qw = 0.25 / s + qx = (m[2, 1] - m[1, 2]) * s + qy = (m[0, 2] - m[2, 0]) * s + qz = (m[1, 0] - m[0, 1]) * s + elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: + s = 2.0 * math.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) + qw = (m[2, 1] - m[1, 2]) / s + qx = 0.25 * s + qy = (m[0, 1] + m[1, 0]) / s + qz = (m[0, 2] + m[2, 0]) / s + elif m[1, 1] > m[2, 2]: + s = 2.0 * math.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) + qw = (m[0, 2] - m[2, 0]) / s + qx = (m[0, 1] + m[1, 0]) / s + qy = 0.25 * s + qz = (m[1, 2] + m[2, 1]) / s + else: + s = 2.0 * math.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) + qw = (m[1, 0] - m[0, 1]) / s + qx = (m[0, 2] + m[2, 0]) / s + qy = (m[1, 2] + m[2, 1]) / s + qz = 0.25 * s + + return ( + float(tvec.flat[0]), float(tvec.flat[1]), float(tvec.flat[2]), + float(qx), float(qy), float(qz), float(qw), + ) + + +class ArucoBackend: + """Wraps cv2.aruco, handling the 4.6 vs 4.7+ API difference.""" + + def __init__(self, logger=None): + self._log = logger + dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_APRILTAG_36h11) + params = cv2.aruco.DetectorParameters() + + try: + self._detector = cv2.aruco.ArucoDetector(dictionary, params) + self._new_api = True + if logger: + logger.info('[apriltag] ArucoDetector (OpenCV ≥ 4.7)') + except AttributeError: + self._dict = dictionary + self._params = params + self._new_api = False + if logger: + logger.info('[apriltag] legacy aruco.detectMarkers (OpenCV 4.6)') + + def detect(self, gray: np.ndarray): + """Returns (corners, ids); ids may be None.""" + if self._new_api: + corners, ids, _ = self._detector.detectMarkers(gray) + else: + corners, ids, _ = cv2.aruco.detectMarkers( + gray, self._dict, parameters=self._params + ) + return corners, ids diff --git a/jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/apriltag_node.py b/jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/apriltag_node.py new file mode 100644 index 0000000..b0c9f73 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/apriltag_node.py @@ -0,0 +1,212 @@ +""" +apriltag_node.py — AprilTag (DICT_APRILTAG_36h11) landmark detector. + +Detects AprilTags in CSI camera frames and publishes results at 10 Hz. +When camera_info is available, estimates full 6-DOF pose per tag via +cv2.solvePnP (SOLVEPNP_IPPE_SQUARE). + +Supports OpenCV 4.6 (legacy detectMarkers) and 4.7+ (ArucoDetector). + +Subscribes: + sensor_msgs/Image default /camera/color/image_raw + /camera_info sensor_msgs/CameraInfo latched intrinsics + +Publishes: + /saltybot/apriltags saltybot_apriltag_msgs/DetectedTagArray 10 Hz + +Parameters: + image_topic str '/camera/color/image_raw' + publish_hz float 10.0 + tag_size_m float 0.10 physical tag side in metres (for pose estimation) + min_margin float 10.0 minimum corner-span pixels to keep a detection +""" + +from __future__ import annotations + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy + +import numpy as np +import cv2 +from cv_bridge import CvBridge + +from sensor_msgs.msg import Image, CameraInfo +from geometry_msgs.msg import Point, Pose + +try: + from saltybot_apriltag_msgs.msg import DetectedTag, DetectedTagArray + _MSGS_OK = True +except ImportError: + _MSGS_OK = False + +from ._aruco_utils import ArucoBackend, rvec_tvec_to_pose + + +_SENSOR_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=5, +) +_LATCHED_QOS = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, +) + +_FAMILY = 'DICT_APRILTAG_36h11' + + +class AprilTagNode(Node): + + def __init__(self): + super().__init__('apriltag_detector') + + self.declare_parameter('image_topic', '/camera/color/image_raw') + self.declare_parameter('publish_hz', 10.0) + self.declare_parameter('tag_size_m', 0.10) + self.declare_parameter('min_margin', 10.0) + + img_topic = self.get_parameter('image_topic').value + pub_hz = self.get_parameter('publish_hz').value + self._tag_size = self.get_parameter('tag_size_m').value + self._min_margin = self.get_parameter('min_margin').value + + self._bridge = CvBridge() + self._backend = ArucoBackend(self.get_logger()) + + self._K: np.ndarray | None = None + self._dist: np.ndarray | None = None + self._latest_img: Image | None = None + + # Derive camera_info topic from image topic namespace + info_topic = img_topic.rsplit('/', 1)[0] + '/camera_info' + + self.create_subscription(Image, img_topic, self._on_image, _SENSOR_QOS) + self.create_subscription(CameraInfo, info_topic, self._on_camera_info, _LATCHED_QOS) + + if _MSGS_OK: + self._pub = self.create_publisher( + DetectedTagArray, '/saltybot/apriltags', 10 + ) + else: + self._pub = None + self.get_logger().warning( + '[apriltag] saltybot_apriltag_msgs not built — ' + 'DetectedTagArray not published' + ) + + self.create_timer(1.0 / pub_hz, self._tick) + + self.get_logger().info( + f'apriltag_detector ready — dict={_FAMILY} ' + f'tag_size={self._tag_size}m topic={img_topic} hz={pub_hz}' + ) + + # ── Callbacks ───────────────────────────────────────────────────────────── + + def _on_image(self, msg: Image) -> None: + self._latest_img = msg + + def _on_camera_info(self, msg: CameraInfo) -> None: + self._K = np.array(msg.k, dtype=np.float64).reshape(3, 3) + self._dist = np.array(msg.d, dtype=np.float64) + + # ── 10 Hz tick ──────────────────────────────────────────────────────────── + + def _tick(self) -> None: + if self._latest_img is None or self._pub is None: + return + + try: + gray = self._bridge.imgmsg_to_cv2( + self._latest_img, desired_encoding='mono8' + ) + except Exception as exc: + self.get_logger().warning(f'[apriltag] decode error: {exc}') + return + + corners, ids = self._backend.detect(gray) + + stamp = self.get_clock().now().to_msg() + arr = self._build_array(corners, ids, self._latest_img.header, stamp) + self._pub.publish(arr) + + # ── Message builder ─────────────────────────────────────────────────────── + + def _build_array(self, corners, ids, src_header, stamp) -> 'DetectedTagArray': + arr = DetectedTagArray() + arr.header.stamp = stamp + arr.header.frame_id = src_header.frame_id + + if ids is None or len(ids) == 0: + arr.count = 0 + return arr + + half = self._tag_size / 2.0 + obj_pts = np.array([ + [-half, half, 0], + [ half, half, 0], + [ half, -half, 0], + [-half, -half, 0], + ], dtype=np.float64) + + for corner_set, tag_id in zip(corners, ids.flatten()): + pts = corner_set.reshape(4, 2) + margin = float(np.mean([ + np.linalg.norm(pts[i] - pts[(i + 1) % 4]) for i in range(4) + ])) + if margin < self._min_margin: + continue + + cx = float(np.mean(pts[:, 0])) + cy = float(np.mean(pts[:, 1])) + + tag = DetectedTag() + tag.header = arr.header + tag.tag_id = int(tag_id) + tag.family = _FAMILY + tag.decision_margin = margin + tag.corners = pts.flatten().tolist() + tag.center = Point(x=cx, y=cy, z=0.0) + + if self._K is not None and self._dist is not None: + ok, rvec, tvec = cv2.solvePnP( + obj_pts, pts.astype(np.float64), + self._K, self._dist, + flags=cv2.SOLVEPNP_IPPE_SQUARE, + ) + if ok: + tx, ty, tz, qx, qy, qz, qw = rvec_tvec_to_pose(rvec, tvec) + tag.pose.position.x = tx + tag.pose.position.y = ty + tag.pose.position.z = tz + tag.pose.orientation.x = qx + tag.pose.orientation.y = qy + tag.pose.orientation.z = qz + tag.pose.orientation.w = qw + tag.pose_valid = True + else: + tag.pose_valid = False + else: + tag.pose_valid = False + + arr.tags.append(tag) + + arr.count = len(arr.tags) + return arr + + +def main(args=None): + rclpy.init(args=args) + node = AprilTagNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_apriltag/setup.cfg b/jetson/ros2_ws/src/saltybot_apriltag/setup.cfg new file mode 100644 index 0000000..ed164d6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_apriltag +[install] +install_scripts=$base/lib/saltybot_apriltag diff --git a/jetson/ros2_ws/src/saltybot_apriltag/setup.py b/jetson/ros2_ws/src/saltybot_apriltag/setup.py new file mode 100644 index 0000000..0dfd5d6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup, find_packages +from glob import glob + +package_name = 'saltybot_apriltag' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', + glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='SaltyLab', + maintainer_email='robot@saltylab.local', + description='AprilTag DICT_APRILTAG_36h11 detector — 10 Hz, 6-DOF pose', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'apriltag_detector = saltybot_apriltag.apriltag_node:main', + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_apriltag/test/test_apriltag.py b/jetson/ros2_ws/src/saltybot_apriltag/test/test_apriltag.py new file mode 100644 index 0000000..a5a36f0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag/test/test_apriltag.py @@ -0,0 +1,142 @@ +""" +test_apriltag.py — Unit tests for AprilTag detector helpers. + +Runs without ROS2 (no rclpy). Tests cover: + - _rvec_tvec_to_pose: rotation/translation → Pose quaternion + - _ArucoBackend: detection on a synthetic rendered tag image + - AprilTagNode._build_array: message construction from mock detections +""" + +from __future__ import annotations + +import math +import sys +import os + +import numpy as np +import pytest +import cv2 + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) + +from saltybot_apriltag._aruco_utils import rvec_tvec_to_pose as _rvec_tvec_to_pose, ArucoBackend as _ArucoBackend + + +# ── _rvec_tvec_to_pose ──────────────────────────────────────────────────────── + +class TestRvecTvecToPose: + # _rvec_tvec_to_pose returns (tx, ty, tz, qx, qy, qz, qw) + + def test_identity_rotation_gives_unit_quaternion(self): + tx, ty, tz, qx, qy, qz, qw = _rvec_tvec_to_pose(np.zeros(3), np.zeros(3)) + assert qw == pytest.approx(1.0, abs=1e-6) + assert qx == pytest.approx(0.0, abs=1e-6) + assert qy == pytest.approx(0.0, abs=1e-6) + assert qz == pytest.approx(0.0, abs=1e-6) + + def test_translation_passed_through(self): + tvec = np.array([1.5, -0.3, 2.0]) + tx, ty, tz, *_ = _rvec_tvec_to_pose(np.zeros(3), tvec) + assert tx == pytest.approx(1.5) + assert ty == pytest.approx(-0.3) + assert tz == pytest.approx(2.0) + + def test_90deg_yaw_quaternion(self): + # 90° rotation around Z axis → qw=cos(45°), qz=sin(45°) + rvec = np.array([0.0, 0.0, math.pi / 2]) + tx, ty, tz, qx, qy, qz, qw = _rvec_tvec_to_pose(rvec, np.zeros(3)) + assert qw == pytest.approx(math.cos(math.pi / 4), abs=1e-5) + assert qz == pytest.approx(math.sin(math.pi / 4), abs=1e-5) + assert qx == pytest.approx(0.0, abs=1e-5) + assert qy == pytest.approx(0.0, abs=1e-5) + + def test_quaternion_is_unit_norm(self): + rvec = np.array([0.3, 0.7, -1.1]) + tvec = np.array([0.5, 0.5, 0.5]) + tx, ty, tz, qx, qy, qz, qw = _rvec_tvec_to_pose(rvec, tvec) + norm = math.sqrt(qx**2 + qy**2 + qz**2 + qw**2) + assert norm == pytest.approx(1.0, abs=1e-5) + + def test_180deg_x_rotation(self): + rvec = np.array([math.pi, 0.0, 0.0]) + tx, ty, tz, qx, qy, qz, qw = _rvec_tvec_to_pose(rvec, np.zeros(3)) + norm = math.sqrt(qx**2 + qy**2 + qz**2 + qw**2) + assert norm == pytest.approx(1.0, abs=1e-5) + + +# ── _ArucoBackend ───────────────────────────────────────────────────────────── + +class _FakeLogger: + def info(self, msg): pass + def warning(self, msg): pass + def debug(self, msg): pass + + +def _render_apriltag_36h11(tag_id: int = 0, size: int = 200) -> np.ndarray: + """ + Render an AprilTag DICT_APRILTAG_36h11 tag into a greyscale image. + Returns an image large enough for reliable detection. + """ + dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_APRILTAG_36h11) + # Draw the marker (OpenCV ≥ 4.6) + tag_img = np.zeros((size, size), dtype=np.uint8) + try: + cv2.aruco.generateImageMarker(dictionary, tag_id, size, tag_img, 1) + except AttributeError: + # Older API + tag_img = cv2.aruco.drawMarker(dictionary, tag_id, size) + + # Embed in white canvas with border + canvas = np.ones((size + 80, size + 80), dtype=np.uint8) * 255 + canvas[40:40 + size, 40:40 + size] = tag_img + return canvas + + +class TestArucoBackend: + + def test_backend_initialises(self): + backend = _ArucoBackend(_FakeLogger()) + assert backend is not None + + def test_detect_returns_tuple(self): + backend = _ArucoBackend(_FakeLogger()) + gray = np.ones((480, 640), dtype=np.uint8) * 200 + corners, ids = backend.detect(gray) + assert isinstance(corners, (list, tuple)) + + def test_blank_image_no_detections(self): + backend = _ArucoBackend(_FakeLogger()) + gray = np.ones((480, 640), dtype=np.uint8) * 200 + _, ids = backend.detect(gray) + assert ids is None or len(ids) == 0 + + def test_rendered_tag_detected(self): + backend = _ArucoBackend(_FakeLogger()) + tag_img = _render_apriltag_36h11(tag_id=5, size=200) + corners, ids = backend.detect(tag_img) + assert ids is not None and len(ids) == 1 + assert int(ids.flatten()[0]) == 5 + + def test_rendered_tag_corners_shape(self): + backend = _ArucoBackend(_FakeLogger()) + tag_img = _render_apriltag_36h11(tag_id=0, size=200) + corners, ids = backend.detect(tag_img) + assert ids is not None + assert len(corners) == 1 + assert corners[0].reshape(4, 2).shape == (4, 2) + + def test_multiple_tags_detected(self): + """Place two tags side-by-side and expect both found.""" + backend = _ArucoBackend(_FakeLogger()) + t0 = _render_apriltag_36h11(tag_id=0, size=150) + t1 = _render_apriltag_36h11(tag_id=1, size=150) + canvas = np.ones((t0.shape[0], t0.shape[1] * 2 + 20), dtype=np.uint8) * 255 + canvas[:, :t0.shape[1]] = t0 + canvas[:, t0.shape[1] + 20:] = t1 + _, ids = backend.detect(canvas) + assert ids is not None and len(ids) == 2 + assert set(ids.flatten().tolist()) == {0, 1} + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/jetson/ros2_ws/src/saltybot_apriltag_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_apriltag_msgs/CMakeLists.txt new file mode 100644 index 0000000..f5d40e4 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag_msgs/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(saltybot_apriltag_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/DetectedTag.msg" + "msg/DetectedTagArray.msg" + DEPENDENCIES std_msgs geometry_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/jetson/ros2_ws/src/saltybot_apriltag_msgs/msg/DetectedTag.msg b/jetson/ros2_ws/src/saltybot_apriltag_msgs/msg/DetectedTag.msg new file mode 100644 index 0000000..a89c5b8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag_msgs/msg/DetectedTag.msg @@ -0,0 +1,18 @@ +# DetectedTag — single AprilTag detection result. + +std_msgs/Header header + +int32 tag_id # numeric ID encoded in the tag +string family # e.g. 'DICT_APRILTAG_36h11' + +float32 decision_margin # detector confidence (higher = more certain) + +# Corner pixel coordinates [x0,y0, x1,y1, x2,y2, x3,y3] (image frame, CW from TL) +float32[8] corners + +# Tag centre in image pixels +geometry_msgs/Point center + +# 6-DOF pose in camera optical frame (only valid when pose_valid=true) +geometry_msgs/Pose pose +bool pose_valid # false if camera_info not yet received diff --git a/jetson/ros2_ws/src/saltybot_apriltag_msgs/msg/DetectedTagArray.msg b/jetson/ros2_ws/src/saltybot_apriltag_msgs/msg/DetectedTagArray.msg new file mode 100644 index 0000000..5be9892 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag_msgs/msg/DetectedTagArray.msg @@ -0,0 +1,7 @@ +# DetectedTagArray — all AprilTags found in one frame. +# Published at ~10 Hz on /saltybot/apriltags. + +std_msgs/Header header + +saltybot_apriltag_msgs/DetectedTag[] tags +int32 count diff --git a/jetson/ros2_ws/src/saltybot_apriltag_msgs/package.xml b/jetson/ros2_ws/src/saltybot_apriltag_msgs/package.xml new file mode 100644 index 0000000..d4b39c2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_apriltag_msgs/package.xml @@ -0,0 +1,22 @@ + + + + saltybot_apriltag_msgs + 0.1.0 + Message types for AprilTag landmark detection. + SaltyLab + MIT + + ament_cmake + rosidl_default_generators + + std_msgs + geometry_msgs + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +