View Javadoc

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   
7   import cz.cuni.amis.pogamut.base.agent.navigation.IStuckDetector;
8   import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
9   import cz.cuni.amis.pogamut.base.utils.logging.LogCategory;
10  import cz.cuni.amis.pogamut.base.utils.math.DistanceUtils;
11  import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
12  import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
13  import cz.cuni.amis.pogamut.base3d.worldview.object.Rotation;
14  import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.AgentInfo;
15  import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.Senses;
16  import cz.cuni.amis.pogamut.ut2004.agent.module.utils.TabooSet;
17  import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004GetBackToNavGraph;
18  import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathRunner;
19  import cz.cuni.amis.pogamut.ut2004.agent.navigation.UT2004RunStraight;
20  import cz.cuni.amis.pogamut.ut2004.agent.navigation.loquenavigator.KefikRunner;
21  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004DistanceStuckDetector;
22  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004PositionStuckDetector;
23  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004TimeStuckDetector;
24  import cz.cuni.amis.pogamut.ut2004.bot.command.AdvancedLocomotion;
25  import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
26  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbcommands.Move;
27  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.EndMessage;
28  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
29  
30  /**
31   * This class is meant to provide easy "get-back-to-navigation-graph-in-order-I-can-safely-navigate-through-map"
32   * implementation. 
33   * 
34   * Automatically uses {@link UT2004TimeStuckDetector}, {@link UT2004PositionStuckDetector} and {@link UT2004DistanceStuckDetector}.
35   * 
36   * @author Jimmy
37   */
38  public class UT2004GetBackToNavGraph implements IUT2004GetBackToNavGraph {
39  
40  	public static final int CLOSE_ENOUGH = 50;
41  
42  	public static final double MAX_ANGLE = UT2004RunStraight.MAX_ANGLE;
43  	
44  	protected UT2004Bot bot;
45  	
46  	protected AgentInfo info;
47  	
48  	protected IUT2004PathRunner runner;
49  	
50  	protected boolean executing;
51  	
52  	protected LogCategory log;
53  	
54  	protected int randomMoveDirection;
55  
56  	protected IWorldEventListener<EndMessage> endListener = new IWorldEventListener<EndMessage>() {
57  		
58  		@Override
59  		public void notify(EndMessage event) {
60  			getBackOnNavGraph();
61  		}
62  		
63  	};
64  	
65  	protected List<IStuckDetector> stuckDetectors = new ArrayList<IStuckDetector>();
66  
67  	protected ILocated focus;
68  	
69  	public UT2004GetBackToNavGraph(UT2004Bot bot, AgentInfo info, AdvancedLocomotion move) {
70  		this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
71  		this.bot = bot;
72  		this.info = info;		
73  		this.runner = new KefikRunner(bot, info, move, log);
74  		
75  		stuckDetectors.add(new UT2004TimeStuckDetector(bot, 3000, 5000));
76  		stuckDetectors.add(new UT2004PositionStuckDetector(bot));
77  		stuckDetectors.add(new UT2004DistanceStuckDetector(bot));
78  		
79  		bot.getWorldView().addEventListener(EndMessage.class, endListener);
80  	}
81  	
82  	@Override
83  	public LogCategory getLog() {
84  		return log;
85  	}
86  	
87  	@Override
88  	public void addStuckDetector(IStuckDetector stuckDetector) {
89  		stuckDetectors.add(stuckDetector);
90  	}
91  	
92  	@Override
93  	public void removeStuckDetector(IStuckDetector stuckDetector) {
94  		stuckDetectors.remove(stuckDetector);
95  	}
96  	
97  	@Override
98  	public void clearStuckDetectors() {
99  		stuckDetectors.clear();
100 	}
101 		
102 	@Override
103 	public NavPoint getNearestNavPoint() {
104 		return info.getNearestNavPoint();
105 	}
106 	
107 	@Override
108 	public boolean isOnNavGraph() {
109 		return info.isOnNavGraph();
110 	}
111 	
112 	@Override
113 	public boolean isExecuting() {
114 		return executing;
115 	}
116 	
117 	@Override
118 	public void setFocus(ILocated located) {
119 		this.focus = located;
120 	}
121 	
122 	@Override
123 	public void backToNavGraph() {
124 		if (executing) return;
125 		
126 		if (log != null && log.isLoggable(Level.INFO)) log.info("STARTED");
127 		
128 		reset();
129 		
130 		initialLocation = info.getLocation();
131 		
132 		for (IStuckDetector stuckDetector : stuckDetectors) {			
133 			stuckDetector.reset();
134 			stuckDetector.setEnabled(true);
135 		}
136 			
137 		executing = true;
138 	}
139 	
140 	@Override
141 	public void stop() {
142 		if (!executing) return;
143 		if (log != null && log.isLoggable(Level.INFO)) log.info("STOPPED");
144 				
145 		executing = false;
146 		reset();
147 		
148 		for (IStuckDetector stuckDetector : stuckDetectors) {
149 			stuckDetector.setEnabled(false);
150 		}
151 	}
152 	
153 	//
154 	// VARIABLES
155 	//
156 	
157 	protected TabooSet<NavPoint> tried;
158 
159 	protected NavPoint tryingNav;
160 	
161 	protected Location initialLocation;
162 	
163 	//
164 	// RESET
165 	// 
166 	
167 	protected void reset() {
168 		if (log != null && log.isLoggable(Level.FINER)) log.finer("Reset");
169 		
170 		if (tried == null) {
171 			tried = new TabooSet<NavPoint>(bot);
172 		} else {
173 			tried.clear();
174 		}
175 		
176 		tryingNav = null;		
177 	}
178 	
179 	//
180 	// EXECUTION
181 	//
182 
183 	protected void getBackOnNavGraph() {
184 		if (!executing) return;
185 	
186 		if (isOnNavGraph()) {
187 			// WE'VE MANAGED TO GET BACK ON NAVIGATION GRAPH
188 			if (log != null && log.isLoggable(Level.INFO)) log.info("Got back to Navigation Graph.");
189 			stop();
190 			return;
191 		}
192 		
193 		while (true) {
194 			if (runToNavPoint()) {
195 				// WE'RE ON THE WAY...
196 				return;
197 			}
198 			// RUNNING FAILED
199 			// => select new navpoint
200 			while (tryingNav == null) {
201 				tryingNav = DistanceUtils.getNearest(tried.filter(bot.getWorldView().getAll(NavPoint.class).values()), info.getLocation());
202 			
203 				// CHECK SUITABILITY
204 				if (tryingNav == null || info.getLocation().getDistance(tryingNav.getLocation()) > 2000) {
205 					// FAILURE, NO MORE SUITABLE NAV POINTS TO TRY
206 					// => clear taboo set and start retrying...
207 					if (tried.size() == 0) {
208 						// TOTAL FAILURE!!! No suitable navpoints :-( lest try to move the bot a bit.
209 						runSomewhere();
210 						//stop();
211 						return;
212 					}
213 					
214 					tried.clear();
215 					tryingNav = null;	
216 					
217 					continue;
218 				}
219 			}
220 			
221 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Trying to get to: " + tryingNav);
222 			
223 			// NAV POINT CHOSEN
224 			initialLocation = info.getLocation();
225 			for (IStuckDetector stuckDetector : stuckDetectors) {
226 				stuckDetector.reset();
227 				stuckDetector.setBotTarget(tryingNav);
228 			}
229 			runner.reset();
230 			
231 			// CONTINUE WITH NEXT CYCLE THAT WILL TRY TO RUN TO tryingNav
232 		}
233 	}
234 	
235 	/**
236 	 * Last resort - keep trying random points 200 UT units from bot location - 90 degrees left, right and backwards. :-)
237 	 */
238 	protected void runSomewhere() {
239 		randomMoveDirection++;
240 		if (randomMoveDirection >= 2)
241 			randomMoveDirection = -1;
242 					
243 		Location backwardsLoc = bot.getLocation();
244 		
245 		Rotation rot = bot.getRotation();
246 		rot.setYaw(rot.getYaw() + randomMoveDirection * 16000);
247 		backwardsLoc = backwardsLoc.sub(rot.toLocation().getNormalized().scale(200));					
248 		
249 		double hDistance = bot.getLocation().getDistance2D(backwardsLoc);
250 		double vDistance = bot.getLocation().getDistanceZ(backwardsLoc);
251 		
252 		double angle = Math.atan(Math.abs(vDistance) / hDistance);		
253 		runner.runToLocation(initialLocation, backwardsLoc, null, focus == null ? backwardsLoc : focus, null, angle < MAX_ANGLE);				
254 	}
255 
256 	protected boolean runToNavPoint() {
257 		if (tryingNav == null) return false;
258 		// WE'RE TRYING TO RUN TO SOME NAVPOINT
259 		
260 		if (log != null && log.isLoggable(Level.FINE)) log.fine("Running to: " + tryingNav);
261 		
262 		double hDistance = bot.getLocation().getDistance2D(tryingNav.getLocation());
263 		double vDistance = bot.getLocation().getDistanceZ(tryingNav.getLocation());
264 		
265 		double angle = Math.atan(Math.abs(vDistance) / hDistance);
266 		
267 		if (runner.runToLocation(initialLocation, tryingNav.getLocation(), null, focus == null ? tryingNav.getLocation() : focus, null, angle < MAX_ANGLE)) {
268 			// RUNNING SEEMS OK
269 			// => check stuck detectors
270 			for (IStuckDetector stuckDetector : stuckDetectors) {
271 				if (stuckDetector.isStuck()) {
272 					// WE'RE STUCK!
273 					runFailed();
274 					return false;
275 				}
276 			}
277 			
278 			return true;
279 		} else {
280 			// RUNNER FAILED...
281 			runFailed();			
282 			return false;
283 		}
284 	}
285 
286 	protected void runFailed() {
287 		// RUNNING FAILED 
288 		// => ban nav point
289 		tried.add(tryingNav);
290 		tryingNav = null;
291 	}
292 
293 }