diff --git a/src/main/java/se/oru/coordination/coordination_oru/AbstractTrajectoryEnvelopeCoordinator.java b/src/main/java/se/oru/coordination/coordination_oru/AbstractTrajectoryEnvelopeCoordinator.java index b5790f02b..6499abccb 100644 --- a/src/main/java/se/oru/coordination/coordination_oru/AbstractTrajectoryEnvelopeCoordinator.java +++ b/src/main/java/se/oru/coordination/coordination_oru/AbstractTrajectoryEnvelopeCoordinator.java @@ -116,6 +116,7 @@ public abstract class AbstractTrajectoryEnvelopeCoordinator { protected HashSet muted = new HashSet(); + protected boolean updateAllPostedMissionsOnce = false; protected boolean yieldIfParking = true; protected boolean checkEscapePoses = true; @@ -241,6 +242,11 @@ public int getControlPeriod() { public void setYieldIfParking(boolean value) { this.yieldIfParking = value; } + + //FIXME + public void setUpdateAllPostedMissionsOnce(boolean value) { + this.updateAllPostedMissionsOnce = value; + } /** * Set whether completely overlapping paths should lead to a warning. @@ -1734,17 +1740,27 @@ public void run() { while (true) { synchronized (solver) { if (!missionsPool.isEmpty()) { - //get the oldest posted mission - int oldestMissionRobotID = -1; - long oldestMissionTime = Long.MAX_VALUE; - for (int robotID : missionsPool.keySet()) { - if (missionsPool.get(robotID).getSecond().compareTo(oldestMissionTime) < 0) { - oldestMissionTime = missionsPool.get(robotID).getSecond().longValue(); - oldestMissionRobotID = robotID; + + //FIXME critical sections should be computed incrementally/asynchronously + if (updateAllPostedMissionsOnce) { + for (int robotID : missionsPool.keySet()) { + envelopesToTrack.add(missionsPool.get(robotID).getFirst()); + } + missionsPool.clear(); + } + else { + //get the oldest posted mission (use TreeSet) + int oldestMissionRobotID = -1; + long oldestMissionTime = Long.MAX_VALUE; + for (int robotID : missionsPool.keySet()) { + if (missionsPool.get(robotID).getSecond().compareTo(oldestMissionTime) < 0) { + oldestMissionTime = missionsPool.get(robotID).getSecond().longValue(); + oldestMissionRobotID = robotID; + } } + envelopesToTrack.add(missionsPool.get(oldestMissionRobotID).getFirst()); + missionsPool.remove(oldestMissionRobotID); } - envelopesToTrack.add(missionsPool.get(oldestMissionRobotID).getFirst()); - missionsPool.remove(oldestMissionRobotID); computeCriticalSections(); startTrackingAddedMissions(); } diff --git a/src/main/java/se/oru/coordination/coordination_oru/motionplanning/AbstractMotionPlanner.java b/src/main/java/se/oru/coordination/coordination_oru/motionplanning/AbstractMotionPlanner.java index 9dff46088..2b5a504c1 100644 --- a/src/main/java/se/oru/coordination/coordination_oru/motionplanning/AbstractMotionPlanner.java +++ b/src/main/java/se/oru/coordination/coordination_oru/motionplanning/AbstractMotionPlanner.java @@ -95,6 +95,7 @@ public PoseSteering[] getPath() { } public PoseSteering[] getPathInv() { + if (this.pathPS == null) return this.pathPS; ArrayList inv = new ArrayList(); for (PoseSteering ps : this.pathPS) inv.add(ps); Collections.reverse(inv); diff --git a/src/main/java/se/oru/coordination/coordination_oru/tests/icaps2018/talk/RandomPathsInMap.java b/src/main/java/se/oru/coordination/coordination_oru/tests/icaps2018/talk/RandomPathsInMap.java index c7f3fdc5d..a0b1ac17b 100644 --- a/src/main/java/se/oru/coordination/coordination_oru/tests/icaps2018/talk/RandomPathsInMap.java +++ b/src/main/java/se/oru/coordination/coordination_oru/tests/icaps2018/talk/RandomPathsInMap.java @@ -226,6 +226,9 @@ public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { } else rsp.setGoals(endLoc); rsp.plan(); + if (rsp.getPath() == null) { + throw new Error("No path found."); + } path = rsp.getPath(); pathInv = rsp.getPathInv(); if (cachePaths) {