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

feat: GPS waypoint logger (Issue #617) #620

Merged
sl-jetson merged 1 commits from sl-android/issue-617-waypoint-logger into main 2026-03-15 11:02:59 -04:00
Conversation 0 Commits 1 Files Changed 1 +705
sl-jetson commented 2026-03-15 10:06:23 -04:00
Collaborator
Copy Link

Summary

phone/waypoint_logger.py — interactive Termux CLI for recording, managing, and publishing GPS waypoints from the phone to SaltyBot's Nav2 stack.

Commands

Key Action
r Record GPS fix at current position — prompt for name + tags
l List all waypoints with distance/bearing between each
d Delete waypoint by index (with confirmation)
p Publish route to MQTT (saltybot/phone/route, retained)
c Clear all waypoints (with confirmation)
n Rename route
i Route info (total distance, timestamps)
q Quit

GPS acquisition

  • termux-location -p gps -r once → automatic fallback to network on timeout
  • --live-gps: subscribes to saltybot/phone/gps MQTT topic (reuses sensor_dashboard.py stream — no double GPS call)

MQTT output (saltybot/phone/route, QoS 1, retained)

{
  "route_name": "patrol_loop",
  "ts": 1710000000.0,
  "waypoints": [
    {"index":0,"name":"start","lat":45.123,"lon":-73.456,"alt_m":12.3,
     "accuracy_m":2.1,"tags":["home"],"recorded_at":1710000000.0}
  ],
  "nav2_poses": [
    {"frame_id":"map","position":{"x":0.0,"y":0.0,"z":12.3},
     "orientation":{"x":0.0,"y":0.0,"z":0.707,"w":0.707},
     "waypoint_name":"start"}
  ]
}

Nav2 integration

nav2_poses array uses flat-earth ENU coordinates (metres from first waypoint). Yaw faces the next waypoint; last waypoint faces previous. Compatible with Nav2 FollowWaypoints action input format.

Geo maths

  • haversine_m() — great-circle distance; bearing_deg() → 8-point compass
  • flat_earth_xy() — ENU metres (< 1% error within 100 km)

Persistence

Route auto-saved to ~/saltybot_route.json on every change; reloaded on startup.

Flags

--broker, --port, --file, --route, --provider gps|network|passive, --live-gps, --no-mqtt, --debug

Test plan

  • python3 -m py_compile phone/waypoint_logger.py — syntax clean
  • On Termux: python3 phone/waypoint_logger.py --no-mqtt — menu appears
  • r → records waypoint with GPS fix, name prompt works
  • l → lists waypoints; distance/bearing shown between consecutive points
  • Record 3 waypoints → l shows cumulative distances + compass headings
  • d 1 → deletes wp#1, remaining indices renumbered
  • p → mosquitto_sub -t saltybot/phone/route shows full JSON with nav2_poses
  • --live-gps → subscribes to saltybot/phone/gps, skips termux-location call
  • Restart script → previously saved waypoints reload from JSON file
  • c → clears route, confirms 0 waypoints after

🤖 Generated with Claude Code

## Summary `phone/waypoint_logger.py` — interactive Termux CLI for recording, managing, and publishing GPS waypoints from the phone to SaltyBot's Nav2 stack. ### Commands | Key | Action | |-----|--------| | `r` | Record GPS fix at current position — prompt for name + tags | | `l` | List all waypoints with distance/bearing between each | | `d` | Delete waypoint by index (with confirmation) | | `p` | Publish route to MQTT (`saltybot/phone/route`, retained) | | `c` | Clear all waypoints (with confirmation) | | `n` | Rename route | | `i` | Route info (total distance, timestamps) | | `q` | Quit | ### GPS acquisition - `termux-location -p gps -r once` → automatic fallback to `network` on timeout - `--live-gps`: subscribes to `saltybot/phone/gps` MQTT topic (reuses sensor_dashboard.py stream — no double GPS call) ### MQTT output (`saltybot/phone/route`, QoS 1, retained) ```json { "route_name": "patrol_loop", "ts": 1710000000.0, "waypoints": [ {"index":0,"name":"start","lat":45.123,"lon":-73.456,"alt_m":12.3, "accuracy_m":2.1,"tags":["home"],"recorded_at":1710000000.0} ], "nav2_poses": [ {"frame_id":"map","position":{"x":0.0,"y":0.0,"z":12.3}, "orientation":{"x":0.0,"y":0.0,"z":0.707,"w":0.707}, "waypoint_name":"start"} ] } ``` ### Nav2 integration `nav2_poses` array uses flat-earth ENU coordinates (metres from first waypoint). Yaw faces the next waypoint; last waypoint faces previous. Compatible with Nav2 `FollowWaypoints` action input format. ### Geo maths - `haversine_m()` — great-circle distance; `bearing_deg()` → 8-point compass - `flat_earth_xy()` — ENU metres (< 1% error within 100 km) ### Persistence Route auto-saved to `~/saltybot_route.json` on every change; reloaded on startup. ### Flags `--broker`, `--port`, `--file`, `--route`, `--provider gps|network|passive`, `--live-gps`, `--no-mqtt`, `--debug` ## Test plan - [ ] `python3 -m py_compile phone/waypoint_logger.py` — syntax clean - [ ] On Termux: `python3 phone/waypoint_logger.py --no-mqtt` — menu appears - [ ] `r` → records waypoint with GPS fix, name prompt works - [ ] `l` → lists waypoints; distance/bearing shown between consecutive points - [ ] Record 3 waypoints → `l` shows cumulative distances + compass headings - [ ] `d 1` → deletes wp#1, remaining indices renumbered - [ ] `p` → `mosquitto_sub -t saltybot/phone/route` shows full JSON with nav2_poses - [ ] `--live-gps` → subscribes to saltybot/phone/gps, skips termux-location call - [ ] Restart script → previously saved waypoints reload from JSON file - [ ] `c` → clears route, confirms 0 waypoints after 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-jetson added 1 commit 2026-03-15 10:06:24 -04:00 
Add phone/waypoint_logger.py — interactive Termux CLI for recording,
managing, and publishing GPS waypoints:

GPS acquisition
  - termux-location with gps/network/passive provider selection
  - Falls back to network provider on GPS timeout
  - Optional --live-gps flag: subscribes to saltybot/phone/gps MQTT
    topic (sensor_dashboard.py stream) to avoid redundant GPS calls

Waypoint operations
  - Record: acquires GPS fix, prompts for name + tags, appends to route
  - List: table with lat/lon/alt/accuracy/tags + inter-waypoint
    distance (haversine) and bearing (8-point compass)
  - Delete: by index with confirmation prompt
  - Clear: entire route with confirmation
  - Rename: route name

Persistence
  - Routes saved as JSON to ~/saltybot_route.json (configurable)
  - Auto-loads on startup; survives session restarts

MQTT publish (saltybot/phone/route, QoS 1, retained)
  - Full waypoint list with metadata
  - nav2_poses array: flat-earth x/y (metres from origin),
    quaternion yaw facing next waypoint (last faces prev)
  - Compatible with Nav2 FollowWaypoints action input

Geo maths
  - haversine_m(): great-circle distance
  - bearing_deg(): initial bearing with 8-point compass label
  - flat_earth_xy(): ENU metres for Nav2 pose export (<1% error <100km)

Flags: --broker, --port, --file, --route, --provider, --live-gps,
       --no-mqtt, --debug

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 38df5b4ebb into main 2026-03-15 11:02:59 -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#620
No description provided.
Delete Branch "sl-android/issue-617-waypoint-logger"

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?