feat: Add UWB integration — follow mode, range presets, UWB status
UWBModels.swift (new):
- FollowMode enum: .gps / .uwb — publishes {"mode":"gps|uwb"} to saltybot/follow/mode
- FollowPreset enum: .close(1.5m) / .medium(3m) / .far(5m) — publishes
{"range_m":N,"preset":"..."} to saltybot/follow/range
- UWBPosition struct: x/y/z + timestamp
- UWBRange struct: anchorID + rangeMetres + timestamp
SensorManager:
- Subscribes to saltybot/uwb/range + saltybot/uwb/position on startStreaming
- handleUWBRange: updates uwbRanges[anchorID] (keyed dict)
- handleUWBPosition: updates uwbPosition + sets uwbActive=true
- UWB staleness watchdog (1Hz timer): clears uwbActive and prunes stale ranges >3s
- setFollowMode(_:) / setFollowPreset(_:): update state + publish to MQTT immediately
- Publishes current follow mode+range on connect
MapContentView:
- UWB status badge (top-left): green/gray dot, "UWB Active|Out of Range",
per-anchor range readouts (e.g. A1 2.34m)
- Follow mode segmented control: GPS | UWB
- Follow range segmented control: Close | Medium | Far (shows metres)
- MapCircle follow-range ring around robot: green inside, orange outside range
- Stats bar: distance turns green + checkmark when user is inside follow range;
UWB (x,y) coord shown when UWB mode active and position known
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
72e3138fb3
commit
4eb2fce08f
@ -14,6 +14,7 @@
|
|||||||
A100000100000000000005AA /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = A100000100000000000005AB /* Assets.xcassets */; };
|
A100000100000000000005AA /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = A100000100000000000005AB /* Assets.xcassets */; };
|
||||||
A100000100000000000009AA /* MQTTClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000009AB /* MQTTClient.swift */; };
|
A100000100000000000009AA /* MQTTClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000009AB /* MQTTClient.swift */; };
|
||||||
A10000010000000000000AAA /* MapContentView.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000AAB /* MapContentView.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 */
|
/* End PBXBuildFile section */
|
||||||
|
|
||||||
/* Begin PBXFileReference 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; };
|
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>"; };
|
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>"; };
|
A10000010000000000000AAB /* MapContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MapContentView.swift; sourceTree = "<group>"; };
|
||||||
|
A10000010000000000000BAB /* UWBModels.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = UWBModels.swift; sourceTree = "<group>"; };
|
||||||
/* End PBXFileReference section */
|
/* End PBXFileReference section */
|
||||||
|
|
||||||
/* Begin PBXFrameworksBuildPhase section */
|
/* Begin PBXFrameworksBuildPhase section */
|
||||||
@ -56,6 +58,7 @@
|
|||||||
A100000100000000000004AB /* WebSocketClient.swift */,
|
A100000100000000000004AB /* WebSocketClient.swift */,
|
||||||
A100000100000000000009AB /* MQTTClient.swift */,
|
A100000100000000000009AB /* MQTTClient.swift */,
|
||||||
A10000010000000000000AAB /* MapContentView.swift */,
|
A10000010000000000000AAB /* MapContentView.swift */,
|
||||||
|
A10000010000000000000BAB /* UWBModels.swift */,
|
||||||
A100000100000000000005AB /* Assets.xcassets */,
|
A100000100000000000005AB /* Assets.xcassets */,
|
||||||
A100000100000000000006AB /* Info.plist */,
|
A100000100000000000006AB /* Info.plist */,
|
||||||
);
|
);
|
||||||
@ -146,6 +149,7 @@
|
|||||||
A100000100000000000004AA /* WebSocketClient.swift in Sources */,
|
A100000100000000000004AA /* WebSocketClient.swift in Sources */,
|
||||||
A100000100000000000009AA /* MQTTClient.swift in Sources */,
|
A100000100000000000009AA /* MQTTClient.swift in Sources */,
|
||||||
A10000010000000000000AAA /* MapContentView.swift in Sources */,
|
A10000010000000000000AAA /* MapContentView.swift in Sources */,
|
||||||
|
A10000010000000000000BAA /* UWBModels.swift in Sources */,
|
||||||
);
|
);
|
||||||
runOnlyForDeploymentPostprocessing = 0;
|
runOnlyForDeploymentPostprocessing = 0;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -1,18 +1,33 @@
|
|||||||
import SwiftUI
|
import SwiftUI
|
||||||
import MapKit
|
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,
|
/// 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 {
|
struct MapContentView: View {
|
||||||
@EnvironmentObject var sensor: SensorManager
|
@EnvironmentObject var sensor: SensorManager
|
||||||
|
|
||||||
@State private var position: MapCameraPosition = .automatic
|
@State private var position: MapCameraPosition = .automatic
|
||||||
@State private var followUser = true
|
@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 {
|
var body: some View {
|
||||||
ZStack(alignment: .bottom) {
|
ZStack(alignment: .bottom) {
|
||||||
map
|
map
|
||||||
overlay
|
VStack(spacing: 0) {
|
||||||
|
Spacer()
|
||||||
|
followControls
|
||||||
|
statsBar
|
||||||
|
}
|
||||||
}
|
}
|
||||||
.ignoresSafeArea(edges: .top)
|
.ignoresSafeArea(edges: .top)
|
||||||
.onChange(of: sensor.userLocation) { _, coord in
|
.onChange(of: sensor.userLocation) { _, coord in
|
||||||
@ -20,20 +35,32 @@ struct MapContentView: View {
|
|||||||
withAnimation(.easeInOut(duration: 0.4)) {
|
withAnimation(.easeInOut(duration: 0.4)) {
|
||||||
position = .camera(MapCamera(
|
position = .camera(MapCamera(
|
||||||
centerCoordinate: coord,
|
centerCoordinate: coord,
|
||||||
distance: 400,
|
distance: 400, heading: 0, pitch: 0
|
||||||
heading: 0,
|
|
||||||
pitch: 0
|
|
||||||
))
|
))
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// MARK: - Map
|
// MARK: - Map content
|
||||||
|
|
||||||
private var map: some View {
|
private var map: some View {
|
||||||
Map(position: $position) {
|
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)
|
// ── User breadcrumb trail (fading blue dots)
|
||||||
let userCrumbs = sensor.userBreadcrumbs
|
let userCrumbs = sensor.userBreadcrumbs
|
||||||
ForEach(userCrumbs.indices, id: \.self) { idx in
|
ForEach(userCrumbs.indices, id: \.self) { idx in
|
||||||
@ -69,15 +96,9 @@ struct MapContentView: View {
|
|||||||
if let userLoc = sensor.userLocation {
|
if let userLoc = sensor.userLocation {
|
||||||
Annotation("You", coordinate: userLoc) {
|
Annotation("You", coordinate: userLoc) {
|
||||||
ZStack {
|
ZStack {
|
||||||
Circle()
|
Circle().fill(.blue.opacity(0.25)).frame(width: 36, height: 36)
|
||||||
.fill(.blue.opacity(0.25))
|
Circle().fill(.blue).frame(width: 16, height: 16)
|
||||||
.frame(width: 36, height: 36)
|
Circle().stroke(.white, lineWidth: 2.5).frame(width: 16, height: 16)
|
||||||
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 {
|
if let robotLoc = sensor.robotLocation {
|
||||||
Annotation("Robot", coordinate: robotLoc) {
|
Annotation("Robot", coordinate: robotLoc) {
|
||||||
ZStack {
|
ZStack {
|
||||||
Circle()
|
Circle().fill(.orange.opacity(0.25)).frame(width: 36, height: 36)
|
||||||
.fill(.orange.opacity(0.25))
|
|
||||||
.frame(width: 36, height: 36)
|
|
||||||
Image(systemName: "car.fill")
|
Image(systemName: "car.fill")
|
||||||
.font(.system(size: 16, weight: .bold))
|
.font(.system(size: 16, weight: .bold))
|
||||||
.foregroundStyle(.white)
|
.foregroundStyle(.white)
|
||||||
@ -99,27 +118,54 @@ struct MapContentView: View {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
.mapStyle(.standard(elevation: .realistic))
|
.mapStyle(.standard(elevation: .realistic))
|
||||||
.onMapCameraChange { _ in
|
.onMapCameraChange { _ in followUser = false }
|
||||||
// User dragged map → stop auto-follow
|
.overlay(alignment: .topTrailing) { recenterButton }
|
||||||
followUser = false
|
.overlay(alignment: .topLeading) { uwbBadge }
|
||||||
}
|
|
||||||
.overlay(alignment: .topTrailing) {
|
|
||||||
followButton
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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 {
|
Button {
|
||||||
followUser = true
|
followUser = true
|
||||||
if let coord = sensor.userLocation {
|
if let coord = sensor.userLocation {
|
||||||
withAnimation {
|
withAnimation {
|
||||||
position = .camera(MapCamera(
|
position = .camera(MapCamera(
|
||||||
centerCoordinate: coord,
|
centerCoordinate: coord,
|
||||||
distance: 400,
|
distance: 400, heading: 0, pitch: 0
|
||||||
heading: 0,
|
|
||||||
pitch: 0
|
|
||||||
))
|
))
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -128,24 +174,84 @@ struct MapContentView: View {
|
|||||||
.padding(10)
|
.padding(10)
|
||||||
.background(.ultraThinMaterial, in: Circle())
|
.background(.ultraThinMaterial, in: Circle())
|
||||||
}
|
}
|
||||||
.padding([.top, .trailing], 16)
|
.padding(.top, 56)
|
||||||
.padding(.top, 44) // below nav bar
|
.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) {
|
HStack(spacing: 20) {
|
||||||
if let dist = sensor.distanceToRobot {
|
if let dist = sensor.distanceToRobot {
|
||||||
statCell(value: distanceString(dist),
|
statCell(
|
||||||
|
value: distanceString(dist),
|
||||||
label: "distance",
|
label: "distance",
|
||||||
icon: "arrow.left.and.right")
|
icon: insideFollowRange ? "checkmark.circle.fill" : "arrow.left.and.right",
|
||||||
|
tint: insideFollowRange ? .green : .primary
|
||||||
|
)
|
||||||
}
|
}
|
||||||
if sensor.robotSpeed > 0.2 {
|
if sensor.robotSpeed > 0.2 {
|
||||||
statCell(value: String(format: "%.1f m/s", sensor.robotSpeed),
|
statCell(value: String(format: "%.1f m/s", sensor.robotSpeed),
|
||||||
label: "robot spd",
|
label: "robot spd",
|
||||||
icon: "speedometer")
|
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 {
|
if !sensor.isStreaming {
|
||||||
Text("Start Follow-Me to stream")
|
Text("Start Follow-Me to stream")
|
||||||
.font(.caption)
|
.font(.caption)
|
||||||
@ -159,22 +265,25 @@ struct MapContentView: View {
|
|||||||
.padding(.horizontal, 16)
|
.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) {
|
HStack(spacing: 6) {
|
||||||
Image(systemName: icon)
|
Image(systemName: icon)
|
||||||
.foregroundStyle(.secondary)
|
.foregroundStyle(tint == .primary ? .secondary : tint)
|
||||||
.font(.caption)
|
.font(.caption)
|
||||||
VStack(alignment: .leading, spacing: 0) {
|
VStack(alignment: .leading, spacing: 0) {
|
||||||
Text(value).font(.headline.monospacedDigit())
|
Text(value).font(.headline.monospacedDigit()).foregroundStyle(tint)
|
||||||
Text(label).font(.caption2).foregroundStyle(.secondary)
|
Text(label).font(.caption2).foregroundStyle(.secondary)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private func distanceString(_ metres: Double) -> String {
|
private func distanceString(_ m: Double) -> String {
|
||||||
metres < 1000
|
m < 1000 ? "\(Int(m)) m" : String(format: "%.1f km", m / 1000)
|
||||||
? "\(Int(metres)) m"
|
|
||||||
: String(format: "%.1f km", metres / 1000)
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -4,8 +4,8 @@ import CoreMotion
|
|||||||
import MapKit
|
import MapKit
|
||||||
import Combine
|
import Combine
|
||||||
|
|
||||||
/// Manages all iPhone sensors, publishes iOS GPS to MQTT, subscribes to robot GPS,
|
/// Manages all iPhone sensors, publishes iOS GPS to MQTT, subscribes to robot GPS
|
||||||
/// and exposes state for the map and status views.
|
/// and UWB data, and exposes state for the map and status views.
|
||||||
final class SensorManager: NSObject, ObservableObject {
|
final class SensorManager: NSObject, ObservableObject {
|
||||||
|
|
||||||
// MARK: - Streaming state
|
// MARK: - Streaming state
|
||||||
@ -25,17 +25,25 @@ final class SensorManager: NSObject, ObservableObject {
|
|||||||
@Published var userLocation: CLLocationCoordinate2D? = nil
|
@Published var userLocation: CLLocationCoordinate2D? = nil
|
||||||
@Published var userBreadcrumbs: [CLLocationCoordinate2D] = []
|
@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 robotLocation: CLLocationCoordinate2D? = nil
|
||||||
@Published var robotBreadcrumbs: [CLLocationCoordinate2D] = []
|
@Published var robotBreadcrumbs: [CLLocationCoordinate2D] = []
|
||||||
@Published var robotSpeed: Double = 0
|
@Published var robotSpeed: Double = 0
|
||||||
|
|
||||||
// MARK: - Derived
|
|
||||||
|
|
||||||
@Published var distanceToRobot: Double? = nil
|
@Published var distanceToRobot: Double? = nil
|
||||||
|
|
||||||
// MARK: - WebSocket config (sensor stream to Orin)
|
// MARK: - UWB (saltybot/uwb/range + saltybot/uwb/position)
|
||||||
|
|
||||||
|
@Published var uwbPosition: UWBPosition? = nil
|
||||||
|
@Published var uwbRanges: [String: UWBRange] = [:] // anchorID → UWBRange
|
||||||
|
@Published var uwbActive: Bool = false
|
||||||
|
|
||||||
|
// MARK: - Follow settings
|
||||||
|
|
||||||
|
@Published var followMode: FollowMode = .gps
|
||||||
|
@Published var followPreset: FollowPreset = .medium
|
||||||
|
|
||||||
|
// MARK: - WebSocket config
|
||||||
|
|
||||||
static let defaultOrinURL = "ws://100.64.0.2:9090"
|
static let defaultOrinURL = "ws://100.64.0.2:9090"
|
||||||
private static let orinURLKey = "orinURL"
|
private static let orinURLKey = "orinURL"
|
||||||
@ -56,12 +64,19 @@ final class SensorManager: NSObject, ObservableObject {
|
|||||||
password: "mqtt_pass",
|
password: "mqtt_pass",
|
||||||
clientID: "saul-t-mote-\(UUID().uuidString.prefix(8))"
|
clientID: "saul-t-mote-\(UUID().uuidString.prefix(8))"
|
||||||
))
|
))
|
||||||
|
|
||||||
private static let iosGPSTopic = "saltybot/ios/gps"
|
private static let iosGPSTopic = "saltybot/ios/gps"
|
||||||
private static let robotGPSTopic = "saltybot/phone/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 maxBreadcrumbs = 60
|
||||||
|
private static let uwbStaleSeconds = 3.0
|
||||||
|
|
||||||
private var lastKnownLocation: CLLocation?
|
private var lastKnownLocation: CLLocation?
|
||||||
private var mqttPublishTimer: Timer?
|
private var mqttPublishTimer: Timer?
|
||||||
|
private var uwbStalenessTimer: Timer?
|
||||||
|
|
||||||
// MARK: - Sensors
|
// MARK: - Sensors
|
||||||
|
|
||||||
@ -81,8 +96,8 @@ final class SensorManager: NSObject, ObservableObject {
|
|||||||
// MARK: - Init
|
// MARK: - Init
|
||||||
|
|
||||||
override init() {
|
override init() {
|
||||||
let urlString = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL
|
let urlStr = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL
|
||||||
self.ws = WebSocketClient(url: URL(string: urlString) ?? URL(string: Self.defaultOrinURL)!)
|
self.ws = WebSocketClient(url: URL(string: urlStr) ?? URL(string: Self.defaultOrinURL)!)
|
||||||
super.init()
|
super.init()
|
||||||
|
|
||||||
locationManager.delegate = self
|
locationManager.delegate = self
|
||||||
@ -98,8 +113,13 @@ final class SensorManager: NSObject, ObservableObject {
|
|||||||
.store(in: &cancellables)
|
.store(in: &cancellables)
|
||||||
|
|
||||||
mqtt.onMessage = { [weak self] topic, payload in
|
mqtt.onMessage = { [weak self] topic, payload in
|
||||||
guard let self, topic == Self.robotGPSTopic else { return }
|
guard let self else { return }
|
||||||
self.handleRobotGPS(payload)
|
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()
|
ws.connect()
|
||||||
mqtt.connect()
|
mqtt.connect()
|
||||||
mqtt.subscribe(topic: Self.robotGPSTopic)
|
mqtt.subscribe(topic: Self.robotGPSTopic)
|
||||||
|
mqtt.subscribe(topic: Self.uwbRangeTopic)
|
||||||
|
mqtt.subscribe(topic: Self.uwbPositionTopic)
|
||||||
requestPermissionsAndStartSensors()
|
requestPermissionsAndStartSensors()
|
||||||
startRateTimer()
|
startRateTimer()
|
||||||
startMQTTPublishTimer()
|
startMQTTPublishTimer()
|
||||||
|
startUWBStalenessTimer()
|
||||||
|
// Publish current follow settings immediately on connect
|
||||||
|
publishFollowMode()
|
||||||
|
publishFollowPreset()
|
||||||
}
|
}
|
||||||
|
|
||||||
func stopStreaming() {
|
func stopStreaming() {
|
||||||
@ -124,6 +150,7 @@ final class SensorManager: NSObject, ObservableObject {
|
|||||||
stopSensors()
|
stopSensors()
|
||||||
rateTimer?.invalidate(); rateTimer = nil
|
rateTimer?.invalidate(); rateTimer = nil
|
||||||
mqttPublishTimer?.invalidate(); mqttPublishTimer = nil
|
mqttPublishTimer?.invalidate(); mqttPublishTimer = nil
|
||||||
|
uwbStalenessTimer?.invalidate(); uwbStalenessTimer = nil
|
||||||
}
|
}
|
||||||
|
|
||||||
func updateURL(_ urlString: String) {
|
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)
|
// MARK: - MQTT GPS publish (1 Hz)
|
||||||
|
|
||||||
private func startMQTTPublishTimer() {
|
private func startMQTTPublishTimer() {
|
||||||
@ -159,14 +208,13 @@ final class SensorManager: NSObject, ObservableObject {
|
|||||||
mqtt.publish(topic: Self.iosGPSTopic, payload: json)
|
mqtt.publish(topic: Self.iosGPSTopic, payload: json)
|
||||||
}
|
}
|
||||||
|
|
||||||
// MARK: - Robot GPS subscription handler
|
// MARK: - Incoming MQTT handlers
|
||||||
|
|
||||||
private func handleRobotGPS(_ payload: String) {
|
private func handleRobotGPS(_ payload: String) {
|
||||||
guard let data = payload.data(using: .utf8),
|
guard let data = payload.data(using: .utf8),
|
||||||
let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any],
|
let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any],
|
||||||
let lat = json["lat"] as? Double,
|
let lat = json["lat"] as? Double,
|
||||||
let lon = json["lon"] as? Double else { return }
|
let lon = json["lon"] as? Double else { return }
|
||||||
|
|
||||||
let coord = CLLocationCoordinate2D(latitude: lat, longitude: lon)
|
let coord = CLLocationCoordinate2D(latitude: lat, longitude: lon)
|
||||||
robotLocation = coord
|
robotLocation = coord
|
||||||
robotSpeed = (json["speed_ms"] as? Double) ?? 0
|
robotSpeed = (json["speed_ms"] as? Double) ?? 0
|
||||||
@ -174,6 +222,39 @@ final class SensorManager: NSObject, ObservableObject {
|
|||||||
updateDistance()
|
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
|
// MARK: - Breadcrumbs + distance
|
||||||
|
|
||||||
private func appendBreadcrumb(_ coord: CLLocationCoordinate2D,
|
private func appendBreadcrumb(_ coord: CLLocationCoordinate2D,
|
||||||
@ -208,7 +289,7 @@ final class SensorManager: NSObject, ObservableObject {
|
|||||||
|
|
||||||
private func startIMU() {
|
private func startIMU() {
|
||||||
guard motionManager.isDeviceMotionAvailable else { return }
|
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
|
motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, _ in
|
||||||
guard let self, let motion else { return }
|
guard let self, let motion else { return }
|
||||||
self.recordEvent(in: &self.imuCounts)
|
self.recordEvent(in: &self.imuCounts)
|
||||||
@ -290,7 +371,7 @@ extension SensorManager: CLLocationManagerDelegate {
|
|||||||
"latitude": loc.coordinate.latitude,
|
"latitude": loc.coordinate.latitude,
|
||||||
"longitude": loc.coordinate.longitude,
|
"longitude": loc.coordinate.longitude,
|
||||||
"altitude": loc.altitude,
|
"altitude": loc.altitude,
|
||||||
"horizontalAccuracy":loc.horizontalAccuracy,
|
"horizontalAccuracy": loc.horizontalAccuracy,
|
||||||
"verticalAccuracy": loc.verticalAccuracy,
|
"verticalAccuracy": loc.verticalAccuracy,
|
||||||
"speed": loc.speed,
|
"speed": loc.speed,
|
||||||
"speedAccuracy": loc.speedAccuracy,
|
"speedAccuracy": loc.speedAccuracy,
|
||||||
|
|||||||
51
SulTee/SulTee/UWBModels.swift
Normal file
51
SulTee/SulTee/UWBModels.swift
Normal file
@ -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 }
|
||||||
|
}
|
||||||
Loading…
x
Reference in New Issue
Block a user