Added tracking functionality to controller

src/de/tud/kom/p2psim/impl/topology/movement/vehicular/sumo/simulation/controller/traci/TraciSimulationController.java

@@ -12,6 +12,7 @@ import java.util.HashMap;
 import java.util.LinkedList;
 import java.util.List;
 import java.util.Map;
+import java.util.Map.Entry;
 import java.util.Random;
 
 import de.tud.kom.p2psim.api.simengine.SimulatorObserver;
@@ -19,6 +20,7 @@ import de.tud.kom.p2psim.impl.simengine.Simulator;
 import de.tud.kom.p2psim.impl.topology.PositionVector;
 import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.VehicleInformationContainer;
 import de.tudarmstadt.maki.simonstrator.api.Randoms;
+import de.tudarmstadt.maki.simonstrator.api.Time;
 import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.EdgeController;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.SimulationSetupExtractor;
@@ -31,6 +33,8 @@ import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.Road
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.SerializableRoadNetwork;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.exception.NoAdditionalRouteAvailableException;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.exception.NoExitAvailableException;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.paths.VehiclePathTracker;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.paths.VehiclePathTrackerFactory;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.routing.DijkstraAlgorithm;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.routing.RoutingAlgorithm;
 import de.tudresden.sumo.cmd.Edge;
@@ -56,6 +60,8 @@ import it.polito.appeal.traci.SumoTraciConnection;
 public class TraciSimulationController implements VehicleController, SimulationSetupExtractor, EdgeController, SimulatorObserver {
 	private static final File TEMP_FILE = new File(new File(System.getProperty("java.io.tmpdir")), "road_network.tmp");
 
+	private static final boolean TRAIN_PATH_PROBABILITIES = true;
+
 	private Random _random = Randoms.getRandom(getClass());
 
 	private static final Map<String, TraciSimulationController> CONTROLLER = new HashMap<>();
@@ -110,7 +116,7 @@ public class TraciSimulationController implements VehicleController, SimulationS
 	}
 
 	@Override
-	public synchronized void init() {
+	public synchronized void init(long pTimeScale) {
 		if (!_initalized) {
 			Random random = Randoms.getRandom("SUMO");
 
@@ -136,12 +142,23 @@ public class TraciSimulationController implements VehicleController, SimulationS
 
 			for (int i = 0; i < _startTime; i++) {
 				System.out.println("Pre-Start Setup: " + i + " of " + _startTime + " steps done.");
-				try {
-					_connection.do_timestep();
-				} catch (Exception e) {
-					throw new AssertionError(e);
+
+				if (TRAIN_PATH_PROBABILITIES) {
+				    Map<String, VehicleInformationContainer> positions = nextStep();
+				    for (Entry<String, VehicleInformationContainer> entry : positions.entrySet()) {
+				        VehiclePathTrackerFactory.getVehiclePathTracker().setEdge(entry.getKey(), entry.getValue().getRoute().getStart(), i * Time.SECOND);
+                    }
+				} else {
+				    try {
+				        _connection.do_timestep();
+				    } catch (Exception e) {
+				        throw new AssertionError(e);
+				    }
 				}
 			}
+			if (TRAIN_PATH_PROBABILITIES) {
+			    VehiclePathTrackerFactory.getVehiclePathTracker().disableTracking();
+            }
 		}
 	}
 
@@ -161,6 +178,13 @@ public class TraciSimulationController implements VehicleController, SimulationS
 		return _positons.get(pVehicleID).getPosition();
 	}
 
+	@Override
+	public double getVehicleLength(String pVehicleID) {
+	    SumoCommand lengthCommand = Vehicle.getLength(pVehicleID);
+	    Object requestObject = requestObject(lengthCommand);
+	    return (Double) requestObject;
+	}
+
 	@Override
 	public double getVehicleHeading(String pVehicleID) {
 		return _positons.get(pVehicleID).getHeading();
@@ -191,10 +215,10 @@ public class TraciSimulationController implements VehicleController, SimulationS
 
 			if (_observedAreaSet) {
 				if (_startX <= sumoPosition.x && sumoPosition.x <= _endX && _startY <= sumoPosition.y && sumoPosition.y <= _endY) {
-					result.add(new PositionVector(sumoPosition.x - _startX, sumoPosition.y - _startY, 0));
+					result.add(new PositionVector(sumoPosition.x - _startX, sumoPosition.y - _startY));
 				}
 			} else {
-				result.add(new PositionVector(sumoPosition.x, sumoPosition.y, 0));
+				result.add(new PositionVector(sumoPosition.x, sumoPosition.y));
 			}
 		}
 
@@ -220,54 +244,61 @@ public class TraciSimulationController implements VehicleController, SimulationS
 		return result;
 	}
 
