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