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

feat: route recording + autonomous replay (#71) #75

Merged
seb merged 1 commits from sl-perception/route-record-replay into main 2026-03-01 01:10:03 -05:00
Conversation 0 Commits 1 Files Changed 10 +1106
sl-perception commented 2026-03-01 01:07:38 -05:00
Collaborator
Copy Link

Summary

Implements Phase 3 "ride once, replay forever" route system for SaltyBot. During a UWB follow-me session, the robot records its GPS path. Next ride: it replicates the route autonomously.

saltybot_routes package — 3 nodes:

route_recorder_node

  • Subscribes: /gps/fix (SIM7600X PR #65), /odom (STM32), /imu/data (D435i heading), /uwb/target (follow-me context PR #66)
  • Samples at 1 Hz, skips waypoints within 2 m of previous (configurable)
  • Stores: timestamp, lat/lon, heading_deg, speed_mps, odom_x/y, gps_status
  • Format: single JSON object per file (/data/routes/<name>.jsonl on NVMe)
  • Services: /route/start_recording, /route/stop_recording, /route/save, /route/discard

route_replayer_node

  • Loads .jsonl route file; converts GPS lat/lon → map-frame ENU (flat-earth, same approach as outdoor_nav)
  • Preserves recorded headings: IMU yaw_deg → ENU quaternion (0°=East)
  • Subsamples at 3 m spacing before sending to Nav2 navigate_through_poses
  • 2 m goal tolerance (matches SIM7600X ±2.5 m CEP)
  • Services: /route/load, /route/start_replay, /route/pause, /route/stop
  • Pause/resume via /route/pause_cmd topic (Bool) or service

route_manager_node

  • /route/list → JSON array: [{name, distance_m, duration_s, recorded_at, waypoint_count}, ...]
  • /route/info → full metadata JSON (no waypoints array — can be large)
  • /route/delete → removes named route file

Supporting files:

  • route_system.launch.py — starts all three nodes with shared save_dir + route_name args
  • route_params.yaml — operational notes + file format documentation inline

Test plan

  • docker compose up -d saltybot-routes (after adding service to docker-compose)
  • Start follow-me session (PR #66), then: 
    ros2 param set /route_recorder route_name "test_loop"
ros2 service call /route/start_recording std_srvs/srv/Trigger '{}'
# ... ride route ...
ros2 service call /route/stop_recording std_srvs/srv/Trigger '{}'
ros2 service call /route/save std_srvs/srv/Trigger '{}'
  • Verify /data/routes/test_loop.jsonl on NVMe
  • ros2 service call /route/list std_srvs/srv/Trigger '{}' — see route in list
  • Load and replay: 
    ros2 param set /route_replayer route_name "test_loop"
ros2 service call /route/load std_srvs/srv/Trigger '{}'
ros2 service call /route/start_replay std_srvs/srv/Trigger '{}'
ros2 topic echo /route/replay_status
  • Test pause: ros2 service call /route/pause std_srvs/srv/Trigger '{}'
  • Test emergency stop: ros2 service call /route/stop std_srvs/srv/Trigger '{}'
  • ros2 service call /route/delete std_srvs/srv/Trigger '{}' — verify file removed

Closes #71.

🤖 Generated with Claude Code

## Summary Implements Phase 3 "ride once, replay forever" route system for SaltyBot. During a UWB follow-me session, the robot records its GPS path. Next ride: it replicates the route autonomously. **`saltybot_routes` package — 3 nodes:** **`route_recorder_node`** - Subscribes: `/gps/fix` (SIM7600X PR #65), `/odom` (STM32), `/imu/data` (D435i heading), `/uwb/target` (follow-me context PR #66) - Samples at 1 Hz, skips waypoints within 2 m of previous (configurable) - Stores: timestamp, lat/lon, heading_deg, speed_mps, odom_x/y, gps_status - Format: single JSON object per file (`/data/routes/<name>.jsonl` on NVMe) - Services: `/route/start_recording`, `/route/stop_recording`, `/route/save`, `/route/discard` **`route_replayer_node`** - Loads `.jsonl` route file; converts GPS lat/lon → map-frame ENU (flat-earth, same approach as outdoor_nav) - Preserves recorded headings: IMU yaw_deg → ENU quaternion (0°=East) - Subsamples at 3 m spacing before sending to Nav2 `navigate_through_poses` - 2 m goal tolerance (matches SIM7600X ±2.5 m CEP) - Services: `/route/load`, `/route/start_replay`, `/route/pause`, `/route/stop` - Pause/resume via `/route/pause_cmd` topic (Bool) or service **`route_manager_node`** - `/route/list` → JSON array: `[{name, distance_m, duration_s, recorded_at, waypoint_count}, ...]` - `/route/info` → full metadata JSON (no waypoints array — can be large) - `/route/delete` → removes named route file **Supporting files:** - `route_system.launch.py` — starts all three nodes with shared `save_dir` + `route_name` args - `route_params.yaml` — operational notes + file format documentation inline ## Test plan - [ ] `docker compose up -d saltybot-routes` (after adding service to docker-compose) - [ ] Start follow-me session (PR #66), then: ```bash ros2 param set /route_recorder route_name "test_loop" ros2 service call /route/start_recording std_srvs/srv/Trigger '{}' # ... ride route ... ros2 service call /route/stop_recording std_srvs/srv/Trigger '{}' ros2 service call /route/save std_srvs/srv/Trigger '{}' ``` - [ ] Verify `/data/routes/test_loop.jsonl` on NVMe - [ ] `ros2 service call /route/list std_srvs/srv/Trigger '{}'` — see route in list - [ ] Load and replay: ```bash ros2 param set /route_replayer route_name "test_loop" ros2 service call /route/load std_srvs/srv/Trigger '{}' ros2 service call /route/start_replay std_srvs/srv/Trigger '{}' ros2 topic echo /route/replay_status ``` - [ ] Test pause: `ros2 service call /route/pause std_srvs/srv/Trigger '{}'` - [ ] Test emergency stop: `ros2 service call /route/stop std_srvs/srv/Trigger '{}'` - [ ] `ros2 service call /route/delete std_srvs/srv/Trigger '{}'` — verify file removed Closes #71. 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-01 01:07:38 -05:00 
Implements Phase 3 ride-once-replay-forever route system.

saltybot_routes package:
- route_recorder_node: samples GPS+odom+heading at 1Hz during follow-me
  rides; 2m waypoint spacing; JSON-Lines .jsonl on NVMe /data/routes/;
  services start_recording/stop_recording/save/discard
- route_replayer_node: loads .jsonl, GPS->ENU flat-earth conversion,
  heading->quaternion, 3m subsampling for Nav2 navigate_through_poses;
  2m GPS tolerance (SIM7600X +-2.5m); pause/resume/stop services
- route_manager_node: list/info/delete services for saved routes
- route_system.launch.py: all three nodes with shared params
- route_params.yaml: waypoint_spacing_m=2.0, replay_spacing_m=3.0

GPS: /gps/fix from SIM7600X (PR #65)
UWB: /uwb/target from follow-me (PR #66)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
seb merged commit d3168b9c07 into main 2026-03-01 01:10:03 -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#75
No description provided.
Delete Branch "sl-perception/route-record-replay"

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?