Adding traffic light model and scenario for priority vehicles

src/de/tud/kom/p2psim/impl/topology/movement/VehicleMovementModel.java

@@ -56,7 +56,7 @@ public class VehicleMovementModel implements MovementModel, EventHandler {
 
 	private static final Location DEFAULT_LOCATION = new PositionVector(Double.NaN, Double.NaN);
 
-	private boolean _reuseComponents = false;
+	private boolean _reuseComponents = true;
 
 	private static VehicleMovementModel MOVEMENT;
 

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

@@ -28,6 +28,7 @@ public class VehicleInformationContainer {
 	private double _heading;
 	private double _speed;
 	private RoadNetworkRoute _route;
+	private double _gap;
 
 	public VehicleInformationContainer(Location pPosition, double pHeading,
 			double pSpeed, RoadNetworkRoute pRoute) {
@@ -56,4 +57,8 @@ public class VehicleInformationContainer {
 	public void setSpeed(double vehicleSpeed) {
 		_speed = vehicleSpeed;
 	}
+
+	public void setGap(double vehicleGap) {
+		_gap = vehicleGap;
+	}
 }
\ No newline at end of file

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

@@ -21,22 +21,22 @@ import de.tudarmstadt.maki.simonstrator.api.Randoms;
 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;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.TrafficLightController;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.VehicleController;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.information.LocationUtils;
-import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetwork;
-import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkEdge;
-import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkLane;
-import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkRoute;
-import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.SerializableRoadNetwork;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.*;
 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.routing.DijkstraAlgorithm;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.routing.RoutingAlgorithm;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.trafficlights.TrafficLight;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.trafficlights.TrafficLightState;
 import de.tudresden.sumo.cmd.Edge;
 import de.tudresden.sumo.cmd.Junction;
 import de.tudresden.sumo.cmd.Lane;
 import de.tudresden.sumo.cmd.Simulation;
 import de.tudresden.sumo.cmd.Vehicle;
+import de.tudresden.sumo.cmd.Trafficlight;
 import de.tudresden.sumo.util.SumoCommand;
 import de.tudresden.ws.container.*;
 import edu.emory.mathcs.backport.java.util.Collections;
@@ -48,7 +48,7 @@ import it.polito.appeal.traci.SumoTraciConnection;
  * @version 1.0 at 06.11.2017
  *
  */
-public class TraciSimulationController implements VehicleController, SimulationSetupExtractor, EdgeController, SimulatorObserver {
+public class TraciSimulationController implements VehicleController, SimulationSetupExtractor, EdgeController, SimulatorObserver, TrafficLightController {
 	private static final File TEMP_FILE = new File(new File(System.getProperty("java.io.tmpdir")), "road_network.tmp");
 
 	private Random _random = Randoms.getRandom(getClass());
@@ -71,6 +71,7 @@ public class TraciSimulationController implements VehicleController, SimulationS
 	private double _endY;
 
 	private Map<String, VehicleInformationContainer> _positons = new HashMap<>();
+	private Map<String, Location> trafficLightsPosition = new HashMap<>();
 
 	private boolean _initalized = false;
 
@@ -98,6 +99,10 @@ public class TraciSimulationController implements VehicleController, SimulationS
 		return CONTROLLER.values().iterator().next();
 	}
 
+	public static TrafficLightController getTrafficLightController() {
+		return CONTROLLER.values().iterator().next();
+	}
+
 	@Override
 	public synchronized void init() {
 		if (!_initalized) {
@@ -265,6 +270,12 @@ public class TraciSimulationController implements VehicleController, SimulationS
 		execute(typeCommand);
 	}
 
+	@Override
+	public void setVehicleGap(String pVehicleID, double gap) {
+	    SumoCommand typeCommand = Vehicle.setMinGap(pVehicleID, gap);
+	    execute(typeCommand);
+	}
+
 	private Location requestVehiclePosition(String pVehicleID) {
 		if (_vehiclesOutOfRange.containsKey(pVehicleID)) {
 			if (_vehiclesOutOfRange.get(pVehicleID) < _step) {
@@ -899,4 +910,107 @@ public class TraciSimulationController implements VehicleController, SimulationS
 		return _observedAreaSet;
 	}
 
+    @Override
+    public List<TrafficLight> getAllTrafficLights() {
+	    SumoCommand trafficLightCommand = Trafficlight.getIDList();
+	    Object object = requestObject(trafficLightCommand);
+	    SumoStringList trafficLightList = (SumoStringList)object;
+
+	    List<TrafficLight> result = new ArrayList<>();
+
+	    for (String trafficLightId : trafficLightList) {
+	        TrafficLight trafficLight = new TrafficLight(trafficLightId, this);
+	        List<String> controlledLanesIds = trafficLight.getControlledLaneIDs();
+
+	        boolean found = false;
+	        for (String laneId : controlledLanesIds) {
+	        	if (isLaneUsable(laneId)) {
+	        		found = true;
+	        		break;
+				}
+			}
+
+			if (found) {
+	        	result.add(trafficLight);
+			}
+		}
+
+		return result;
+    }
+
+    @Override
+    public List<String> getControlledLanes(String pTrafficLightID) {
+        SumoCommand trafficLightCommand = Trafficlight.getControlledLanes(pTrafficLightID);
+        Object object = requestObject(trafficLightCommand);
+        SumoStringList list = (SumoStringList)object;
+
+		List<String> result = new ArrayList<>();
+
+        for (String lane : list) {
+        	result.add(lane);
+		}
+
+		return result;
+    }
+
+    @Override
+    public Map<RoadNetworkLink, TrafficLightState> getTrafficLightStateForEachLane(String pTrafficLightID) {
+	    SumoCommand trafficLightCommand = Trafficlight.getRedYellowGreenState(pTrafficLightID);
+	    Object trafficLightObject = requestObject(trafficLightCommand);
+	    String trafficLightStates = (String)trafficLightObject;
+
+	    Map<RoadNetworkLink, TrafficLightState> result = new HashMap<>();
+	    List<RoadNetworkLink> controlledLinks = getControlledLinks(pTrafficLightID);
+
+	    for (int i = 0 ; i < controlledLinks.size() ; ++i) {
+	    	switch (trafficLightStates.toLowerCase().charAt(i)) {
+				case 'g':
+					result.put(controlledLinks.get(i), TrafficLightState.GREEN);
+					break;
+				case 'y':
+					result.put(controlledLinks.get(i), TrafficLightState.YELLOW);
+					break;
+				case 'r':
+					result.put(controlledLinks.get(i), TrafficLightState.RED);
+					break;
+				case 'o':
+					result.put(controlledLinks.get(i), TrafficLightState.OFF);
+					break;
+				default:
+					throw new AssertionError("Unexpected value!");
+			}
+		}
+
+        return result;
+    }
+
+	@Override
+	public Location getTrafficLightPosition(String pTrafficLightID) {
+		return trafficLightsPosition.get(pTrafficLightID);
+	}
+
+	public List<RoadNetworkLink> getControlledLinks(String pTrafficLightID) {
+		SumoCommand trafficLightCommand = Trafficlight.getControlledLinks(pTrafficLightID);
+		Object controlledLinksObject = requestObject(trafficLightCommand);
+		SumoLinkList controlledLanes = (SumoLinkList)controlledLinksObject;
+
+		List<RoadNetworkLink> result = new ArrayList<>();
+
+		for (SumoLink link : controlledLanes) {
+			RoadNetworkLink roadNetworkLink = new RoadNetworkLink(link.from, link.to);
+			result.add(roadNetworkLink);
+		}
+
+		return result;
+	}
+
+	private boolean isLaneUsable(String laneID) {
+		List<Location> laneShape = getLaneShape(laneID);
+		for (Location location : laneShape) {
+			if (0 <= location.getLongitude() && location.getLongitude() <= _endX - _startX && 0 <= location.getLatitude() && location.getLatitude() <= _endY - _startY) {
+				return true;
+			}
+		}
+		return false;
+	}
 }

src/de/tud/kom/p2psim/impl/topology/movement/vehicular/sumo/simulation/controller/xml/XMLSimulationController.java

@@ -215,6 +215,11 @@ public class XMLSimulationController implements VehicleController, SimulationSet
 		_vehicleDataInformationHandler.getVehiclePositions().get(pVehicleID).setSpeed(pVehicleSpeed);
 	}
 
+	@Override
+	public void setVehicleGap(String pVehicleID, double gap) {
+		_vehicleDataInformationHandler.getVehiclePositions().get(pVehicleID).setGap(gap);
+	}
+
 	@Override
 	public double getVehicleSpeed(String pVehicleID) {
 		return _vehicleDataInformationHandler.getVehiclePositions().get(pVehicleID).getSpeed();

src/de/tud/kom/p2psim/impl/topology/placement/TrafficLightPlacementModel.java

+package de.tud.kom.p2psim.impl.topology.placement;
+
+import de.tud.kom.p2psim.api.topology.movement.MovementModel;
+import de.tud.kom.p2psim.api.topology.movement.SimLocationActuator;
+import de.tud.kom.p2psim.impl.topology.PositionVector;
+import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.traci.TraciSimulationController;
+import de.tudarmstadt.maki.simonstrator.api.EventHandler;
+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.SimulationSetupExtractor;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.TrafficLightController;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.trafficlights.TrafficLight;
+import de.tudarmstadt.maki.simonstrator.api.util.XMLConfigurableConstructor;
+
+import java.util.HashMap;
+import java.util.LinkedList;
+import java.util.List;
+import java.util.Map;
+
+public class TrafficLightPlacementModel implements MovementModel, EventHandler {
+    private static TrafficLightPlacementModel PLACEMENT;
+    private final List<SimLocationActuator> components;
+    private final Map<String, SimLocationActuator> idComponentMatcher;
+
+    private final String sumoExe;
+    private final String sumoConfigFile;
+    private final int offsetX;
+    private final int offsetY;
+    private final int width;
+    private final int height;
+    private final long timestepConversion = Time.SECOND;
+
+    private double scenarioWidth = 0;
+    private double scenarioHeight = 0;
+
+    private static final Location DEFAULT_LOCATION = new PositionVector(Double.NaN, Double.NaN);
+    private boolean initialized = false;
+
+    private TrafficLightController controller;
+    private SimulationSetupExtractor extractor;
+
+    @XMLConfigurableConstructor({ "sumoExe", "sumoConfigFile", "offsetX", "offsetY", "width", "height" })
+    public TrafficLightPlacementModel(String sumoExe, String sumoConfigFile, String offsetX, String offsetY, String width, String height) {
+        PLACEMENT = this;
+        this.components = new LinkedList<>();
+        this.idComponentMatcher = new HashMap<>();
+        this.sumoExe = sumoExe;
+        this.sumoConfigFile = sumoConfigFile;
+        this.offsetX = Integer.parseInt(offsetX);
+        this.offsetY = Integer.parseInt(offsetY);
+        this.width = Integer.parseInt(width);
+        this.height = Integer.parseInt(height);
+    }
+
+    @Override
+    public void eventOccurred(Object content, int type) {
+        long currentTime = Time.getCurrentTime() / timestepConversion;
+
+		while (controller.getStep() - controller.getStart() < currentTime) {
+			if (!controller.nextStep()) {
+				return;
+			}
+		}
+
+		List<TrafficLight> allTrafficLights = controller.getAllTrafficLights();
+
+		for (int i = 0 ; i < allTrafficLights.size() ; ++i) {
+            TrafficLight trafficlight = allTrafficLights.get(i);
+
+        }
+
+		/*
+        List<String> allVehicles = _controller.getAllVehicles();
+
+        for (int i = 0; i < allVehicles.size(); i++) {
+            String vehicle = allVehicles.get(i);
+
+            Location position = _controller.getVehiclePosition(vehicle);
+
+            if (position == null) {
+                allVehicles.remove(i--);
+                continue;
+            }
+
+            SimLocationActuator component = requestSimActuator(vehicle);
+
+            try {
+                RoutedNetLayer routedNetLayer = component.getHost().getComponent(RoutedNetLayer.class);
+                for (SimNetInterface netInterface : routedNetLayer.getSimNetworkInterfaces()) {
+                    if (netInterface.isOffline()) {
+                        netInterface.goOnline();
+                    }
+                }
+            } catch (ComponentNotAvailableException e) {
+                e.printStackTrace();
+            }
+
+            component.updateCurrentLocation(new PositionVector(position.getLongitude(), position.getLatitude()));
+
+            component.setMovementSpeed(_controller.getVehicleSpeed(vehicle));
+
+        }
+
+        if (allVehicles.size() != idComponentMatcher.size()) {
+            ArrayList<String> registeredVehicles = new ArrayList<>(idComponentMatcher.keySet());
+            for (int i = 0; i < registeredVehicles.size(); i++) {
+                String vehicle = registeredVehicles.get(i);
+                if (!allVehicles.contains(vehicle)) {
+                    addFreeHost(vehicle);
+                }
+            }
+        }
+
+        if (Time.getCurrentTime() < 5 * Time.SECOND) {
+            for (SimLocationActuator simLocationActuator : freeComponents) {
+                simLocationActuator.updateCurrentLocation(DEFAULT_LOCATION);
+            }
+        }
+
+        // Reschedule next step
+        Event.scheduleWithDelay(timeBetweenMoveOperations, this, null, 0);
+        */
+    }
+
+    @Override
+    public void addComponent(SimLocationActuator actuator) {
+        actuator.updateCurrentLocation(DEFAULT_LOCATION);
+        components.add(actuator);
+    }
+
+    @Override
+    public void placeComponent(SimLocationActuator actuator) {
+        actuator.updateCurrentLocation(DEFAULT_LOCATION);
+        if (!initialized) {
+            initializeModel();
+        }
+    }
+
+    protected void initializeModel() {
+        if (!initialized) {
+            if (sumoExe != null) {
+                TraciSimulationController traciSimulationController = TraciSimulationController.createSimulationController(sumoExe, sumoConfigFile);
+                controller = traciSimulationController;
+                controller.setObservedArea(offsetX, offsetY, offsetX + width, offsetY + height);
+				controller.init();
+				controller.nextStep();
+
+				extractor = traciSimulationController;
+            }
+
+            scenarioWidth = extractor.getScenarioWidth();
+            scenarioHeight = extractor.getScenarioHeight();
+
+            System.out.println("Initialization: done.");
+        }
+    }
+
+    @Override
+    public void setTimeBetweenMoveOperations(long time) {
+
+    }
+}

src/de/tud/kom/p2psim/impl/vehicular/DefaultVehicleInformationComponent.java

@@ -135,10 +135,15 @@ public class DefaultVehicleInformationComponent implements VehicleInformationCom
 	}
 
 	@Override
-	public void setVehicleSpeed(String vehicleID, double vehicleSpeed) {
+	public void setVehicleSpeed(double vehicleSpeed) {
 		controller.setVehicleSpeed(vehicleID, vehicleSpeed);
 	}
 
+	@Override
+	public void setVehicleGap(double vehicleGap) {
+		controller.setVehicleGap(vehicleID, vehicleGap);
+	}
+
 	@Override
 	public void resetVehicleID() {
 		vehicleID = null;