User Story #73 » 0001-feat-engine-non-root-followers-adopt-the-root-s-cons.patch
| common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt | ||
|---|---|---|
|
/** This device's own target id, or null if not yet identified. */
|
||
|
fun selfId(): DeviceId? = selfTargetId
|
||
|
/**
|
||
|
* True when this device is a NON-ROOT follower of the shared frame — it must DEFER the anchor
|
||
|
* constellation solve to the ROOT's authoritative shape instead of re-deriving its own.
|
||
|
*
|
||
|
* Every device initializes its frame with the deterministically-elected root as the origin
|
||
|
* ([initializeAsRoot] with `currentAnchorId`, driven each platform roster tick), so ONLY the
|
||
|
* root has `selfTargetId == topology.frame.originDeviceId`. A follower already receives the
|
||
|
* root's broadcast solved positions and seats them as reference points; if it ALSO ran its own
|
||
|
* [bootstrapReferenceConstellation] / [refineAnchorConstellation] on its own biased ranges +
|
||
|
* local AoA vantage it would OVERRIDE those adopted positions with a DIVERGENT shape — the root
|
||
|
* showing one constellation and every other anchor a different one, none ever locking (the
|
||
|
* field failure). Deferring makes the whole mesh converge on the single ROOT-authored frame:
|
||
|
* self-solved, general to any solvable layout, no manual coordinates. Returns false (solve
|
||
|
* locally, as before) until BOTH a self id and a frame origin are known, so the root and every
|
||
|
* existing single-engine / test path stay byte-identical.
|
||
|
*/
|
||
|
private fun defersConstellationToRoot(): Boolean {
|
||
|
val self = selfTargetId ?: return false
|
||
|
val root = frameManager.topology?.frame?.originDeviceId ?: return false
|
||
|
return root != self
|
||
|
}
|
||
|
/** Latest physically-valid AoA azimuth (radians) THIS device measured to each peer anchor,
|
||
|
* captured in [maybeUpdateSelfYaw]. This is the heading reference the distance-only
|
||
|
* constellation solve lacks: ranges fix the anchor SHAPE, these bearings fix its yaw and
|
||
| ... | ... | |
|
// advertise it immediately rather than a tick late.
|
||
|
anyReferenceAnchorInMotion() -> ConstellationFrameState.RESOLVING_MOTION
|
||
|
frameEstablished -> ConstellationFrameState.LOCKED
|
||
|
// A non-root follower runs no local solve (it ADOPTS the root's frame), so [frameEstablished]
|
||
|
// never latches here. Once it has seated the root + peers as reference points it is on a
|
||
|
// stable SHARED frame — advertise LOCKED, not a perpetual CONVERGING, to the diagnostics and
|
||
|
// the yaw-reconcile gate. Gated on a populated reference set so a not-yet-seated follower
|
||
|
// still reads as acquiring.
|
||
|
defersConstellationToRoot() &&
|
||
|
frameManager.getAllReferencePoints().size >= config.minAnchorsForFusion ->
|
||
|
ConstellationFrameState.LOCKED
|
||
|
// Distinguish the INITIAL convergence (never locked — still gathering a complete, stable,
|
||
|
// trustworthy solve; the accuracy>speed wait) from RE-solving a frame that HAD been locked
|
||
|
// and was broken by an anchor drop / settling back after a disruption.
|
||
| ... | ... | |
|
if (frameEstablished) return 0
|
||
|
val topo = frameManager.topology ?: return 0
|
||
|
val rootId = topo.frame.originDeviceId
|
||
|
// A non-root follower ADOPTS the root's authoritative constellation; it must not re-refine
|
||
|
// its own reference points from local ranges. See [defersConstellationToRoot].
|
||
|
if (defersConstellationToRoot()) return 0
|
||
|
// The anchor constellation is the FULL reference-point set (the root plus every
|
||
|
// placed peer anchor), NOT topo.anchors. On-device the peer anchors are seeded as
|
||
|
// mesh points (registerMeshNode / placeReferenceAnchor), so topo.anchors holds ONLY
|
||
| ... | ... | |
|
val topo = frameManager.topology
|
||
|
?: run { pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "no-topology", 0, edgeCount, 0, emptyList()); return 0 }
|
||
|
val rootId = topo.frame.originDeviceId
|
||
|
// A NON-ROOT follower DEFERS to the root's authoritative constellation (already seated as
|
||
|
// reference points from the root's broadcast solve) instead of re-solving its own divergent
|
||
|
// shape from local biased ranges — the mesh-wide convergence fix. See [defersConstellationToRoot].
|
||
|
// The root (self == origin) is unaffected and still owns the solve.
|
||
|
if (defersConstellationToRoot()) {
|
||
|
val adopted = frameManager.getAllReferencePoints()
|
||
|
pipelineTrace?.constellationBootstrapped(
|
||
|
clock.now().microseconds, "adopt-root", adopted.size, edgeCount, 0,
|
||
|
adopted.map { MofePipelineTrace.RefPoint(it.id, it.position) },
|
||
|
)
|
||
|
return 0
|
||
|
}
|
||
|
// Anchor set = every node engaged in inter-anchor ranging, plus the root.
|
||
|
val ids = (interAnchorDistances.keys.flatMap { listOf(it.first, it.second) } + rootId).toSet()
|
||
|
if (ids.size < config.minAnchorsForFusion) {
|
||
| common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationAdoptionTest.kt | ||
|---|---|---|
|
package com.aether.mofe.engine
|
||
|
import com.aether.mofe.integration.MofeTestHarness
|
||
|
import com.aether.mofe.model.ConstellationFrameState
|
||
|
import com.aether.mofe.model.DeviceId
|
||
|
import com.aether.mofe.model.MofeConfig
|
||
|
import com.aether.mofe.model.Vector3D
|
||
|
import kotlin.test.Test
|
||
|
import kotlin.test.assertEquals
|
||
|
import kotlin.test.assertTrue
|
||
|
/**
|
||
|
* #61 — mesh-wide convergence. A NON-ROOT device ADOPTS the ROOT's authoritative constellation
|
||
|
* (the positions it already receives over the band and seats as reference points) and DEFERS its
|
||
|
* own local solve, instead of every device independently re-deriving a divergent shape from its
|
||
|
* own biased ranges + local AoA vantage. Survey-free — no manual coordinates; the source is the
|
||
|
* root's own self-solved frame.
|
||
|
*
|
||
|
* Guards the two failures seen on-device (capture4): the root showing one layout while each other
|
||
|
* anchor showed a DIFFERENT one, and no device's frame ever locking (`CONVERGING` forever).
|
||
|
*
|
||
|
* NOTE: the helpers are plain functions taking the harness, NOT extensions on `MofeTestHarness` —
|
||
|
* the harness has its own `root` member that would shadow this class's `root` inside a receiver.
|
||
|
*/
|
||
|
class ConstellationAdoptionTest {
|
||
|
private val root = DeviceId("A1") // deterministically-elected root == frame origin
|
||
|
private val a2 = DeviceId("A2")
|
||
|
private val a3 = DeviceId("A3")
|
||
|
private val a4 = DeviceId("A4")
|
||
|
/** The ROOT's authoritative, broadcast constellation (root at the origin). */
|
||
|
private val authoritative = mapOf(
|
||
|
root to Vector3D(0.0, 0.0, 0.0),
|
||
|
a2 to Vector3D(2.0, 0.0, 0.0),
|
||
|
a3 to Vector3D(0.0, 2.0, 0.0),
|
||
|
a4 to Vector3D(1.0, 1.0, 0.6),
|
||
|
)
|
||
|
private val ids = listOf(root, a2, a3, a4)
|
||
|
/** Seat the root's authoritative positions as this device's reference points — what the
|
||
|
* platform roster tick does via placeReferenceAnchor from the root's broadcast solve
|
||
|
* (registering the origin itself is a no-op, mirroring the on-device guard). */
|
||
|
private fun adoptRootFrame(h: MofeTestHarness) {
|
||
|
for ((id, pos) in authoritative) h.engine.registerReferenceAnchor(id, pos)
|
||
|
}
|
||
|
/** Feed inter-anchor ranges for a shape into the engine (populates the local solve inputs). */
|
||
|
private fun feedRanges(h: MofeTestHarness, shape: Map<DeviceId, Vector3D>, rounds: Int = 10) {
|
||
|
repeat(rounds) {
|
||
|
for (i in ids.indices) for (j in i + 1 until ids.size) {
|
||
|
h.engine.processInterAnchorRanging(
|
||
|
ids[i], ids[j], shape.getValue(ids[i]).distanceTo(shape.getValue(ids[j])))
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
/** A CONFLICTING shape — the biased local ranges that, if the follower were allowed to solve,
|
||
|
* would place a DIFFERENT constellation and override the adopted one. */
|
||
|
private val rogue = mapOf(
|
||
|
root to Vector3D(0.0, 0.0, 0.0),
|
||
|
a2 to Vector3D(3.0, 0.0, 0.0), // ~1 m longer than authoritative
|
||
|
a3 to Vector3D(0.0, 3.0, 0.0),
|
||
|
a4 to Vector3D(1.5, 1.5, 1.2),
|
||
|
)
|
||
|
@Test
|
||
|
fun follower_adopts_root_constellation_and_defers_its_local_solve() {
|
||
|
val h = MofeTestHarness(MofeConfig()).build()
|
||
|
h.engine.initializeAsRoot(root) // every device inits its frame with the ELECTED root as origin
|
||
|
h.engine.setSelfId(a2) // …but THIS device is A2, a NON-root follower
|
||
|
adoptRootFrame(h)
|
||
|
// (1) the adopted frame is present, and reads as a stable SHARED frame (not perpetual CONVERGING)
|
||
|
val adopted = h.frameManager.getAllReferencePoints().associate { it.id to it.position }
|
||
|
assertEquals(authoritative.keys, adopted.keys, "follower must hold the full root constellation")
|
||
|
for ((id, p) in authoritative) {
|
||
|
assertTrue(adopted.getValue(id).distanceTo(p) < 1e-9, "$id must match the root's position")
|
||
|
}
|
||
|
assertEquals(
|
||
|
ConstellationFrameState.LOCKED, h.engine.constellationFrameState(),
|
||
|
"a follower on the adopted root frame advertises LOCKED, not CONVERGING",
|
||
|
)
|
||
|
// (2) the follower's own solve DEFERS. Give it a COMPLETE, solvable — but CONFLICTING — set of
|
||
|
// inter-anchor ranges: un-gated it WOULD re-solve to the rogue shape and overwrite the frame.
|
||
|
feedRanges(h, rogue)
|
||
|
assertEquals(0, h.engine.refineAnchorConstellation(), "follower must not refine its own frame")
|
||
|
repeat(5) {
|
||
|
assertEquals(
|
||
|
0, h.engine.bootstrapReferenceConstellation(),
|
||
|
"follower must not re-solve its own frame despite complete local ranges",
|
||
|
)
|
||
|
}
|
||
|
val after = h.frameManager.getAllReferencePoints().associate { it.id to it.position }
|
||
|
for ((id, p) in authoritative) {
|
||
|
assertTrue(
|
||
|
after.getValue(id).distanceTo(p) < 1e-9,
|
||
|
"$id must stay on the adopted root position, not the rogue local solve",
|
||
|
)
|
||
|
}
|
||
|
assertEquals(ConstellationFrameState.LOCKED, h.engine.constellationFrameState())
|
||
|
}
|
||
|
@Test
|
||
|
fun root_still_solves_its_own_constellation() {
|
||
|
// The gate is role-specific: the ROOT (self == origin) stays byte-identical — with the SAME
|
||
|
// range feed that a follower ignores, the root DOES solve + seat its constellation.
|
||
|
val h = MofeTestHarness(MofeConfig()).build()
|
||
|
h.engine.initializeAsRoot(root)
|
||
|
h.engine.setSelfId(root) // this device IS the root
|
||
|
feedRanges(h, authoritative, rounds = 8)
|
||
|
assertTrue(
|
||
|
h.engine.bootstrapReferenceConstellation() > 0,
|
||
|
"the root must still solve + seat its constellation from inter-anchor ranges",
|
||
|
)
|
||
|
}
|
||
|
}
|
||