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

feat: person-following control loop #55

Merged
seb merged 1 commits from sl-controls/person-follower into main 2026-02-28 23:25:40 -05:00
Conversation 0 Commits 1 Files Changed 9 +976
sl-webui commented 2026-02-28 23:23:12 -05:00
Collaborator
Copy Link

Summary

Adds saltybot_follower ROS2 package — proportional person-following controller for Phase 2b.

Architecture

sl-jetson person_detector
  → /person/target (PoseStamped, base_link frame)
    → person_follower_node        ← this PR
      → /cmd_vel (Twist)
        → cmd_vel_bridge_node (PR #46, ramp+deadman+mode gate)
          → STM32 AUTONOMOUS drive

Controller

  • Proportional control: linear.x = kp_linear × (distance − follow_dist), angular.z = kp_angular × bearing
  • Follow distance: 1.5m, ±0.3m dead zone to prevent jitter at target
  • Velocity caps: max 0.5 m/s linear, 1.0 rad/s angular (conservative for balance robot)
  • Obstacle override: checks forward corridor in Nav2 /local_costmap/costmap; zeroes forward cmd_vel if occupied cell ≥ threshold; turning still allowed

Lost-target state machine

State Trigger Output
FOLLOWING /person/target received Proportional cmd_vel
STOPPING Lost > lost_timeout (2s) Zero cmd_vel
SEARCHING Lost > search_timeout (5s) Slow rotation (0.3 rad/s)

Mode integration

  • follow_enabled param (default true) — disable without stopping node: ros2 param set /person_follower follow_enabled false
  • STM32 AUTONOMOUS gate handled by cmd_vel_bridge_node (PR #46)

Files

File Purpose
saltybot_follower/person_follower_node.py ROS2 node (314 lines)
config/person_follower_params.yaml All params with comments
launch/person_follower.launch.py All params as launch args
test/test_person_follower.py 53 pytest tests, no ROS2 runtime
package.xml, setup.py, setup.cfg Package metadata

Test coverage

53 tests covering: clamp, geometry (distance/bearing), linear/angular cmd, obstacle suppression, costmap check, state machine transitions, search/stop behaviour, follow_enabled gate.

Usage

# Defaults (1.5m, 0.5m/s, 2s lost timeout):
ros2 launch saltybot_follower person_follower.launch.py

# Tighter indoor following:
ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.0

# Disable at runtime:
ros2 param set /person_follower follow_enabled false

# Run tests:
pytest jetson/ros2_ws/src/saltybot_follower/test/test_person_follower.py

Prerequisites

  • cmd_vel_bridge_node (saltybot_bridge PR #46) running on the serial port
  • sl-jetson person_detector publishing /person/target
  • Nav2 local costmap (optional; obstacle check skipped if no messages)
## Summary Adds `saltybot_follower` ROS2 package — proportional person-following controller for Phase 2b. ### Architecture ``` sl-jetson person_detector → /person/target (PoseStamped, base_link frame) → person_follower_node ← this PR → /cmd_vel (Twist) → cmd_vel_bridge_node (PR #46, ramp+deadman+mode gate) → STM32 AUTONOMOUS drive ``` ### Controller - **Proportional control**: `linear.x = kp_linear × (distance − follow_dist)`, `angular.z = kp_angular × bearing` - **Follow distance**: 1.5m, ±0.3m dead zone to prevent jitter at target - **Velocity caps**: max 0.5 m/s linear, 1.0 rad/s angular (conservative for balance robot) - **Obstacle override**: checks forward corridor in Nav2 `/local_costmap/costmap`; zeroes forward cmd_vel if occupied cell ≥ threshold; turning still allowed ### Lost-target state machine | State | Trigger | Output | |-------|---------|--------| | `FOLLOWING` | `/person/target` received | Proportional cmd_vel | | `STOPPING` | Lost > `lost_timeout` (2s) | Zero cmd_vel | | `SEARCHING` | Lost > `search_timeout` (5s) | Slow rotation (0.3 rad/s) | ### Mode integration - `follow_enabled` param (default `true`) — disable without stopping node: `ros2 param set /person_follower follow_enabled false` - STM32 AUTONOMOUS gate handled by cmd_vel_bridge_node (PR #46) ### Files | File | Purpose | |------|---------| | `saltybot_follower/person_follower_node.py` | ROS2 node (314 lines) | | `config/person_follower_params.yaml` | All params with comments | | `launch/person_follower.launch.py` | All params as launch args | | `test/test_person_follower.py` | 53 pytest tests, no ROS2 runtime | | `package.xml`, `setup.py`, `setup.cfg` | Package metadata | ### Test coverage 53 tests covering: clamp, geometry (distance/bearing), linear/angular cmd, obstacle suppression, costmap check, state machine transitions, search/stop behaviour, follow_enabled gate. ### Usage ```bash # Defaults (1.5m, 0.5m/s, 2s lost timeout): ros2 launch saltybot_follower person_follower.launch.py # Tighter indoor following: ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.0 # Disable at runtime: ros2 param set /person_follower follow_enabled false # Run tests: pytest jetson/ros2_ws/src/saltybot_follower/test/test_person_follower.py ``` ### Prerequisites - `cmd_vel_bridge_node` (saltybot_bridge PR #46) running on the serial port - sl-jetson `person_detector` publishing `/person/target` - Nav2 local costmap (optional; obstacle check skipped if no messages)
sl-webui added 1 commit 2026-02-28 23:23:13 -05:00 
Adds saltybot_follower ROS2 package — proportional person-following
controller that bridges sl-jetson's /person/target detections to Nav2
/cmd_vel, with the cmd_vel_bridge_node (PR #46) providing safety wrapping.

Controller features:
  - Proportional control: linear.x ∝ distance error, angular.z ∝ bearing
  - Follow distance: 1.5m default with ±0.3m dead zone (no jitter at target)
  - Max speed: 0.5 m/s linear, 1.0 rad/s angular (conservative for balance)
  - Obstacle override: zeroes forward cmd_vel when Nav2 local costmap
    detects obstacle in forward corridor; turning still allowed
  - Lost-target state machine:
      FOLLOWING  → person visible
      STOPPING   → lost > 2s, publish zero
      SEARCHING  → lost > 5s, slow rotation (0.3 rad/s) to re-acquire
  - Mode integration: follow_enabled param (toggle via ros2 param set)
    independently gates the controller; cmd_vel bridge gates on md=2

Deliverables:
  saltybot_follower/person_follower_node.py   — ROS2 node (314 lines)
  config/person_follower_params.yaml          — all params documented
  launch/person_follower.launch.py            — all params as launch args
  test/test_person_follower.py                — 53 pytest tests, no ROS2
  package.xml / setup.py / setup.cfg          — package metadata

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
seb merged commit fcd59ead80 into main 2026-02-28 23:25:40 -05:00
seb deleted branch sl-controls/person-follower 2026-02-28 23:25:41 -05: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#55
No description provided.
Delete Branch "sl-controls/person-follower"

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?