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 void addStuckDetector(IStuckDetector stuckDetector) {
84  		stuckDetectors.add(stuckDetector);
85  	}
86  	
87  	@Override
88  	public void removeStuckDetector(IStuckDetector stuckDetector) {
89  		stuckDetectors.remove(stuckDetector);
90  	}
91  	
92  	@Override
93  	public void clearStuckDetectors() {
94  		stuckDetectors.clear();
95  	}
96  		
97  	@Override
98  	public NavPoint getNearestNavPoint() {
99  		return info.getNearestNavPoint();
100 	}
101 	
102 	@Override
103 	public boolean isOnNavGraph() {
104 		return info.isOnNavGraph();
105 	}
106 	
107 	@Override
108 	public boolean isExecuting() {
109 		return executing;
110 	}
111 	
112 	@Override
113 	public void setFocus(ILocated located) {
114 		this.focus = located;
115 	}
116 	
117 	@Override
118 	public void backToNavGraph() {
119 		if (executing) return;
120 		
121 		if (log != null && log.isLoggable(Level.INFO)) log.info("STARTED");
122 		
123 		reset();
124 		
125 		initialLocation = info.getLocation();
126 		
127 		for (IStuckDetector stuckDetector : stuckDetectors) {			
128 			stuckDetector.reset();
129 			stuckDetector.setEnabled(true);
130 		}
131 			
132 		executing = true;
133 	}
134 	
135 	@Override
136 	public void stop() {
137 		if (!executing) return;
138 		if (log != null && log.isLoggable(Level.INFO)) log.info("STOPPED");
139 				
140 		executing = false;
141 		reset();
142 		
143 		for (IStuckDetector stuckDetector : stuckDetectors) {
144 			stuckDetector.setEnabled(false);
145 		}
146 	}
147 	
148 	//
149 	// VARIABLES
150 	//
151 	
152 	protected TabooSet<NavPoint> tried;
153 
154 	protected NavPoint tryingNav;
155 	
156 	protected Location initialLocation;
157 	
158 	//
159 	// RESET
160 	// 
161 	
162 	protected void reset() {
163 		if (log != null && log.isLoggable(Level.FINER)) log.finer("Reset");
164 		
165 		if (tried == null) {
166 			tried = new TabooSet<NavPoint>(bot);
167 		} else {
168 			tried.clear();
169 		}
170 		
171 		tryingNav = null;		
172 	}
173 	
174 	//
175 	// EXECUTION
176 	//
177 
178 	protected void getBackOnNavGraph() {
179 		if (!executing) return;
180 	
181 		if (isOnNavGraph()) {
182 			// WE'VE MANAGED TO GET BACK ON NAVIGATION GRAPH
183 			if (log != null && log.isLoggable(Level.INFO)) log.info("Got back to Navigation Graph.");
184 			stop();
185 			return;
186 		}
187 		
188 		while (true) {
189 			if (runToNavPoint()) {
190 				// WE'RE ON THE WAY...
191 				return;
192 			}
193 			// RUNNING FAILED
194 			// => select new navpoint
195 			while (tryingNav == null) {
196 				tryingNav = DistanceUtils.getNearest(tried.filter(bot.getWorldView().getAll(NavPoint.class).values()), info.getLocation());
197 			
198 				// CHECK SUITABILITY
199 				if (tryingNav == null || info.getLocation().getDistance(tryingNav.getLocation()) > 2000) {
200 					// FAILURE, NO MORE SUITABLE NAV POINTS TO TRY
201 					// => clear taboo set and start retrying...
202 					if (tried.size() == 0) {
203 						// TOTAL FAILURE!!! No suitable navpoints :-( lest try to move the bot a bit.
204 						runSomewhere();
205 						//stop();
206 						return;
207 					}
208 					
209 					tried.clear();
210 					tryingNav = null;	
211 					
212 					continue;
213 				}
214 			}
215 			
216 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Trying to get to: " + tryingNav);
217 			
218 			// NAV POINT CHOSEN
219 			initialLocation = info.getLocation();
220 			for (IStuckDetector stuckDetector : stuckDetectors) {
221 				stuckDetector.reset();
222 				stuckDetector.setBotTarget(tryingNav);
223 			}
224 			runner.reset();
225 			
226 			// CONTINUE WITH NEXT CYCLE THAT WILL TRY TO RUN TO tryingNav
227 		}
228 	}
229 	
230 	/**
231 	 * Last resort - keep trying random points 200 UT units from bot location - 90 degrees left, right and backwards. :-)
232 	 */
233 	protected void runSomewhere() {
234 		randomMoveDirection++;
235 		if (randomMoveDirection >= 2)
236 			randomMoveDirection = -1;
237 					
238 		Location backwardsLoc = bot.getLocation();
239 		
240 		Rotation rot = bot.getRotation();
241 		rot.setYaw(rot.getYaw() + randomMoveDirection * 16000);
242 		backwardsLoc = backwardsLoc.sub(rot.toLocation().getNormalized().scale(200));					
243 		
244 		double hDistance = bot.getLocation().getDistance2D(backwardsLoc);
245 		double vDistance = bot.getLocation().getDistanceZ(backwardsLoc);
246 		
247 		double angle = Math.atan(Math.abs(vDistance) / hDistance);		
248 		runner.runToLocation(initialLocation, backwardsLoc, null, focus == null ? backwardsLoc : focus, null, angle < MAX_ANGLE);				
249 	}
250 
251 	protected boolean runToNavPoint() {
252 		if (tryingNav == null) return false;
253 		// WE'RE TRYING TO RUN TO SOME NAVPOINT
254 		
255 		if (log != null && log.isLoggable(Level.FINE)) log.fine("Running to: " + tryingNav);
256 		
257 		double hDistance = bot.getLocation().getDistance2D(tryingNav.getLocation());
258 		double vDistance = bot.getLocation().getDistanceZ(tryingNav.getLocation());
259 		
260 		double angle = Math.atan(Math.abs(vDistance) / hDistance);
261 		
262 		if (runner.runToLocation(initialLocation, tryingNav.getLocation(), null, focus == null ? tryingNav.getLocation() : focus, null, angle < MAX_ANGLE)) {
263 			// RUNNING SEEMS OK
264 			// => check stuck detectors
265 			for (IStuckDetector stuckDetector : stuckDetectors) {
266 				if (stuckDetector.isStuck()) {
267 					// WE'RE STUCK!
268 					runFailed();
269 					return false;
270 				}
271 			}
272 			
273 			return true;
274 		} else {
275 			// RUNNER FAILED...
276 			runFailed();			
277 			return false;
278 		}
279 	}
280 
281 	protected void runFailed() {
282 		// RUNNING FAILED 
283 		// => ban nav point
284 		tried.add(tryingNav);
285 		tryingNav = null;
286 	}
287 
288 }