diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj index 1a6ff66..a65c916 100644 --- a/SulTee/SulTee.xcodeproj/project.pbxproj +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -14,6 +14,7 @@ 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 */; }; + A10000010000000000000BAA /* UWBModels.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000BAB /* UWBModels.swift */; }; /* End PBXBuildFile section */ /* Begin PBXFileReference section */ @@ -26,6 +27,7 @@ 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 = ""; }; + A10000010000000000000BAB /* UWBModels.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = UWBModels.swift; sourceTree = ""; }; /* End PBXFileReference section */ /* Begin PBXFrameworksBuildPhase section */ @@ -56,6 +58,7 @@ A100000100000000000004AB /* WebSocketClient.swift */, A100000100000000000009AB /* MQTTClient.swift */, A10000010000000000000AAB /* MapContentView.swift */, + A10000010000000000000BAB /* UWBModels.swift */, A100000100000000000005AB /* Assets.xcassets */, A100000100000000000006AB /* Info.plist */, ); @@ -146,6 +149,7 @@ A100000100000000000004AA /* WebSocketClient.swift in Sources */, A100000100000000000009AA /* MQTTClient.swift in Sources */, A10000010000000000000AAA /* MapContentView.swift in Sources */, + A10000010000000000000BAA /* UWBModels.swift in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/SulTee/SulTee/MapContentView.swift b/SulTee/SulTee/MapContentView.swift index c44dc15..d317324 100644 --- a/SulTee/SulTee/MapContentView.swift +++ b/SulTee/SulTee/MapContentView.swift @@ -1,18 +1,33 @@ import SwiftUI import MapKit +extension CLLocationCoordinate2D: @retroactive Equatable { + public static func == (lhs: CLLocationCoordinate2D, rhs: CLLocationCoordinate2D) -> Bool { + lhs.latitude == rhs.latitude && lhs.longitude == rhs.longitude + } +} + /// Full-screen map showing user (blue) and robot (orange) positions, -/// a follow-path line between them, and fading breadcrumb trails for both. +/// a follow-path line, fading breadcrumb trails, UWB status, and follow controls. struct MapContentView: View { @EnvironmentObject var sensor: SensorManager - @State private var position: MapCameraPosition = .automatic - @State private var followUser = true + @State private var position: MapCameraPosition = .automatic + @State private var followUser = true + + private var insideFollowRange: Bool { + guard let d = sensor.distanceToRobot else { return false } + return d <= sensor.followPreset.metres + } var body: some View { ZStack(alignment: .bottom) { map - overlay + VStack(spacing: 0) { + Spacer() + followControls + statsBar + } } .ignoresSafeArea(edges: .top) .onChange(of: sensor.userLocation) { _, coord in @@ -20,20 +35,32 @@ struct MapContentView: View { withAnimation(.easeInOut(duration: 0.4)) { position = .camera(MapCamera( centerCoordinate: coord, - distance: 400, - heading: 0, - pitch: 0 + distance: 400, heading: 0, pitch: 0 )) } } } } - // MARK: - Map + // MARK: - Map content private var map: some View { Map(position: $position) { + // ── Follow range circle around robot + if let robotLoc = sensor.robotLocation { + MapCircle(center: robotLoc, radius: sensor.followPreset.metres) + .foregroundStyle( + insideFollowRange + ? Color.green.opacity(0.12) + : Color.orange.opacity(0.12) + ) + .stroke( + insideFollowRange ? Color.green : Color.orange, + style: StrokeStyle(lineWidth: 1.5) + ) + } + // ── User breadcrumb trail (fading blue dots) let userCrumbs = sensor.userBreadcrumbs ForEach(userCrumbs.indices, id: \.self) { idx in @@ -59,7 +86,7 @@ struct MapContentView: View { } // ── Follow path line: robot → user - if let userLoc = sensor.userLocation, + if let userLoc = sensor.userLocation, let robotLoc = sensor.robotLocation { MapPolyline(coordinates: [robotLoc, userLoc]) .stroke(.yellow, style: StrokeStyle(lineWidth: 2, dash: [6, 4])) @@ -69,15 +96,9 @@ struct MapContentView: View { 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) + 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) } } } @@ -86,9 +107,7 @@ struct MapContentView: View { if let robotLoc = sensor.robotLocation { Annotation("Robot", coordinate: robotLoc) { ZStack { - Circle() - .fill(.orange.opacity(0.25)) - .frame(width: 36, height: 36) + Circle().fill(.orange.opacity(0.25)).frame(width: 36, height: 36) Image(systemName: "car.fill") .font(.system(size: 16, weight: .bold)) .foregroundStyle(.white) @@ -99,27 +118,54 @@ struct MapContentView: View { } } .mapStyle(.standard(elevation: .realistic)) - .onMapCameraChange { _ in - // User dragged map → stop auto-follow - followUser = false - } - .overlay(alignment: .topTrailing) { - followButton - } + .onMapCameraChange { _ in followUser = false } + .overlay(alignment: .topTrailing) { recenterButton } + .overlay(alignment: .topLeading) { uwbBadge } } - // MARK: - Re-centre button + // MARK: - UWB status badge (top-left) - private var followButton: some View { + private var uwbBadge: some View { + VStack(alignment: .leading, spacing: 4) { + HStack(spacing: 6) { + Circle() + .fill(sensor.uwbActive ? Color.green : Color.gray) + .frame(width: 9, height: 9) + .shadow(color: sensor.uwbActive ? .green : .clear, radius: 4) + Text(sensor.uwbActive ? "UWB Active" : "UWB Out of Range") + .font(.caption2.bold()) + .foregroundStyle(sensor.uwbActive ? .primary : .secondary) + } + + // Per-anchor ranges + let sortedRanges = sensor.uwbRanges.values.sorted { $0.anchorID < $1.anchorID } + if !sortedRanges.isEmpty { + HStack(spacing: 8) { + ForEach(sortedRanges) { r in + Text("\(r.anchorID) \(r.rangeMetres, specifier: "%.2f")m") + .font(.caption2) + .foregroundStyle(.secondary) + } + } + } + } + .padding(.horizontal, 10) + .padding(.vertical, 7) + .background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 10)) + .padding(.top, 56) // below status bar + .padding(.leading, 16) + } + + // MARK: - Re-centre button (top-right) + + private var recenterButton: some View { Button { followUser = true if let coord = sensor.userLocation { withAnimation { position = .camera(MapCamera( centerCoordinate: coord, - distance: 400, - heading: 0, - pitch: 0 + distance: 400, heading: 0, pitch: 0 )) } } @@ -128,24 +174,84 @@ struct MapContentView: View { .padding(10) .background(.ultraThinMaterial, in: Circle()) } - .padding([.top, .trailing], 16) - .padding(.top, 44) // below nav bar + .padding(.top, 56) + .padding(.trailing, 16) } - // MARK: - Stats overlay + // MARK: - Follow controls panel - private var overlay: some View { + private var followControls: some View { + VStack(spacing: 10) { + + // Follow mode: GPS | UWB + VStack(alignment: .leading, spacing: 4) { + Label("Follow Mode", systemImage: "location.circle") + .font(.caption.bold()) + .foregroundStyle(.secondary) + Picker("Follow Mode", selection: Binding( + get: { sensor.followMode }, + set: { sensor.setFollowMode($0) } + )) { + ForEach(FollowMode.allCases) { mode in + Text(mode.rawValue).tag(mode) + } + } + .pickerStyle(.segmented) + } + + // Follow range: Close | Medium | Far + VStack(alignment: .leading, spacing: 4) { + HStack { + Label("Follow Range", systemImage: "circle.dashed") + .font(.caption.bold()) + .foregroundStyle(.secondary) + Spacer() + Text("\(sensor.followPreset.metres, specifier: "%.1f") m") + .font(.caption.monospacedDigit()) + .foregroundStyle(.secondary) + } + Picker("Range", selection: Binding( + get: { sensor.followPreset }, + set: { sensor.setFollowPreset($0) } + )) { + ForEach(FollowPreset.allCases) { preset in + Text(preset.rawValue).tag(preset) + } + } + .pickerStyle(.segmented) + } + } + .padding(.horizontal, 16) + .padding(.vertical, 12) + .background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 16)) + .padding(.horizontal, 16) + .padding(.bottom, 8) + } + + // MARK: - Stats bar + + private var statsBar: some View { HStack(spacing: 20) { if let dist = sensor.distanceToRobot { - statCell(value: distanceString(dist), - label: "distance", - icon: "arrow.left.and.right") + statCell( + value: distanceString(dist), + label: "distance", + icon: insideFollowRange ? "checkmark.circle.fill" : "arrow.left.and.right", + tint: insideFollowRange ? .green : .primary + ) } if sensor.robotSpeed > 0.2 { statCell(value: String(format: "%.1f m/s", sensor.robotSpeed), label: "robot spd", icon: "speedometer") } + if sensor.followMode == .uwb, let pos = sensor.uwbPosition { + statCell( + value: String(format: "(%.1f, %.1f)", pos.x, pos.y), + label: "UWB pos", + icon: "waveform" + ) + } if !sensor.isStreaming { Text("Start Follow-Me to stream") .font(.caption) @@ -159,22 +265,25 @@ struct MapContentView: View { .padding(.horizontal, 16) } - private func statCell(value: String, label: String, icon: String) -> some View { + // MARK: - Helpers + + private func statCell(value: String, + label: String, + icon: String, + tint: Color = .primary) -> some View { HStack(spacing: 6) { Image(systemName: icon) - .foregroundStyle(.secondary) + .foregroundStyle(tint == .primary ? .secondary : tint) .font(.caption) VStack(alignment: .leading, spacing: 0) { - Text(value).font(.headline.monospacedDigit()) + Text(value).font(.headline.monospacedDigit()).foregroundStyle(tint) Text(label).font(.caption2).foregroundStyle(.secondary) } } } - private func distanceString(_ metres: Double) -> String { - metres < 1000 - ? "\(Int(metres)) m" - : String(format: "%.1f km", metres / 1000) + private func distanceString(_ m: Double) -> String { + m < 1000 ? "\(Int(m)) m" : String(format: "%.1f km", m / 1000) } } diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 41db57e..34a9121 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -4,8 +4,8 @@ import CoreMotion import MapKit import Combine -/// Manages all iPhone sensors, publishes iOS GPS to MQTT, subscribes to robot GPS, -/// and exposes state for the map and status views. +/// Manages all iPhone sensors, publishes iOS GPS to MQTT, subscribes to robot GPS +/// and UWB data, and exposes state for the map and status views. final class SensorManager: NSObject, ObservableObject { // MARK: - Streaming state @@ -25,19 +25,27 @@ final class SensorManager: NSObject, ObservableObject { @Published var userLocation: CLLocationCoordinate2D? = nil @Published var userBreadcrumbs: [CLLocationCoordinate2D] = [] - // MARK: - Robot position (from MQTT saltybot/phone/gps) + // MARK: - Robot position (saltybot/phone/gps) @Published var robotLocation: CLLocationCoordinate2D? = nil @Published var robotBreadcrumbs: [CLLocationCoordinate2D] = [] @Published var robotSpeed: Double = 0 + @Published var distanceToRobot: Double? = nil - // MARK: - Derived + // MARK: - UWB (saltybot/uwb/range + saltybot/uwb/position) - @Published var distanceToRobot: Double? = nil + @Published var uwbPosition: UWBPosition? = nil + @Published var uwbRanges: [String: UWBRange] = [:] // anchorID → UWBRange + @Published var uwbActive: Bool = false - // MARK: - WebSocket config (sensor stream to Orin) + // MARK: - Follow settings - static let defaultOrinURL = "ws://100.64.0.2:9090" + @Published var followMode: FollowMode = .gps + @Published var followPreset: FollowPreset = .medium + + // MARK: - WebSocket config + + static let defaultOrinURL = "ws://100.64.0.2:9090" private static let orinURLKey = "orinURL" private(set) var ws: WebSocketClient @@ -56,12 +64,19 @@ final class SensorManager: NSObject, ObservableObject { password: "mqtt_pass", clientID: "saul-t-mote-\(UUID().uuidString.prefix(8))" )) - 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? + private static let iosGPSTopic = "saltybot/ios/gps" + private static let robotGPSTopic = "saltybot/phone/gps" + private static let uwbRangeTopic = "saltybot/uwb/range" + private static let uwbPositionTopic = "saltybot/uwb/position" + private static let followModeTopic = "saltybot/follow/mode" + private static let followRangeTopic = "saltybot/follow/range" + private static let maxBreadcrumbs = 60 + private static let uwbStaleSeconds = 3.0 + + private var lastKnownLocation: CLLocation? + private var mqttPublishTimer: Timer? + private var uwbStalenessTimer: Timer? // MARK: - Sensors @@ -76,13 +91,13 @@ final class SensorManager: NSObject, ObservableObject { private var imuCounts: [Date] = [] private var headingCounts: [Date] = [] private var baroCounts: [Date] = [] - private var rateTimer: Timer? + private var rateTimer: Timer? // MARK: - Init override init() { - let urlString = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL - self.ws = WebSocketClient(url: URL(string: urlString) ?? URL(string: Self.defaultOrinURL)!) + let urlStr = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL + self.ws = WebSocketClient(url: URL(string: urlStr) ?? URL(string: Self.defaultOrinURL)!) super.init() locationManager.delegate = self @@ -98,8 +113,13 @@ final class SensorManager: NSObject, ObservableObject { .store(in: &cancellables) mqtt.onMessage = { [weak self] topic, payload in - guard let self, topic == Self.robotGPSTopic else { return } - self.handleRobotGPS(payload) + guard let self else { return } + switch topic { + case Self.robotGPSTopic: self.handleRobotGPS(payload) + case Self.uwbRangeTopic: self.handleUWBRange(payload) + case Self.uwbPositionTopic: self.handleUWBPosition(payload) + default: break + } } } @@ -111,9 +131,15 @@ final class SensorManager: NSObject, ObservableObject { ws.connect() mqtt.connect() mqtt.subscribe(topic: Self.robotGPSTopic) + mqtt.subscribe(topic: Self.uwbRangeTopic) + mqtt.subscribe(topic: Self.uwbPositionTopic) requestPermissionsAndStartSensors() startRateTimer() startMQTTPublishTimer() + startUWBStalenessTimer() + // Publish current follow settings immediately on connect + publishFollowMode() + publishFollowPreset() } func stopStreaming() { @@ -122,8 +148,9 @@ 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 + uwbStalenessTimer?.invalidate(); uwbStalenessTimer = nil } func updateURL(_ urlString: String) { @@ -134,6 +161,28 @@ final class SensorManager: NSObject, ObservableObject { } } + /// Change follow mode and publish to MQTT immediately. + func setFollowMode(_ mode: FollowMode) { + followMode = mode + publishFollowMode() + } + + /// Change follow range preset and publish to MQTT immediately. + func setFollowPreset(_ preset: FollowPreset) { + followPreset = preset + publishFollowPreset() + } + + // MARK: - MQTT publish helpers + + private func publishFollowMode() { + mqtt.publish(topic: Self.followModeTopic, payload: followMode.mqttPayload) + } + + private func publishFollowPreset() { + mqtt.publish(topic: Self.followRangeTopic, payload: followPreset.mqttPayload) + } + // MARK: - MQTT GPS publish (1 Hz) private func startMQTTPublishTimer() { @@ -159,14 +208,13 @@ final class SensorManager: NSObject, ObservableObject { mqtt.publish(topic: Self.iosGPSTopic, payload: json) } - // MARK: - Robot GPS subscription handler + // MARK: - Incoming MQTT handlers 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 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 @@ -174,6 +222,39 @@ final class SensorManager: NSObject, ObservableObject { updateDistance() } + private func handleUWBRange(_ payload: String) { + guard let data = payload.data(using: .utf8), + let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any], + let anchorID = json["anchor_id"] as? String, + let rangeM = json["range_m"] as? Double else { return } + uwbRanges[anchorID] = UWBRange(anchorID: anchorID, + rangeMetres: rangeM, + timestamp: Date()) + } + + private func handleUWBPosition(_ payload: String) { + guard let data = payload.data(using: .utf8), + let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any], + let x = json["x"] as? Double, + let y = json["y"] as? Double, + let z = json["z"] as? Double else { return } + uwbPosition = UWBPosition(x: x, y: y, z: z, timestamp: Date()) + uwbActive = true + } + + // MARK: - UWB staleness watchdog + + private func startUWBStalenessTimer() { + uwbStalenessTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in + guard let self else { return } + let cutoff = Date().addingTimeInterval(-Self.uwbStaleSeconds) + if let pos = self.uwbPosition, pos.timestamp < cutoff { + self.uwbActive = false + } + self.uwbRanges = self.uwbRanges.filter { $0.value.timestamp > cutoff } + } + } + // MARK: - Breadcrumbs + distance private func appendBreadcrumb(_ coord: CLLocationCoordinate2D, @@ -208,7 +289,7 @@ final class SensorManager: NSObject, ObservableObject { private func startIMU() { guard motionManager.isDeviceMotionAvailable else { return } - motionManager.deviceMotionUpdateInterval = 1.0 / 100.0 // 100 Hz + motionManager.deviceMotionUpdateInterval = 1.0 / 100.0 motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, _ in guard let self, let motion else { return } self.recordEvent(in: &self.imuCounts) @@ -287,15 +368,15 @@ extension SensorManager: CLLocationManagerDelegate { "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 ] ]) } diff --git a/SulTee/SulTee/UWBModels.swift b/SulTee/SulTee/UWBModels.swift new file mode 100644 index 0000000..90efff3 --- /dev/null +++ b/SulTee/SulTee/UWBModels.swift @@ -0,0 +1,51 @@ +import Foundation + +// MARK: - Follow mode + +enum FollowMode: String, CaseIterable, Identifiable { + case gps = "GPS" + case uwb = "UWB" + var id: String { rawValue } + + var mqttPayload: String { + "{\"mode\":\"\(rawValue.lowercased())\"}" + } +} + +// MARK: - Follow range preset + +enum FollowPreset: String, CaseIterable, Identifiable { + case close = "Close" + case medium = "Medium" + case far = "Far" + var id: String { rawValue } + + var metres: Double { + switch self { + case .close: return 1.5 + case .medium: return 3.0 + case .far: return 5.0 + } + } + + var mqttPayload: String { + let d: [String: Any] = ["range_m": metres, "preset": rawValue.lowercased()] + guard let data = try? JSONSerialization.data(withJSONObject: d), + let str = String(data: data, encoding: .utf8) else { return "{}" } + return str + } +} + +// MARK: - UWB data types + +struct UWBPosition { + let x, y, z: Double + let timestamp: Date +} + +struct UWBRange: Identifiable { + let anchorID: String + let rangeMetres: Double + let timestamp: Date + var id: String { anchorID } +}