1 package org.opentrafficsim.road.network.lane.object.sensor;
2
3 import static org.junit.Assert.assertEquals;
4 import static org.opentrafficsim.core.gtu.GTUType.TRUCK;
5
6 import java.rmi.RemoteException;
7 import java.util.ArrayList;
8 import java.util.LinkedHashSet;
9 import java.util.List;
10 import java.util.Set;
11
12 import javax.naming.NamingException;
13
14 import org.djunits.unit.AccelerationUnit;
15 import org.djunits.unit.DurationUnit;
16 import org.djunits.unit.LengthUnit;
17 import org.djunits.unit.SpeedUnit;
18 import org.djunits.unit.TimeUnit;
19 import org.djunits.value.vdouble.scalar.Acceleration;
20 import org.djunits.value.vdouble.scalar.Duration;
21 import org.djunits.value.vdouble.scalar.Length;
22 import org.djunits.value.vdouble.scalar.Speed;
23 import org.djunits.value.vdouble.scalar.Time;
24 import org.opentrafficsim.base.parameters.Parameters;
25 import org.opentrafficsim.core.compatibility.Compatible;
26 import org.opentrafficsim.core.dsol.OTSModelInterface;
27 import org.opentrafficsim.core.geometry.OTSGeometryException;
28 import org.opentrafficsim.core.geometry.OTSPoint3D;
29 import org.opentrafficsim.core.gtu.GTUDirectionality;
30 import org.opentrafficsim.core.gtu.GTUException;
31 import org.opentrafficsim.core.gtu.GTUType;
32 import org.opentrafficsim.core.gtu.RelativePosition;
33 import org.opentrafficsim.core.gtu.RelativePosition.TYPE;
34 import org.opentrafficsim.core.network.LongitudinalDirectionality;
35 import org.opentrafficsim.core.network.NetworkException;
36 import org.opentrafficsim.core.network.OTSNetwork;
37 import org.opentrafficsim.core.network.OTSNode;
38 import org.opentrafficsim.road.DefaultTestParameters;
39 import org.opentrafficsim.road.gtu.lane.AbstractLaneBasedGTU;
40 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
41 import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
42 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
43 import org.opentrafficsim.road.gtu.lane.tactical.following.FixedAccelerationModel;
44 import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
45 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Egoistic;
46 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
47 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
48 import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
49 import org.opentrafficsim.road.network.factory.LaneFactory;
50 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
51 import org.opentrafficsim.road.network.lane.Lane;
52 import org.opentrafficsim.road.network.lane.LaneType;
53 import org.opentrafficsim.simulationengine.SimpleSimulator;
54
55 import nl.tudelft.simulation.dsol.SimRuntimeException;
56 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
57 import nl.tudelft.simulation.dsol.simulators.DEVSSimulator;
58 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
59 import nl.tudelft.simulation.event.EventInterface;
60 import nl.tudelft.simulation.event.EventListenerInterface;
61
62
63
64
65
66
67
68
69
70
71
72
73 public class TrafficLightSensorTest implements EventListenerInterface
74 {
75
76
77
78
79
80
81
82 private static DEVSSimulator.TimeDoubleUnit makeSimulator() throws SimRuntimeException, NamingException
83 {
84 return new SimpleSimulator(Time.ZERO, Duration.ZERO, new Duration(1, DurationUnit.HOUR), new OTSModelInterface()
85 {
86
87
88 private static final long serialVersionUID = 1L;
89
90
91 private SimulatorInterface<Time, Duration, SimTimeDoubleUnit> sim;
92
93 @Override
94 public void constructModel(final SimulatorInterface<Time, Duration, SimTimeDoubleUnit> theSimulator)
95 throws SimRuntimeException
96 {
97 this.sim = theSimulator;
98 }
99
100 @Override
101 public SimulatorInterface<Time, Duration, SimTimeDoubleUnit> getSimulator()
102 {
103 return this.sim;
104 }
105
106
107 @Override
108 public OTSNetwork getNetwork()
109 {
110 return null;
111 }
112 });
113 }
114
115
116
117
118
119
120
121
122
123
124
125
126 private static Lane[] buildNetwork(final double[] lengths, final DEVSSimulator.TimeDoubleUnit simulator)
127 throws NetworkException, NamingException, OTSGeometryException, SimRuntimeException
128 {
129 OTSNetwork network = new OTSNetwork("network");
130 OTSNode prevNode = null;
131 Lane[] result = new Lane[lengths.length];
132 LaneType laneType = LaneType.FREEWAY;
133 Speed speedLimit = new Speed(50, SpeedUnit.KM_PER_HOUR);
134 double cumulativeLength = 0;
135 for (int nodeNumber = 0; nodeNumber <= lengths.length; nodeNumber++)
136 {
137 OTSNode node = new OTSNode(network, "node" + nodeNumber, new OTSPoint3D(cumulativeLength, 0, 0));
138 if (null != prevNode)
139 {
140 LongitudinalDirectionality direction = lengths[nodeNumber - 1] > 0 ? LongitudinalDirectionality.DIR_PLUS
141 : LongitudinalDirectionality.DIR_MINUS;
142 OTSNode fromNode = LongitudinalDirectionality.DIR_PLUS == direction ? prevNode : node;
143 OTSNode toNode = LongitudinalDirectionality.DIR_PLUS == direction ? node : prevNode;
144 int laneOffset = LongitudinalDirectionality.DIR_PLUS == direction ? 0 : -1;
145 result[nodeNumber - 1] = LaneFactory.makeMultiLane(network, "Link" + nodeNumber, fromNode, toNode, null, 1,
146 laneOffset, laneOffset, laneType, speedLimit, simulator)[0];
147 System.out.println("Created lane with center line " + result[nodeNumber - 1].getCenterLine()
148 + ", directionality " + direction);
149 }
150 if (nodeNumber < lengths.length)
151 {
152 cumulativeLength += Math.abs(lengths[nodeNumber]);
153 }
154 prevNode = node;
155 }
156
157 Lane lastLane = result[lengths.length - 1];
158 Length sinkPosition = new Length(lengths[lengths.length - 1] > 0 ? lastLane.getLength().si - 10 : 10, LengthUnit.METER);
159 new SinkSensor(lastLane, sinkPosition, simulator);
160 return result;
161 }
162
163
164
165
166
167
168
169
170 private DirectedLanePosition findLaneAndPosition(final Lane[] lanes, final Length position) throws GTUException
171 {
172 Length remainingLength = position;
173 for (Lane lane : lanes)
174 {
175 if (lane.getLength().ge(remainingLength))
176 {
177 boolean reverse =
178 lane.getParentLink().getEndNode().getPoint().x < lane.getParentLink().getStartNode().getPoint().x;
179 if (reverse)
180 {
181 remainingLength = lane.getLength().minus(remainingLength);
182 }
183 return new DirectedLanePosition(lane, remainingLength,
184 reverse ? GTUDirectionality.DIR_MINUS : GTUDirectionality.DIR_PLUS);
185 }
186 remainingLength = remainingLength.minus(lane.getLength());
187 }
188 return null;
189 }
190
191
192
193
194
195
196
197
198
199
200 public final void trafficLightSensorTest()
201 throws NetworkException, NamingException, OTSGeometryException, SimRuntimeException, GTUException
202 {
203 double[][] lengthLists =
204 { { 101.1, -1, 1, -1, 1, -900 }, { 1000 }, { -1000 }, { 101.1, 900 }, { 101.1, 1, 1, 1, 1, 900 }, };
205 for (double[] lengthList : lengthLists)
206 {
207 for (int pos = 50; pos < 130; pos++)
208 {
209 System.out.println("Number of lanes is " + lengthList.length + " pos is " + pos);
210 DEVSSimulator.TimeDoubleUnit simulator = makeSimulator();
211 Lane[] lanes = buildNetwork(lengthList, simulator);
212 OTSNetwork network = (OTSNetwork) lanes[0].getParentLink().getNetwork();
213 Length a = new Length(100, LengthUnit.METER);
214 Length b = new Length(120, LengthUnit.METER);
215 DirectedLanePosition pA = findLaneAndPosition(lanes, a);
216 DirectedLanePosition pB = findLaneAndPosition(lanes, b);
217 String sensorId = "D123";
218 TYPE entryPosition = RelativePosition.FRONT;
219 TYPE exitPosition = RelativePosition.REAR;
220 List<Lane> intermediateLanes = null;
221 if (lanes.length > 2)
222 {
223 intermediateLanes = new ArrayList<>();
224 for (Lane lane : lanes)
225 {
226 if (lane.equals(pA.getLane()))
227 {
228 continue;
229 }
230 if (lane.equals(pB.getLane()))
231 {
232 break;
233 }
234 intermediateLanes.add(lane);
235 }
236 }
237 TrafficLightSensor tls = new TrafficLightSensor(sensorId, pA.getLane(), pA.getPosition(), pB.getLane(),
238 pB.getPosition(), intermediateLanes, entryPosition, exitPosition, simulator, Compatible.EVERYTHING);
239 assertEquals("Id should match the provided id", sensorId, tls.getId());
240 assertEquals("Simulator should match", simulator, tls.getSimulator());
241 assertEquals("Entry position", entryPosition, tls.getPositionTypeEntry());
242 assertEquals("Exit position", exitPosition, tls.getPositionTypeExit());
243 assertEquals("Position a", pA.getPosition().si, tls.getLanePositionA().si, 0.00001);
244 assertEquals("Position b", pB.getPosition().si, tls.getLanePositionB().si, 0.00001);
245 this.loggedEvents.clear();
246 assertEquals("event list is empty", 0, this.loggedEvents.size());
247 tls.addListener(this, NonDirectionalOccupancySensor.NON_DIRECTIONAL_OCCUPANCY_SENSOR_TRIGGER_ENTRY_EVENT);
248 tls.addListener(this, NonDirectionalOccupancySensor.NON_DIRECTIONAL_OCCUPANCY_SENSOR_TRIGGER_EXIT_EVENT);
249 assertEquals("event list is empty", 0, this.loggedEvents.size());
250
251 GTUType gtuType = TRUCK;
252 Length gtuLength = new Length(17, LengthUnit.METER);
253 Length gtuWidth = new Length(2, LengthUnit.METER);
254 Speed maximumSpeed = new Speed(90, SpeedUnit.KM_PER_HOUR);
255 LaneBasedGTU gtu = new LaneBasedIndividualGTU("GTU1", gtuType, gtuLength, gtuWidth, maximumSpeed,
256 gtuLength.multiplyBy(0.5), simulator, network);
257 Set<DirectedLanePosition> initialLongitudinalPositions = new LinkedHashSet<>(1);
258 Length initialPosition = new Length(pos, LengthUnit.METER);
259 DirectedLanePosition gtuPosition = findLaneAndPosition(lanes, initialPosition);
260 initialLongitudinalPositions.add(new DirectedLanePosition(gtuPosition.getLane(), gtuPosition.getPosition(),
261 gtuPosition.getGtuDirection()));
262 Parameters parameters = DefaultTestParameters.create();
263 LaneChangeModel laneChangeModel = new Egoistic();
264 GTUFollowingModelOld gtuFollowingModel = new FixedAccelerationModel(
265 new Acceleration(0, AccelerationUnit.METER_PER_SECOND_2), new Duration(10, DurationUnit.SECOND));
266 LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
267 new LaneBasedCFLCTacticalPlanner(gtuFollowingModel, laneChangeModel, gtu), gtu);
268 gtu.setParameters(parameters);
269 Speed initialSpeed = new Speed(10, SpeedUnit.METER_PER_SECOND);
270 if (lanes.length == 6 && pos >= 103)
271 {
272 System.out.println("let op. InitialLongitudinalPositions: " + initialLongitudinalPositions);
273 }
274 ((AbstractLaneBasedGTU) gtu).init(strategicalPlanner, initialLongitudinalPositions, initialSpeed);
275 if (initialPosition.plus(gtuLength.divideBy(2)).lt(a) || initialPosition.minus(gtuLength.divideBy(2)).gt(b))
276 {
277 assertEquals("event list is empty", 0, this.loggedEvents.size());
278 }
279 else
280 {
281 if (1 != this.loggedEvents.size())
282 {
283
284
285
286 }
287 }
288 Time stopTime = new Time(100, TimeUnit.BASE_SECOND);
289 while (simulator.getSimulatorTime().lt(stopTime))
290 {
291
292 simulator.step();
293 }
294 System.out.println("simulation time is now " + simulator);
295 if (initialPosition.minus(gtuLength.divideBy(2)).lt(b))
296 {
297 assertEquals("event list contains 2 events", 2, this.loggedEvents.size());
298 }
299 else
300 {
301 assertEquals("event list contains 0 events", 0, this.loggedEvents.size());
302 }
303 simulator.cleanUp();
304 }
305 }
306 }
307
308
309 private List<EventInterface> loggedEvents = new ArrayList<>();
310
311
312 @Override
313 public final void notify(final EventInterface event) throws RemoteException
314 {
315 System.out.println("Received event " + event);
316 this.loggedEvents.add(event);
317 }
318
319 }