1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.HashMap;
4 import java.util.Map;
5
6 import org.djunits.value.vdouble.scalar.LinearDensity;
7 import org.djunits.value.vdouble.scalar.Speed;
8 import org.djunits.value.vdouble.scalar.Time;
9 import org.djutils.exceptions.Try;
10 import org.opentrafficsim.base.TimeStampedObject;
11 import org.opentrafficsim.base.parameters.ParameterException;
12 import org.opentrafficsim.base.parameters.ParameterTypeLength;
13 import org.opentrafficsim.base.parameters.ParameterTypes;
14 import org.opentrafficsim.base.parameters.Parameters;
15 import org.opentrafficsim.core.gtu.GTUException;
16 import org.opentrafficsim.core.network.NetworkException;
17 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
18 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
19 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
20 import org.opentrafficsim.road.gtu.lane.perception.categories.AnticipationSpeed.SpeedSet;
21 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
22
23
24
25
26
27
28
29
30
31
32
33
34
35 public class AnticipationTrafficPerception extends LaneBasedAbstractPerceptionCategory implements TrafficPerception
36 {
37
38
39 private static final long serialVersionUID = 20180313L;
40
41
42 protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
43
44
45 private Time lastSpeedTime = null;
46
47
48 private Map<RelativeLane, Double> antFromLeft = new HashMap<>();
49
50
51 private Map<RelativeLane, Double> antInLane = new HashMap<>();
52
53
54 private Map<RelativeLane, Double> antFromRight = new HashMap<>();
55
56
57 private Map<RelativeLane, Speed> speed = new HashMap<>();
58
59
60 private Map<RelativeLane, TimeStampedObject<LinearDensity>> density = new HashMap<>();
61
62
63 private static final AnticipationDensity DENSITY = new AnticipationDensity();
64
65
66
67
68
69 public AnticipationTrafficPerception(final LanePerception perception)
70 {
71 super(perception);
72 }
73
74
75 @Override
76 public Speed getSpeed(final RelativeLane lane) throws ParameterException
77 {
78 Time now = Try.assign(() -> getTimestamp(), "");
79 if (this.lastSpeedTime == null || this.lastSpeedTime.si < now.si)
80 {
81
82 this.antFromLeft.clear();
83 this.antInLane.clear();
84 this.antFromRight.clear();
85 this.speed.clear();
86 this.lastSpeedTime = now;
87 }
88 Speed vAnt = this.speed.get(lane);
89 if (vAnt == null)
90 {
91 LaneBasedGTU gtu = Try.assign(() -> getPerception().getGtu(), "");
92 Speed desiredSpeed = gtu.getDesiredSpeed();
93 vAnt = anticipationSpeed(lane, gtu.getParameters(),
94 getPerception().getPerceptionCategoryOrNull(NeighborsPerception.class),
95 getPerception().getPerceptionCategoryOrNull(InfrastructurePerception.class), desiredSpeed);
96 this.speed.put(lane, vAnt);
97 }
98 return vAnt;
99 }
100
101
102
103
104
105
106
107
108
109
110
111
112 private Speed anticipationSpeed(final RelativeLane lane, final Parameters params, final NeighborsPerception neighbors,
113 final InfrastructurePerception infra, final Speed desiredSpeed) throws ParameterException
114 {
115 if (!this.antInLane.containsKey(lane))
116 {
117 anticipateSpeedFromLane(lane, params, neighbors, desiredSpeed);
118 }
119 double v = this.antInLane.get(lane);
120 if (infra.getCrossSection().contains(lane.getLeft()))
121 {
122 if (!this.antFromLeft.containsKey(lane))
123 {
124 anticipateSpeedFromLane(lane.getLeft(), params, neighbors, desiredSpeed);
125 }
126 double fromLeft = this.antFromLeft.get(lane);
127 v = v < fromLeft ? v : fromLeft;
128 }
129 if (infra.getCrossSection().contains(lane.getRight()))
130 {
131 if (!this.antFromRight.containsKey(lane))
132 {
133 anticipateSpeedFromLane(lane.getRight(), params, neighbors, desiredSpeed);
134 }
135 double fromRight = this.antFromRight.get(lane);
136 v = v < fromRight ? v : fromRight;
137 }
138 return Speed.createSI(v);
139 }
140
141
142
143
144
145
146
147
148
149 private void anticipateSpeedFromLane(final RelativeLane lane, final Parameters params, final NeighborsPerception neighbors,
150 final Speed desiredSpeed) throws ParameterException
151 {
152 AnticipationSpeed anticipationSpeed = new AnticipationSpeed(desiredSpeed, params.getParameter(LOOKAHEAD), lane);
153 SpeedSet speedSet = neighbors.getLeaders(lane).collect(anticipationSpeed);
154 this.antFromLeft.put(lane.getRight(), speedSet.getRight().si);
155 this.antInLane.put(lane, speedSet.getCurrent().si);
156 this.antFromRight.put(lane.getLeft(), speedSet.getLeft().si);
157 }
158
159
160 @Override
161 public LinearDensity getDensity(final RelativeLane lane)
162 {
163 Time now = Try.assign(() -> getTimestamp(), "");
164 TimeStampedObject<LinearDensity> tK = this.density.get(lane);
165 if (tK == null || tK.getTimestamp().si < now.si)
166 {
167 LinearDensity k =
168 getPerception().getPerceptionCategoryOrNull(NeighborsPerception.class).getLeaders(lane).collect(DENSITY);
169 this.density.put(lane, new TimeStampedObject<>(k, now));
170 return k;
171 }
172 else
173 {
174 return tK.getObject();
175 }
176 }
177
178
179 @Override
180 public void updateAll() throws GTUException, NetworkException, ParameterException
181 {
182
183 }
184
185
186 @Override
187 public String toString()
188 {
189 return "AnticipationTrafficPerception [lastSpeedTime=" + this.lastSpeedTime + ", speed=" + this.speed + ", density="
190 + this.density + "]";
191 }
192
193 }