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