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

[P0] Follow-me controller — sensor fusion + path planning + motor commands #366

New Issue
Closed
opened 2026-03-03 15:06:52 -05:00 by seb · 0 comments
seb commented 2026-03-03 15:06:52 -05:00
Owner
Copy Link

Goal

The high-level follow-me brain: fuse all sensors, plan path, send motor commands to keep 2-3m behind Tee at up to 20km/h.

Architecture

UWB (/saltybot/uwb_target) ---\
RealSense (/saltybot/target_track) ---> Fusion Node ---> Path Planner ---> Motor Commands
LIDAR (/scan) ---> Costmap --------/
IMU (/saltybot/imu) ---------------/

Requirements

  • Sensor fusion: combine UWB (bearing+distance) + camera (visual track) + LIDAR (obstacles) + IMU (orientation)
  • Target state estimation: Kalman filter for position, velocity, heading prediction
  • Path planning: reactive controller (not full nav2 initially) — follow target while avoiding obstacles
  • Speed control: match target speed, maintain 2-3m following distance
  • Emergency stop: obstacle in path OR target lost for >2s OR IMU indicates fall
  • Graceful degradation: if camera lost, follow UWB alone; if UWB lost, follow camera alone; if both lost, STOP
  • Self-balancing PID runs on FC (already working) — this node sends speed/turn commands only

Modes

  1. IDLE: balanced, stationary, waiting for target
  2. FOLLOW: actively tracking and following
  3. SEARCH: target lost, rotating to reacquire
  4. ESTOP: emergency stop, motors off

Dependencies

  • #362 UART link working
  • #363 Person tracking
  • #364 LIDAR obstacle avoidance
  • #365 UWB integration

Notes

  • Start simple: camera-only following at walking speed in the lab
  • Add LIDAR avoidance
  • Add UWB when hardware arrives
  • Scale up speed gradually — do NOT test 20km/h until all sensors fused
## Goal The high-level follow-me brain: fuse all sensors, plan path, send motor commands to keep 2-3m behind Tee at up to 20km/h. ## Architecture ``` UWB (/saltybot/uwb_target) ---\ RealSense (/saltybot/target_track) ---> Fusion Node ---> Path Planner ---> Motor Commands LIDAR (/scan) ---> Costmap --------/ IMU (/saltybot/imu) ---------------/ ``` ## Requirements - Sensor fusion: combine UWB (bearing+distance) + camera (visual track) + LIDAR (obstacles) + IMU (orientation) - Target state estimation: Kalman filter for position, velocity, heading prediction - Path planning: reactive controller (not full nav2 initially) — follow target while avoiding obstacles - Speed control: match target speed, maintain 2-3m following distance - Emergency stop: obstacle in path OR target lost for >2s OR IMU indicates fall - Graceful degradation: if camera lost, follow UWB alone; if UWB lost, follow camera alone; if both lost, STOP - Self-balancing PID runs on FC (already working) — this node sends speed/turn commands only ## Modes 1. IDLE: balanced, stationary, waiting for target 2. FOLLOW: actively tracking and following 3. SEARCH: target lost, rotating to reacquire 4. ESTOP: emergency stop, motors off ## Dependencies - #362 UART link working - #363 Person tracking - #364 LIDAR obstacle avoidance - #365 UWB integration ## Notes - Start simple: camera-only following at walking speed in the lab - Add LIDAR avoidance - Add UWB when hardware arrives - Scale up speed gradually — do NOT test 20km/h until all sensors fused
Sign in to join this conversation.
No Branch/Tag Specified
Branches Tags
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#366
No description provided.
Delete Branch "%!s()"

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?