-	@Override
-	public boolean nextStep() {
-		if (Simulator.getEndTime() == Simulator.getCurrentTime()) {
-			return false;
-		}
-		try {
-			_connection.do_timestep();
+	private Map<String, VehicleInformationContainer> nextStep() {
+        try {
+            _connection.do_timestep();
 
-			try {
-				synchronized (_positons) {
-					_positons.clear();
+            try {
+                Map<String, VehicleInformationContainer> vehiclePositions = new HashMap<>();
 
-					int temp = (Integer) _connection.do_job_get(Simulation.getCurrentTime());
+                List<String> allVehicles = requestAllVehicles();
+                for (String vehicle : allVehicles) {
+                    Location position = requestVehiclePosition(vehicle);
+                    if (position != null) {
+                        double heading = requestVehicleHeading(vehicle);
+                        double speed = requestVehicleSpeed(vehicle);
+                        RoadNetworkRoute route = requestVehicleRoute(vehicle);
 
-					_step = temp / 1000.0;
+                        VehicleInformationContainer informationContainer = new VehicleInformationContainer(position, heading, speed, route);
 
-					if (_start == -1) {
-						_start = _step;
-					}
+                        vehiclePositions.put(vehicle, informationContainer);
+                    }
+                }
+                return vehiclePositions;
+            } catch (Exception e) {
+                throw new AssertionError(e);
+            }
+        } catch (Exception e) {
+            throw new AssertionError(e);
+        }
+    }
 
-					Map<String, VehicleInformationContainer> vehiclePositions = new HashMap<>();
+	@Override
+	public boolean nextStep(long pTimeScale) {
+		if (Simulator.getEndTime() == Simulator.getCurrentTime()) {
+			return false;
+		}
+		Map<String, VehicleInformationContainer> vehiclePositions = nextStep();
 
-					List<String> allVehicles = requestAllVehicles();
-					for (String vehicle : allVehicles) {
-						Location position = requestVehiclePosition(vehicle);
-						if (position != null) {
-							double heading = requestVehicleHeading(vehicle);
-							double speed = requestVehicleSpeed(vehicle);
-							RoadNetworkRoute route = requestVehicleRoute(vehicle);
+        try {
+            int temp = (Integer) _connection.do_job_get(Simulation.getCurrentTime());
 
-							VehicleInformationContainer informationContainer = new VehicleInformationContainer(position, heading, speed, route);
+            _step = temp / (pTimeScale / (double)Time.MILLISECOND);
 
-							vehiclePositions.put(vehicle, informationContainer);
-						}
-					}
-					_positons = vehiclePositions;
-				}
-			} catch (Exception e) {
-				e.printStackTrace();
-			}
+            if (_start == -1) {
+                _start = _step;
+            }
+        } catch (Exception e) {
+            throw new AssertionError(e);
+        }
 
-			return true;
-		} catch (RuntimeException e) {
-			throw e;
-		} catch (Exception e) {
-			e.printStackTrace();
+		if (vehiclePositions != null) {
+		    _positons = vehiclePositions;
+		    return true;
+		} else {
+		    _positons.clear();
+		    return false;
 		}
-		return false;
 	}
 
 	private Location requestVehiclePosition(String pVehicleID) {
@@ -287,7 +318,7 @@ public class TraciSimulationController implements VehicleController, SimulationS
 
 		if (_observedAreaSet) {
 			if (_startX <= sumoPosition.x && sumoPosition.x <= _endX && _startY <= sumoPosition.y && sumoPosition.y <= _endY) {
-				return new PositionVector(sumoPosition.x - _startX, sumoPosition.y - _startY, 0);
+				return new PositionVector(sumoPosition.x - _startX, sumoPosition.y - _startY);
 			} else {
 				double diffX = _startX - sumoPosition.x;
 				if (diffX < 0 || sumoPosition.x - _endX < diffX) {
@@ -312,7 +343,7 @@ public class TraciSimulationController implements VehicleController, SimulationS
 				return null;
 			}
 		} else {
-			return new PositionVector(sumoPosition.x, sumoPosition.y, 0);
+			return new PositionVector(sumoPosition.x, sumoPosition.y);
 		}
 	}
 
@@ -329,7 +360,6 @@ public class TraciSimulationController implements VehicleController, SimulationS
 
 		Object angleObject = requestObject(angleCommand);
 
-
 		if (angleObject != null) {
 			return (Double) angleObject;
 		}
@@ -844,8 +874,11 @@ public class TraciSimulationController implements VehicleController, SimulationS
 			length += (double) object;
 
 		}
-
-		return length / (_roadNetwork.getEdge(pEdgeID).getLaneAmount());
+		if ((_roadNetwork.getEdge(pEdgeID).getLaneAmount()) != 0) {
+		    return length / (_roadNetwork.getEdge(pEdgeID).getLaneAmount());
+		} else {
+		    return 0.0;
+		}
 	}
 
 	@Override