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