Facial Tracking (No-PTZ)

1. Experiment Objective

OpenCV Haar Cascade is used to detect faces, and the car rotates to track so that the face stays 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)
  → face_tracker_node
    ├── Image processing timer (15Hz): image conversion → grayscale → Haar Cascade detection → largest face selection
    └── Body control timer (10Hz): pixel error → PD angular velocity + face width distance following → /cmd_vel (BEST_EFFORT)

3. Core Topic Quick Reference

Firmware Uplink (Sensor Data)

Firmware Downlink (Control Commands)

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 Facial Tracking (Terminal 2)

Tracking mode (rotation tracking only):

xxxxxxxxxx
ros2 launch microros_v2_no_ptz_visual_control_pkg face_tracker.launch.py

4. Operation Instructions

4. Experiment Phenomenon

In Tracking mode:

• When the face moves left or right in the frame, the car automatically rotates to track it, keeping the face at the center of the frame
• When multiple faces are present, the largest one is automatically selected as the tracking target

In Follow mode:

• While rotating to track, the car adjusts its distance to the target based on the width of the face box

When the target is lost:

• After the face moves out of the frame or turns away, the car stops moving
• When the face reappears, tracking resumes automatically

5. Code Explanation

Source path:

• Launch (track): ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/face_tracker.launch.py
• Launch (follow): ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/face_follow.launch.py
• Node source: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/face_tracker_node.py

1. Core Node Analysis

face_tracker_node uses the OpenCV Haar Cascade classifier to detect faces. Core workflow:

• Image processing (15Hz): raw Image frame → cv_bridge conversion → grayscale → face_cascade.detectMultiScale() → select the largest face as the target
• Body control (10Hz): deviation between the face center x-coordinate and the image center → PD computes angular velocity; in Follow mode the face width deviation → PD computes linear velocity

Constructor — load the Haar Cascade model, create publishers/subscribers:

​x
# Source file: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/face_tracker_node.py
​
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._has_target = False
    self._lost_frames = 0
    self._target_x: Optional[float] = None
    self._target_y: Optional[float] = None
    self._target_w: Optional[int] = None  # Face box width (used for distance following)
​
    self._declare_params()
    self._load_params()
​
    # Load the OpenCV Haar Cascade face detection model
    cascade_path = cv.data.haarcascades + "haarcascade_frontalface_default.xml"
    self._face_cascade = cv.CascadeClassifier(cascade_path)
​
    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 — facial PD tracking:

xxxxxxxxxx
# Source file: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/face_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
        tw = self._target_w
​
    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:
        now = time.perf_counter()
        dt = now - self._prev_t if self._prev_t > 0 else 1.0 / self._body_rate
        dt = max(dt, 1e-3)
        d_err = (err_x - self._prev_err) / dt
        self._prev_err = err_x; self._prev_t = now
        az = self._angular_kp * err_x + self._angular_kd * d_err
        az = clamp(az, -self._max_az, self._max_az)
        if self._reverse_body:
            az = -az
​
    cmd = Twist()
    cmd.angular.z = az
​
    # Distance following: face width deviation → PD linear velocity
    if self._dist_follow and tw is not None:
        err_fw = self._target_face_w - tw
        if abs(err_fw) > self._dist_tol_fw:
            now = time.perf_counter()
            dt = now - self._prev_dt if self._prev_dt > 0 else 1.0 / self._body_rate
            dt = max(dt, 1e-3)
            d_err_fw = (err_fw - self._prev_derr) / dt
            self._prev_derr = err_fw; self._prev_dt = now
            lx = self._dist_kp * err_fw + self._dist_kd * d_err_fw
            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: haarcascade_frontalface_default.xml only detects frontal faces (low detection rate for side faces). detectMultiScale returns all face rectangles, and the one with the largest area is taken as the tracking target.

2. Key Parameters

Parameter definition file:

• Launch: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/face_tracker.launch.py
• Node: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/face_tracker_node.py

3. Node Description

6. Common Problems