1 package org.opentrafficsim.road.gtu.lane.changing;
2
3 import static org.junit.Assert.assertEquals;
4
5 import java.util.Collection;
6 import java.util.LinkedHashMap;
7 import java.util.LinkedHashSet;
8 import java.util.Map;
9 import java.util.Set;
10
11 import javax.naming.NamingException;
12
13 import org.djunits.unit.DurationUnit;
14 import org.djunits.unit.LengthUnit;
15 import org.djunits.unit.UNITS;
16 import org.djunits.value.vdouble.scalar.Acceleration;
17 import org.djunits.value.vdouble.scalar.Duration;
18 import org.djunits.value.vdouble.scalar.Length;
19 import org.djunits.value.vdouble.scalar.Speed;
20 import org.djunits.value.vdouble.scalar.Time;
21 import org.junit.Test;
22 import org.opentrafficsim.base.parameters.ParameterSet;
23 import org.opentrafficsim.base.parameters.ParameterTypes;
24 import org.opentrafficsim.core.dsol.AbstractOTSModel;
25 import org.opentrafficsim.core.dsol.OTSSimulator;
26 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
27 import org.opentrafficsim.core.geometry.OTSGeometryException;
28 import org.opentrafficsim.core.geometry.OTSLine3D;
29 import org.opentrafficsim.core.geometry.OTSPoint3D;
30 import org.opentrafficsim.core.gtu.GTUDirectionality;
31 import org.opentrafficsim.core.gtu.GTUType;
32 import org.opentrafficsim.core.gtu.RelativePosition;
33 import org.opentrafficsim.core.network.LateralDirectionality;
34 import org.opentrafficsim.core.network.LinkType;
35 import org.opentrafficsim.core.network.NetworkException;
36 import org.opentrafficsim.core.network.OTSNode;
37 import org.opentrafficsim.road.DefaultTestParameters;
38 import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
39 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
40 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTUSimple;
41 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
42 import org.opentrafficsim.road.gtu.lane.tactical.following.AbstractIDM;
43 import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
44 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.AbstractLaneChangeModel;
45 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Altruistic;
46 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Egoistic;
47 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneMovementStep;
48 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
49 import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
50 import org.opentrafficsim.road.network.OTSRoadNetwork;
51 import org.opentrafficsim.road.network.RoadNetwork;
52 import org.opentrafficsim.road.network.lane.CrossSectionLink;
53 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
54 import org.opentrafficsim.road.network.lane.Lane;
55 import org.opentrafficsim.road.network.lane.LaneType;
56 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
57
58 import nl.tudelft.simulation.dsol.SimRuntimeException;
59
60
61
62
63
64
65
66
67
68
69
70 public class LaneChangeModelTest extends AbstractOTSModel implements UNITS
71 {
72
73 private static final long serialVersionUID = 20150313;
74
75
76 private OTSRoadNetwork network = new OTSRoadNetwork("lane change model test network", true);
77
78
79
80 public LaneChangeModelTest()
81 {
82 super(new OTSSimulator());
83 }
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98 private static CrossSectionLink makeLink(final RoadNetwork network, final String name, final OTSNode from, final OTSNode to,
99 final Length width, final OTSSimulatorInterface simulator) throws OTSGeometryException, NetworkException
100 {
101
102
103 OTSPoint3D[] coordinates = new OTSPoint3D[] {new OTSPoint3D(from.getPoint().x, from.getPoint().y, 0),
104 new OTSPoint3D(to.getPoint().x, to.getPoint().y, 0)};
105 OTSLine3D line = new OTSLine3D(coordinates);
106 CrossSectionLink link = new CrossSectionLink(network, name, from, to, network.getLinkType(LinkType.DEFAULTS.ROAD), line,
107 simulator, LaneKeepingPolicy.KEEPRIGHT);
108 return link;
109 }
110
111
112
113
114
115
116
117
118
119
120
121
122
123 private static Lane makeLane(final CrossSectionLink link, final String id, final LaneType laneType, final Length latPos,
124 final Length width) throws NamingException, NetworkException, OTSGeometryException
125 {
126 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
127 speedMap.put(link.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(100, KM_PER_HOUR));
128
129 Lane result = new Lane(link, id, latPos, latPos, width, width, laneType, speedMap);
130 return result;
131 }
132
133
134
135
136
137
138
139
140
141
142
143
144
145 public static Lane[] makeMultiLane(final RoadNetwork network, final String name, final OTSNode from, final OTSNode to,
146 final LaneType laneType, final int laneCount, final OTSSimulatorInterface simulator) throws Exception
147 {
148 Length width = new Length(laneCount * 4.0, METER);
149 final CrossSectionLink link = makeLink(network, name, from, to, width, simulator);
150 Lane[] result = new Lane[laneCount];
151 width = new Length(4.0, METER);
152 for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
153 {
154
155 Length latPos = new Length((-0.5 - laneIndex) * width.getSI(), METER);
156 result[laneIndex] = makeLane(link, "lane." + laneIndex, laneType, latPos, width);
157 }
158 return result;
159 }
160
161
162
163
164
165 @Test
166 public final void changeRight() throws Exception
167 {
168 GTUType gtuType = this.network.getGtuType(GTUType.DEFAULTS.CAR);
169 LaneType laneType = this.network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
170 int laneCount = 2;
171 this.simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), this);
172 Lane[] lanes =
173 makeMultiLane(this.network, "Road with two lanes", new OTSNode(this.network, "From", new OTSPoint3D(0, 0, 0)),
174 new OTSNode(this.network, "To", new OTSPoint3D(200, 0, 0)), laneType, laneCount, this.simulator);
175
176
177
178 assertEquals("Leftmost lane should not have accessible adjacent lanes on the LEFT side", 0,
179 lanes[0].accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtuType, GTUDirectionality.DIR_PLUS).size());
180 assertEquals("Leftmost lane should have one accessible adjacent lane on the RIGHT side", 1,
181 lanes[0].accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtuType, GTUDirectionality.DIR_PLUS).size());
182 assertEquals("Rightmost lane should have one accessible adjacent lane on the LEFT side", 1,
183 lanes[1].accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtuType, GTUDirectionality.DIR_PLUS).size());
184 assertEquals("Rightmost lane should not have accessible adjacent lanes on the RIGHT side", 0,
185 lanes[1].accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtuType, GTUDirectionality.DIR_PLUS).size());
186
187 Set<DirectedLanePosition> initialLongitudinalPositions = new LinkedHashSet<>(1);
188 initialLongitudinalPositions
189 .add(new DirectedLanePosition(lanes[1], new Length(100, METER), GTUDirectionality.DIR_PLUS));
190 AbstractLaneChangeModel laneChangeModel = new Egoistic();
191 ParameterSet parameters = DefaultTestParameters.create();
192
193
194
195 LaneBasedIndividualGTU car = new LaneBasedIndividualGTU("ReferenceCar", gtuType, new Length(4, METER),
196 new Length(2, METER), new Speed(150, KM_PER_HOUR), Length.createSI(2.0), this.simulator, this.network);
197 LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
198 new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), laneChangeModel, car), car);
199 car.setParameters(parameters);
200 car.init(strategicalPlanner, initialLongitudinalPositions, new Speed(100, KM_PER_HOUR));
201 car.getTacticalPlanner().getPerception().perceive();
202 Collection<Headway> sameLaneGTUs = new LinkedHashSet<>();
203 sameLaneGTUs.add(new HeadwayGTUSimple(car.getId(), car.getGTUType(), Length.ZERO, Length.ZERO, car.getLength(),
204 car.getSpeed(), car.getAcceleration(), null));
205 Collection<Headway> preferredLaneGTUs = new LinkedHashSet<>();
206 Collection<Headway> nonPreferredLaneGTUs = new LinkedHashSet<>();
207 LaneMovementStep laneChangeModelResult = laneChangeModel.computeLaneChangeAndAcceleration(car, sameLaneGTUs,
208 preferredLaneGTUs, nonPreferredLaneGTUs, new Speed(100, KM_PER_HOUR), new Acceleration(0.3, METER_PER_SECOND_2),
209 new Acceleration(0.1, METER_PER_SECOND_2), new Acceleration(-0.3, METER_PER_SECOND_2));
210
211 assertEquals("Vehicle want to change to the right lane", LateralDirectionality.RIGHT,
212 laneChangeModelResult.getLaneChangeDirection());
213 Length rear = car.position(lanes[0], car.getRear());
214 Length front = car.position(lanes[0], car.getFront());
215 Length reference = car.position(lanes[0], RelativePosition.REFERENCE_POSITION);
216
217
218
219 Length vehicleLength = front.minus(rear);
220 Length collisionStart = reference.minus(vehicleLength);
221 Length collisionEnd = reference.plus(vehicleLength);
222 for (double pos = collisionStart.getSI() + 0.01; pos < collisionEnd.getSI() - 0.01; pos += 0.1)
223 {
224 Set<DirectedLanePosition> otherLongitudinalPositions = new LinkedHashSet<>(1);
225 otherLongitudinalPositions
226 .add(new DirectedLanePosition(lanes[1], new Length(pos, METER), GTUDirectionality.DIR_PLUS));
227
228 parameters = DefaultTestParameters.create();
229
230
231
232
233
234
235
236
237
238
239
240 LaneBasedIndividualGTU collisionCar =
241 new LaneBasedIndividualGTU("LaneChangeBlockingCarAt" + pos, gtuType, vehicleLength, new Length(2, METER),
242 new Speed(150, KM_PER_HOUR), vehicleLength.multiplyBy(0.5), this.simulator, this.network);
243 strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
244 new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), laneChangeModel, collisionCar), collisionCar);
245 collisionCar.setParameters(parameters);
246 collisionCar.init(strategicalPlanner, otherLongitudinalPositions, new Speed(100, KM_PER_HOUR));
247 preferredLaneGTUs.clear();
248 HeadwayGTUSimple collisionHWGTU = new HeadwayGTUSimple(collisionCar.getId(), collisionCar.getGTUType(),
249 new Length(pos - reference.getSI(), LengthUnit.SI), collisionCar.getLength(), collisionCar.getWidth(),
250 collisionCar.getSpeed(), collisionCar.getAcceleration(), null);
251 preferredLaneGTUs.add(collisionHWGTU);
252 laneChangeModelResult = new Egoistic().computeLaneChangeAndAcceleration(car, sameLaneGTUs, preferredLaneGTUs,
253 nonPreferredLaneGTUs, new Speed(100, KM_PER_HOUR), new Acceleration(0.3, METER_PER_SECOND_2),
254 new Acceleration(0.1, METER_PER_SECOND_2), new Acceleration(-0.3, METER_PER_SECOND_2));
255
256 assertEquals("Vehicle cannot to change to the right lane because that would result in an immediate collision", null,
257 laneChangeModelResult.getLaneChangeDirection());
258 }
259 for (double pos = 0; pos < 180; pos += 5)
260 {
261 Set<DirectedLanePosition> otherLongitudinalPositions = new LinkedHashSet<>(1);
262 otherLongitudinalPositions
263 .add(new DirectedLanePosition(lanes[1], new Length(pos, METER), GTUDirectionality.DIR_PLUS));
264
265 parameters = new ParameterSet();
266 parameters.setParameter(ParameterTypes.A, new Acceleration(1, METER_PER_SECOND_2));
267 parameters.setParameter(ParameterTypes.B, new Acceleration(1.5, METER_PER_SECOND_2));
268 parameters.setParameter(ParameterTypes.S0, new Length(2, METER));
269 parameters.setParameter(ParameterTypes.T, new Duration(1, SECOND));
270 parameters.setParameter(ParameterTypes.A, new Acceleration(1, METER_PER_SECOND_2));
271 parameters.setDefaultParameter(ParameterTypes.LOOKAHEAD);
272 parameters.setDefaultParameter(ParameterTypes.LOOKBACKOLD);
273 parameters.setParameter(AbstractIDM.DELTA, 1d);
274
275
276
277
278 LaneBasedIndividualGTU otherCar =
279 new LaneBasedIndividualGTU("OtherCarAt" + pos, gtuType, vehicleLength, new Length(2, METER),
280 new Speed(150, KM_PER_HOUR), vehicleLength.multiplyBy(0.5), this.simulator, this.network);
281 strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
282 new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), laneChangeModel, otherCar), otherCar);
283 otherCar.setParameters(parameters);
284 otherCar.init(strategicalPlanner, otherLongitudinalPositions, new Speed(100, KM_PER_HOUR));
285 preferredLaneGTUs.clear();
286 HeadwayGTUSimple collisionHWGTU = new HeadwayGTUSimple(otherCar.getId(), otherCar.getGTUType(),
287 new Length(pos - car.position(lanes[0], car.getReference()).getSI(), LengthUnit.SI), otherCar.getLength(),
288 otherCar.getWidth(), otherCar.getSpeed(), otherCar.getAcceleration(), null);
289 preferredLaneGTUs.add(collisionHWGTU);
290 laneChangeModelResult = new Egoistic().computeLaneChangeAndAcceleration(car, sameLaneGTUs, preferredLaneGTUs,
291 nonPreferredLaneGTUs, new Speed(100, KM_PER_HOUR), new Acceleration(0.3, METER_PER_SECOND_2),
292 new Acceleration(0.1, METER_PER_SECOND_2), new Acceleration(-0.3, METER_PER_SECOND_2));
293
294 laneChangeModelResult = new Altruistic().computeLaneChangeAndAcceleration(car, sameLaneGTUs, preferredLaneGTUs,
295 nonPreferredLaneGTUs, new Speed(100, KM_PER_HOUR), new Acceleration(0.3, METER_PER_SECOND_2),
296 new Acceleration(0.1, METER_PER_SECOND_2), new Acceleration(-0.3, METER_PER_SECOND_2));
297
298
299
300
301 }
302 }
303
304
305
306
307
308
309 @Override
310 public void constructModel() throws SimRuntimeException
311 {
312
313 }
314
315
316 @Override
317 public final OTSRoadNetwork getNetwork()
318 {
319 return this.network;
320 }
321
322 }