feat: Rename to SAUL-T-MOTE, add map with user + robot positions and follow path
Rename: - CFBundleDisplayName = "SAUL-T-MOTE" in Info.plist - navigationTitle updated to "SAUL-T-MOTE" in StatusView - MQTT clientID prefix changed to "saul-t-mote-" Map view (MapContentView.swift, MapKit): - Blue marker + fading breadcrumb trail for user (iPhone GPS) - Orange car marker + fading breadcrumb trail for robot (Pixel 5) - Dashed yellow line from robot → user (follow path) - Bottom overlay: distance between user and robot, robot speed - Auto-follow camera tracks user; manual drag disables it; re-centre button restores - MapPolyline for trails, per-point Annotation for fading breadcrumb dots Robot GPS subscription (saltybot/phone/gps): - MQTTClient extended with SUBSCRIBE (QoS 0) + incoming PUBLISH parser (handles variable-length remaining-length, multi-packet frames) - Subscriptions persisted and re-sent on reconnect (CONNACK handler) - SensorManager.handleRobotGPS() updates robotLocation, robotSpeed, robotBreadcrumbs, distanceToRobot iOS GPS publish unchanged (saltybot/ios/gps, 1 Hz) — PR #2 intact. ContentView restructured as TabView: - Tab 1: Status (sensor rates, WS URL, follow-me button) - Tab 2: Map Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
f39b9d432d
commit
0ad2b2f5c0
67
CLAUDE.md
Normal file
67
CLAUDE.md
Normal file
@ -0,0 +1,67 @@
|
||||
# sl-ios — iOS Companion App Agent (Sul-Tee)
|
||||
|
||||
## Role
|
||||
You are sl-ios, a SaltyLab agent building the iOS companion app ("Sul-Tee") for SaltyBot follow-me mode. The app runs on iPhone 15 Pro and streams GPS, IMU, magnetometer, and barometer data over WebSocket to the Jetson Orin.
|
||||
|
||||
## Scope
|
||||
- Swift / SwiftUI native iOS app (iOS 17+, iPhone 15 Pro target)
|
||||
- CoreLocation (dual-frequency GPS L1+L5), CoreMotion (IMU, mag, baro)
|
||||
- WebSocket client streaming sensor data to Orin
|
||||
- Background operation (must keep streaming when phone locked/backgrounded)
|
||||
- Simple status UI: connection state, sensor rates, bot distance
|
||||
- Start/stop follow-me button, haptic alerts from bot
|
||||
- Future: LiDAR depth data passthrough
|
||||
|
||||
## Tech Stack
|
||||
- **Language:** Swift
|
||||
- **UI:** SwiftUI
|
||||
- **Sensors:** CoreLocation, CoreMotion, CMAltimeter
|
||||
- **Networking:** URLSessionWebSocketTask (native WebSocket)
|
||||
- **Protocol:** JSON over WebSocket (binary optimization later)
|
||||
- **Target:** iPhone 15 Pro, iOS 17+
|
||||
- **Xcode dev account:** vayrette@gmail.com (team Z37N597UWY)
|
||||
|
||||
## Architecture Context
|
||||
- UWB ranging is handled by ESP32 DW1000 anchors on the bot (NOT Apple U1)
|
||||
- iPhone provides GPS + IMU + mag + baro over WiFi/WebSocket to Orin
|
||||
- Orin fuses phone sensors + UWB ranges for position estimate
|
||||
- Orin IP: 192.168.86.158 (saltylab-orin)
|
||||
|
||||
## Repository
|
||||
- Repo: `seb/saltylab-ios` on gitea.vayrette.com
|
||||
- Target branch: `origin/main`
|
||||
- PR login: `--login sl-ios` (via tea CLI, or sl-jetson if no access)
|
||||
|
||||
## Git Rules (MANDATORY)
|
||||
1. Always rebase before starting: `git fetch origin && git rebase origin/main`
|
||||
2. Always rebase before pushing: `git fetch origin && git rebase origin/main`
|
||||
3. Branch naming: `sl-ios/issue-<N>-<slug>`
|
||||
|
||||
## MQTT Communication
|
||||
```bash
|
||||
# Send message to max (PM):
|
||||
AGENT_NAME=sl-ios ~/agent-mqtt/agent-send max "your message"
|
||||
|
||||
# Read inbox:
|
||||
~/agent-mqtt/agent-read 2>/dev/null | tail -15
|
||||
```
|
||||
Prioritize messages from max in your inbox.
|
||||
|
||||
## PR Workflow
|
||||
```bash
|
||||
tea pr create --login sl-ios --repo seb/saltylab-ios \
|
||||
--title 'feat: <description> (Issue #N)' \
|
||||
--description '<details>' --base main
|
||||
```
|
||||
If push fails (permission denied), report via MQTT — sl-jetson will push for you.
|
||||
|
||||
## Tab Naming
|
||||
Update iTerm tab to reflect state:
|
||||
- Working: `printf '\e]1;%s\a' "sl-ios - issue-<N>"`
|
||||
- Done: `printf '\e]1;%s\a' "sl-ios - reported to max"`
|
||||
- Idle: `printf '\e]1;%s\a' "sl-ios - idle"`
|
||||
|
||||
## Reference
|
||||
- Issue #709: https://gitea.vayrette.com/seb/saltylab-firmware/issues/709
|
||||
- UWB firmware branch: `salty/uwb-tag-display-wireless`
|
||||
- SaltyBot architecture: see saltylab-firmware repo docs
|
||||
@ -13,6 +13,7 @@
|
||||
A100000100000000000004AA /* WebSocketClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000004AB /* WebSocketClient.swift */; };
|
||||
A100000100000000000005AA /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = A100000100000000000005AB /* Assets.xcassets */; };
|
||||
A100000100000000000009AA /* MQTTClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000009AB /* MQTTClient.swift */; };
|
||||
A10000010000000000000AAA /* MapContentView.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000AAB /* MapContentView.swift */; };
|
||||
/* End PBXBuildFile section */
|
||||
|
||||
/* Begin PBXFileReference section */
|
||||
@ -24,6 +25,7 @@
|
||||
A100000100000000000006AB /* Info.plist */ = {isa = PBXFileReference; lastKnownFileType = text.plist.xml; path = Info.plist; sourceTree = "<group>"; };
|
||||
A100000100000000000007AB /* SulTee.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = SulTee.app; sourceTree = BUILT_PRODUCTS_DIR; };
|
||||
A100000100000000000009AB /* MQTTClient.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MQTTClient.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000AAB /* MapContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MapContentView.swift; sourceTree = "<group>"; };
|
||||
/* End PBXFileReference section */
|
||||
|
||||
/* Begin PBXFrameworksBuildPhase section */
|
||||
@ -53,6 +55,7 @@
|
||||
A100000100000000000003AB /* SensorManager.swift */,
|
||||
A100000100000000000004AB /* WebSocketClient.swift */,
|
||||
A100000100000000000009AB /* MQTTClient.swift */,
|
||||
A10000010000000000000AAB /* MapContentView.swift */,
|
||||
A100000100000000000005AB /* Assets.xcassets */,
|
||||
A100000100000000000006AB /* Info.plist */,
|
||||
);
|
||||
@ -142,6 +145,7 @@
|
||||
A100000100000000000003AA /* SensorManager.swift in Sources */,
|
||||
A100000100000000000004AA /* WebSocketClient.swift in Sources */,
|
||||
A100000100000000000009AA /* MQTTClient.swift in Sources */,
|
||||
A10000010000000000000AAA /* MapContentView.swift in Sources */,
|
||||
);
|
||||
runOnlyForDeploymentPostprocessing = 0;
|
||||
};
|
||||
|
||||
7
SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata
generated
Normal file
7
SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata
generated
Normal file
@ -0,0 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Workspace
|
||||
version = "1.0">
|
||||
<FileRef
|
||||
location = "self:">
|
||||
</FileRef>
|
||||
</Workspace>
|
||||
@ -1,7 +1,24 @@
|
||||
import SwiftUI
|
||||
|
||||
// MARK: - Root (tab container)
|
||||
|
||||
struct ContentView: View {
|
||||
@EnvironmentObject var sensor: SensorManager
|
||||
|
||||
var body: some View {
|
||||
TabView {
|
||||
StatusView()
|
||||
.tabItem { Label("Status", systemImage: "antenna.radiowaves.left.and.right") }
|
||||
MapContentView()
|
||||
.tabItem { Label("Map", systemImage: "map.fill") }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Status tab
|
||||
|
||||
private struct StatusView: View {
|
||||
@EnvironmentObject var sensor: SensorManager
|
||||
@AppStorage("orinURL") private var orinURL: String = SensorManager.defaultOrinURL
|
||||
@State private var editingURL: String = ""
|
||||
@FocusState private var urlFieldFocused: Bool
|
||||
@ -14,21 +31,33 @@ struct ContentView: View {
|
||||
Divider()
|
||||
sensorRatesGrid
|
||||
Divider()
|
||||
if let dist = sensor.botDistanceMeters {
|
||||
Text("Bot distance: \(dist, specifier: "%.1f") m")
|
||||
.font(.title2)
|
||||
.bold()
|
||||
if let dist = sensor.distanceToRobot {
|
||||
distanceRow(dist)
|
||||
}
|
||||
Spacer()
|
||||
followMeButton
|
||||
}
|
||||
.padding()
|
||||
.navigationTitle("Sul-Tee")
|
||||
.navigationTitle("SAUL-T-MOTE")
|
||||
.onAppear { editingURL = orinURL }
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Subviews
|
||||
// MARK: Connection banner
|
||||
|
||||
private var connectionBanner: some View {
|
||||
HStack(spacing: 12) {
|
||||
Circle()
|
||||
.fill(wsColor)
|
||||
.frame(width: 14, height: 14)
|
||||
Text(wsLabel)
|
||||
.font(.headline)
|
||||
Spacer()
|
||||
}
|
||||
.padding(.top, 8)
|
||||
}
|
||||
|
||||
// MARK: Orin URL field
|
||||
|
||||
private var orinURLField: some View {
|
||||
VStack(alignment: .leading, spacing: 4) {
|
||||
@ -59,32 +88,22 @@ struct ContentView: View {
|
||||
sensor.updateURL(editingURL)
|
||||
}
|
||||
|
||||
private var connectionBanner: some View {
|
||||
HStack(spacing: 12) {
|
||||
Circle()
|
||||
.fill(wsColor)
|
||||
.frame(width: 14, height: 14)
|
||||
Text(wsLabel)
|
||||
.font(.headline)
|
||||
Spacer()
|
||||
}
|
||||
.padding(.top, 8)
|
||||
}
|
||||
// MARK: Sensor rates grid
|
||||
|
||||
private var sensorRatesGrid: some View {
|
||||
Grid(horizontalSpacing: 20, verticalSpacing: 12) {
|
||||
GridRow {
|
||||
rateCell(icon: "location.fill", label: "GPS", rate: sensor.gpsRate, unit: "Hz")
|
||||
rateCell(icon: "gyroscope", label: "IMU", rate: sensor.imuRate, unit: "Hz")
|
||||
rateCell(icon: "location.fill", label: "GPS", rate: sensor.gpsRate)
|
||||
rateCell(icon: "gyroscope", label: "IMU", rate: sensor.imuRate)
|
||||
}
|
||||
GridRow {
|
||||
rateCell(icon: "location.north.fill", label: "Heading", rate: sensor.headingRate, unit: "Hz")
|
||||
rateCell(icon: "barometer", label: "Baro", rate: sensor.baroRate, unit: "Hz")
|
||||
rateCell(icon: "location.north.fill", label: "Heading", rate: sensor.headingRate)
|
||||
rateCell(icon: "barometer", label: "Baro", rate: sensor.baroRate)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private func rateCell(icon: String, label: String, rate: Double, unit: String) -> some View {
|
||||
private func rateCell(icon: String, label: String, rate: Double) -> some View {
|
||||
VStack(spacing: 4) {
|
||||
Image(systemName: icon)
|
||||
.font(.title2)
|
||||
@ -92,7 +111,7 @@ struct ContentView: View {
|
||||
Text(label)
|
||||
.font(.caption)
|
||||
.foregroundStyle(.secondary)
|
||||
Text("\(Int(rate)) \(unit)")
|
||||
Text("\(Int(rate)) Hz")
|
||||
.font(.title3.monospacedDigit())
|
||||
.bold()
|
||||
}
|
||||
@ -101,13 +120,23 @@ struct ContentView: View {
|
||||
.background(.quaternary, in: RoundedRectangle(cornerRadius: 12))
|
||||
}
|
||||
|
||||
// MARK: Distance row
|
||||
|
||||
private func distanceRow(_ dist: Double) -> some View {
|
||||
HStack {
|
||||
Image(systemName: "arrow.left.and.right")
|
||||
Text(dist < 1000
|
||||
? "Robot \(Int(dist)) m away"
|
||||
: String(format: "Robot %.1f km away", dist / 1000))
|
||||
.font(.title2).bold()
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: Follow-Me button
|
||||
|
||||
private var followMeButton: some View {
|
||||
Button {
|
||||
if sensor.isStreaming {
|
||||
sensor.stopStreaming()
|
||||
} else {
|
||||
sensor.startStreaming()
|
||||
}
|
||||
sensor.isStreaming ? sensor.stopStreaming() : sensor.startStreaming()
|
||||
} label: {
|
||||
Label(
|
||||
sensor.isStreaming ? "Stop Follow-Me" : "Start Follow-Me",
|
||||
@ -124,7 +153,7 @@ struct ContentView: View {
|
||||
.padding(.bottom, 8)
|
||||
}
|
||||
|
||||
// MARK: - Helpers
|
||||
// MARK: Helpers
|
||||
|
||||
private var wsColor: Color {
|
||||
switch sensor.wsState {
|
||||
|
||||
@ -10,6 +10,8 @@
|
||||
<string>$(PRODUCT_BUNDLE_IDENTIFIER)</string>
|
||||
<key>CFBundleInfoDictionaryVersion</key>
|
||||
<string>6.0</string>
|
||||
<key>CFBundleDisplayName</key>
|
||||
<string>SAUL-T-MOTE</string>
|
||||
<key>CFBundleName</key>
|
||||
<string>$(PRODUCT_NAME)</string>
|
||||
<key>CFBundlePackageType</key>
|
||||
|
||||
@ -1,8 +1,8 @@
|
||||
import Foundation
|
||||
import Network
|
||||
|
||||
/// Minimal MQTT 3.1.1 client — CONNECT + PUBLISH (QoS 0) + PINGREQ only.
|
||||
/// Sufficient for 1 Hz telemetry publishing; no subscription support needed.
|
||||
/// Minimal MQTT 3.1.1 client — CONNECT + PUBLISH (QoS 0) + SUBSCRIBE (QoS 0) + PINGREQ.
|
||||
/// Supports both publish and subscribe; no QoS 1/2 needed for this use-case.
|
||||
final class MQTTClient {
|
||||
|
||||
struct Config {
|
||||
@ -17,10 +17,16 @@ final class MQTTClient {
|
||||
enum State { case disconnected, connecting, connected }
|
||||
|
||||
private(set) var state: State = .disconnected
|
||||
|
||||
/// Called on the main queue for every received PUBLISH message: (topic, payload).
|
||||
var onMessage: ((String, String) -> Void)?
|
||||
|
||||
private var config: Config
|
||||
private var connection: NWConnection?
|
||||
private var pingTimer: DispatchSourceTimer?
|
||||
private var shouldRun = false
|
||||
private var subscriptions: [String] = [] // persisted across reconnects
|
||||
private var nextPacketID: UInt16 = 1
|
||||
private let queue = DispatchQueue(label: "mqtt.client", qos: .utility)
|
||||
|
||||
init(config: Config) {
|
||||
@ -47,21 +53,27 @@ final class MQTTClient {
|
||||
/// Publish a UTF-8 string payload to `topic` at QoS 0.
|
||||
func publish(topic: String, payload: String) {
|
||||
guard state == .connected else { return }
|
||||
let packet = buildPublish(topic: topic, payload: payload)
|
||||
connection?.send(content: packet, completion: .idempotent)
|
||||
connection?.send(content: buildPublish(topic: topic, payload: payload),
|
||||
completion: .idempotent)
|
||||
}
|
||||
|
||||
/// Subscribe to `topic` at QoS 0. Stored and re-sent automatically on reconnect.
|
||||
func subscribe(topic: String) {
|
||||
if !subscriptions.contains(topic) { subscriptions.append(topic) }
|
||||
guard state == .connected else { return }
|
||||
sendSubscribe(topic: topic)
|
||||
}
|
||||
|
||||
// MARK: - Connection lifecycle
|
||||
|
||||
private func openConnection() {
|
||||
state = .connecting
|
||||
let host = NWEndpoint.Host(config.host)
|
||||
let port = NWEndpoint.Port(rawValue: config.port)!
|
||||
connection = NWConnection(host: host, port: port, using: .tcp)
|
||||
connection?.stateUpdateHandler = { [weak self] newState in
|
||||
self?.handleStateChange(newState)
|
||||
}
|
||||
connection?.start(queue: queue)
|
||||
let conn = NWConnection(host: NWEndpoint.Host(config.host),
|
||||
port: NWEndpoint.Port(rawValue: config.port)!,
|
||||
using: .tcp)
|
||||
conn.stateUpdateHandler = { [weak self] s in self?.handleStateChange(s) }
|
||||
conn.start(queue: queue)
|
||||
connection = conn
|
||||
scheduleRead()
|
||||
}
|
||||
|
||||
@ -82,32 +94,69 @@ final class MQTTClient {
|
||||
|
||||
private func reconnectIfNeeded() {
|
||||
guard shouldRun else { return }
|
||||
queue.asyncAfter(deadline: .now() + 3) { [weak self] in
|
||||
self?.openConnection()
|
||||
}
|
||||
queue.asyncAfter(deadline: .now() + 3) { [weak self] in self?.openConnection() }
|
||||
}
|
||||
|
||||
// MARK: - Read loop (to receive CONNACK / PINGRESP)
|
||||
// MARK: - Read loop
|
||||
|
||||
private func scheduleRead() {
|
||||
connection?.receive(minimumIncompleteLength: 2, maximumLength: 256) { [weak self] data, _, _, error in
|
||||
connection?.receive(minimumIncompleteLength: 2, maximumLength: 4096) { [weak self] data, _, _, error in
|
||||
guard let self else { return }
|
||||
if let data, !data.isEmpty {
|
||||
self.handleIncoming(data)
|
||||
}
|
||||
if let data, !data.isEmpty { self.handleIncoming(data) }
|
||||
if error == nil { self.scheduleRead() }
|
||||
}
|
||||
}
|
||||
|
||||
/// Parse one or more MQTT packets from `data`.
|
||||
private func handleIncoming(_ data: Data) {
|
||||
guard let first = data.first else { return }
|
||||
switch first {
|
||||
case 0x20: // CONNACK
|
||||
state = .connected
|
||||
case 0xD0: // PINGRESP — no action needed
|
||||
break
|
||||
default:
|
||||
break
|
||||
var i = data.startIndex
|
||||
|
||||
while i < data.endIndex {
|
||||
let firstByte = data[i]
|
||||
i = data.index(after: i)
|
||||
let packetType = firstByte & 0xF0
|
||||
|
||||
// Decode variable-length remaining-length field
|
||||
var multiplier = 1
|
||||
var remaining = 0
|
||||
var lenByte: UInt8 = 0
|
||||
repeat {
|
||||
guard i < data.endIndex else { return }
|
||||
lenByte = data[i]
|
||||
i = data.index(after: i)
|
||||
remaining += Int(lenByte & 0x7F) * multiplier
|
||||
multiplier *= 128
|
||||
} while lenByte & 0x80 != 0
|
||||
|
||||
guard let payloadEnd = data.index(i, offsetBy: remaining, limitedBy: data.endIndex) else { break }
|
||||
|
||||
switch packetType {
|
||||
case 0x20: // CONNACK
|
||||
state = .connected
|
||||
for topic in subscriptions { sendSubscribe(topic: topic) }
|
||||
|
||||
case 0x30: // PUBLISH (QoS 0 — no packet identifier)
|
||||
var j = i
|
||||
if data.distance(from: j, to: payloadEnd) >= 2 {
|
||||
let topicLen = Int(data[j]) << 8 | Int(data[data.index(after: j)])
|
||||
j = data.index(j, offsetBy: 2)
|
||||
if let topicEnd = data.index(j, offsetBy: topicLen, limitedBy: payloadEnd) {
|
||||
let topic = String(bytes: data[j..<topicEnd], encoding: .utf8) ?? ""
|
||||
let payload = String(bytes: data[topicEnd..<payloadEnd], encoding: .utf8) ?? ""
|
||||
let t = topic, p = payload
|
||||
DispatchQueue.main.async { self.onMessage?(t, p) }
|
||||
}
|
||||
}
|
||||
|
||||
case 0x90: // SUBACK — ignore
|
||||
break
|
||||
case 0xD0: // PINGRESP — ignore
|
||||
break
|
||||
default:
|
||||
break
|
||||
}
|
||||
|
||||
i = payloadEnd
|
||||
}
|
||||
}
|
||||
|
||||
@ -118,33 +167,27 @@ final class MQTTClient {
|
||||
t.schedule(deadline: .now() + Double(config.keepAlive / 2),
|
||||
repeating: Double(config.keepAlive / 2))
|
||||
t.setEventHandler { [weak self] in
|
||||
self?.sendPing()
|
||||
self?.connection?.send(content: Data([0xC0, 0x00]), completion: .idempotent)
|
||||
}
|
||||
t.resume()
|
||||
pingTimer = t
|
||||
}
|
||||
|
||||
private func sendPing() {
|
||||
let packet = Data([0xC0, 0x00])
|
||||
connection?.send(content: packet, completion: .idempotent)
|
||||
}
|
||||
|
||||
// MARK: - MQTT packet builders
|
||||
|
||||
private func sendConnect() {
|
||||
let packet = buildConnect()
|
||||
connection?.send(content: packet, completion: .idempotent)
|
||||
connection?.send(content: buildConnect(), completion: .idempotent)
|
||||
}
|
||||
|
||||
private func buildConnect() -> Data {
|
||||
var payload = Data()
|
||||
payload += mqttString("MQTT") // protocol name
|
||||
payload.append(0x04) // protocol level (3.1.1)
|
||||
payload.append(0xC2) // flags: username + password + clean session
|
||||
payload += uint16BE(config.keepAlive) // keep-alive
|
||||
payload += mqttString(config.clientID) // client ID
|
||||
payload += mqttString(config.username) // username
|
||||
payload += mqttString(config.password) // password
|
||||
payload += mqttString("MQTT")
|
||||
payload.append(0x04) // protocol level 3.1.1
|
||||
payload.append(0xC2) // flags: username + password + clean session
|
||||
payload += uint16BE(config.keepAlive)
|
||||
payload += mqttString(config.clientID)
|
||||
payload += mqttString(config.username)
|
||||
payload += mqttString(config.password)
|
||||
return mqttPacket(type: 0x10, payload: payload)
|
||||
}
|
||||
|
||||
@ -155,6 +198,16 @@ final class MQTTClient {
|
||||
return mqttPacket(type: 0x30, payload: body)
|
||||
}
|
||||
|
||||
private func sendSubscribe(topic: String) {
|
||||
var payload = Data()
|
||||
payload += uint16BE(nextPacketID)
|
||||
nextPacketID &+= 1
|
||||
payload += mqttString(topic)
|
||||
payload.append(0x00) // QoS 0
|
||||
connection?.send(content: mqttPacket(type: 0x82, payload: payload),
|
||||
completion: .idempotent)
|
||||
}
|
||||
|
||||
// MARK: - Encoding helpers
|
||||
|
||||
private func mqttPacket(type: UInt8, payload: Data) -> Data {
|
||||
|
||||
184
SulTee/SulTee/MapContentView.swift
Normal file
184
SulTee/SulTee/MapContentView.swift
Normal file
@ -0,0 +1,184 @@
|
||||
import SwiftUI
|
||||
import MapKit
|
||||
|
||||
/// Full-screen map showing user (blue) and robot (orange) positions,
|
||||
/// a follow-path line between them, and fading breadcrumb trails for both.
|
||||
struct MapContentView: View {
|
||||
@EnvironmentObject var sensor: SensorManager
|
||||
|
||||
@State private var position: MapCameraPosition = .automatic
|
||||
@State private var followUser = true
|
||||
|
||||
var body: some View {
|
||||
ZStack(alignment: .bottom) {
|
||||
map
|
||||
overlay
|
||||
}
|
||||
.ignoresSafeArea(edges: .top)
|
||||
.onChange(of: sensor.userLocation) { _, coord in
|
||||
if followUser, let coord {
|
||||
withAnimation(.easeInOut(duration: 0.4)) {
|
||||
position = .camera(MapCamera(
|
||||
centerCoordinate: coord,
|
||||
distance: 400,
|
||||
heading: 0,
|
||||
pitch: 0
|
||||
))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Map
|
||||
|
||||
private var map: some View {
|
||||
Map(position: $position) {
|
||||
|
||||
// ── User breadcrumb trail (fading blue dots)
|
||||
let userCrumbs = sensor.userBreadcrumbs
|
||||
ForEach(userCrumbs.indices, id: \.self) { idx in
|
||||
let opacity = Double(idx + 1) / Double(max(userCrumbs.count, 1))
|
||||
Annotation("", coordinate: userCrumbs[idx]) {
|
||||
Circle()
|
||||
.fill(.blue.opacity(opacity * 0.6))
|
||||
.frame(width: 7, height: 7)
|
||||
}
|
||||
.annotationTitles(.hidden)
|
||||
}
|
||||
|
||||
// ── Robot breadcrumb trail (fading orange dots)
|
||||
let robotCrumbs = sensor.robotBreadcrumbs
|
||||
ForEach(robotCrumbs.indices, id: \.self) { idx in
|
||||
let opacity = Double(idx + 1) / Double(max(robotCrumbs.count, 1))
|
||||
Annotation("", coordinate: robotCrumbs[idx]) {
|
||||
Circle()
|
||||
.fill(.orange.opacity(opacity * 0.6))
|
||||
.frame(width: 7, height: 7)
|
||||
}
|
||||
.annotationTitles(.hidden)
|
||||
}
|
||||
|
||||
// ── Follow path line: robot → user
|
||||
if let userLoc = sensor.userLocation,
|
||||
let robotLoc = sensor.robotLocation {
|
||||
MapPolyline(coordinates: [robotLoc, userLoc])
|
||||
.stroke(.yellow, style: StrokeStyle(lineWidth: 2, dash: [6, 4]))
|
||||
}
|
||||
|
||||
// ── User marker (blue)
|
||||
if let userLoc = sensor.userLocation {
|
||||
Annotation("You", coordinate: userLoc) {
|
||||
ZStack {
|
||||
Circle()
|
||||
.fill(.blue.opacity(0.25))
|
||||
.frame(width: 36, height: 36)
|
||||
Circle()
|
||||
.fill(.blue)
|
||||
.frame(width: 16, height: 16)
|
||||
Circle()
|
||||
.stroke(.white, lineWidth: 2.5)
|
||||
.frame(width: 16, height: 16)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── Robot marker (orange)
|
||||
if let robotLoc = sensor.robotLocation {
|
||||
Annotation("Robot", coordinate: robotLoc) {
|
||||
ZStack {
|
||||
Circle()
|
||||
.fill(.orange.opacity(0.25))
|
||||
.frame(width: 36, height: 36)
|
||||
Image(systemName: "car.fill")
|
||||
.font(.system(size: 16, weight: .bold))
|
||||
.foregroundStyle(.white)
|
||||
.padding(6)
|
||||
.background(.orange, in: Circle())
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
.mapStyle(.standard(elevation: .realistic))
|
||||
.onMapCameraChange { _ in
|
||||
// User dragged map → stop auto-follow
|
||||
followUser = false
|
||||
}
|
||||
.overlay(alignment: .topTrailing) {
|
||||
followButton
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Re-centre button
|
||||
|
||||
private var followButton: some View {
|
||||
Button {
|
||||
followUser = true
|
||||
if let coord = sensor.userLocation {
|
||||
withAnimation {
|
||||
position = .camera(MapCamera(
|
||||
centerCoordinate: coord,
|
||||
distance: 400,
|
||||
heading: 0,
|
||||
pitch: 0
|
||||
))
|
||||
}
|
||||
}
|
||||
} label: {
|
||||
Image(systemName: followUser ? "location.fill" : "location")
|
||||
.padding(10)
|
||||
.background(.ultraThinMaterial, in: Circle())
|
||||
}
|
||||
.padding([.top, .trailing], 16)
|
||||
.padding(.top, 44) // below nav bar
|
||||
}
|
||||
|
||||
// MARK: - Stats overlay
|
||||
|
||||
private var overlay: some View {
|
||||
HStack(spacing: 20) {
|
||||
if let dist = sensor.distanceToRobot {
|
||||
statCell(value: distanceString(dist),
|
||||
label: "distance",
|
||||
icon: "arrow.left.and.right")
|
||||
}
|
||||
if sensor.robotSpeed > 0.2 {
|
||||
statCell(value: String(format: "%.1f m/s", sensor.robotSpeed),
|
||||
label: "robot spd",
|
||||
icon: "speedometer")
|
||||
}
|
||||
if !sensor.isStreaming {
|
||||
Text("Start Follow-Me to stream")
|
||||
.font(.caption)
|
||||
.foregroundStyle(.secondary)
|
||||
}
|
||||
}
|
||||
.padding(.horizontal, 20)
|
||||
.padding(.vertical, 12)
|
||||
.background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 16))
|
||||
.padding(.bottom, 24)
|
||||
.padding(.horizontal, 16)
|
||||
}
|
||||
|
||||
private func statCell(value: String, label: String, icon: String) -> some View {
|
||||
HStack(spacing: 6) {
|
||||
Image(systemName: icon)
|
||||
.foregroundStyle(.secondary)
|
||||
.font(.caption)
|
||||
VStack(alignment: .leading, spacing: 0) {
|
||||
Text(value).font(.headline.monospacedDigit())
|
||||
Text(label).font(.caption2).foregroundStyle(.secondary)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private func distanceString(_ metres: Double) -> String {
|
||||
metres < 1000
|
||||
? "\(Int(metres)) m"
|
||||
: String(format: "%.1f km", metres / 1000)
|
||||
}
|
||||
}
|
||||
|
||||
#Preview {
|
||||
MapContentView()
|
||||
.environmentObject(SensorManager())
|
||||
}
|
||||
@ -1,30 +1,47 @@
|
||||
import Foundation
|
||||
import CoreLocation
|
||||
import CoreMotion
|
||||
import MapKit
|
||||
import Combine
|
||||
|
||||
/// Manages all iPhone sensors and forwards data to the WebSocket client
|
||||
/// and MQTT broker (topic: saltybot/ios/gps, 1 Hz).
|
||||
/// Manages all iPhone sensors, publishes iOS GPS to MQTT, subscribes to robot GPS,
|
||||
/// and exposes state for the map and status views.
|
||||
final class SensorManager: NSObject, ObservableObject {
|
||||
|
||||
// MARK: - Published state for UI
|
||||
// MARK: - Streaming state
|
||||
|
||||
@Published var isStreaming = false
|
||||
@Published var wsState: WebSocketClient.ConnectionState = .disconnected
|
||||
@Published var gpsRate: Double = 0
|
||||
@Published var imuRate: Double = 0
|
||||
@Published var headingRate: Double = 0
|
||||
@Published var baroRate: Double = 0
|
||||
@Published var botDistanceMeters: Double? = nil
|
||||
|
||||
// MARK: - WebSocket
|
||||
// MARK: - Sensor rates (Hz)
|
||||
|
||||
@Published var gpsRate: Double = 0
|
||||
@Published var imuRate: Double = 0
|
||||
@Published var headingRate: Double = 0
|
||||
@Published var baroRate: Double = 0
|
||||
|
||||
// MARK: - User (phone) position
|
||||
|
||||
@Published var userLocation: CLLocationCoordinate2D? = nil
|
||||
@Published var userBreadcrumbs: [CLLocationCoordinate2D] = []
|
||||
|
||||
// MARK: - Robot position (from MQTT saltybot/phone/gps)
|
||||
|
||||
@Published var robotLocation: CLLocationCoordinate2D? = nil
|
||||
@Published var robotBreadcrumbs: [CLLocationCoordinate2D] = []
|
||||
@Published var robotSpeed: Double = 0
|
||||
|
||||
// MARK: - Derived
|
||||
|
||||
@Published var distanceToRobot: Double? = nil
|
||||
|
||||
// MARK: - WebSocket config (sensor stream to Orin)
|
||||
|
||||
static let defaultOrinURL = "ws://100.64.0.2:9090"
|
||||
private static let orinURLKey = "orinURL"
|
||||
|
||||
private(set) var ws: WebSocketClient
|
||||
|
||||
/// Current Orin WebSocket URL string (persisted in UserDefaults).
|
||||
var orinURLString: String {
|
||||
get { UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL }
|
||||
set { UserDefaults.standard.set(newValue, forKey: Self.orinURLKey) }
|
||||
@ -33,29 +50,32 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
// MARK: - MQTT
|
||||
|
||||
private let mqtt = MQTTClient(config: .init(
|
||||
host: "192.168.87.29",
|
||||
port: 1883,
|
||||
host: "192.168.87.29",
|
||||
port: 1883,
|
||||
username: "mqtt_seb",
|
||||
password: "mqtt_pass",
|
||||
clientID: "sultee-ios-\(UUID().uuidString.prefix(8))"
|
||||
clientID: "saul-t-mote-\(UUID().uuidString.prefix(8))"
|
||||
))
|
||||
private static let mqttGPSTopic = "saltybot/ios/gps"
|
||||
private static let iosGPSTopic = "saltybot/ios/gps"
|
||||
private static let robotGPSTopic = "saltybot/phone/gps"
|
||||
private static let maxBreadcrumbs = 60
|
||||
|
||||
private var lastKnownLocation: CLLocation?
|
||||
private var mqttPublishTimer: Timer?
|
||||
|
||||
// MARK: - Sensors
|
||||
|
||||
private let locationManager = CLLocationManager()
|
||||
private let motionManager = CMMotionManager()
|
||||
private let altimeter = CMAltimeter()
|
||||
private var cancellables = Set<AnyCancellable>()
|
||||
private let motionManager = CMMotionManager()
|
||||
private let altimeter = CMAltimeter()
|
||||
private var cancellables = Set<AnyCancellable>()
|
||||
|
||||
// MARK: - Rate counters (counts per second)
|
||||
// MARK: - Rate counters
|
||||
|
||||
private var gpsCounts: [Date] = []
|
||||
private var imuCounts: [Date] = []
|
||||
private var gpsCounts: [Date] = []
|
||||
private var imuCounts: [Date] = []
|
||||
private var headingCounts: [Date] = []
|
||||
private var baroCounts: [Date] = []
|
||||
private var baroCounts: [Date] = []
|
||||
private var rateTimer: Timer?
|
||||
|
||||
// MARK: - Init
|
||||
@ -64,9 +84,9 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
let urlString = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL
|
||||
self.ws = WebSocketClient(url: URL(string: urlString) ?? URL(string: Self.defaultOrinURL)!)
|
||||
super.init()
|
||||
|
||||
locationManager.delegate = self
|
||||
locationManager.desiredAccuracy = kCLLocationAccuracyBestForNavigation
|
||||
// Use dual-frequency GPS (L1+L5) on iPhone 15 Pro — automatic when accuracy is set to Best
|
||||
locationManager.distanceFilter = kCLDistanceFilterNone
|
||||
locationManager.allowsBackgroundLocationUpdates = true
|
||||
locationManager.pausesLocationUpdatesAutomatically = false
|
||||
@ -76,6 +96,11 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
.receive(on: DispatchQueue.main)
|
||||
.assign(to: \.wsState, on: self)
|
||||
.store(in: &cancellables)
|
||||
|
||||
mqtt.onMessage = { [weak self] topic, payload in
|
||||
guard let self, topic == Self.robotGPSTopic else { return }
|
||||
self.handleRobotGPS(payload)
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Public control
|
||||
@ -85,6 +110,7 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
isStreaming = true
|
||||
ws.connect()
|
||||
mqtt.connect()
|
||||
mqtt.subscribe(topic: Self.robotGPSTopic)
|
||||
requestPermissionsAndStartSensors()
|
||||
startRateTimer()
|
||||
startMQTTPublishTimer()
|
||||
@ -96,14 +122,10 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
ws.disconnect()
|
||||
mqtt.disconnect()
|
||||
stopSensors()
|
||||
rateTimer?.invalidate()
|
||||
rateTimer = nil
|
||||
mqttPublishTimer?.invalidate()
|
||||
mqttPublishTimer = nil
|
||||
rateTimer?.invalidate(); rateTimer = nil
|
||||
mqttPublishTimer?.invalidate(); mqttPublishTimer = nil
|
||||
}
|
||||
|
||||
/// Call when the user edits the Orin URL. Persists the value and updates
|
||||
/// the client URL; takes effect on the next connect().
|
||||
func updateURL(_ urlString: String) {
|
||||
guard !isStreaming else { return }
|
||||
orinURLString = urlString
|
||||
@ -134,7 +156,37 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
]
|
||||
guard let data = try? JSONSerialization.data(withJSONObject: payload),
|
||||
let json = String(data: data, encoding: .utf8) else { return }
|
||||
mqtt.publish(topic: Self.mqttGPSTopic, payload: json)
|
||||
mqtt.publish(topic: Self.iosGPSTopic, payload: json)
|
||||
}
|
||||
|
||||
// MARK: - Robot GPS subscription handler
|
||||
|
||||
private func handleRobotGPS(_ payload: String) {
|
||||
guard let data = payload.data(using: .utf8),
|
||||
let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any],
|
||||
let lat = json["lat"] as? Double,
|
||||
let lon = json["lon"] as? Double else { return }
|
||||
|
||||
let coord = CLLocationCoordinate2D(latitude: lat, longitude: lon)
|
||||
robotLocation = coord
|
||||
robotSpeed = (json["speed_ms"] as? Double) ?? 0
|
||||
appendBreadcrumb(coord, to: &robotBreadcrumbs)
|
||||
updateDistance()
|
||||
}
|
||||
|
||||
// MARK: - Breadcrumbs + distance
|
||||
|
||||
private func appendBreadcrumb(_ coord: CLLocationCoordinate2D,
|
||||
to list: inout [CLLocationCoordinate2D]) {
|
||||
list.append(coord)
|
||||
if list.count > Self.maxBreadcrumbs { list.removeFirst() }
|
||||
}
|
||||
|
||||
private func updateDistance() {
|
||||
guard let user = userLocation, let robot = robotLocation else { return }
|
||||
let a = CLLocation(latitude: user.latitude, longitude: user.longitude)
|
||||
let b = CLLocation(latitude: robot.latitude, longitude: robot.longitude)
|
||||
distanceToRobot = a.distance(from: b)
|
||||
}
|
||||
|
||||
// MARK: - Sensor start / stop
|
||||
@ -157,34 +209,17 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
private func startIMU() {
|
||||
guard motionManager.isDeviceMotionAvailable else { return }
|
||||
motionManager.deviceMotionUpdateInterval = 1.0 / 100.0 // 100 Hz
|
||||
motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, error in
|
||||
motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, _ in
|
||||
guard let self, let motion else { return }
|
||||
self.recordEvent(in: &self.imuCounts)
|
||||
let ts = Date().timeIntervalSince1970
|
||||
self.ws.send([
|
||||
"type": "imu",
|
||||
"timestamp": ts,
|
||||
"timestamp": Date().timeIntervalSince1970,
|
||||
"data": [
|
||||
"accel": [
|
||||
"x": motion.userAcceleration.x,
|
||||
"y": motion.userAcceleration.y,
|
||||
"z": motion.userAcceleration.z
|
||||
],
|
||||
"gyro": [
|
||||
"x": motion.rotationRate.x,
|
||||
"y": motion.rotationRate.y,
|
||||
"z": motion.rotationRate.z
|
||||
],
|
||||
"attitude": [
|
||||
"roll": motion.attitude.roll,
|
||||
"pitch": motion.attitude.pitch,
|
||||
"yaw": motion.attitude.yaw
|
||||
],
|
||||
"gravity": [
|
||||
"x": motion.gravity.x,
|
||||
"y": motion.gravity.y,
|
||||
"z": motion.gravity.z
|
||||
],
|
||||
"accel": ["x": motion.userAcceleration.x, "y": motion.userAcceleration.y, "z": motion.userAcceleration.z],
|
||||
"gyro": ["x": motion.rotationRate.x, "y": motion.rotationRate.y, "z": motion.rotationRate.z],
|
||||
"attitude": ["roll": motion.attitude.roll, "pitch": motion.attitude.pitch, "yaw": motion.attitude.yaw],
|
||||
"gravity": ["x": motion.gravity.x, "y": motion.gravity.y, "z": motion.gravity.z],
|
||||
"magneticField": [
|
||||
"x": motion.magneticField.field.x,
|
||||
"y": motion.magneticField.field.y,
|
||||
@ -198,16 +233,14 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
|
||||
private func startBarometer() {
|
||||
guard CMAltimeter.isRelativeAltitudeAvailable() else { return }
|
||||
altimeter.startRelativeAltitudeUpdates(to: .main) { [weak self] data, error in
|
||||
altimeter.startRelativeAltitudeUpdates(to: .main) { [weak self] data, _ in
|
||||
guard let self, let data else { return }
|
||||
self.recordEvent(in: &self.baroCounts)
|
||||
self.ws.send([
|
||||
"type": "baro",
|
||||
"timestamp": Date().timeIntervalSince1970,
|
||||
"data": [
|
||||
"relativeAltitude": data.relativeAltitude.doubleValue,
|
||||
"pressure": data.pressure.doubleValue
|
||||
]
|
||||
"data": ["relativeAltitude": data.relativeAltitude.doubleValue,
|
||||
"pressure": data.pressure.doubleValue]
|
||||
])
|
||||
}
|
||||
}
|
||||
@ -220,22 +253,19 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
}
|
||||
}
|
||||
|
||||
private func recordEvent(in list: inout [Date]) {
|
||||
list.append(Date())
|
||||
}
|
||||
private func recordEvent(in list: inout [Date]) { list.append(Date()) }
|
||||
|
||||
private func updateRates() {
|
||||
let cutoff = Date().addingTimeInterval(-1.0)
|
||||
gpsCounts = gpsCounts.filter { $0 > cutoff }
|
||||
imuCounts = imuCounts.filter { $0 > cutoff }
|
||||
gpsCounts = gpsCounts.filter { $0 > cutoff }
|
||||
imuCounts = imuCounts.filter { $0 > cutoff }
|
||||
headingCounts = headingCounts.filter { $0 > cutoff }
|
||||
baroCounts = baroCounts.filter { $0 > cutoff }
|
||||
|
||||
baroCounts = baroCounts.filter { $0 > cutoff }
|
||||
DispatchQueue.main.async {
|
||||
self.gpsRate = Double(self.gpsCounts.count)
|
||||
self.imuRate = Double(self.imuCounts.count)
|
||||
self.gpsRate = Double(self.gpsCounts.count)
|
||||
self.imuRate = Double(self.imuCounts.count)
|
||||
self.headingRate = Double(self.headingCounts.count)
|
||||
self.baroRate = Double(self.baroCounts.count)
|
||||
self.baroRate = Double(self.baroCounts.count)
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -247,20 +277,25 @@ extension SensorManager: CLLocationManagerDelegate {
|
||||
func locationManager(_ manager: CLLocationManager, didUpdateLocations locations: [CLLocation]) {
|
||||
guard let loc = locations.last else { return }
|
||||
lastKnownLocation = loc
|
||||
let coord = loc.coordinate
|
||||
userLocation = coord
|
||||
appendBreadcrumb(coord, to: &userBreadcrumbs)
|
||||
updateDistance()
|
||||
recordEvent(in: &gpsCounts)
|
||||
|
||||
ws.send([
|
||||
"type": "gps",
|
||||
"timestamp": loc.timestamp.timeIntervalSince1970,
|
||||
"data": [
|
||||
"latitude": loc.coordinate.latitude,
|
||||
"longitude": loc.coordinate.longitude,
|
||||
"altitude": loc.altitude,
|
||||
"horizontalAccuracy": loc.horizontalAccuracy,
|
||||
"verticalAccuracy": loc.verticalAccuracy,
|
||||
"speed": loc.speed,
|
||||
"speedAccuracy": loc.speedAccuracy,
|
||||
"course": loc.course,
|
||||
"courseAccuracy": loc.courseAccuracy
|
||||
"latitude": loc.coordinate.latitude,
|
||||
"longitude": loc.coordinate.longitude,
|
||||
"altitude": loc.altitude,
|
||||
"horizontalAccuracy":loc.horizontalAccuracy,
|
||||
"verticalAccuracy": loc.verticalAccuracy,
|
||||
"speed": loc.speed,
|
||||
"speedAccuracy": loc.speedAccuracy,
|
||||
"course": loc.course,
|
||||
"courseAccuracy": loc.courseAccuracy
|
||||
]
|
||||
])
|
||||
}
|
||||
@ -272,11 +307,9 @@ extension SensorManager: CLLocationManagerDelegate {
|
||||
"timestamp": Date().timeIntervalSince1970,
|
||||
"data": [
|
||||
"magneticHeading": newHeading.magneticHeading,
|
||||
"trueHeading": newHeading.trueHeading,
|
||||
"trueHeading": newHeading.trueHeading,
|
||||
"headingAccuracy": newHeading.headingAccuracy,
|
||||
"x": newHeading.x,
|
||||
"y": newHeading.y,
|
||||
"z": newHeading.z
|
||||
"x": newHeading.x, "y": newHeading.y, "z": newHeading.z
|
||||
]
|
||||
])
|
||||
}
|
||||
@ -292,8 +325,7 @@ extension SensorManager: CLLocationManagerDelegate {
|
||||
manager.startUpdatingLocation()
|
||||
manager.startUpdatingHeading()
|
||||
}
|
||||
default:
|
||||
break
|
||||
default: break
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user