Mediapipe Human Pose Follow (PTZ)

1. Experiment Objective

PTZ pan-tilt + car body collaborative human pose following — MediaPipe Pose detects 33 landmarks, uses the mean center of all landmarks as the tracking target, keeps the tilt axis fixed (fixed_pitch_deg=-10°), and coordinates horizontal PD + body movement.

2. Experiment Principle

1. Key Terms

2. Technical Architecture

Node chain:

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

Three-layer coordinated control:

• PTZ PD control (20 Hz): the human center x pixel error → PD (pd_kp=3.0, pd_kd=1.5, the highest gains — human movement is large) → feedforward compensation → /cmd_ptz_angle.x. The tilt direction is fixed and not tracked; fixed_pitch_deg (-10°) is always output, avoiding tilt jitter caused by changes in human height.
• Body angular-velocity PD control (10 Hz): pan-tilt horizontal angle × body_share (0.5) → error shared with the body → PD (angular_kp=0.8, angular_kd=0.05) → limiting ±0.12 rad/s.
• Distance-follow PD control (10 Hz): deviation between the human bbox height and target_pose_size_px (320 px) → PD (distance_kp=0.008, distance_kd=0.003) → limiting ±0.20 m/s.

Lost-frame tolerance: no person for ≥5 consecutive frames → freeze the body.

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 Human Pose Follow (Terminal 2)

Open a new terminal and run:

xxxxxxxxxx
ros2 launch microros_v2_ptz_visual_control_pkg pose_follow_ptz.launch.py

After the node starts, the initial PTZ angle is published automatically, and the Space key toggles the idle/tracking state. At this point the pan-tilt is powered on but not yet in tracking mode.

4. Operation Instructions

Interaction:

Workflow:

1. After the program starts, it is in the idle state by default; MediaPipe pose detection is already running but outputs no control commands
2. Press Space to enter the tracking state; the pan-tilt tracks the human center horizontally, and the body turns + follows distance in coordination
3. Press i to return to idle, press q to exit

Safety and Edge Cases:

• No person ≥5 frames → freeze the body (lost_frame_threshold=5)
• Tilt fixed at fixed_pitch_deg (-10°), not involved in PD tracking
• Pan-tilt limiting: horizontal [-90°, 90°], tilt [-90°, 20°]
• Velocity limiting: angular velocity ±0.12 rad/s, linear velocity ±0.20 m/s

5. How to Stop

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

4. Experiment Observations

• idle: Pose detection runs but outputs no control; the frame shows the human landmarks and the center marker
• tracking + human present: The pan-tilt tracks the human center horizontally (purple dot), and the body turns quickly (angular_kp=0.8) + follows distance
• Tilt not tracked: always points at fixed_pitch_deg (-10°); changes in the person's height do not affect the pan-tilt
• Human moves left/right: high-gain PD (3.0/1.5) responds to the pan-tilt quickly, and the high body angular-velocity gain (0.8) turns in coordination
• Human moves forward/back: distance following based on the bbox size, up to 0.20 m/s
• No person ≥5 frames: the body freezes and the frame shows the lost count

5. Program Analysis

Source code paths:

• Launch file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/launch/pose_follow_ptz.launch.py
• Node file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/pose_follow_ptz_node.py

1. Core Node Analysis

PoseFollowPtzNode inherits from rclpy.node.Node and internally contains three timer loops: image processing (15 Hz), PTZ control (20 Hz), body control (10 Hz), plus key polling (20 Hz).

Node initialization — __init__:

​x
# Source file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/pose_follow_ptz_node.py
​
class PoseFollowPtzNode(Node):
​
    def __init__(self) -> None:
        super().__init__(DEFAULT_NODE_NAME)
        self._image_lock = threading.Lock()
        self._pose_lock = threading.Lock()
​
        self._latest_frame: Optional[np.ndarray] = None
        self._latest_seq = 0
        self._processed_seq = 0
​
        self._state = STATE_IDLE
        self._shutdown = False
​
        self._has_pose = False
        self._has_target = False
        self._lost_frames = 0
        self._target_x: Optional[float] = None
        self._target_y: Optional[float] = None
        self._target_size_px: Optional[float] = None
