fixing trackers synchronization in truncateEnvelope and reverseEnvelope functions.

Author: annamannucci

src/main/java/se/oru/coordination/coordination_oru/TrajectoryEnvelopeCoordinator.java

@@ -1316,7 +1316,10 @@ public boolean truncateEnvelope(int robotID, boolean ensureDynamicFeasibility) {
 			}
 
 			TrajectoryEnvelope te = this.getCurrentTrajectoryEnvelope(robotID);
-			AbstractTrajectoryEnvelopeTracker tet = this.trackers.get(robotID); 
+			AbstractTrajectoryEnvelopeTracker tet = null;
+			synchronized(trackers) {
+				tet = this.trackers.get(robotID); 
+			}
 
 			int earliestStoppingPathIndex = -1;
 
@@ -1326,10 +1329,7 @@ public boolean truncateEnvelope(int robotID, boolean ensureDynamicFeasibility) {
 
 				//Check if you were already slowing down to stop in your critical point.
 				int lastCommunicatedCP = -1;
-				synchronized (trackers) {
-					//if (breakingPathIndex == 0) trackers.get(robotID).resetStartingTimeInMillis();
-					lastCommunicatedCP = communicatedCPs.get(trackers.get(robotID)).getFirst();
-				}
+				lastCommunicatedCP = communicatedCPs.get(tet).getFirst();
 				earliestStoppingPathIndex = (lastCommunicatedCP != -1) ? Math.min(lastCommunicatedCP, earliestStoppingPathIndex) : earliestStoppingPathIndex;
 
 				//Compute and add new TE, remove old TE (both driving and final parking)
@@ -1359,18 +1359,18 @@ public boolean reverseEnvelope(int robotID) {
 			}
 
 			TrajectoryEnvelope te = this.getCurrentTrajectoryEnvelope(robotID);
-			AbstractTrajectoryEnvelopeTracker tet = this.trackers.get(robotID);
+			AbstractTrajectoryEnvelopeTracker tet = null;
+			synchronized(trackers) {
+				tet = this.trackers.get(robotID); 
+			}
 
 			int earliestStoppingPathIndex = -1;
 
 			if (!(tet instanceof TrajectoryEnvelopeTrackerDummy)) {
 				earliestStoppingPathIndex = this.getForwardModel(robotID).getEarliestStoppingPathIndex(te, this.getRobotReport(robotID));
 				//Check if you were already slowing down to stop in your critical point.
 				int lastCommunicatedCP = -1;
-				synchronized (trackers) {
-					//if (breakingPathIndex == 0) trackers.get(robotID).resetStartingTimeInMillis();
-					lastCommunicatedCP = communicatedCPs.get(trackers.get(robotID)).getFirst();
-				}
+				lastCommunicatedCP = communicatedCPs.get(tet).getFirst();
 				earliestStoppingPathIndex = (lastCommunicatedCP != -1) ? Math.min(lastCommunicatedCP, earliestStoppingPathIndex) : earliestStoppingPathIndex;
 
 				if (earliestStoppingPathIndex != -1) {