AprilTag Machine Code Follow (PTZ)

1. Experiment Objective

PTZ pan-tilt + car body collaborative AprilTag following — dt_apriltags detects the AprilTag, the 3D pose Z-axis enables precise distance following, the pan-tilt locks onto the Tag center with PD, and the body homes in coordination.

2. Experiment Principle

1. Key Terms

2. Technical Architecture

Node chain:

State estimation layer → april_tag_follow_ptz_node → /cmd_ptz_angle (pan-tilt angle) + /cmd_vel (body velocity)

Three-layer coordinated control:

• PTZ PD control (30 Hz): Tag center pixel error → PD (pd_kp=1.2, pd_kd=0.25) → step limiting (±1.2°) → angular-rate feedforward compensation (body_ff_gain=1.3) → /cmd_ptz_angle. Deadband 50 px (larger; AprilTag detection is stable and needs no fine-tuning).
• Body angular-velocity PD control (10 Hz): pan-tilt horizontal angle × body_share (0.5) → error shared with the body → PD (angular_kp=0.2, angular_kd=0.06) → limiting ±0.12 rad/s.
• 3D distance-follow PD control (10 Hz): deviation between the Tag's Z-axis depth and target_tag_pose_z (0.35 m) → PD (distance_kp=0.6, distance_kd=0.2, the highest gains — meter-level precision) → limiting ±0.12 m/s.

Multiple-Tag selection and visualization:

tag_select_strategy=largest selects the primary Tag. On-screen overlay: camera coordinate axes (X/Y/Z), the primary Tag as a purple polygon + tag_id, and the other Tags as green polygons.

3. Core Topics Quick Reference

Firmware Uplink (ESP32 → ROS2):

Firmware Downlink (ROS2 → ESP32 via micro_ros_agent):

ROS2 Internal (published by the state estimation layer):

3. Experiment Steps

1. Hardware Preparation

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

ros2 launch microros_v2_bringup car_base.launch.py

3. Start AprilTag Follow (Terminal 2)

Open a new terminal and run:

xxxxxxxxxx
ros2 launch microros_v2_ptz_visual_control_pkg april_tag_follow_ptz.launch.py

After the node starts, the camera intrinsics (camera.yaml) are loaded automatically, and the Space key toggles the idle/tracking state. Without a calibration file, it automatically degrades to pixel-size distance following.

4. Operation Instructions

Interaction:

Workflow:

1. After the program starts, it is in the idle state by default; AprilTag detection is already running but outputs no control commands
2. Place the AprilTag within the camera's field of view, then press Space to enter the tracking state
3. The pan-tilt locks onto the Tag center, the body turns to home, and a distance of about 0.35 m is maintained automatically
4. Press i to return to idle, press q to exit

Safety and Edge Cases:

• dt_apriltags unavailable: an ImportError is logged; the node does not crash
• Camera YAML unavailable: pose estimation is disabled; only 2D pixel tracking and distance following are performed (degraded mode)
• No Tag ≥5 frames → freeze the body (lost_frame_threshold=5)
• Pan-tilt limiting: horizontal [-90°, 90°], tilt [-90°, 20°]
• Velocity limiting: angular velocity ±0.12 rad/s, linear velocity ±0.12 m/s
• quad_decimate ≥1.0, validated at runtime

5. How to Stop

Press Ctrl+C to stop Terminal 2 → Terminal 1 in order.

4. Experiment Observations

• idle: AprilTag detection runs but outputs no control; the frame shows the Tag border and id
• tracking + Tag: The pan-tilt locks onto the Tag center, the body turns to home, and 3D distance following maintains a depth of about 0.35 m
• With camera calibration: the pose X/Y/Z is displayed precisely, and distance control is based on the meter-level Z axis
• Without camera calibration: degrades to pixel-size distance following (based on the Tag area)
• Tag moves forward/back: the car adjusts forward/back to maintain a 0.35 m ± 0.06 m depth
• Multiple Tags: the primary Tag is marked purple + the others green, selected by the largest strategy
• Tag gone ≥5 frames: the body freezes

5. Program Analysis

Source code paths:

• Launch file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/launch/april_tag_follow_ptz.launch.py
• Node file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/april_tag_follow_ptz_node.py

1. Core Node Analysis

AprilTagFollowPtzNode inherits from rclpy.node.Node; detection and display are done in the image callback, with two independent control timers for PTZ/body. It supports dynamic parameter hot-reloading and rqt dynamic tuning.

Node initialization — __init__:

​x
# Source file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/april_tag_follow_ptz_node.py
​
class AprilTagFollowPtzNode(Node):
​
    def __init__(self) -> None:
        super().__init__(DEFAULT_NODE_NAME)
        self._tgt_lock = threading.Lock()
        self._latest_frame: Optional[np.ndarray] = None
​
        self._state = STATE_IDLE
        self._shutdown = False
​
        self._has_target = False
        self._lost_frames = 0
        self._tag_id = -1
        self._target_x: Optional[float] = None
        self._target_y: Optional[float] = None
        self._tag_pose_x: Optional[float] = None
        self._tag_pose_y: Optional[float] = None
        self._tag_pose_z: Optional[float] = None
        self._tag_corners: List[Tuple[int, int]] = []
