Coordinator¶
coordination_oru.abstract_trajectory_envelope_coordinator
¶
AbstractTrajectoryEnvelopeCoordinator: coordination for a fleet of robots.
Ported from the Java class of the same name. Java's synchronized(solver)
(which nearly every public method wraps its whole body in, and which nests
synchronized(trackers)/synchronized(allCriticalSections)/
synchronized(currentDependencies)/synchronized(stoppingPoints)
inside it) becomes a single asyncio.Lock acquired at the outer entry
points (the coordination tick, addMissions, placeRobot, and the
tracker-finished callback); asyncio is cooperative and none of the ported
logic below awaits mid-section, so one lock at the outer boundary gives the
same atomicity as Java's nested monitors without risking deadlock from
non-reentrant re-acquisition.
Java's per-robot Threads (the coordinator inference thread, tracker
threads, stopping-point waiting threads) become asyncio.Tasks.
Java's ground-envelope/sub-envelope temporal dispatch machinery (deadlines,
getAllSubEnvelopes) is dropped — missions here are always a single flat
envelope, so it has no observable effect; see
:mod:coordination_oru.abstract_trajectory_envelope_tracker.
log = get_logger(__name__)
module-attribute
¶
PARKING_DURATION = 3000
module-attribute
¶
DEFAULT_STOPPING_TIME = 5000
module-attribute
¶
DEFAULT_ROBOT_TRACKING_PERIOD = 30
module-attribute
¶
TRAILING_PATH_POINTS = 3
module-attribute
¶
CriticalSection
¶
te1 = te1
instance-attribute
¶
te2 = te2
instance-attribute
¶
te1Start = te1Start
instance-attribute
¶
te2Start = te2Start
instance-attribute
¶
te1End = te1End
instance-attribute
¶
te2End = te2End
instance-attribute
¶
te1Break = -1
instance-attribute
¶
te2Break = -1
instance-attribute
¶
getTe1()
¶
getTe2()
¶
getTe1Start()
¶
getTe2Start()
¶
getTe1End()
¶
getTe2End()
¶
getTe1Break()
¶
getTe2Break()
¶
setTe1Break(te1Break)
¶
setTe2Break(te2Break)
¶
Dependency
¶
teWaiting = teWaiting
instance-attribute
¶
teDriving = teDriving
instance-attribute
¶
waitingPoint = waitingPoint
instance-attribute
¶
thresholdPoint = thresholdPoint
instance-attribute
¶
robotIDWaiting = teWaiting.getRobotID()
instance-attribute
¶
robotIDDriving = teDriving.getRobotID() if teDriving is not None else 0
instance-attribute
¶
compareTo(other)
¶
getWaitingPose()
¶
getReleasingPose()
¶
getWaitingTrajectoryEnvelope()
¶
getDrivingTrajectoryEnvelope()
¶
getWaitingPoint()
¶
getReleasingPoint()
¶
getWaitingRobotID()
¶
getDrivingRobotID()
¶
ForwardModel
¶
Pose
dataclass
¶
x
instance-attribute
¶
y
instance-attribute
¶
theta
instance-attribute
¶
z = math.nan
class-attribute
instance-attribute
¶
roll = math.nan
class-attribute
instance-attribute
¶
pitch = math.nan
class-attribute
instance-attribute
¶
is_3d()
¶
distance_xy(other)
¶
getX()
¶
getY()
¶
getTheta()
¶
distanceTo(other)
¶
interpolate(other, ratio)
¶
Linear interpolation towards other; theta via shortest arc.
TrajectoryEnvelope
dataclass
¶
A robot's planned trajectory expressed as an STP-aware swept envelope.
envelope_id is assigned by the
:class:~coordination_oru.metacsp.spatial.trajectory_envelope_solver.TrajectoryEnvelopeSolver
that creates it; start_node / end_node are the STP variable indices
for this envelope's start and end times.
envelope_id
instance-attribute
¶
robot_id
instance-attribute
¶
path
instance-attribute
¶
start_node
instance-attribute
¶
end_node
instance-attribute
¶
spatial_envelope
instance-attribute
¶
footprint
instance-attribute
¶
component = 'Driving'
class-attribute
instance-attribute
¶
nominal_duration = 0.0
class-attribute
instance-attribute
¶
completed = False
class-attribute
instance-attribute
¶
metadata = field(default_factory=dict)
class-attribute
instance-attribute
¶
length
property
¶
pose_at(index)
¶
waypoint_footprint(index)
¶
getID()
¶
getRobotID()
¶
getPathLength()
¶
getSpatialEnvelope()
¶
getFootprint()
¶
getTrajectory()
¶
makeFootprint(ps)
¶
getComponent()
¶
getSequenceNumberStart()
¶
getSequenceNumberEnd()
¶
getSequenceNumber(x, y)
¶
Index of the path point closest to (x, y).
Mirrors Java's getSequenceNumber(Coordinate) used to locate
stopping points along the path.
TrajectoryEnvelopeSolver
dataclass
¶
Tracks envelopes and exposes timing queries through an STP network.
max_envelopes = 64
class-attribute
instance-attribute
¶
stp = field(init=False)
class-attribute
instance-attribute
¶
create_envelope(robot_id, path, footprint, *, nominal_duration=math.nan, earliest_start=None, latest_start=None)
¶
createEnvelopeNoParking(robotID, path, component, footprint)
¶
createParkingEnvelope(robotID, duration, pose, location, footprint)
¶
envelopes()
¶
all_envelopes()
¶
get(envelope_id)
¶
mark_completed(envelope_id)
¶
Retire an envelope: flag it (envelopes() excludes it from the
active set consumed by critical-section/dependency computation, but
it stays in all_envelopes() for introspection) and detach its
two STP variables (remove_variable — ports Java's
removeConstraints + removeVariable), freeing their slots for
reuse rather than letting the temporal network's working set grow
for the coordinator's entire lifetime.
add_ordering(first, second, *, gap_lb=0.0, gap_ub=math.inf)
¶
Constrain first to finish before second starts.
gap_lb / gap_ub define the allowed gap between
end(first) and start(second). Defaults to "any non-negative
gap" — the most common case for critical-section serialisation.
add_allen_constraint(rel, a, b, bounds=None)
¶
earliest_start(envelope)
¶
latest_start(envelope)
¶
earliest_end(envelope)
¶
latest_end(envelope)
¶
is_consistent()
¶
Mission
¶
robotID = robotID
instance-attribute
¶
path = tuple(path)
instance-attribute
¶
order = next(_mission_order_counter)
instance-attribute
¶
fromLocation = fromLocation if fromLocation is not None else str(self.path[0].getPose())
instance-attribute
¶
toLocation = toLocation if toLocation is not None else str(self.path[-1].getPose())
instance-attribute
¶
fromPose = fromPose if fromPose is not None else self.path[0].getPose()
instance-attribute
¶
toPose = toPose if toPose is not None else self.path[-1].getPose()
instance-attribute
¶
stoppingPoints = []
instance-attribute
¶
stoppingPointDurations = []
instance-attribute
¶
compareTo(other)
¶
setStoppingPoint(pose, duration)
¶
clearStoppingPoints()
¶
getStoppingPoints()
¶
setToLocation(location)
¶
setFromLocation(location)
¶
getRobotID()
¶
getPath()
¶
setPath(path)
¶
getFromLocation()
¶
getToLocation()
¶
getFromPose()
¶
setFromPose(fromPose)
¶
getToPose()
¶
setToPose(toPose)
¶
RobotReport
¶
robotID = robotID
instance-attribute
¶
pose = pose
instance-attribute
¶
pathIndex = pathIndex
instance-attribute
¶
velocity = velocity
instance-attribute
¶
distanceTraveled = distanceTraveled
instance-attribute
¶
criticalPoint = criticalPoint
instance-attribute
¶
getRobotID()
¶
getPose()
¶
getPathIndex()
¶
getVelocity()
¶
getDistanceTraveled()
¶
getCriticalPoint()
¶
TrackingCallback
¶
TrajectoryEnvelopeTrackerDummy
¶
AbstractTrajectoryEnvelopeTracker
¶
Bases: ABC
te = te
instance-attribute
¶
traj = te.getTrajectory()
instance-attribute
¶
temporalResolution = temporalResolution
instance-attribute
¶
externalCPCounter = -1
instance-attribute
¶
reportCounter = -1
instance-attribute
¶
criticalPoint = -1
instance-attribute
¶
trackingPeriodInMillis = trackingPeriodInMillis
instance-attribute
¶
cb = cb
instance-attribute
¶
tec = tec
instance-attribute
¶
startingTimeInMillis = tec.getCurrentTimeInMillis()
instance-attribute
¶
getTrackingPeriod()
¶
getStartingTimeInMillis()
¶
resetStartingTimeInMillis()
¶
onTrajectoryEnvelopeUpdate()
abstractmethod
¶
updateTrajectoryEnvelope(te)
¶
setCanStartTracking()
¶
canStartTracking()
¶
waitUntilCanStartTracking()
async
¶
setCriticalPoint(criticalPoint)
abstractmethod
¶
setCriticalPointWithCounter(criticalPointToSet, externalCPCounter)
¶
setReportCounter(reportCounter)
¶
getReportCounter()
¶
getCriticalPoint()
¶
getTrackingPeriodInMillis()
¶
getLastRobotReport()
¶
getRobotReport()
abstractmethod
¶
onPositionUpdate()
¶
getCurrentTimeInMillis()
abstractmethod
¶
startTracking()
abstractmethod
¶
trackingStarted()
¶
getTrajectoryEnvelope()
¶
finishTracking()
¶
cancel()
¶
AbstractMotionPlanner
¶
Bases: ABC
start = None
instance-attribute
¶
goal = ()
instance-attribute
¶
footprintCoords = None
instance-attribute
¶
pathPS = None
instance-attribute
¶
setFootprint(*coords)
¶
setStart(p)
¶
setGoals(*poses)
¶
getPath()
¶
addObstacles(geoms)
¶
clearObstacles()
¶
getObstacles()
¶
doPlanning()
abstractmethod
¶
Populate self.pathPS; return True iff planning succeeded.
plan()
¶
writeDebugImage()
¶
RobotAtCriticalSection
¶
AbstractTrajectoryEnvelopeCoordinator
¶
Bases: ABC
CONTROL_PERIOD = CONTROL_PERIOD
instance-attribute
¶
TEMPORAL_RESOLUTION = TEMPORAL_RESOLUTION
instance-attribute
¶
DEFAULT_ROBOT_TRACKING_PERIOD = DEFAULT_ROBOT_TRACKING_PERIOD
instance-attribute
¶
overlay = False
instance-attribute
¶
quiet = False
instance-attribute
¶
totalMsgsSent = 0
instance-attribute
¶
totalMsgsReTx = 0
instance-attribute
¶
criticalSectionCounter = 0
instance-attribute
¶
solver = None
instance-attribute
¶
missionsPool = []
instance-attribute
¶
envelopesToTrack = []
instance-attribute
¶
currentParkingEnvelopes = []
instance-attribute
¶
allCriticalSections = set()
instance-attribute
¶
CSToDepsOrder = {}
instance-attribute
¶
depsToCS = {}
instance-attribute
¶
escapingCSToWaitingRobotIDandCP = {}
instance-attribute
¶
stoppingPoints = {}
instance-attribute
¶
stoppingTimes = {}
instance-attribute
¶
stoppingPointTimers = {}
instance-attribute
¶
trackers = {}
instance-attribute
¶
currentDependencies = {}
instance-attribute
¶
communicatedCPs = {}
instance-attribute
¶
externalCPCounters = {}
instance-attribute
¶
comparators = []
instance-attribute
¶
forwardModels = {}
instance-attribute
¶
footprints = {}
instance-attribute
¶
maxFootprintDimensions = {}
instance-attribute
¶
robotTrackingPeriodInMillis = {}
instance-attribute
¶
robotMaxVelocity = {}
instance-attribute
¶
robotMaxAcceleration = {}
instance-attribute
¶
muted = set()
instance-attribute
¶
yieldIfParking = True
instance-attribute
¶
checkEscapePoses = True
instance-attribute
¶
trackingCallbacks = {}
instance-attribute
¶
inferenceCallback = None
instance-attribute
¶
motionPlanners = {}
instance-attribute
¶
packetLossProbability = network_configuration.PROBABILITY_OF_PACKET_LOSS
instance-attribute
¶
MAX_TX_DELAY = network_configuration.getMaximumTxDelay()
instance-attribute
¶
maxFaultsProbability = network_configuration.PROBABILITY_OF_PACKET_LOSS
instance-attribute
¶
numberOfReplicas = 1
instance-attribute
¶
isDriving = {}
instance-attribute
¶
criticalPointSafetyMargin = 0.0
instance-attribute
¶
getMaxFootprintDimension(robotID)
¶
getFootprint(robotID)
¶
getFootprintPolygon(robotID)
¶
setFootprint(robotID, *coordinates)
¶
computeMaxFootprintDimension(coords)
¶
setForwardModel(robotID, fm)
¶
getForwardModel(robotID)
¶
setRobotTrackingPeriodInMillis(robotID, trackingPeriodInMillis)
¶
getRobotTrackingPeriodInMillis(robotID)
¶
setCriticalPointSafetyMargin(margin)
¶
Stop yielding robots at least margin metres of arc length (along
their own path) before the first conflicting pose of a critical
section, in addition to the classic TRAILING_PATH_POINTS backoff.
0.0 (the default) preserves the original behaviour exactly.
setRobotMaxVelocity(robotID, maxVelocity)
¶
setRobotMaxAcceleration(robotID, maxAcceleration)
¶
getRobotMaxVelocity(robotID)
¶
getRobotMaxAcceleration(robotID)
¶
setNetworkParameters(packetLossProbability, max_tx_delay, maxFaultsProbability)
¶
setInferenceCallback(cb)
¶
getControlPeriod()
¶
getTemporalResolution()
¶
setYieldIfParking(value)
¶
setCheckEscapePoses(value)
¶
toggleMute(robotID)
¶
mute(robotID)
¶
unMute(robotID)
¶
getMuted()
¶
getCurrentTimeInMillis()
abstractmethod
¶
setupSolver(max_envelopes=64)
¶
startInference()
async
¶
stopInference()
async
¶
isStartedInference()
¶
onNewMissionDispatched(robotID)
¶
onCriticalSectionUpdate()
¶
getDrivingEnvelopes()
¶
isParked(robotID)
¶
isDrivingRobot(robotID)
¶
getIdleRobots()
¶
getAllRobotIDs()
¶
getRobotReport(robotID)
¶
getCurrentDependencies()
¶
getCurrentSuperEnvelope(robotID)
¶
getCurrentTrajectoryEnvelope(robotID)
¶
addTrackingCallback(robotID, cb)
¶
setVisualization(viz)
¶
addComparator(c)
¶
setMotionPlanner(robotID, mp)
¶
getMotionPlanner(robotID)
¶
inParkingPose(robotID)
¶
setCriticalPoint(robotID, criticalPoint, retransmitt)
¶
placeRobot(robotID, currentPose=None, parking=None, location=None)
¶
isFree(robotID)
¶
atStoppingPoint(robotID)
¶
spawnWaitingThread(robotID, index, duration)
¶
getObstaclesInCriticalPoints(robotIDs)
¶
getObstaclesFromWaitingRobots(robotID)
¶
makeObstacles(robotID, *obstaclePoses)
¶
doReplanning(mp, fromPose, toPose, obstaclesToConsider=())
async
¶
Plan off the event loop: mp.plan() may take seconds (or, for a
remote planner, must itself wait on messages delivered by this very
loop), so the whole motion-planner interaction runs in a worker
thread.
updateDependencies()
abstractmethod
¶
canExitCriticalSection(drivingCurrentIndex, waitingCurrentIndex, drivingTE, waitingTE, lastIndexOfCSDriving)
¶
getCriticalPoint(yieldingRobotID, cs, leadingRobotCurrentPathIndex)
¶
isAhead(cs, rr1, rr2)
¶
computeCriticalSections()
¶
filterCriticalSections()
¶
getCriticalSections(te1, minStart1, te2, minStart2, checkEscapePoses, maxDimensionOfSmallestRobot)
staticmethod
¶
cleanUpRobotCS(robotID, lastWaitingPoint)
¶
cleanUp(te)
¶
Retire an envelope no longer in use — ports Java's cleanUp
(solver.removeConstraints + solver.removeVariable, verified
against the real org.metacsp.framework.ConstraintSolver /
ConstraintNetwork source). Java's network is a sparse graph
(removeVariable deletes the vertex outright; memory is bounded
by live entries alone); this port's TrajectoryEnvelopeSolver
instead uses a dense STP distance matrix sized for the whole
process's variable count, so the equivalent fix
(TrajectoryEnvelopeSolver.mark_completed) both excludes the
envelope from solver.envelopes() — the active set
computeCriticalSections considers, so a retired envelope can't
spuriously conflict with robots long gone from that space — and
detaches its two STP variables, freeing their matrix slots for
reuse so the working set stays bounded by concurrently-active
envelopes rather than growing for the coordinator's entire
lifetime. Also drops it from currentParkingEnvelopes if
present.
cleanUpStaleParkingEnvelope(robotID)
¶
A robot's parking envelope from before its current mission
stays in currentParkingEnvelopes until that mission finishes
through the normal onTrackingFinished path (which retires it
there) — a caller that ends a mission a different way (e.g.
recovery's hard-drop cancel) must retire it here instead, or it
leaks for the coordinator's remaining lifetime.
startTrackingAddedMissions()
¶
addMissions(*missions)
¶
getNewTracker(te, cb)
abstractmethod
¶
_DefaultForwardModel
¶
Bases: ForwardModel
get_logger(name=None, **bindings)
¶
A lazy proxy, not a materialized logger: don't call .bind() here.
BoundLoggerLazyProxy.bind() immediately freezes in whatever
structlog.configure() last set (processors, wrapper class, logger
factory) onto a concrete BoundLogger. Every call site does
log = get_logger(__name__) at module import time, which for
downstream consumers happens before their own structlog.configure()
runs — eagerly binding would permanently lock these loggers onto
structlog's unconfigured defaults (console-only, print-based), deaf to
any later reconfiguration. structlog.get_logger(name, **bindings)
passes bindings through as initial context instead, keeping the proxy
lazy so it materializes against whatever config is active at first log
call.
coordination_oru.trajectory_envelope_coordinator
¶
TrajectoryEnvelopeCoordinator: the full dependency/deadlock pipeline.
Ported from Java's TrajectoryEnvelopeCoordinator. Two mutually exclusive
strategies for updateDependencies():
localCheckAndRevise(default) — per-critical-section local ordering, with local reordering (callLocalReordering) and/or one-path replanning (callOnePathReplan) to break detected nonlive cycles.globalCheckAndRevise(behindavoidDeadlockGlobally, off by default — exponential worst case) — precedence pre-loaded via FCFS/previous decisions, then revised per a global heuristic while checking that no reversal introduces a nonlive cycle across the whole fleet.
breakDeadlocksByReplanning calls into AbstractMotionPlanner — with no
planner injected (the default), replanning attempts simply fail and the
local-reordering / artificial-dependency fallback takes over, exactly as in
Java when no planner is configured for a robot.
log = get_logger(__name__)
module-attribute
¶
AbstractTrajectoryEnvelopeCoordinator
¶
Bases: ABC
CONTROL_PERIOD = CONTROL_PERIOD
instance-attribute
¶
TEMPORAL_RESOLUTION = TEMPORAL_RESOLUTION
instance-attribute
¶
DEFAULT_ROBOT_TRACKING_PERIOD = DEFAULT_ROBOT_TRACKING_PERIOD
instance-attribute
¶
overlay = False
instance-attribute
¶
quiet = False
instance-attribute
¶
totalMsgsSent = 0
instance-attribute
¶
totalMsgsReTx = 0
instance-attribute
¶
criticalSectionCounter = 0
instance-attribute
¶
solver = None
instance-attribute
¶
missionsPool = []
instance-attribute
¶
envelopesToTrack = []
instance-attribute
¶
currentParkingEnvelopes = []
instance-attribute
¶
allCriticalSections = set()
instance-attribute
¶
CSToDepsOrder = {}
instance-attribute
¶
depsToCS = {}
instance-attribute
¶
escapingCSToWaitingRobotIDandCP = {}
instance-attribute
¶
stoppingPoints = {}
instance-attribute
¶
stoppingTimes = {}
instance-attribute
¶
stoppingPointTimers = {}
instance-attribute
¶
trackers = {}
instance-attribute
¶
currentDependencies = {}
instance-attribute
¶
communicatedCPs = {}
instance-attribute
¶
externalCPCounters = {}
instance-attribute
¶
comparators = []
instance-attribute
¶
forwardModels = {}
instance-attribute
¶
footprints = {}
instance-attribute
¶
maxFootprintDimensions = {}
instance-attribute
¶
robotTrackingPeriodInMillis = {}
instance-attribute
¶
robotMaxVelocity = {}
instance-attribute
¶
robotMaxAcceleration = {}
instance-attribute
¶
muted = set()
instance-attribute
¶
yieldIfParking = True
instance-attribute
¶
checkEscapePoses = True
instance-attribute
¶
trackingCallbacks = {}
instance-attribute
¶
inferenceCallback = None
instance-attribute
¶
motionPlanners = {}
instance-attribute
¶
packetLossProbability = network_configuration.PROBABILITY_OF_PACKET_LOSS
instance-attribute
¶
MAX_TX_DELAY = network_configuration.getMaximumTxDelay()
instance-attribute
¶
maxFaultsProbability = network_configuration.PROBABILITY_OF_PACKET_LOSS
instance-attribute
¶
numberOfReplicas = 1
instance-attribute
¶
isDriving = {}
instance-attribute
¶
criticalPointSafetyMargin = 0.0
instance-attribute
¶
getMaxFootprintDimension(robotID)
¶
getFootprint(robotID)
¶
getFootprintPolygon(robotID)
¶
setFootprint(robotID, *coordinates)
¶
computeMaxFootprintDimension(coords)
¶
setForwardModel(robotID, fm)
¶
getForwardModel(robotID)
¶
setRobotTrackingPeriodInMillis(robotID, trackingPeriodInMillis)
¶
getRobotTrackingPeriodInMillis(robotID)
¶
setCriticalPointSafetyMargin(margin)
¶
Stop yielding robots at least margin metres of arc length (along
their own path) before the first conflicting pose of a critical
section, in addition to the classic TRAILING_PATH_POINTS backoff.
0.0 (the default) preserves the original behaviour exactly.
setRobotMaxVelocity(robotID, maxVelocity)
¶
setRobotMaxAcceleration(robotID, maxAcceleration)
¶
getRobotMaxVelocity(robotID)
¶
getRobotMaxAcceleration(robotID)
¶
setNetworkParameters(packetLossProbability, max_tx_delay, maxFaultsProbability)
¶
setInferenceCallback(cb)
¶
getControlPeriod()
¶
getTemporalResolution()
¶
setYieldIfParking(value)
¶
setCheckEscapePoses(value)
¶
toggleMute(robotID)
¶
mute(robotID)
¶
unMute(robotID)
¶
getMuted()
¶
getCurrentTimeInMillis()
abstractmethod
¶
setupSolver(max_envelopes=64)
¶
startInference()
async
¶
stopInference()
async
¶
isStartedInference()
¶
onNewMissionDispatched(robotID)
¶
onCriticalSectionUpdate()
¶
getDrivingEnvelopes()
¶
isParked(robotID)
¶
isDrivingRobot(robotID)
¶
getIdleRobots()
¶
getAllRobotIDs()
¶
getRobotReport(robotID)
¶
getCurrentDependencies()
¶
getCurrentSuperEnvelope(robotID)
¶
getCurrentTrajectoryEnvelope(robotID)
¶
addTrackingCallback(robotID, cb)
¶
setVisualization(viz)
¶
addComparator(c)
¶
setMotionPlanner(robotID, mp)
¶
getMotionPlanner(robotID)
¶
inParkingPose(robotID)
¶
setCriticalPoint(robotID, criticalPoint, retransmitt)
¶
placeRobot(robotID, currentPose=None, parking=None, location=None)
¶
isFree(robotID)
¶
atStoppingPoint(robotID)
¶
spawnWaitingThread(robotID, index, duration)
¶
getObstaclesInCriticalPoints(robotIDs)
¶
getObstaclesFromWaitingRobots(robotID)
¶
makeObstacles(robotID, *obstaclePoses)
¶
doReplanning(mp, fromPose, toPose, obstaclesToConsider=())
async
¶
Plan off the event loop: mp.plan() may take seconds (or, for a
remote planner, must itself wait on messages delivered by this very
loop), so the whole motion-planner interaction runs in a worker
thread.
updateDependencies()
abstractmethod
¶
canExitCriticalSection(drivingCurrentIndex, waitingCurrentIndex, drivingTE, waitingTE, lastIndexOfCSDriving)
¶
getCriticalPoint(yieldingRobotID, cs, leadingRobotCurrentPathIndex)
¶
isAhead(cs, rr1, rr2)
¶
computeCriticalSections()
¶
filterCriticalSections()
¶
getCriticalSections(te1, minStart1, te2, minStart2, checkEscapePoses, maxDimensionOfSmallestRobot)
staticmethod
¶
cleanUpRobotCS(robotID, lastWaitingPoint)
¶
cleanUp(te)
¶
Retire an envelope no longer in use — ports Java's cleanUp
(solver.removeConstraints + solver.removeVariable, verified
against the real org.metacsp.framework.ConstraintSolver /
ConstraintNetwork source). Java's network is a sparse graph
(removeVariable deletes the vertex outright; memory is bounded
by live entries alone); this port's TrajectoryEnvelopeSolver
instead uses a dense STP distance matrix sized for the whole
process's variable count, so the equivalent fix
(TrajectoryEnvelopeSolver.mark_completed) both excludes the
envelope from solver.envelopes() — the active set
computeCriticalSections considers, so a retired envelope can't
spuriously conflict with robots long gone from that space — and
detaches its two STP variables, freeing their matrix slots for
reuse so the working set stays bounded by concurrently-active
envelopes rather than growing for the coordinator's entire
lifetime. Also drops it from currentParkingEnvelopes if
present.
cleanUpStaleParkingEnvelope(robotID)
¶
A robot's parking envelope from before its current mission
stays in currentParkingEnvelopes until that mission finishes
through the normal onTrackingFinished path (which retires it
there) — a caller that ends a mission a different way (e.g.
recovery's hard-drop cancel) must retire it here instead, or it
leaks for the coordinator's remaining lifetime.
startTrackingAddedMissions()
¶
addMissions(*missions)
¶
getNewTracker(te, cb)
abstractmethod
¶
CriticalSection
¶
te1 = te1
instance-attribute
¶
te2 = te2
instance-attribute
¶
te1Start = te1Start
instance-attribute
¶
te2Start = te2Start
instance-attribute
¶
te1End = te1End
instance-attribute
¶
te2End = te2End
instance-attribute
¶
te1Break = -1
instance-attribute
¶
te2Break = -1
instance-attribute
¶
getTe1()
¶
getTe2()
¶
getTe1Start()
¶
getTe2Start()
¶
getTe1End()
¶
getTe2End()
¶
getTe1Break()
¶
getTe2Break()
¶
setTe1Break(te1Break)
¶
setTe2Break(te2Break)
¶
Dependency
¶
teWaiting = teWaiting
instance-attribute
¶
teDriving = teDriving
instance-attribute
¶
waitingPoint = waitingPoint
instance-attribute
¶
thresholdPoint = thresholdPoint
instance-attribute
¶
robotIDWaiting = teWaiting.getRobotID()
instance-attribute
¶
robotIDDriving = teDriving.getRobotID() if teDriving is not None else 0
instance-attribute
¶
compareTo(other)
¶
getWaitingPose()
¶
getReleasingPose()
¶
getWaitingTrajectoryEnvelope()
¶
getDrivingTrajectoryEnvelope()
¶
getWaitingPoint()
¶
getReleasingPoint()
¶
getWaitingRobotID()
¶
getDrivingRobotID()
¶
RobotAtCriticalSection
¶
RobotReport
¶
robotID = robotID
instance-attribute
¶
pose = pose
instance-attribute
¶
pathIndex = pathIndex
instance-attribute
¶
velocity = velocity
instance-attribute
¶
distanceTraveled = distanceTraveled
instance-attribute
¶
criticalPoint = criticalPoint
instance-attribute
¶
getRobotID()
¶
getPose()
¶
getPathIndex()
¶
getVelocity()
¶
getDistanceTraveled()
¶
getCriticalPoint()
¶
TrajectoryEnvelopeTrackerDummy
¶
AbstractTrajectoryEnvelopeTracker
¶
Bases: ABC
te = te
instance-attribute
¶
traj = te.getTrajectory()
instance-attribute
¶
temporalResolution = temporalResolution
instance-attribute
¶
externalCPCounter = -1
instance-attribute
¶
reportCounter = -1
instance-attribute
¶
criticalPoint = -1
instance-attribute
¶
trackingPeriodInMillis = trackingPeriodInMillis
instance-attribute
¶
cb = cb
instance-attribute
¶
tec = tec
instance-attribute
¶
startingTimeInMillis = tec.getCurrentTimeInMillis()
instance-attribute
¶
getTrackingPeriod()
¶
getStartingTimeInMillis()
¶
resetStartingTimeInMillis()
¶
onTrajectoryEnvelopeUpdate()
abstractmethod
¶
updateTrajectoryEnvelope(te)
¶
setCanStartTracking()
¶
canStartTracking()
¶
waitUntilCanStartTracking()
async
¶
setCriticalPoint(criticalPoint)
abstractmethod
¶
setCriticalPointWithCounter(criticalPointToSet, externalCPCounter)
¶
setReportCounter(reportCounter)
¶
getReportCounter()
¶
getCriticalPoint()
¶
getTrackingPeriodInMillis()
¶
getLastRobotReport()
¶
getRobotReport()
abstractmethod
¶
onPositionUpdate()
¶
getCurrentTimeInMillis()
abstractmethod
¶
startTracking()
abstractmethod
¶
trackingStarted()
¶
getTrajectoryEnvelope()
¶
finishTracking()
¶
cancel()
¶
PoseSteering
dataclass
¶
TrajectoryEnvelope
dataclass
¶
A robot's planned trajectory expressed as an STP-aware swept envelope.
envelope_id is assigned by the
:class:~coordination_oru.metacsp.spatial.trajectory_envelope_solver.TrajectoryEnvelopeSolver
that creates it; start_node / end_node are the STP variable indices
for this envelope's start and end times.
envelope_id
instance-attribute
¶
robot_id
instance-attribute
¶
path
instance-attribute
¶
start_node
instance-attribute
¶
end_node
instance-attribute
¶
spatial_envelope
instance-attribute
¶
footprint
instance-attribute
¶
component = 'Driving'
class-attribute
instance-attribute
¶
nominal_duration = 0.0
class-attribute
instance-attribute
¶
completed = False
class-attribute
instance-attribute
¶
metadata = field(default_factory=dict)
class-attribute
instance-attribute
¶
length
property
¶
pose_at(index)
¶
waypoint_footprint(index)
¶
getID()
¶
getRobotID()
¶
getPathLength()
¶
getSpatialEnvelope()
¶
getFootprint()
¶
getTrajectory()
¶
makeFootprint(ps)
¶
getComponent()
¶
getSequenceNumberStart()
¶
getSequenceNumberEnd()
¶
getSequenceNumber(x, y)
¶
Index of the path point closest to (x, y).
Mirrors Java's getSequenceNumber(Coordinate) used to locate
stopping points along the path.
TrajectoryEnvelopeCoordinator
¶
Bases: AbstractTrajectoryEnvelopeCoordinator
currentOrdersGraph = nx.DiGraph()
instance-attribute
¶
currentCyclesList = {}
instance-attribute
¶
replanningStoppingPoints = {}
instance-attribute
¶
breakDeadlocksByReordering = True
instance-attribute
¶
breakDeadlocksByReplanning = True
instance-attribute
¶
avoidDeadlockGlobally = False
instance-attribute
¶
nonliveStatesDetected = 0
instance-attribute
¶
nonliveStatesAvoided = 0
instance-attribute
¶
currentOrdersHeurusticallyDecided = 0
instance-attribute
¶
nonliveCyclesOld = []
instance-attribute
¶
replanningTrialsCounter = 0
instance-attribute
¶
successfulReplanningTrialsCounter = 0
instance-attribute
¶
forceCriticalPointReTransmission = {}
instance-attribute
¶
staticReplan = False
instance-attribute
¶
isBlocked = False
instance-attribute
¶
isDeadlockedFlag = False
instance-attribute
¶
deadlockedCycles = []
instance-attribute
¶
deadlockedCallback = None
instance-attribute
¶
replanFailedCallback = None
instance-attribute
¶
fake = False
instance-attribute
¶
isBlockedFleet()
¶
setBreakDeadlocks(global_, reorder, replan)
¶
setDeadlockedCallback(cb)
¶
setReplanFailedCallback(cb)
¶
Called from :meth:rePlanPath — after the
replanningStoppingPoints pins are popped — when no robot in the
set could be replanned, with the robotsToReplan set. Lets the
deployment decide what to do with an unresolvable deadlock (e.g.
cancel the missions and park the robots). Not called when the replan
task is cancelled.
setStaticReplan(value)
¶
setFakeCoordination(fake)
¶
depsToGraph(deps)
¶
nonlivePair(dep1, dep2)
¶
findSimpleNonliveCycles(g)
¶
computeClosestDependencies(allDeps, artificialDeps)
¶
computeIsDeadlocked()
¶
Recompute :attr:isDeadlockedFlag without firing the
deadlocked callback — safe for observers (e.g. viewers) to poll.
Also records :attr:deadlockedCycles: every nonlive cycle whose
robots have all come to rest at their communicated critical points
([] when none has).
isDeadlocked()
¶
findAndRepairNonliveCycles(currentDeps, artificialDeps, reversibleDeps, currentReports)
¶
callLocalReordering(nonlive_cycles, artificialDeps, g, reversibleDeps, allDeps, currentReports)
¶
replanEnvelope(robotID, onlyIfDeadlocks=False)
¶
callOnePathReplan(cycle, g)
¶
spawnReplanning(robotsToReplan, allConnectedRobots)
¶
stopInference()
async
¶
setMaxCPDependencies(robotIDs)
¶
rePlanPath(robotsToReplan, robotsAsObstacles)
async
¶
Three-phase lock scope per robot: gather dep/pose/obstacle info
under self._lock, run the (possibly slow, possibly remote) plan
with the lock released — the involved robots stay pinned via
replanningStoppingPoints — and re-acquire only for the
replacePath commit.
replacePath(robotID, newPath, breakingPathIndex, lockedRobotIDs, concatenatePaths=True)
¶
truncateEnvelope(robotID)
¶
truncateEnvelopeAt(robotID, pathIndex)
¶
reverseEnvelope(robotID)
¶
getOrder(robotTracker1, robotReport1, robotTracker2, robotReport2, cs)
¶
updateDependencies()
¶
localCheckAndRevise()
¶
sendCriticalPoint(robotID, currentReports)
¶
cleanUpRobotCS(robotID, lastWaitingPoint)
¶
deleteEdge(edge)
¶
deleteEdges(edgesToDelete)
¶
addEdges(edgesToAdd)
¶
updateGraph(edgesToDelete, edgesToAdd)
¶
globalCheckAndRevise()
¶
get_logger(name=None, **bindings)
¶
A lazy proxy, not a materialized logger: don't call .bind() here.
BoundLoggerLazyProxy.bind() immediately freezes in whatever
structlog.configure() last set (processors, wrapper class, logger
factory) onto a concrete BoundLogger. Every call site does
log = get_logger(__name__) at module import time, which for
downstream consumers happens before their own structlog.configure()
runs — eagerly binding would permanently lock these loggers onto
structlog's unconfigured defaults (console-only, print-based), deaf to
any later reconfiguration. structlog.get_logger(name, **bindings)
passes bindings through as initial context instead, keeping the proxy
lazy so it materializes against whatever config is active at first log
call.
coordination_oru.critical_section
¶
CriticalSection: a quadruple (te1, te2, [te1Start,te1End], [te2Start,te2End]).
te1/te2 overlap when robot 1 is between path indices te1Start and
te1End while robot 2 is between te2Start and te2End. Ported
verbatim from Java's CriticalSection, including the symmetric
te1/te2 swap in __eq__/__hash__ and the exclusion of the
mutable te1Break/te2Break fields from the hash (they mutate after
construction, so including them would break hash stability while the CS
lives in a set/dict — this is the fix for the "CS-identity/priority keying
bug" the Java original avoids too).
TrajectoryEnvelope
dataclass
¶
A robot's planned trajectory expressed as an STP-aware swept envelope.
envelope_id is assigned by the
:class:~coordination_oru.metacsp.spatial.trajectory_envelope_solver.TrajectoryEnvelopeSolver
that creates it; start_node / end_node are the STP variable indices
for this envelope's start and end times.
envelope_id
instance-attribute
¶
robot_id
instance-attribute
¶
path
instance-attribute
¶
start_node
instance-attribute
¶
end_node
instance-attribute
¶
spatial_envelope
instance-attribute
¶
footprint
instance-attribute
¶
component = 'Driving'
class-attribute
instance-attribute
¶
nominal_duration = 0.0
class-attribute
instance-attribute
¶
completed = False
class-attribute
instance-attribute
¶
metadata = field(default_factory=dict)
class-attribute
instance-attribute
¶
length
property
¶
pose_at(index)
¶
waypoint_footprint(index)
¶
getID()
¶
getRobotID()
¶
getPathLength()
¶
getSpatialEnvelope()
¶
getFootprint()
¶
getTrajectory()
¶
makeFootprint(ps)
¶
getComponent()
¶
getSequenceNumberStart()
¶
getSequenceNumberEnd()
¶
getSequenceNumber(x, y)
¶
Index of the path point closest to (x, y).
Mirrors Java's getSequenceNumber(Coordinate) used to locate
stopping points along the path.
CriticalSection
¶
te1 = te1
instance-attribute
¶
te2 = te2
instance-attribute
¶
te1Start = te1Start
instance-attribute
¶
te2Start = te2Start
instance-attribute
¶
te1End = te1End
instance-attribute
¶
te2End = te2End
instance-attribute
¶
te1Break = -1
instance-attribute
¶
te2Break = -1
instance-attribute
¶
getTe1()
¶
getTe2()
¶
getTe1Start()
¶
getTe2Start()
¶
getTe1End()
¶
getTe2End()
¶
getTe1Break()
¶
getTe2Break()
¶
setTe1Break(te1Break)
¶
setTe2Break(te2Break)
¶
coordination_oru.dependency
¶
Dependency: the tuple (teWaiting, teDriving, waitingPoint, thresholdPoint).
The robot navigating teWaiting should not go beyond path index
waitingPoint until the robot navigating teDriving reaches path index
thresholdPoint. teDriving may be None (a stopping-point-only
dependency), in which case robotIDDriving is 0, matching Java.
Note (ported verbatim from the Java javadoc): __eq__ and __lt__ give
different results for dependencies involving different robot pairs with the
same critical point — __lt__ orders by waiting/threshold point only,
while __eq__ also compares the envelope pair. Be sure to use the right
one when adding/removing dependencies from a given data structure.
Pose
dataclass
¶
x
instance-attribute
¶
y
instance-attribute
¶
theta
instance-attribute
¶
z = math.nan
class-attribute
instance-attribute
¶
roll = math.nan
class-attribute
instance-attribute
¶
pitch = math.nan
class-attribute
instance-attribute
¶
is_3d()
¶
distance_xy(other)
¶
getX()
¶
getY()
¶
getTheta()
¶
distanceTo(other)
¶
interpolate(other, ratio)
¶
Linear interpolation towards other; theta via shortest arc.
TrajectoryEnvelope
dataclass
¶
A robot's planned trajectory expressed as an STP-aware swept envelope.
envelope_id is assigned by the
:class:~coordination_oru.metacsp.spatial.trajectory_envelope_solver.TrajectoryEnvelopeSolver
that creates it; start_node / end_node are the STP variable indices
for this envelope's start and end times.
envelope_id
instance-attribute
¶
robot_id
instance-attribute
¶
path
instance-attribute
¶
start_node
instance-attribute
¶
end_node
instance-attribute
¶
spatial_envelope
instance-attribute
¶
footprint
instance-attribute
¶
component = 'Driving'
class-attribute
instance-attribute
¶
nominal_duration = 0.0
class-attribute
instance-attribute
¶
completed = False
class-attribute
instance-attribute
¶
metadata = field(default_factory=dict)
class-attribute
instance-attribute
¶
length
property
¶
pose_at(index)
¶
waypoint_footprint(index)
¶
getID()
¶
getRobotID()
¶
getPathLength()
¶
getSpatialEnvelope()
¶
getFootprint()
¶
getTrajectory()
¶
makeFootprint(ps)
¶
getComponent()
¶
getSequenceNumberStart()
¶
getSequenceNumberEnd()
¶
getSequenceNumber(x, y)
¶
Index of the path point closest to (x, y).
Mirrors Java's getSequenceNumber(Coordinate) used to locate
stopping points along the path.
Dependency
¶
teWaiting = teWaiting
instance-attribute
¶
teDriving = teDriving
instance-attribute
¶
waitingPoint = waitingPoint
instance-attribute
¶
thresholdPoint = thresholdPoint
instance-attribute
¶
robotIDWaiting = teWaiting.getRobotID()
instance-attribute
¶
robotIDDriving = teDriving.getRobotID() if teDriving is not None else 0
instance-attribute
¶
compareTo(other)
¶
getWaitingPose()
¶
getReleasingPose()
¶
getWaitingTrajectoryEnvelope()
¶
getDrivingTrajectoryEnvelope()
¶
getWaitingPoint()
¶
getReleasingPoint()
¶
getWaitingRobotID()
¶
getDrivingRobotID()
¶
coordination_oru.mission
¶
Mission: a goal for a robot, reached via a path connecting two poses.
_mission_order_counter = count(0)
module-attribute
¶
Pose
dataclass
¶
x
instance-attribute
¶
y
instance-attribute
¶
theta
instance-attribute
¶
z = math.nan
class-attribute
instance-attribute
¶
roll = math.nan
class-attribute
instance-attribute
¶
pitch = math.nan
class-attribute
instance-attribute
¶
is_3d()
¶
distance_xy(other)
¶
getX()
¶
getY()
¶
getTheta()
¶
distanceTo(other)
¶
interpolate(other, ratio)
¶
Linear interpolation towards other; theta via shortest arc.
PoseSteering
dataclass
¶
Mission
¶
robotID = robotID
instance-attribute
¶
path = tuple(path)
instance-attribute
¶
order = next(_mission_order_counter)
instance-attribute
¶
fromLocation = fromLocation if fromLocation is not None else str(self.path[0].getPose())
instance-attribute
¶
toLocation = toLocation if toLocation is not None else str(self.path[-1].getPose())
instance-attribute
¶
fromPose = fromPose if fromPose is not None else self.path[0].getPose()
instance-attribute
¶
toPose = toPose if toPose is not None else self.path[-1].getPose()
instance-attribute
¶
stoppingPoints = []
instance-attribute
¶
stoppingPointDurations = []
instance-attribute
¶
compareTo(other)
¶
setStoppingPoint(pose, duration)
¶
clearStoppingPoints()
¶
getStoppingPoints()
¶
setToLocation(location)
¶
setFromLocation(location)
¶
getRobotID()
¶
getPath()
¶
setPath(path)
¶
getFromLocation()
¶
getToLocation()
¶
getFromPose()
¶
setFromPose(fromPose)
¶
getToPose()
¶
setToPose(toPose)
¶
coordination_oru.robot_report
¶
RobotReport: telemetry snapshot issued by a tracker.
Pose
dataclass
¶
x
instance-attribute
¶
y
instance-attribute
¶
theta
instance-attribute
¶
z = math.nan
class-attribute
instance-attribute
¶
roll = math.nan
class-attribute
instance-attribute
¶
pitch = math.nan
class-attribute
instance-attribute
¶
is_3d()
¶
distance_xy(other)
¶
getX()
¶
getY()
¶
getTheta()
¶
distanceTo(other)
¶
interpolate(other, ratio)
¶
Linear interpolation towards other; theta via shortest arc.
RobotReport
¶
robotID = robotID
instance-attribute
¶
pose = pose
instance-attribute
¶
pathIndex = pathIndex
instance-attribute
¶
velocity = velocity
instance-attribute
¶
distanceTraveled = distanceTraveled
instance-attribute
¶
criticalPoint = criticalPoint
instance-attribute
¶
getRobotID()
¶
getPose()
¶
getPathIndex()
¶
getVelocity()
¶
getDistanceTraveled()
¶
getCriticalPoint()
¶
coordination_oru.robot_at_critical_section
¶
RobotAtCriticalSection: a (report, CS) pair used by ordering heuristics.
CriticalSection
¶
te1 = te1
instance-attribute
¶
te2 = te2
instance-attribute
¶
te1Start = te1Start
instance-attribute
¶
te2Start = te2Start
instance-attribute
¶
te1End = te1End
instance-attribute
¶
te2End = te2End
instance-attribute
¶
te1Break = -1
instance-attribute
¶
te2Break = -1
instance-attribute
¶
getTe1()
¶
getTe2()
¶
getTe1Start()
¶
getTe2Start()
¶
getTe1End()
¶
getTe2End()
¶
getTe1Break()
¶
getTe2Break()
¶
setTe1Break(te1Break)
¶
setTe2Break(te2Break)
¶
RobotReport
¶
robotID = robotID
instance-attribute
¶
pose = pose
instance-attribute
¶
pathIndex = pathIndex
instance-attribute
¶
velocity = velocity
instance-attribute
¶
distanceTraveled = distanceTraveled
instance-attribute
¶
criticalPoint = criticalPoint
instance-attribute
¶
getRobotID()
¶
getPose()
¶
getPathIndex()
¶
getVelocity()
¶
getDistanceTraveled()
¶
getCriticalPoint()
¶
coordination_oru.forward_model
¶
ForwardModel: predicts whether/where a robot can stop.
ConstantAccelerationForwardModel matches the Java class of the same
name. The Java version forward-simulates with the RK4 integrator at 0.1 ms
steps; since the dynamics it integrates are piecewise-constant acceleration
(Derivative.compute_acceleration is bang-bang toward a velocity cap),
the trajectory has an exact closed form, which is used here instead: the
RK4 loop cost ~100 ms per call in pure Python and starved the asyncio
event loop that the simulation and the web viewer share.
State
¶
TrajectoryEnvelope
dataclass
¶
A robot's planned trajectory expressed as an STP-aware swept envelope.
envelope_id is assigned by the
:class:~coordination_oru.metacsp.spatial.trajectory_envelope_solver.TrajectoryEnvelopeSolver
that creates it; start_node / end_node are the STP variable indices
for this envelope's start and end times.
envelope_id
instance-attribute
¶
robot_id
instance-attribute
¶
path
instance-attribute
¶
start_node
instance-attribute
¶
end_node
instance-attribute
¶
spatial_envelope
instance-attribute
¶
footprint
instance-attribute
¶
component = 'Driving'
class-attribute
instance-attribute
¶
nominal_duration = 0.0
class-attribute
instance-attribute
¶
completed = False
class-attribute
instance-attribute
¶
metadata = field(default_factory=dict)
class-attribute
instance-attribute
¶
length
property
¶
pose_at(index)
¶
waypoint_footprint(index)
¶
getID()
¶
getRobotID()
¶
getPathLength()
¶
getSpatialEnvelope()
¶
getFootprint()
¶
getTrajectory()
¶
makeFootprint(ps)
¶
getComponent()
¶
getSequenceNumberStart()
¶
getSequenceNumberEnd()
¶
getSequenceNumber(x, y)
¶
Index of the path point closest to (x, y).
Mirrors Java's getSequenceNumber(Coordinate) used to locate
stopping points along the path.
RobotReport
¶
robotID = robotID
instance-attribute
¶
pose = pose
instance-attribute
¶
pathIndex = pathIndex
instance-attribute
¶
velocity = velocity
instance-attribute
¶
distanceTraveled = distanceTraveled
instance-attribute
¶
criticalPoint = criticalPoint
instance-attribute
¶
getRobotID()
¶
getPose()
¶
getPathIndex()
¶
getVelocity()
¶
getDistanceTraveled()
¶
getCriticalPoint()
¶
ForwardModel
¶
ConstantAccelerationForwardModel
¶
Bases: ForwardModel
maxAccel = maxAccel
instance-attribute
¶
maxVel = maxVel
instance-attribute
¶
temporalResolution = temporalResolution
instance-attribute
¶
controlPeriodInMillis = controlPeriodInMillis
instance-attribute
¶
trackingPeriodInMillis = trackingPeriodInMillis
instance-attribute
¶
canStop(te, currentState, targetPathIndex, useVelocity)
¶
getEarliestStoppingPathIndex(te, currentState)
¶
_robot_report_at(traj, aux_state, robot_id, critical_point)
¶
computeDistance(traj, startIndex, endIndex)
¶
_accelerate_capped(v0, accel, vcap, duration)
¶
Distance travelled and final velocity after duration seconds of
acceleration accel toward the velocity cap (zero acceleration at or
above it) — the closed form of what the Java RK4 loop integrates.
coordination_oru.network_configuration
¶
NetworkConfiguration: shared network-delay/packet-loss parameters.
Module-level state mirrors Java's static fields on the class of the same
name (shared process-wide, exactly like a Java static field).