​
        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_size_err = 0.0
        self._prev_linear_t = 0.0
​
        self._measured_fps = 0.0
        self._fps_count = 0
        self._fps_last_t = time.perf_counter()
        self._win_ready = False
        self._last_print_t = 0.0
​
        self._declare_params()
        self._load_params()
​
        self._h_deg = float(self._init_h_deg)
        self._p_deg = clamp(float(self._fixed_pitch_deg), self._p_min, self._p_max)
​
        self._pose_det = PoseDetector(
            detectionCon=float(self._pose_conf),
            trackCon=float(self._pose_track_conf),
        )
​
        video_qos = 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, 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._img_timer = self.create_timer(
            1.0 / max(float(self._img_rate), MINIMUM_TIMER_RATE_HZ), self._process_image
        )
        self._ptz_timer = self.create_timer(
            1.0 / max(float(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._ensure_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_ptz:
            self._start_init_ptz()
​
        self._param_cb = self.add_on_set_parameters_callback(self._on_param_update)
​
        self.get_logger().info(
            f"Pose follow PTZ+body started. SPACE=toggle, i=idle, q=quit. "
            f"ptz={self._ptz_rate:.0f}Hz img={self._img_rate:.0f}Hz body={self._body_rate:.0f}Hz"
        )

PTZ control core — _ptz_tick (fixed-pitch implementation):

xxxxxxxxxx
# Source file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/pose_follow_ptz_node.py
​
    def _ptz_tick(self) -> None:
        if self._state != STATE_TRACKING:
            return
​
        with self._pose_lock:
            tx = self._target_x
            ty = self._target_y
            has_target = self._has_target
​
        if not has_target or tx is None or ty is None:
            self._reset_ptz_pd()
            return
​
        icx = float(self._proc_w) * 0.5
        icy = float(self._proc_h) * 0.5
        pex = float(tx - icx)
        pey = float(ty - icy)
​
        cx = abs(pex) < self._db_x
        cy = abs(pey) < self._db_y
        if cx:
            pex = 0.0
        if cy:
            pey = 0.0
​
        n = time.perf_counter()
        dt = n - self._prev_ctrl_t if self._prev_ctrl_t > 0.0 else 1.0 / max(float(self._ptz_rate), MINIMUM_TIMER_RATE_HZ)
        dt = max(dt, 1e-3)
​
        nex = pex / icx
        ney = pey / icy
        dx = (nex - self._prev_err_x) / dt
        dy = (ney - self._prev_err_y) / dt
​
        ff = self._body_ff_gain * self._last_body_az
        rx = clamp((self._kp * nex + self._kd * dx - ff) * self._max_step, -self._max_step, self._max_step)
        ry = 0.0  # pitch is fixed: no PD control output
​
        a = self._step_alpha
        sx = (1.0 - a) * self._last_sx + a * rx
        sy = (1.0 - a) * self._last_sy + a * ry
        if cx:
            sx = 0.0
            nex = 0.0
        if cy:
            sy = 0.0
            ney = 0.0
​
        self._prev_err_x = nex
        self._prev_err_y = ney
        self._prev_ctrl_t = n
        self._last_sx = sx
        self._last_sy = sy
​
        if self._rev_h:
            sx = -sx
        dh = clamp(self._h_deg + sx, self._h_min, self._h_max)
        dp = clamp(self._fixed_pitch_deg, self._p_min, self._p_max)
​
        sa = self._ptz_alpha
        self._h_deg = clamp((1.0 - sa) * self._h_deg + sa * dh, self._h_min, self._h_max)
        self._p_deg = dp
​
        self._pub_ptz()
        self._log_ctrl(pex, pey)

The fixed-pitch key lines: ry = 0.0 and dp = clamp(self._fixed_pitch_deg, self._p_min, self._p_max) — the tilt direction performs no PD error computation and always outputs the fixed_pitch_deg angle.

2. Key Parameters

Parameter definition files:

• Launch file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/launch/pose_follow_ptz.launch.py
• Node file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/pose_follow_ptz_node.py

Pan-tilt PTZ control parameters:

Body control parameters:

Distance-follow parameters:

Pose detection parameters:

6. Frequently Asked Questions