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