seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat: Person-following head tracking (Issue #549) #555

Merged
sl-jetson merged 1 commits from sl-perception/issue-549-head-tracking into main 2026-03-14 11:36:30 -04:00
Conversation 0 Commits 1 Files Changed 8 +561
sl-perception commented 2026-03-14 10:28:43 -04:00
Collaborator
Copy Link

Summary

Implements saltybot_head_tracking — a ROS2 Python node that provides automatic person-following by computing pan/tilt angle setpoints from YOLOv8n person detection bounding boxes.

  • Subscribes to /saltybot/objects (DetectedObjectArray), filters for class_id==0 (person)
  • Selects best target: score = 0.6 * 1/(1+dist_m) + 0.4 * confidence — prefers closest, breaks ties by confidence; falls back to confidence-only when distance_m==0
  • Dual independent PID controllers (pan + tilt) with anti-windup integral clamp
  • Dead-zone filter (10 px default) suppresses micro-jitter
  • Publishes geometry_msgs/Point to /saltybot/gimbal/cmd: x=pan_deg, y=tilt_deg, z=confidence
  • Publishes JSON state string to /saltybot/head_tracking/state

State machine

State Transition
IDLE → TRACKING on first detection
TRACKING → LOST when no detection for ~2 control cycles
LOST hold last angle for hold_duration_s (3 s default) → CENTERING
CENTERING drive to (0°, 0°) at center_speed_deg_s (20°/s) → IDLE

Key parameters (head_tracking_params.yaml)

Parameter Default Description
pan/tilt_kp 0.08 Proportional gain
pan/tilt_ki 0.002 Integral gain
pan/tilt_kd 0.015 Derivative gain
pan_min/max_deg ±60° Pan servo limits
tilt_min/max_deg ±30° Tilt servo limits
hold_duration_s 3.0 Hold time after loss
dead_zone_px 10 Ignore errors < this
control_hz 20 PID loop rate

Integration with pan_tilt_node

saltybot_pan_tilt subscribes to /saltybot/target_track (pixel coords).
head_tracking publishes to /saltybot/gimbal/cmd (angle degrees).
Bridge via topic remap in pan_tilt launch: target_track → gimbal/cmd
(or update pan_tilt_node to accept angle setpoints directly).

Files

  • jetson/ros2_ws/src/saltybot_head_tracking/saltybot_head_tracking/head_tracking_node.py
  • config/head_tracking_params.yaml
  • launch/head_tracking.launch.py
  • package.xml, setup.py, setup.cfg, resource/

Test plan

  • colcon build --packages-select saltybot_head_tracking succeeds
  • Node starts: ros2 run saltybot_head_tracking head_tracking
  • State IDLE on startup, TRACKING after first detection on /saltybot/objects
  • /saltybot/gimbal/cmd publishes at 20 Hz
  • Pan/tilt angles converge toward zero error when target is centred
  • State transitions: TRACKING → LOST (no detection) → CENTERING after 3 s → IDLE
  • Closest person selected when multiple persons in frame
  • Dead-zone: no gimbal movement for errors < 10 px
  • Servo limit clamping: angles stay within ±60° pan, ±30° tilt

Closes #549

## Summary Implements `saltybot_head_tracking` — a ROS2 Python node that provides automatic person-following by computing pan/tilt angle setpoints from YOLOv8n person detection bounding boxes. - Subscribes to `/saltybot/objects` (DetectedObjectArray), filters for `class_id==0` (person) - Selects best target: `score = 0.6 * 1/(1+dist_m) + 0.4 * confidence` — prefers closest, breaks ties by confidence; falls back to confidence-only when `distance_m==0` - Dual independent PID controllers (pan + tilt) with anti-windup integral clamp - Dead-zone filter (10 px default) suppresses micro-jitter - Publishes `geometry_msgs/Point` to `/saltybot/gimbal/cmd`: `x=pan_deg, y=tilt_deg, z=confidence` - Publishes JSON state string to `/saltybot/head_tracking/state` ### State machine | State | Transition | |-------|------------| | IDLE | → TRACKING on first detection | | TRACKING | → LOST when no detection for ~2 control cycles | | LOST | hold last angle for `hold_duration_s` (3 s default) → CENTERING | | CENTERING | drive to (0°, 0°) at `center_speed_deg_s` (20°/s) → IDLE | ### Key parameters (`head_tracking_params.yaml`) | Parameter | Default | Description | |-----------|---------|-------------| | `pan/tilt_kp` | 0.08 | Proportional gain | | `pan/tilt_ki` | 0.002 | Integral gain | | `pan/tilt_kd` | 0.015 | Derivative gain | | `pan_min/max_deg` | ±60° | Pan servo limits | | `tilt_min/max_deg` | ±30° | Tilt servo limits | | `hold_duration_s` | 3.0 | Hold time after loss | | `dead_zone_px` | 10 | Ignore errors < this | | `control_hz` | 20 | PID loop rate | ### Integration with pan_tilt_node `saltybot_pan_tilt` subscribes to `/saltybot/target_track` (pixel coords). `head_tracking` publishes to `/saltybot/gimbal/cmd` (angle degrees). Bridge via topic remap in `pan_tilt` launch: `target_track → gimbal/cmd` (or update `pan_tilt_node` to accept angle setpoints directly). ### Files - `jetson/ros2_ws/src/saltybot_head_tracking/saltybot_head_tracking/head_tracking_node.py` - `config/head_tracking_params.yaml` - `launch/head_tracking.launch.py` - `package.xml`, `setup.py`, `setup.cfg`, `resource/` ## Test plan - [ ] `colcon build --packages-select saltybot_head_tracking` succeeds - [ ] Node starts: `ros2 run saltybot_head_tracking head_tracking` - [ ] State IDLE on startup, TRACKING after first detection on `/saltybot/objects` - [ ] `/saltybot/gimbal/cmd` publishes at 20 Hz - [ ] Pan/tilt angles converge toward zero error when target is centred - [ ] State transitions: TRACKING → LOST (no detection) → CENTERING after 3 s → IDLE - [ ] Closest person selected when multiple persons in frame - [ ] Dead-zone: no gimbal movement for errors < 10 px - [ ] Servo limit clamping: angles stay within ±60° pan, ±30° tilt Closes #549
sl-perception added 1 commit 2026-03-14 10:28:44 -04:00 
Add saltybot_head_tracking — ROS2 Python node for automatic person-
following using dual-axis PID control targeting the pan/tilt camera head.

Pipeline:
  1. Subscribe to /saltybot/objects (DetectedObjectArray from YOLOv8n)
  2. Filter for class_id==0 (person); select best target by score:
       score = 0.6 * 1/(1+dist_m)  +  0.4 * confidence
     (falls back to confidence-only when distance_m==0 / unknown)
  3. Compute pixel error of bbox centre from image centre
  4. Apply dead-zone (10 px default) to suppress micro-jitter
  5. Convert pixel error to angle error via camera FOV
  6. Independent PID controllers for pan and tilt axes
  7. Accumulate PID output into absolute angle setpoint
  8. Publish geometry_msgs/Point to /saltybot/gimbal/cmd:
       x = pan_angle_deg, y = tilt_angle_deg, z = confidence

State machine:
  IDLE      -> waiting for first detection
  TRACKING  -> active PID
  LOST      -> hold last angle for hold_duration_s (3 s)
  CENTERING -> return to (0, 0) at 20 deg/s -> IDLE

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 8e21201dd4 into main 2026-03-14 11:36:30 -04:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#555
No description provided.
Delete Branch "sl-perception/issue-549-head-tracking"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?