1 package org.opentrafficsim.demo.lanechange;
2
3 import java.awt.BorderLayout;
4 import java.awt.Frame;
5 import java.awt.geom.Line2D;
6 import java.lang.reflect.InvocationTargetException;
7 import java.rmi.RemoteException;
8 import java.util.ArrayList;
9 import java.util.Collection;
10 import java.util.HashSet;
11 import java.util.LinkedHashSet;
12 import java.util.Set;
13
14 import javax.naming.NamingException;
15 import javax.swing.JFrame;
16 import javax.swing.JPanel;
17 import javax.swing.SwingUtilities;
18 import javax.swing.event.EventListenerList;
19
20 import org.djunits.unit.UNITS;
21 import org.djunits.value.vdouble.scalar.Acceleration;
22 import org.djunits.value.vdouble.scalar.DoubleScalar;
23 import org.djunits.value.vdouble.scalar.Duration;
24 import org.djunits.value.vdouble.scalar.Length;
25 import org.djunits.value.vdouble.scalar.Speed;
26 import org.djunits.value.vdouble.scalar.Time;
27 import org.jfree.chart.ChartFactory;
28 import org.jfree.chart.ChartPanel;
29 import org.jfree.chart.JFreeChart;
30 import org.jfree.chart.StandardChartTheme;
31 import org.jfree.chart.axis.NumberAxis;
32 import org.jfree.chart.event.PlotChangeEvent;
33 import org.jfree.chart.plot.Plot;
34 import org.jfree.chart.plot.PlotOrientation;
35 import org.jfree.chart.renderer.xy.XYLineAndShapeRenderer;
36 import org.jfree.data.DomainOrder;
37 import org.jfree.data.general.DatasetChangeListener;
38 import org.jfree.data.general.DatasetGroup;
39 import org.jfree.data.xy.XYDataset;
40 import org.opentrafficsim.core.dsol.OTSModelInterface;
41 import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
42 import org.opentrafficsim.core.geometry.OTSGeometryException;
43 import org.opentrafficsim.core.geometry.OTSPoint3D;
44 import org.opentrafficsim.core.gtu.GTUDirectionality;
45 import org.opentrafficsim.core.gtu.GTUException;
46 import org.opentrafficsim.core.gtu.GTUType;
47 import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
48 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
49 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
50 import org.opentrafficsim.core.network.LongitudinalDirectionality;
51 import org.opentrafficsim.core.network.Network;
52 import org.opentrafficsim.core.network.NetworkException;
53 import org.opentrafficsim.core.network.OTSNetwork;
54 import org.opentrafficsim.core.network.OTSNode;
55 import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
56 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
57 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTUSimple;
58 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
59 import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
60 import org.opentrafficsim.road.gtu.lane.tactical.following.IDMOld;
61 import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
62 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Altruistic;
63 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Egoistic;
64 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
65 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneMovementStep;
66 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
67 import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
68 import org.opentrafficsim.road.network.factory.LaneFactory;
69 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
70 import org.opentrafficsim.road.network.lane.Lane;
71 import org.opentrafficsim.road.network.lane.LaneType;
72 import org.opentrafficsim.simulationengine.SimpleSimulator;
73
74 import nl.tudelft.simulation.dsol.SimRuntimeException;
75 import nl.tudelft.simulation.dsol.gui.swing.TablePanel;
76 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
77
78
79
80
81
82
83
84
85
86
87
88 public class LaneChangeGraph extends JFrame implements OTSModelInterface, UNITS
89 {
90
91 private static final long serialVersionUID = 20141118L;
92
93
94 static final double[] STANDARDSPEEDS = { 30, 50, 80, 100, 120 };
95
96
97 private GTUFollowingModelOld carFollowingModel;
98
99
100 private ChartPanel[][] charts;
101
102
103 private static final Length LOWERBOUND = new Length(-500, METER);
104
105
106 private static final Length MIDPOINT = new Length(0, METER);
107
108
109 private static final Length UPPERBOUND = new Length(500, METER);
110
111
112 private static LaneChangeGraph lcs;
113
114
115 private OTSNetwork network = new OTSNetwork("network");
116
117
118
119
120
121
122 LaneChangeGraph(final String title, final JPanel mainPanel)
123 {
124 super(title);
125 setContentPane(mainPanel);
126 setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
127 this.charts = new ChartPanel[2][STANDARDSPEEDS.length];
128 }
129
130
131
132
133
134
135
136
137
138
139
140
141 public static void main(final String[] args) throws NamingException, NetworkException, SimRuntimeException, GTUException,
142 OTSGeometryException, ParameterException, OperationalPlanException
143 {
144 try
145 {
146 SwingUtilities.invokeAndWait(new Runnable()
147 {
148
149 @Override
150 public void run()
151 {
152 try
153 {
154 buildGUI(args);
155 }
156 catch (NamingException | NetworkException | SimRuntimeException | GTUException exception)
157 {
158 exception.printStackTrace();
159 }
160 }
161
162 });
163 }
164 catch (InvocationTargetException | InterruptedException exception)
165 {
166 exception.printStackTrace();
167 }
168 for (int row = 0; row < lcs.charts.length; row++)
169 {
170 LaneChangeModel laneChangeModel = 0 == row ? new Egoistic() : new Altruistic();
171 for (int index = 0; index < STANDARDSPEEDS.length; index++)
172 {
173 Speed speed = new Speed(STANDARDSPEEDS[index], KM_PER_HOUR);
174
175 double startSpeedDifference = -30;
176 double endSpeedDifference = startSpeedDifference + 60;
177 ChartData data = (ChartData) lcs.charts[row][index].getChart().getXYPlot().getDataset();
178 int beginRightKey = data.addSeries("Begin of no lane change to right");
179 int endRightKey = data.addSeries("End of no lane change to right");
180 int beginLeftKey = data.addSeries("Begin of no lane change to left");
181 int endLeftKey = data.addSeries("End of no lane change to left");
182 for (double speedDifference = startSpeedDifference; speedDifference <= endSpeedDifference; speedDifference += 1)
183 {
184 Length criticalHeadway = lcs.findDecisionPoint(LaneChangeGraph.LOWERBOUND, MIDPOINT, speed,
185 new Speed(speedDifference, KM_PER_HOUR), laneChangeModel, true);
186 if (null != criticalHeadway)
187 {
188 data.addXYPair(beginRightKey, speedDifference, criticalHeadway.getInUnit(METER));
189 }
190 criticalHeadway = lcs.findDecisionPoint(MIDPOINT, LaneChangeGraph.UPPERBOUND, speed,
191 new Speed(speedDifference, KM_PER_HOUR), laneChangeModel, true);
192 if (null != criticalHeadway)
193 {
194 data.addXYPair(endRightKey, speedDifference, criticalHeadway.getInUnit(METER));
195 }
196 criticalHeadway = lcs.findDecisionPoint(LaneChangeGraph.LOWERBOUND, MIDPOINT, speed,
197 new Speed(speedDifference, KM_PER_HOUR), laneChangeModel, false);
198 if (null != criticalHeadway)
199 {
200 data.addXYPair(beginLeftKey, speedDifference, criticalHeadway.getInUnit(METER));
201 }
202 else
203 {
204 lcs.findDecisionPoint(LaneChangeGraph.LOWERBOUND, MIDPOINT, speed,
205 new Speed(speedDifference, KM_PER_HOUR), laneChangeModel, false);
206 }
207 criticalHeadway = lcs.findDecisionPoint(MIDPOINT, LaneChangeGraph.UPPERBOUND, speed,
208 new Speed(speedDifference, KM_PER_HOUR), laneChangeModel, false);
209 if (null != criticalHeadway)
210 {
211 data.addXYPair(endLeftKey, speedDifference, criticalHeadway.getInUnit(METER));
212 }
213 Plot plot = lcs.charts[row][index].getChart().getPlot();
214 plot.notifyListeners(new PlotChangeEvent(plot));
215 }
216 }
217 }
218
219 }
220
221
222
223
224
225
226
227
228
229 public static void buildGUI(final String[] args) throws NamingException, NetworkException, SimRuntimeException, GTUException
230 {
231 JPanel mainPanel = new JPanel(new BorderLayout());
232 lcs = new LaneChangeGraph("Lane change graphs", mainPanel);
233 TablePanel chartsPanel = new TablePanel(STANDARDSPEEDS.length, 2);
234 mainPanel.add(chartsPanel, BorderLayout.CENTER);
235 for (int index = 0; index < STANDARDSPEEDS.length; index++)
236 {
237 lcs.charts[0][index] =
238 new ChartPanel(lcs.createChart(String.format("Egoistic reference car at %.0fkm/h", STANDARDSPEEDS[index]),
239 STANDARDSPEEDS[index]));
240 chartsPanel.setCell(lcs.charts[0][index], index, 0);
241 }
242 for (int index = 0; index < STANDARDSPEEDS.length; index++)
243 {
244 lcs.charts[1][index] =
245 new ChartPanel(lcs.createChart(String.format("Altruistic reference car at %.0fkm/h", STANDARDSPEEDS[index]),
246 STANDARDSPEEDS[index]));
247 chartsPanel.setCell(lcs.charts[1][index], index, 1);
248 }
249 lcs.pack();
250 lcs.setExtendedState(Frame.MAXIMIZED_BOTH);
251 lcs.setVisible(true);
252 }
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271 private Length findDecisionPoint(Length low, Length high, final Speed referenceSpeed, final Speed speedDifference,
272 final LaneChangeModel laneChangeModel, final boolean mergeRight) throws NamingException, NetworkException,
273 SimRuntimeException, GTUException, OTSGeometryException, ParameterException, OperationalPlanException
274 {
275
276 Network network = new OTSNetwork("lane change graph network");
277 GTUType gtuType = new GTUType("car");
278 Set<GTUType> compatibility = new HashSet<>();
279 compatibility.add(gtuType);
280 LaneType laneType = new LaneType("CarLane", compatibility);
281 final Speed speedLimit = new Speed(120, KM_PER_HOUR);
282
283 Lane[] lanes = LaneFactory.makeMultiLane(network, "Road with two lanes",
284 new OTSNode(network, "From", new OTSPoint3D(LOWERBOUND.getSI(), 0, 0)),
285 new OTSNode(network, "To", new OTSPoint3D(UPPERBOUND.getSI(), 0, 0)), null, 2, laneType, speedLimit, null,
286 LongitudinalDirectionality.DIR_PLUS);
287
288 Set<DirectedLanePosition> initialLongitudinalPositions = new LinkedHashSet<>(1);
289 initialLongitudinalPositions
290 .add(new DirectedLanePosition(lanes[mergeRight ? 0 : 1], new Length(0, METER), GTUDirectionality.DIR_PLUS));
291
292
293
294 SimpleSimulator simpleSimulator =
295 new SimpleSimulator(new Time(0.0, SECOND), new Duration(0.0, SECOND), new Duration(3600.0, SECOND), this);
296 this.carFollowingModel = new IDMPlusOld(new Acceleration(1, METER_PER_SECOND_2),
297 new Acceleration(1.5, METER_PER_SECOND_2), new Length(2, METER), new Duration(1, SECOND), 1d);
298 this.carFollowingModel = new IDMOld(new Acceleration(1, METER_PER_SECOND_2), new Acceleration(1.5, METER_PER_SECOND_2),
299 new Length(2, METER), new Duration(1, SECOND), 1d);
300
301 BehavioralCharacteristics behavioralCharacteristics = new BehavioralCharacteristics();
302 LaneBasedIndividualGTU referenceCar = new LaneBasedIndividualGTU("ReferenceCar", gtuType, new Length(4, METER),
303 new Length(2, METER), new Speed(150, KM_PER_HOUR), simpleSimulator, this.network);
304 LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(behavioralCharacteristics,
305 new LaneBasedCFLCTacticalPlanner(this.carFollowingModel, laneChangeModel, referenceCar), referenceCar);
306 referenceCar.init(strategicalPlanner, initialLongitudinalPositions, referenceSpeed);
307 Collection<Headway> sameLaneGTUs = new LinkedHashSet<>();
308 sameLaneGTUs.add(new HeadwayGTUSimple(referenceCar.getId(), referenceCar.getGTUType(), Length.ZERO,
309 referenceCar.getLength(), referenceCar.getSpeed(), null));
310
311
312 LaneMovementStep lowResult = computeLaneChange(referenceCar, sameLaneGTUs, speedLimit, laneChangeModel, low, lanes[1],
313 speedDifference, mergeRight);
314 LaneMovementStep highResult = computeLaneChange(referenceCar, sameLaneGTUs, speedLimit, laneChangeModel, high, lanes[1],
315 speedDifference, mergeRight);
316 Length mid = null;
317 if (lowResult.getLaneChangeDirection() != highResult.getLaneChangeDirection())
318 {
319
320 final double delta = 0.1;
321 final int stepsNeeded = (int) Math.ceil(Math.log(DoubleScalar.minus(high, low).getSI() / delta) / Math.log(2));
322 for (int step = 0; step < stepsNeeded; step++)
323 {
324 Length mutableMid = low.plus(high).divideBy(2);
325 mid = mutableMid;
326 LaneMovementStep midResult = computeLaneChange(referenceCar, sameLaneGTUs, speedLimit, laneChangeModel, mid,
327 lanes[1], speedDifference, mergeRight);
328
329 if (midResult.getLaneChangeDirection() != lowResult.getLaneChangeDirection())
330 {
331 high = mid;
332 highResult = midResult;
333 }
334 else
335 {
336 low = mid;
337 lowResult = midResult;
338 }
339 }
340 }
341 else
342 {
343
344 computeLaneChange(referenceCar, sameLaneGTUs, speedLimit, laneChangeModel, low, lanes[1], speedDifference,
345 mergeRight);
346 }
347 return mid;
348 }
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368 private LaneMovementStep computeLaneChange(final LaneBasedIndividualGTU referenceCar,
369 final Collection<Headway> sameLaneGTUs, final Speed speedLimit, final LaneChangeModel laneChangeModel,
370 final Length otherCarPosition, final Lane otherCarLane, final Speed deltaV, final boolean mergeRight)
371 throws NamingException, NetworkException, SimRuntimeException, GTUException, OTSGeometryException,
372 ParameterException, OperationalPlanException
373 {
374 Set<DirectedLanePosition> initialLongitudinalPositions = new LinkedHashSet<>(1);
375 initialLongitudinalPositions.add(new DirectedLanePosition(otherCarLane, otherCarPosition, GTUDirectionality.DIR_PLUS));
376 BehavioralCharacteristics behavioralCharacteristics = new BehavioralCharacteristics();
377 LaneBasedIndividualGTU otherCar =
378 new LaneBasedIndividualGTU("otherCar", referenceCar.getGTUType(), new Length(4, METER), new Length(2, METER),
379 new Speed(150, KM_PER_HOUR), referenceCar.getSimulator(), this.network);
380 LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(behavioralCharacteristics,
381 new LaneBasedCFLCTacticalPlanner(this.carFollowingModel, laneChangeModel, otherCar), otherCar);
382 otherCar.init(strategicalPlanner, initialLongitudinalPositions, referenceCar.getSpeed().plus(deltaV));
383 Collection<Headway> preferredLaneGTUs = new LinkedHashSet<>();
384 Collection<Headway> nonPreferredLaneGTUs = new LinkedHashSet<>();
385 Length referenceCarPosition = referenceCar.position(
386 referenceCar.positions(referenceCar.getReference()).keySet().iterator().next(), referenceCar.getReference());
387 Headway otherHeadwayGTU = new HeadwayGTUSimple(otherCar.getId(), otherCar.getGTUType(),
388 otherCarPosition.minus(referenceCarPosition), otherCar.getLength(), otherCar.getSpeed(), null);
389 if (mergeRight)
390 {
391 preferredLaneGTUs.add(otherHeadwayGTU);
392 }
393 else
394 {
395 sameLaneGTUs.add(otherHeadwayGTU);
396 }
397
398
399 LaneMovementStep result = laneChangeModel.computeLaneChangeAndAcceleration(referenceCar, sameLaneGTUs,
400 mergeRight ? preferredLaneGTUs : null, mergeRight ? null : nonPreferredLaneGTUs, speedLimit,
401 new Acceleration(0.3, METER_PER_SECOND_2), new Acceleration(0.1, METER_PER_SECOND_2),
402 new Acceleration(-0.3, METER_PER_SECOND_2));
403
404 sameLaneGTUs.remove(otherHeadwayGTU);
405 otherCar.destroy();
406 return result;
407 }
408
409
410
411
412
413
414 private JFreeChart createChart(final String caption, final double speed)
415 {
416 ChartFactory.setChartTheme(new StandardChartTheme("JFree/Shadow", false));
417 ChartData chartData = new ChartData();
418 JFreeChart chartPanel =
419 ChartFactory.createXYLineChart(caption, "", "", chartData, PlotOrientation.VERTICAL, false, false, false);
420 NumberAxis xAxis = new NumberAxis("\u2192 " + "\u0394v (other car speed minus reference car speed) [km/h]");
421 xAxis.setAutoRangeIncludesZero(true);
422 double minimumDifference = -30;
423 xAxis.setRange(minimumDifference, minimumDifference + 60);
424 xAxis.setStandardTickUnits(NumberAxis.createIntegerTickUnits());
425 NumberAxis yAxis = new NumberAxis("\u2192 " + "gross headway (\u0394s) [m]");
426 yAxis.setAutoRangeIncludesZero(true);
427 yAxis.setRange(LOWERBOUND.getSI(), UPPERBOUND.getSI());
428 yAxis.setInverted(true);
429 chartPanel.getXYPlot().setDomainAxis(xAxis);
430 chartPanel.getXYPlot().setRangeAxis(yAxis);
431 final XYLineAndShapeRenderer renderer = (XYLineAndShapeRenderer) chartPanel.getXYPlot().getRenderer();
432 renderer.setBaseLinesVisible(true);
433 renderer.setBaseShapesVisible(false);
434 renderer.setBaseShape(new Line2D.Float(0, 0, 0, 0));
435 return chartPanel;
436 }
437
438
439 @Override
440 public void constructModel(final SimulatorInterface<Time, Duration, OTSSimTimeDouble> simulator)
441 throws SimRuntimeException, RemoteException
442 {
443
444 }
445
446
447 @Override
448 public final SimulatorInterface<Time, Duration, OTSSimTimeDouble> getSimulator() throws RemoteException
449 {
450 return null;
451 }
452
453 }
454
455
456 class ChartData implements XYDataset
457 {
458
459
460 private ArrayList<ArrayList<Double>> xValues = new ArrayList<>();
461
462
463 private ArrayList<ArrayList<Double>> yValues = new ArrayList<>();
464
465
466 private ArrayList<String> seriesKeys = new ArrayList<>();
467
468
469 private transient EventListenerList listenerList = new EventListenerList();
470
471
472 private DatasetGroup datasetGroup = null;
473
474
475
476
477
478
479 public final int addSeries(final String seriesName)
480 {
481 this.xValues.add(new ArrayList<Double>());
482 this.yValues.add(new ArrayList<Double>());
483 this.seriesKeys.add(seriesName);
484 return this.xValues.size() - 1;
485 }
486
487
488
489
490
491
492
493 public final void addXYPair(final int seriesKey, final double x, final double y)
494 {
495 this.xValues.get(seriesKey).add(x);
496 this.yValues.get(seriesKey).add(y);
497 }
498
499
500 @Override
501 public final int getSeriesCount()
502 {
503 return this.seriesKeys.size();
504 }
505
506
507 @Override
508 public final Comparable<?> getSeriesKey(final int series)
509 {
510 return this.seriesKeys.get(series);
511 }
512
513
514 @Override
515 public final int indexOf(@SuppressWarnings("rawtypes") final Comparable seriesKey)
516 {
517 return this.seriesKeys.indexOf(seriesKey);
518 }
519
520
521 @Override
522 public final void addChangeListener(final DatasetChangeListener listener)
523 {
524 this.listenerList.add(DatasetChangeListener.class, listener);
525 }
526
527
528 @Override
529 public final void removeChangeListener(final DatasetChangeListener listener)
530 {
531 this.listenerList.remove(DatasetChangeListener.class, listener);
532 }
533
534
535 @Override
536 public final DatasetGroup getGroup()
537 {
538 return this.datasetGroup;
539 }
540
541
542 @Override
543 public final void setGroup(final DatasetGroup group)
544 {
545 this.datasetGroup = group;
546 }
547
548
549 @Override
550 public final DomainOrder getDomainOrder()
551 {
552 return DomainOrder.ASCENDING;
553 }
554
555
556 @Override
557 public final int getItemCount(final int series)
558 {
559 return this.xValues.get(series).size();
560 }
561
562
563 @Override
564 public final Number getX(final int series, final int item)
565 {
566 return this.xValues.get(series).get(item);
567 }
568
569
570 @Override
571 public final double getXValue(final int series, final int item)
572 {
573 return this.xValues.get(series).get(item);
574 }
575
576
577 @Override
578 public final Number getY(final int series, final int item)
579 {
580 return this.yValues.get(series).get(item);
581 }
582
583
584 @Override
585 public final double getYValue(final int series, final int item)
586 {
587 return this.yValues.get(series).get(item);
588 }
589
590 }