AprilTag Machine Code Tracking (No-PTZ)

1. Experiment Objective

The camera tracks an AprilTag marker, and the car rotates to keep the Tag at the center of the frame. It supports Tracking mode (rotation only) and Follow mode (rotation + distance keeping).

2. Experiment Principle

1. Keyword Explanation

2. Technical Architecture

/camera/image_raw (BEST_EFFORT)
  → april_tag_tracker_node
    ├── Image processing (15Hz): image conversion → flip → apriltag.Detector.detect() → Tag corners + pose
    └── Body control (10Hz): pixel error → PD angular velocity + Tag depth Z-value distance following → /cmd_vel (BEST_EFFORT)

Follow mode feature: distance following is based not on pixel size but on the Z value of the Tag 3D pose (actual depth, in meters), providing higher precision.

3. Core Topic Quick Reference

Firmware Downlink

3. Experiment Steps

1. Hardware Preparation

2. Start the Agent + Camera + Joint Model (Terminal 1)

xxxxxxxxxx
ros2 launch microros_v2_bringup car_base.launch.py

3. Start AprilTag Tracking (Terminal 2)

Tracking mode (rotation tracking only):

xxxxxxxxxx
ros2 launch microros_v2_no_ptz_visual_control_pkg april_tag_tracker.launch.py

5. Operation Instructions

4. Experiment Phenomenon

• When the AprilTag moves left or right in the frame, the car automatically rotates to track it, keeping the Tag at the center of the frame
• The terminal outputs the Tag's 3D pose information in real time (translation x/y/z and rotation angles)
• The Tag border and center point are visualized
• Follow mode: keeps a constant distance based on the Tag depth Z value
• After the Tag moves out of the frame, the car stops; when it reappears, tracking resumes automatically

5. Code Explanation

Source path:

• Launch (track): ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/april_tag_tracker.launch.py
• Launch (follow): ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/april_tag_follow.launch.py
• Node source: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/april_tag_tracker_node.py

1. Core Node Analysis

april_tag_tracker_node uses the apriltag Python library to detect AprilTag markers. Core workflow:

• Image processing (15Hz): raw Image frame conversion → grayscale → detector.detect(gray) → extract Tag corners + pose
• 3D pose estimation: uses the PnP algorithm with the known physical Tag size (tag_size) and camera intrinsics to compute the 6-DoF pose
• Body control (10Hz): deviation between the Tag center x-coordinate and the image center → PD angular velocity; Tag depth Z-value deviation → PD linear velocity
• quad_decimate: lowers the detection resolution to speed up processing (the larger the value, the faster, but small Tags at long distance may be missed)

Constructor — create the AprilTag detector:

​x
# Source file: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/april_tag_tracker_node.py
​
def __init__(self) -> None:
    super().__init__(DEFAULT_NODE_NAME)
    self._tgt_lock = threading.Lock()
    self._state = STATE_IDLE
    self._has_target = False
    self._lost_frames = 0
    self._target_x: Optional[float] = None
    self._target_z: Optional[float] = None  # Tag depth Z value (used for distance following)
​
    self._declare_params()
    self._load_params()
​
    # Create the AprilTag detector
    self._detector = apriltag.Detector(
        apriltag.DetectorOptions(
            families=self._tag_family,
            quad_decimate=float(self._quad_decimate),
        )
    )
​
    best_effort = QoSProfile(
        history=QoSHistoryPolicy.KEEP_LAST, depth=1,
        reliability=QoSReliabilityPolicy.BEST_EFFORT,
        durability=QoSDurabilityPolicy.VOLATILE,
    )
    self._img_sub = self.create_subscription(
        Image, self._img_topic, self._on_image, best_effort)
    self._cmd_pub = self.create_publisher(Twist, self._cmd_topic, best_effort)
​
    self._img_timer = self.create_timer(
        1.0 / max(float(self._img_rate), MINIMUM_TIMER_RATE_HZ), self._process_image)
    self._body_timer = self.create_timer(
        1.0 / max(float(self._body_rate), MINIMUM_TIMER_RATE_HZ), self._body_tick)

Body control callback — 3D Z-value distance following:

xxxxxxxxxx
# Source file: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/april_tag_tracker_node.py
​
def _body_tick(self) -> None:
    """Body control timer (10Hz)."""
    if self._state != STATE_TRACKING or self._shutdown:
        self._pub_zero()
        return
​
    with self._tgt_lock:
        has = self._has_target
        tx = self._target_x
        tz = self._target_z
​
    if not has or tx is None:
        self._pub_zero()
        return
​
    # Horizontal pixel error → PD angular velocity
    cx = self._img_w * 0.5
    err_x = tx - cx
    if abs(err_x) <= self._deadband_x:
        az = 0.0
    else:
        az = self._angular_kp * err_x
        az = clamp(az, -self._max_az, self._max_az)
        if self._reverse_body:
            az = -az
​
    cmd = Twist()
    cmd.angular.z = az
​
    # Distance following: based on the 3D depth Z value (not pixel size)
    if self._dist_follow and tz is not None and tz > 0:
        err_z = self._target_z_m - tz  # _target_z_m unit: meters
        if abs(err_z) > self._dist_tol_z:
            lx = self._dist_kp * err_z
            lx = clamp(lx, -self._max_lx, self._max_lx)
            if self._reverse_linear:
                lx = -lx
            cmd.linear.x = lx
​
    self._cmd_pub.publish(cmd)

Key logic: Unlike the pixel-size-based distance following in color/face tracking, AprilTag distance following directly uses the 3D pose Z value (actual depth), so the actual size of the target does not need to be calibrated. The tag_size parameter must match the actual physical side length of the printed Tag (default 0.048 m); otherwise the pose estimation will be inaccurate.

2. Key Parameters

Parameter definition file:

• Launch: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/april_tag_tracker.launch.py
• Node: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/april_tag_tracker_node.py

3. Node Description

6. Common Problems