View Javadoc

1   /*
2    * Copyright (C) 2014 AMIS research group, Faculty of Mathematics and Physics, Charles University in Prague, Czech Republic
3    *
4    * This program is free software: you can redistribute it and/or modify
5    * it under the terms of the GNU General Public License as published by
6    * the Free Software Foundation, either version 3 of the License, or
7    * (at your option) any later version.
8    *
9    * This program is distributed in the hope that it will be useful,
10   * but WITHOUT ANY WARRANTY; without even the implied warranty of
11   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12   * GNU General Public License for more details.
13   *
14   * You should have received a copy of the GNU General Public License
15   * along with this program.  If not, see <http://www.gnu.org/licenses/>.
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   * Accelerated version of {@link UT2004Pathexecutor}. Uses
55   * {@link LocationUpdate} as impulse for navigation logic cycle. Should be used
56   * with accelerated versions of the stuck detectors.
57   *
58   * @author Bogo
59   * @param <PATH_ELEMENT>
60   */
61  public class UT2004AcceleratedPathExecutor<PATH_ELEMENT extends ILocated> extends BasePathExecutor<PATH_ELEMENT> implements IUT2004PathExecutor<PATH_ELEMENT>, IUT2004PathExecutorHelper<PATH_ELEMENT> {
62  
63      /**
64       * When doing {@link UT2004PathExecutor#extendPath(List)}, how many OLD
65       * (already passed by elements) should be left in the merged path.
66       *
67       * Some nodes are needed due to lift/teleport navigation!
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      * Current focus of the bot.
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                     // GLOBAL TIMEOUT
152                     newState.setGlobalTimeout(true);
153                 } else {
154                     // STUCK DETECTED
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      * Merges path together.
204      *
205      * @param currPath
206      * @param currIndex
207      * @param morePath
208      * @return
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 //			if (mergedIndex < 0 && element == currPathElement) mergedIndex = mergedPath.size()-1;
224         }
225         for (int i = (mergeFirst ? 1 : 0); i < morePath.size(); ++i) {
226             PATH_ELEMENT element = morePath.get(i);
227             mergedPath.add(element);
228 //			if (mergedIndex < 0 && element == currPathElement) mergedIndex = mergedPath.size()-1;
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      * If the path is not zero-length, recalls
249      * {@link IUT2004PathNavigator#newPath(List)} and set the path into the
250      * GB2004 via {@link SetRoute}.
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      * Sets the path into the GB2004 via {@link SetRoute} whenever switch occurs
272      * and the rest of the path is greater than 32 path elements.
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 }