1 package org.opentrafficsim.road.gtu.lane.plan.operational;
2
3 import java.util.ArrayList;
4 import java.util.List;
5
6 import org.djunits.unit.AccelerationUnit;
7 import org.djunits.unit.SpeedUnit;
8 import org.djunits.unit.TimeUnit;
9 import org.djunits.value.ValueException;
10 import org.djunits.value.vdouble.scalar.Acceleration;
11 import org.djunits.value.vdouble.scalar.Length;
12 import org.djunits.value.vdouble.scalar.Speed;
13 import org.djunits.value.vdouble.scalar.Time;
14 import org.opentrafficsim.core.geometry.OTSGeometryException;
15 import org.opentrafficsim.core.geometry.OTSLine3D;
16 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
17 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.SpeedSegment;
18 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
19 import org.opentrafficsim.core.math.Solver;
20 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
21 import org.opentrafficsim.road.network.lane.Lane;
22
23 /**
24 * Builder for several often used operational plans, based on a list of lanes. E.g., decelerate to come to a full stop at the
25 * end of a shape; accelerate to reach a certain speed at the end of a curve; drive constant on a curve; decelerate or
26 * accelerate to reach a given end speed at the end of a curve, etc.<br>
27 * TODO driving with negative speeds (backward driving) is not yet supported.
28 * <p>
29 * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
30 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
31 * </p>
32 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
33 * initial version Nov 15, 2015 <br>
34 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
35 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
36 */
37 public final class LaneOperationalPlanBuilder
38 {
39 /** Maximum acceleration for unbounded accelerations: 1E12 m/s2. */
40 private static final Acceleration MAX_ACCELERATION = new Acceleration(1E12, AccelerationUnit.SI);
41
42 /** Maximum deceleration for unbounded accelerations: -1E12 m/s2. */
43 private static final Acceleration MAX_DECELERATION = new Acceleration(-1E12, AccelerationUnit.SI);
44
45 /** Private constructor. */
46 private LaneOperationalPlanBuilder()
47 {
48 // class should not be instantiated
49 }
50
51 /**
52 * Build a plan with a path and a given start speed to try to reach a provided end speed, exactly at the end of the curve.
53 * The acceleration (and deceleration) are capped by maxAcceleration and maxDeceleration. Therefore, there is no guarantee
54 * that the end speed is actually reached by this plan.
55 * @param gtu the GTU for debugging purposes
56 * @param lanes a list of connected Lanes to do the driving on
57 * @param firstLanePosition position on the first lane with the reference point of the GTU
58 * @param distance distance to drive for reaching the end speed
59 * @param startTime the current time or a time in the future when the plan should start
60 * @param startSpeed the speed at the start of the path
61 * @param endSpeed the required end speed
62 * @param maxAcceleration the maximum acceleration that can be applied, provided as a POSITIVE number
63 * @param maxDeceleration the maximum deceleration that can be applied, provided as a NEGATIVE number
64 * @return the operational plan to accomplish the given end speed
65 * @throws OperationalPlanException when the plan cannot be generated, e.g. because of a path that is too short
66 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
67 * constructed segment list differ more than a given threshold
68 * @throws OTSGeometryException in case the lanes are not connected or firstLanePosition is larger than the length of the
69 * first lane
70 */
71 public static LaneBasedOperationalPlan buildGradualAccelerationPlan(final LaneBasedGTU gtu, final List<Lane> lanes,
72 final Length.Rel firstLanePosition, final Length.Rel distance, final Time.Abs startTime,
73 final Speed startSpeed, final Speed endSpeed, final Acceleration maxAcceleration,
74 final Acceleration maxDeceleration) throws OperationalPlanException, OTSGeometryException
75 {
76 OTSLine3D path = makePath(lanes, firstLanePosition, distance);
77 OperationalPlan.Segment segment;
78 if (startSpeed.eq(endSpeed))
79 {
80 segment = new SpeedSegment(distance.divideBy(startSpeed));
81 }
82 else
83 {
84 try
85 {
86 // t = 2x / (vt + v0); a = (vt - v0) / t
87 Time.Rel duration = distance.multiplyBy(2.0).divideBy(endSpeed.plus(startSpeed));
88 Acceleration acceleration = endSpeed.minus(startSpeed).divideBy(duration);
89 if (acceleration.si < 0.0 && acceleration.lt(maxDeceleration))
90 {
91 acceleration = maxDeceleration;
92 duration =
93 new Time.Rel(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
94 TimeUnit.SI);
95 }
96 if (acceleration.si > 0.0 && acceleration.gt(maxAcceleration))
97 {
98 acceleration = maxAcceleration;
99 duration =
100 new Time.Rel(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
101 TimeUnit.SI);
102 }
103 segment = new OperationalPlan.AccelerationSegment(duration, acceleration);
104 }
105 catch (ValueException ve)
106 {
107 throw new OperationalPlanException(ve);
108 }
109 }
110 ArrayList<OperationalPlan.Segment> segmentList = new ArrayList<>();
111 segmentList.add(segment);
112 return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, lanes);
113 }
114
115 /**
116 * Build a plan with a path and a given start speed to try to reach a provided end speed, exactly at the end of the curve.
117 * The acceleration (and deceleration) are capped by maxAcceleration and maxDeceleration. Therefore, there is no guarantee
118 * that the end speed is actually reached by this plan.
119 * @param lanes a list of connected Lanes to do the driving on
120 * @param firstLanePosition position on the first lane with the reference point of the GTU
121 * @param distance distance to drive for reaching the end speed
122 * @return the driving path as a line
123 * @throws OperationalPlanException when the length of the lanes is less than the distance when we start at the
124 * firstLanePosition on the first lane, or when the lanes list contains no elements
125 * @throws OTSGeometryException in case the lanes are not connected or firstLanePosition is larger than the length of the
126 * first lane
127 */
128 public static OTSLine3D makePath(final List<Lane> lanes,
129 final Length.Rel firstLanePosition, final Length.Rel distance) throws OperationalPlanException,
130 OTSGeometryException
131 {
132 if (lanes.size() == 0)
133 {
134 throw new OperationalPlanException("LaneOperationalPlanBuilder.makePath got a lanes list with size = 0");
135 }
136 OTSLine3D path = lanes.get(0).getCenterLine().extract(firstLanePosition, lanes.get(0).getLength());
137 for (int i = 1; i < lanes.size(); i++)
138 {
139 path = OTSLine3D.concatenate(path, lanes.get(i).getCenterLine());
140 }
141 return path.extract(0.0, distance.si);
142 }
143
144 /**
145 * Build a plan with a path and a given start speed to reach a provided end speed, exactly at the end of the curve.
146 * Acceleration and deceleration are virtually unbounded (1E12 m/s2) to reach the end speed (e.g., to come to a complete
147 * stop).
148 * @param gtu the GTU for debugging purposes
149 * @param lanes a list of connected Lanes to do the driving on
150 * @param firstLanePosition position on the first lane with the reference point of the GTU
151 * @param distance distance to drive for reaching the end speed
152 * @param startTime the current time or a time in the future when the plan should start
153 * @param startSpeed the speed at the start of the path
154 * @param endSpeed the required end speed
155 * @return the operational plan to accomplish the given end speed
156 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
157 * constructed segment list differ more than a given threshold
158 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
159 * first lane
160 */
161 public static LaneBasedOperationalPlan buildGradualAccelerationPlan(final LaneBasedGTU gtu, final List<Lane> lanes,
162 final Length.Rel firstLanePosition, final Length.Rel distance, final Time.Abs startTime,
163 final Speed startSpeed, final Speed endSpeed) throws OperationalPlanException, OTSGeometryException
164 {
165 return buildGradualAccelerationPlan(gtu, lanes, firstLanePosition, distance, startTime, startSpeed, endSpeed,
166 MAX_ACCELERATION, MAX_DECELERATION);
167 }
168
169 /**
170 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
171 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
172 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
173 * before completing the path, a truncated path that ends where the GTU stops is used instead.
174 * @param gtu the GTU for debugging purposes
175 * @param lanes a list of connected Lanes to do the driving on
176 * @param firstLanePosition position on the first lane with the reference point of the GTU
177 * @param distance distance to drive for reaching the end speed
178 * @param startTime the current time or a time in the future when the plan should start
179 * @param startSpeed the speed at the start of the path
180 * @param endSpeed the required end speed
181 * @param acceleration the acceleration to use if endSpeed > startSpeed, provided as a POSITIVE number
182 * @param deceleration the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
183 * @return the operational plan to accomplish the given end speed
184 * @throws OperationalPlanException when the construction of the operational path fails
185 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
186 * first lane
187 */
188 public static LaneBasedOperationalPlan buildMaximumAccelerationPlan(final LaneBasedGTU gtu, final List<Lane> lanes,
189 final Length.Rel firstLanePosition, final Length.Rel distance, final Time.Abs startTime,
190 final Speed startSpeed, final Speed endSpeed, final Acceleration acceleration, final Acceleration deceleration)
191 throws OperationalPlanException, OTSGeometryException
192 {
193 OTSLine3D path = makePath(lanes, firstLanePosition, distance);
194 ArrayList<OperationalPlan.Segment> segmentList = new ArrayList<>();
195 if (startSpeed.eq(endSpeed))
196 {
197 segmentList.add(new OperationalPlan.SpeedSegment(distance.divideBy(startSpeed)));
198 }
199 else
200 {
201 try
202 {
203 if (endSpeed.gt(startSpeed))
204 {
205 Time.Rel t = endSpeed.minus(startSpeed).divideBy(acceleration);
206 Length.Rel x =
207 startSpeed.multiplyBy(t).plus(acceleration.multiplyBy(0.5).multiplyBy(t).multiplyBy(t));
208 if (x.ge(distance))
209 {
210 // we cannot reach the end speed in the given distance with the given acceleration
211 Time.Rel duration =
212 new Time.Rel(
213 Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
214 TimeUnit.SI);
215 segmentList.add(new OperationalPlan.AccelerationSegment(duration, acceleration));
216 }
217 else
218 {
219 // we reach the (higher) end speed before the end of the segment. Make two segments.
220 segmentList.add(new OperationalPlan.AccelerationSegment(t, acceleration));
221 Time.Rel duration = distance.minus(x).divideBy(endSpeed);
222 segmentList.add(new OperationalPlan.SpeedSegment(duration));
223 }
224 }
225 else
226 {
227 Time.Rel t = endSpeed.minus(startSpeed).divideBy(deceleration);
228 Length.Rel x =
229 startSpeed.multiplyBy(t).plus(deceleration.multiplyBy(0.5).multiplyBy(t).multiplyBy(t));
230 if (x.ge(distance))
231 {
232 // we cannot reach the end speed in the given distance with the given deceleration
233 Time.Rel duration =
234 new Time.Rel(
235 Solver.firstSolutionAfter(0, deceleration.si / 2, startSpeed.si, -distance.si),
236 TimeUnit.SI);
237 segmentList.add(new OperationalPlan.AccelerationSegment(duration, deceleration));
238 }
239 else
240 {
241 if (endSpeed.si == 0.0)
242 {
243 // if endSpeed == 0, we cannot reach the end of the path. Therefore, build a partial path.
244 OTSLine3D partialPath = path.truncate(x.si);
245 segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
246 return new LaneBasedOperationalPlan(gtu, partialPath, startTime, startSpeed, segmentList,
247 lanes);
248 }
249 // we reach the (lower) end speed, larger than zero, before the end of the segment. Make two segments.
250 segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
251 Time.Rel duration = distance.minus(x).divideBy(endSpeed);
252 segmentList.add(new OperationalPlan.SpeedSegment(duration));
253 }
254 }
255 }
256 catch (ValueException ve)
257 {
258 throw new OperationalPlanException(ve);
259 }
260
261 }
262 return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, lanes);
263 }
264
265 /**
266 * Build a plan with a path and a given start speed to try to come to a stop with a given deceleration. If the GTU can stop
267 * before completing the given path, a truncated path that ends where the GTU stops is used instead. There is no guarantee
268 * that the OperationalPlan will lead to a complete stop.
269 * @param gtu the GTU for debugging purposes
270 * @param lanes a list of connected Lanes to do the driving on
271 * @param firstLanePosition position on the first lane with the reference point of the GTU
272 * @param distance distance to drive for reaching the end speed
273 * @param startTime the current time or a time in the future when the plan should start
274 * @param startSpeed the speed at the start of the path
275 * @param deceleration the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
276 * @return the operational plan to accomplish the given end speed
277 * @throws OperationalPlanException when construction of the operational path fails
278 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
279 * first lane
280 */
281 public static LaneBasedOperationalPlan buildStopPlan(final LaneBasedGTU gtu, final List<Lane> lanes,
282 final Length.Rel firstLanePosition, final Length.Rel distance, final Time.Abs startTime,
283 final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
284 {
285 return buildMaximumAccelerationPlan(gtu, lanes, firstLanePosition, distance, startTime, startSpeed, new Speed(
286 0.0, SpeedUnit.SI), new Acceleration(1.0, AccelerationUnit.SI), deceleration);
287 }
288 }