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