Face Follow (PTZ)

1. Experiment Objective

PTZ pan-tilt + car body collaborative face following — Haar Cascade detects the face, the pan-tilt locks onto the face center with PD, the body turns to home based on the s1 angle, and the follow distance is maintained based on the face bbox width.

2. Experiment Principle

1. Key Terms

2. Technical Architecture

Node chain: state estimation (ekf_filter_node + camera_node) → face_follow_ptz_node → /cmd_ptz_angle (pan-tilt) + /cmd_vel (body)

Three-layer coordination: PTZ PD (30 Hz) + body angular-velocity PD (10 Hz) + distance-follow PD (10 Hz)

3. Core Topics Quick Reference

Firmware Uplink

Firmware Downlink

ROS2 Internal

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

xxxxxxxxxx
ros2 launch microros_v2_ptz_visual_control_pkg face_follow_ptz.launch.py

4. Operation Instructions

Workflow:

The architecture is the same as Color Follow: Haar detection is done in the image callback, and the PTZ control (30 Hz) and body control (10 Hz) dual timers run independently.

1. Face detection (Haar Cascade): after decoding, the image is scaled to 640×480, converted to grayscale, and the OpenCV Haar cascade classifier detectMultiScale detects faces (face_min_width/face_min_height=80 px). The largest face is taken as the primary target, yielding the face center (cx, cy) and bbox width.

2. N-frame lost-frame tolerance: the target is marked lost and the body frozen only after consecutive no-face detections reach lost_frame_threshold (5 frames).

3. Three-layer coordinated control:

   • PTZ PD (30 Hz): normalized error → PD (pd_kp=2.0, pd_kd=0.45, higher gains for face movement) → angular-rate feedforward compensation → publish /cmd_ptz_angle
   • Body angular-velocity PD (10 Hz): s1 × body_share (0.5) → PD (angular_kp=0.2, angular_kd=0.06) → /cmd_vel.angular.z
   • Distance follow (10 Hz): deviation between the face bbox width and target_face_width (280 px) → PD (distance_kp=0.008, distance_kd=0.003) → /cmd_vel.linear.x

Interaction:

Safety and Edge Cases:

• If the classifier fails to load, an error is logged when the Haar XML file is unavailable; the node does not crash
• Target lost ≥ 5 frames: marked lost, PD reset, body frozen (zero velocity)
• s1 is zeroed within 3°, avoiding fine-tuning jitter
• Pan-tilt angle limiting: horizontal [-90°, 90°], tilt [-90°, 20°]
• Velocity limiting: angular velocity ±0.15 rad/s, linear velocity ±0.12 m/s
• On exit, stop first, then os._exit(0)

5. How to Stop

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

4. Experiment Observations

• idle state: The frame shows face detection in real time, the pan-tilt does not move, and the body is stationary
• tracking + face detected: The pan-tilt locks onto the face center with PD, the body turns automatically with s1, and simultaneously moves forward/back according to the face size to maintain a bbox width of about 280 px
• Person approaches (face grows): the car moves back to maintain the distance
• Person moves away (face shrinks): the car moves forward to approach
• Face deviates from center: the body takes on 50% of the turning, the pan-tilt takes the rest + feedforward compensates for the body rotation
• Profile face / brief occlusion: occasional Haar detection misses → filtered by the N-frame tolerance
• Face gone > 5 frames: the body freezes, waiting for it to reappear

5. Program Analysis

Source code paths:

• Launch: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/launch/face_follow_ptz.launch.py
• Node: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/face_follow_ptz_node.py

1. Core Node Analysis

FaceFollowPtzNode completes the parameter declaration, face classifier loading, subscriber/publisher creation, and dual-timer initialization in __init__:

​x
# Source file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/microros_v2_ptz_visual_control_pkg/face_follow_ptz_node.py
​
class FaceFollowPtzNode(Node):
​
    def __init__(self) -> None:
        super().__init__(DEFAULT_NODE_NAME)
        self._tgt_lock = threading.Lock()
        self._cv_bridge = CvBridge()
        self._latest_frame: Optional[np.ndarray] = None
​
        self._state = STATE_IDLE
        self._shutdown = False
​
        # Target state
        self._has_target = False
        self._lost_frames = 0; self._lost_frame_threshold = 5
        self._target_x: Optional[float] = None
        self._target_y: Optional[float] = None
        self._target_r: Optional[float] = None  # width of the detected face box
​
        # PTZ
        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
​
        # Face detection
        self._face_cascade = None
​
        # Body PD state
        self._prev_s1 = 0.0; self._prev_body_t = 0.0; self._body_share = 0.5
        self._prev_s1_eff = 0.0
        self._last_body_az = 0.0; self._body_ff_gain = 1.3
​
        # Distance-follow state
        self._target_face_w = 80.0
        self._prev_r_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._declare_params()
        self._load_params()
        self._h_deg = self._init_h_deg; self._p_deg = self._init_p_deg
​
        # Load the face cascade classifier
        self._reload_face_cascade()
​
        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)
​
        # Timers
        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_debug_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"Face follow PTZ+body started. SPACE=toggle, q=quit. "
                              f"ptz={self._ptz_rate:.0f}Hz body={self._body_rate:.0f}Hz")

Face detection method _detect_face: converts the frame to grayscale, detects faces with CascadeClassifier.detectMultiScale, takes the largest face as the primary target, and computes the center coordinates and bbox width to update the target state.

PTZ control method _ptz_tick: the same PD control structure as Color Follow — normalized error, PD step computation, angular-rate feedforward compensation, and dual EMA smoothing before publishing /cmd_ptz_angle. Face following uses higher gains (pd_kp=2.0, pd_kd=0.45) to accommodate fast face movement.

Body control method _body_tick: s1 is scaled by body_share (0.5), then PD computes the angular velocity; the deviation between the face bbox width and target_face_width is fed into PD for the linear velocity, and the merged /cmd_vel is published.

2. Key Parameters

Parameter definition file: ~/microros_ws/src/microros_v2_ptz_visual_control_pkg/launch/face_follow_ptz.launch.py

6. Frequently Asked Questions