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