From 72e3138fb3118bae01c77cf15936aa33356256ac Mon Sep 17 00:00:00 2001 From: sl-ios Date: Sat, 4 Apr 2026 11:41:11 -0400 Subject: [PATCH] feat: Rename to SAUL-T-MOTE, add map with user + robot positions and follow path MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- CLAUDE.md | 67 ++++++ SulTee/SulTee.xcodeproj/project.pbxproj | 4 + .../contents.xcworkspacedata | 7 + SulTee/SulTee/ContentView.swift | 87 +++++--- SulTee/SulTee/Info.plist | 2 + SulTee/SulTee/MQTTClient.swift | 137 ++++++++---- SulTee/SulTee/MapContentView.swift | 184 ++++++++++++++++ SulTee/SulTee/SensorManager.swift | 196 ++++++++++-------- 8 files changed, 531 insertions(+), 153 deletions(-) create mode 100644 CLAUDE.md create mode 100644 SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata create mode 100644 SulTee/SulTee/MapContentView.swift diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..3fb1c23 --- /dev/null +++ b/CLAUDE.md @@ -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--` + +## 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: (Issue #N)' \ + --description '
' --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-"` +- 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 diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj index c3a2c72..1a6ff66 100644 --- a/SulTee/SulTee.xcodeproj/project.pbxproj +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -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 = ""; }; 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 = ""; }; + A10000010000000000000AAB /* MapContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MapContentView.swift; sourceTree = ""; }; /* 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; }; diff --git a/SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata b/SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata new file mode 100644 index 0000000..919434a --- /dev/null +++ b/SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata @@ -0,0 +1,7 @@ + + + + + diff --git a/SulTee/SulTee/ContentView.swift b/SulTee/SulTee/ContentView.swift index c9b1ad0..6ca50da 100644 --- a/SulTee/SulTee/ContentView.swift +++ b/SulTee/SulTee/ContentView.swift @@ -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 { diff --git a/SulTee/SulTee/Info.plist b/SulTee/SulTee/Info.plist index 8e3361b..d00c04b 100644 --- a/SulTee/SulTee/Info.plist +++ b/SulTee/SulTee/Info.plist @@ -10,6 +10,8 @@ $(PRODUCT_BUNDLE_IDENTIFIER) CFBundleInfoDictionaryVersion 6.0 + CFBundleDisplayName + SAUL-T-MOTE CFBundleName $(PRODUCT_NAME) CFBundlePackageType diff --git a/SulTee/SulTee/MQTTClient.swift b/SulTee/SulTee/MQTTClient.swift index 88df9bc..8072714 100644 --- a/SulTee/SulTee/MQTTClient.swift +++ b/SulTee/SulTee/MQTTClient.swift @@ -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.. 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 { diff --git a/SulTee/SulTee/MapContentView.swift b/SulTee/SulTee/MapContentView.swift new file mode 100644 index 0000000..c44dc15 --- /dev/null +++ b/SulTee/SulTee/MapContentView.swift @@ -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()) +} diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 9604f0a..41db57e 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -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() + private let motionManager = CMMotionManager() + private let altimeter = CMAltimeter() + private var cancellables = Set() - // 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 } } }