1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.pathfollowing;
18
19 import cz.cuni.amis.pogamut.base.agent.navigation.IPathExecutionEstimator;
20 import cz.cuni.amis.pogamut.base.agent.navigation.IPathExecutorState;
21 import cz.cuni.amis.pogamut.base.agent.navigation.IStuckDetector;
22 import cz.cuni.amis.pogamut.base.agent.navigation.PathExecutorState;
23 import cz.cuni.amis.pogamut.base.agent.navigation.impl.BasePathExecutor;
24 import cz.cuni.amis.pogamut.base.agent.navigation.impl.PrecomputedPathFuture;
25 import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
26 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectEventListener;
27 import cz.cuni.amis.pogamut.base.communication.worldview.object.event.WorldObjectFirstEncounteredEvent;
28 import cz.cuni.amis.pogamut.base.utils.Pogamut;
29 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
30 import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.AgentInfo;
31 import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathExecutor;
32 import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathExecutorHelper;
33 import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathNavigator;
34 import cz.cuni.amis.pogamut.ut2004.agent.navigation.UT2004PathExecutorStuckState;
35 import cz.cuni.amis.pogamut.ut2004.agent.navigation.loquenavigator.LoqueNavigator;
36 import cz.cuni.amis.pogamut.ut2004.agent.navigation.timeoutestimator.UT2004BasicTimeoutEstimator;
37 import cz.cuni.amis.pogamut.ut2004.bot.command.AdvancedLocomotion;
38 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
39 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbcommands.SetRoute;
40 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.BotKilled;
41 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.LocationUpdate;
42 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
43 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Self;
44 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.SelfMessage;
45 import cz.cuni.amis.pogamut.ut2004.utils.PogamutUT2004Property;
46 import cz.cuni.amis.utils.NullCheck;
47 import cz.cuni.amis.utils.Tuple2;
48 import java.util.ArrayList;
49 import java.util.List;
50 import java.util.logging.Level;
51 import java.util.logging.Logger;
52
53
54
55
56
57
58
59
60
61 public class UT2004AcceleratedPathExecutor<PATH_ELEMENT extends ILocated> extends BasePathExecutor<PATH_ELEMENT> implements IUT2004PathExecutor<PATH_ELEMENT>, IUT2004PathExecutorHelper<PATH_ELEMENT> {
62
63
64
65
66
67
68
69 public static final int PATH_MERGE_CUTOFF = 3;
70
71 private IUT2004PathNavigator<PATH_ELEMENT> navigator;
72
73 private UT2004Bot bot;
74
75 private Self self;
76
77 private long pathExecutionStart = Long.MIN_VALUE;
78
79 private double pathExecutionTimeout = Double.POSITIVE_INFINITY;
80
81 private IWorldObjectEventListener<Self, WorldObjectFirstEncounteredEvent<Self>> selfListener = new IWorldObjectEventListener<Self, WorldObjectFirstEncounteredEvent<Self>>() {
82 @Override
83 public void notify(WorldObjectFirstEncounteredEvent<Self> event) {
84 log.fine("NEW SELF ARRIVED!");
85 self = event.getObject();
86 }
87 };
88
89 private final IWorldEventListener<LocationUpdate> locationUpdateMessageListener = new IWorldEventListener<LocationUpdate>() {
90 @Override
91 public void notify(LocationUpdate event) {
92 eventLocationUpdateMessage();
93 }
94 };
95
96 private final IWorldEventListener<BotKilled> botKilledListener = new IWorldEventListener<BotKilled>() {
97 @Override
98 public void notify(BotKilled event) {
99 stop();
100 }
101 };
102
103 private IPathExecutionEstimator<PATH_ELEMENT> timeoutEstimator;
104
105
106
107
108 private ILocated focus;
109
110 private Boolean sendingSetRoute = Pogamut.getPlatform().getBooleanProperty(PogamutUT2004Property.POGAMUT_UT2004_PATH_EXECUTOR_SEND_SET_ROUTE.getKey());
111
112 public UT2004AcceleratedPathExecutor(UT2004Bot bot, AgentInfo info, AdvancedLocomotion body, IUT2004PathNavigator<PATH_ELEMENT> navigator) {
113 this(bot, info, body, navigator, null);
114 }
115
116 public UT2004AcceleratedPathExecutor(UT2004Bot bot, AgentInfo info, AdvancedLocomotion move, IUT2004PathNavigator<PATH_ELEMENT> navigator, Logger log) {
117 super(log);
118 if (getLog() == null) {
119 setLog(bot.getLogger().getCategory(getClass().getSimpleName()));
120 }
121 NullCheck.check(bot, "bot");
122 if (sendingSetRoute == null) {
123 sendingSetRoute = false;
124 }
125 this.bot = bot;
126 this.navigator = navigator;
127 if (this.navigator == null) {
128 this.navigator = new LoqueNavigator<PATH_ELEMENT>(bot, info, move, getLog());
129 }
130 this.navigator.setBot(bot);
131 this.navigator.setExecutor(this);
132 bot.getWorldView().addObjectListener(Self.class, WorldObjectFirstEncounteredEvent.class, selfListener);
133 bot.getWorldView().addEventListener(LocationUpdate.class, locationUpdateMessageListener);
134 bot.getWorldView().addEventListener(BotKilled.class, botKilledListener);
135 this.timeoutEstimator = new UT2004BasicTimeoutEstimator<PATH_ELEMENT>();
136 }
137
138 public UT2004AcceleratedPathExecutor<PATH_ELEMENT> setTimeoutEstimator(IPathExecutionEstimator<PATH_ELEMENT> timeoutEstimator) {
139 this.timeoutEstimator = timeoutEstimator;
140 return this;
141 }
142
143 @Override
144 protected IPathExecutorState createState(PathExecutorState state) {
145 switch (state) {
146 case STUCK:
147 UT2004PathExecutorStuckState newState = new UT2004PathExecutorStuckState();
148
149 IStuckDetector detector = checkStuckDetectors();
150 if (detector == null) {
151
152 newState.setGlobalTimeout(true);
153 } else {
154
155 newState.setStuckDetector(detector);
156 }
157
158 newState.setLink(navigator.getCurrentLink());
159
160 return newState;
161 default:
162 return super.createState(state);
163 }
164 }
165
166 @Override
167 public void extendPath(List<PATH_ELEMENT> morePath) {
168 synchronized (mutex) {
169 if (morePath == null) {
170 log.warning("Cannot extendPath() with NULL path.");
171 return;
172 }
173 if (morePath.isEmpty()) {
174 log.warning("Cannot extendPath() with 0-sized path.");
175 return;
176 }
177 List<PATH_ELEMENT> currPath = getPath();
178 if (currPath == null) {
179 log.warning("Does not follow any path, cannot extendPath() now!");
180 return;
181 }
182 int currIndex = getPathElementIndex();
183
184 Tuple2<List<PATH_ELEMENT>, Integer> mergedPathAndIndex = mergePath(currPath, currIndex, morePath);
185 List<PATH_ELEMENT> newPath = mergedPathAndIndex.getFirst();
186 int newPathIndex = mergedPathAndIndex.getSecond();
187
188 this.pathFuture = new PrecomputedPathFuture<PATH_ELEMENT>(newPath.get(0), newPath.get(newPath.size() - 1), newPath);
189
190 int previousPathIndexDelta = this.pathElementIndex - this.previousPathElementIndex;
191
192 this.pathElementIndex = newPathIndex;
193 this.previousPathElementIndex = newPathIndex - previousPathIndexDelta;
194 if (this.previousPathElementIndex < 0) {
195 this.previousPathElementIndex = 0;
196 }
197
198 navigator.pathExtended(newPath, pathElementIndex);
199 }
200 }
201
202
203
204
205
206
207
208
209
210 protected Tuple2<List<PATH_ELEMENT>, Integer> mergePath(List<PATH_ELEMENT> currPath, int currIndex, List<PATH_ELEMENT> morePath) {
211 PATH_ELEMENT currPathElement = (currIndex >= 0 && currIndex < currPath.size() ? currPath.get(currIndex) : null);
212 PATH_ELEMENT lastCurrPathElement = currPath.get(currPath.size() - 1);
213 PATH_ELEMENT firstMorePathElement = morePath.get(0);
214 boolean mergeFirst = lastCurrPathElement.getLocation().getDistance(firstMorePathElement.getLocation()) < 50;
215 int cutOffIndex = (currIndex > PATH_MERGE_CUTOFF ? currIndex - PATH_MERGE_CUTOFF : 0);
216 int newPathSize = currPath.size() - cutOffIndex + morePath.size() + (mergeFirst ? -1 : 0);
217 List<PATH_ELEMENT> mergedPath = new ArrayList<PATH_ELEMENT>(newPathSize);
218 int mergedIndex = currIndex - cutOffIndex;
219
220 for (int i = cutOffIndex; i < currPath.size(); ++i) {
221 PATH_ELEMENT element = currPath.get(i);
222 mergedPath.add(element);
223
224 }
225 for (int i = (mergeFirst ? 1 : 0); i < morePath.size(); ++i) {
226 PATH_ELEMENT element = morePath.get(i);
227 mergedPath.add(element);
228
229 }
230
231 return new Tuple2<List<PATH_ELEMENT>, Integer>(mergedPath, mergedIndex);
232 }
233
234 @Override
235 public NavPointNeighbourLink getCurrentLink() {
236 return navigator.getCurrentLink();
237 }
238
239 @Override
240 protected void stopped() {
241 }
242
243 @Override
244 protected void followPathImpl() {
245 }
246
247
248
249
250
251
252 @Override
253 protected void pathComputedImpl() {
254 if (getPath().isEmpty()) {
255 targetReached();
256 } else {
257 if (sendingSetRoute) {
258 bot.getAct().act(new SetRoute().setRoute(getPath()));
259 }
260 navigator.newPath(getPath());
261 pathExecutionStart = System.currentTimeMillis();
262 calculateTimeout();
263 }
264 }
265
266 @Override
267 protected void pathComputationFailedImpl() {
268 }
269
270
271
272
273
274 @Override
275 protected void switchToAnotherPathElementImpl() {
276 List<PATH_ELEMENT> path = getPath();
277 if (path == null) {
278 return;
279 }
280 if (path.size() > 31 + getPathElementIndex()) {
281 List<PATH_ELEMENT> pathPart = new ArrayList<PATH_ELEMENT>(32);
282 for (int i = getPathElementIndex(); i < path.size() && i < getPathElementIndex() + 31; ++i) {
283 pathPart.add(path.get(i));
284 }
285 bot.getAct().act(new SetRoute().setRoute(pathPart));
286 }
287
288 PATH_ELEMENT pathElement = getPathElement();
289 for (IStuckDetector detector : getStuckDetectors()) {
290 detector.setBotTarget(pathElement);
291 }
292 }
293
294 protected void calculateTimeout() {
295 IPathExecutionEstimator<PATH_ELEMENT> estimator = timeoutEstimator;
296 if (estimator != null) {
297 pathExecutionTimeout = estimator.getTimeout(getPath());
298 } else {
299 pathExecutionTimeout = Long.MAX_VALUE;
300 }
301
302 }
303
304 protected void eventLocationUpdateMessage() {
305 synchronized (mutex) {
306 if (inState(PathExecutorState.PATH_COMPUTED) || inState(PathExecutorState.SWITCHED_TO_ANOTHER_PATH_ELEMENT)) {
307 log.fine("LOCATION UPDATE -> NAVIGATE()");
308 navigate();
309 }
310 }
311 }
312
313 protected void navigate() {
314 if (log != null && log.isLoggable(Level.FINER)) {
315 log.finer("navigating");
316 }
317 double timeDelta = System.currentTimeMillis() - pathExecutionStart;
318 if (timeDelta > pathExecutionTimeout) {
319 if (log != null && log.isLoggable(Level.WARNING)) {
320 log.log(Level.FINER, "TIMEOUT! ({0}ms)", pathExecutionTimeout);
321 }
322 stuck();
323 return;
324 }
325 IStuckDetector detector = checkStuckDetectors();
326 if (detector != null) {
327 if (log != null && log.isLoggable(Level.WARNING)) {
328 log.log(Level.WARNING, "{0} has reported that the bot has stuck", detector.getClass().getSimpleName());
329 }
330 stuck();
331 } else {
332 navigator.navigate(focus);
333 }
334 }
335
336 @Override
337 public double getRemainingDistance() {
338 double result = 0;
339
340 List<PATH_ELEMENT> path = getPath();
341
342 if (path == null) {
343 return 0;
344 }
345
346 int currPathIndex = getPathElementIndex();
347
348 if (currPathIndex >= path.size()) {
349 return 0;
350 }
351 if (currPathIndex < 0) {
352 currPathIndex = 0;
353 }
354
355 result += self.getLocation().getDistance(path.get(currPathIndex).getLocation());
356 ++currPathIndex;
357
358 for (int i = currPathIndex; i < path.size(); ++i) {
359 result += path.get(i - 1).getLocation().getDistance(path.get(i).getLocation());
360 }
361
362 return result;
363 }
364
365 @Override
366 public ILocated getFocus() {
367 return this.focus;
368 }
369
370 @Override
371 public void setFocus(ILocated located) {
372 this.focus = located;
373 }
374
375 @Override
376 public List<IStuckDetector> getStuckDetectors() {
377 return stuckDetectors;
378 }
379
380 @Override
381 protected void preStuckImpl() {
382 super.preStuckImpl();
383 }
384
385 @Override
386 protected void stuckImpl() {
387 }
388
389 @Override
390 protected void stopImpl() {
391 super.stopImpl();
392 }
393
394 @Override
395 protected void preTargetReachedImpl() {
396 super.preTargetReachedImpl();
397 }
398
399 @Override
400 protected void targetReachedImpl() {
401 }
402
403 public IUT2004PathNavigator<PATH_ELEMENT> getNavigator() {
404 return navigator;
405 }
406 }