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