seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat(jetson): AprilTag landmark detector — DICT_APRILTAG_36h11 (issue #191)

Browse Source 
saltybot_apriltag_msgs (ament_cmake):
• DetectedTag.msg     — tag_id, family, decision_margin, corners[8],
                        center, 6-DOF pose + pose_valid flag
• DetectedTagArray.msg — DetectedTag[], count

saltybot_apriltag (ament_python, single node):
• _aruco_utils.py    — ROS2-free: ArucoBackend (cv2 4.6/4.7+ API shim),
                        rvec_tvec_to_pose (Rodrigues → Shepperd quaternion)
• apriltag_node.py   — 10 Hz timer; subscribes image + latched camera_info;
                        cv2.solvePnP IPPE_SQUARE for 6-DOF pose when K
                        available; publishes /saltybot/apriltags
• test/test_apriltag.py — 11 unit tests (11/11 pass, no ROS2 needed):
                          pose math + rendered tag detection + multi-tag

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-perception 2026-03-02 11:07:35 -05:00
parent 6dbbbb9adc
commit d6d7e7b75a
12 changed files with 563 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
jetson/ros2_ws/src/saltybot_apriltag/package.xml Normal file
View File

@ -0,0 +1,28 @@
<?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>saltybot_apriltag</name>
<version>0.1.0</version>
<description>
AprilTag (DICT_APRILTAG_36h11) landmark detector for SaltyBot CSI cameras.
Detects tags in incoming frames and publishes DetectedTagArray at 10 Hz.
</description>
<maintainer email="robot@saltylab.local">SaltyLab</maintainer>
<license>MIT</license>
<depend>rclpy</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>saltybot_apriltag_msgs</depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>python3-opencv</exec_depend>
<test_depend>pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
jetson/ros2_ws/src/saltybot_apriltag/resource/saltybot_apriltag Normal file
View File

0
jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/__init__.py Normal file
View File

85
jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/_aruco_utils.py Normal file
View File

@ -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

212
jetson/ros2_ws/src/saltybot_apriltag/saltybot_apriltag/apriltag_node.py Normal file
View File

@ -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:
<image_topic> sensor_msgs/Image default /camera/color/image_raw
<image_topic_dir>/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()

4
jetson/ros2_ws/src/saltybot_apriltag/setup.cfg Normal file
View File

@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/saltybot_apriltag
[install]
install_scripts=$base/lib/saltybot_apriltag

29
jetson/ros2_ws/src/saltybot_apriltag/setup.py Normal file
View File

@ -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',
],
},
)

142
jetson/ros2_ws/src/saltybot_apriltag/test/test_apriltag.py Normal file
View File

@ -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'])

16
jetson/ros2_ws/src/saltybot_apriltag_msgs/CMakeLists.txt Normal file
View File

@ -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()

18
jetson/ros2_ws/src/saltybot_apriltag_msgs/msg/DetectedTag.msg Normal file
View File

@ -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

7
jetson/ros2_ws/src/saltybot_apriltag_msgs/msg/DetectedTagArray.msg Normal file
View File

@ -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

22
jetson/ros2_ws/src/saltybot_apriltag_msgs/package.xml Normal file
View File

@ -0,0 +1,22 @@
<?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>saltybot_apriltag_msgs</name>
<version>0.1.0</version>
<description>Message types for AprilTag landmark detection.</description>
<maintainer email="robot@saltylab.local">SaltyLab</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>