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