​
        self._h_deg = 0.0
        self._p_deg = 0.0
        self._prev_err_x = 0.0
        self._prev_err_y = 0.0
        self._prev_ctrl_t = 0.0
        self._last_sx = 0.0
        self._last_sy = 0.0
​
        self._prev_s1 = 0.0
        self._prev_s1_eff = 0.0
        self._prev_body_t = 0.0
        self._last_body_az = 0.0
​
        self._prev_z_err = 0.0
        self._prev_linear_t = 0.0
​
        self._fps = 0.0
        self._fps_n = 0
        self._fps_t = time.perf_counter()
        self._win_ok = False
        self._last_print_t = 0.0
​
        self._detector = None
        self._camera_params: Optional[Tuple[float, float, float, float]] = None
​
        self._declare_params()
        self._load_params()
        self._h_deg = self._init_h_deg
        self._p_deg = self._init_p_deg
        self._init_detector()
​
        video_qos = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=1,
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            durability=QoSDurabilityPolicy.VOLATILE,
        )
​
        self._sub = self.create_subscription(
            Image, self._img_topic, self._on_image, video_qos
        )
        self._ptz_pub = self.create_publisher(Vector3, self._ptz_topic, 10)
        self._cmd_pub = self.create_publisher(Twist, self._cmd_topic, 10)
​
        self._ptz_timer = self.create_timer(
            1.0 / max(self._ptz_rate, MINIMUM_TIMER_RATE_HZ), self._ptz_tick
        )
        self._body_timer = None
        self._rebuild_body_timer(False)
        self._key_timer = None
        if self._show_win:
            self._init_win()
            self._key_timer = self.create_timer(0.05, self._poll_keys)
​
        self._init_ptz_timer = None
        self._init_ptz_remain = 0
        if self._pub_init:
            self._start_init_ptz()
​
        self._param_cb = self.add_on_set_parameters_callback(self._on_param_update)
        self.get_logger().info(
            f"AprilTag follow PTZ+body started. SPACE=toggle, i=idle, q=quit. "
            f"ptz={self._ptz_rate:.0f}Hz body={self._body_rate:.0f}Hz "
            f"family={self._tag_family} tag_size={self._tag_size:.3f}m"
        )

Distance-follow core — _body_tick (3D Z-axis PD control):

xxxxxxxxxx
# Source file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/april_tag_follow_ptz_node.py
​
    def _body_tick(self) -> None:
        if self._state != STATE_TRACKING:
            self._prev_s1 = 0.0
            self._prev_s1_eff = 0.0
            self._prev_body_t = 0.0
            self._pub_zero_cmd()
            return
​
        with self._tgt_lock:
            ok = self._has_target
            pose_z = self._tag_pose_z
        if not ok:
            self._prev_s1 = 0.0
            self._prev_s1_eff = 0.0
            self._prev_body_t = 0.0
            self._pub_zero_cmd()
            return
​
        s1 = self._h_deg
        if abs(s1) < self._ang_tol:
            s1 = 0.0
        s1_eff = s1 * self._body_share
​
        n = time.perf_counter()
        dt = n - self._prev_body_t if self._prev_body_t > 0.0 else 1.0 / max(self._body_rate, MINIMUM_TIMER_RATE_HZ)
        dt = max(dt, 1e-3)
        ds1_eff = (s1_eff - self._prev_s1_eff) / dt
        az = self._ang_kp * s1_eff + self._ang_kd * ds1_eff
        self._prev_s1 = s1
        self._prev_s1_eff = s1_eff
        self._prev_body_t = n
​
        if self._rev_body:
            az = -az
        az = clamp(az, -self._max_az, self._max_az)
        self._last_body_az = az
​
        if self._enable_dist and pose_z is not None:
            z_err = pose_z - self._target_pose_z
            if abs(z_err) < self._dist_tol_z:
                z_err = 0.0
            dt_l = n - self._prev_linear_t if self._prev_linear_t > 0.0 else 1.0 / max(self._body_rate, MINIMUM_TIMER_RATE_HZ)
            dt_l = max(dt_l, 1e-3)
            dz = (z_err - self._prev_z_err) / dt_l
            lx = self._dist_kp * z_err + self._dist_kd * dz
            self._prev_z_err = z_err
            self._prev_linear_t = n
            if self._rev_linear:
                lx = -lx
            lx = clamp(lx, -self._max_lx, self._max_lx)
        else:
            self._prev_z_err = 0.0
            lx = 0.0
​
        tw = Twist()
        tw.angular.z = float(az)
        tw.linear.x = float(lx)
        self._cmd_pub.publish(tw)

The 3D distance-follow key line: z_err = pose_z - self._target_pose_z — the pose_t[2] (Z-axis depth, in meters) returned by the AprilTag pose estimation is used as the error input, and after PD computation the linear.x velocity is output. The distance_kp=0.6 / distance_kd=0.2 gains are significantly higher than the pixel-size scheme, because the meter-level error range is larger.

2. Key Parameters

Parameter definition files:

• Launch file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/launch/april_tag_follow_ptz.launch.py
• Node file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/april_tag_follow_ptz_node.py

Pan-tilt PTZ control parameters:

Body control parameters:

Distance-follow parameters (3D pose Z-axis):

AprilTag detection parameters:

6. Frequently Asked Questions