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   import java.util.logging.Logger;
7   
8   import cz.cuni.amis.pogamut.base.agent.navigation.IStuckDetector;
9   import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
10  import cz.cuni.amis.pogamut.base.utils.logging.LogCategory;
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.ut2004.agent.module.sensor.AgentInfo;
14  import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.Senses;
15  import cz.cuni.amis.pogamut.ut2004.agent.navigation.loquenavigator.KefikRunner;
16  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004DistanceStuckDetector;
17  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004PositionStuckDetector;
18  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004TimeStuckDetector;
19  import cz.cuni.amis.pogamut.ut2004.bot.command.AdvancedLocomotion;
20  import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
21  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbcommands.Stop;
22  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.EndMessage;
23  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Player;
24  
25  /**
26   * Run straight class is meant to provide "straight running" combined with "stuck detection".
27   * 
28   * Automatically uses {@link UT2004TimeStuckDetector}, {@link UT2004PositionStuckDetector} and {@link UT2004DistanceStuckDetector}.
29   * 
30   * @author Jimmy
31   */
32  public class UT2004RunStraight implements IUT2004RunStraight {
33  
34  	public static final int CLOSE_ENOUGH = 50;
35  
36  	public static final double AT_PLAYER = 100;
37  	
38  	public static final double AT_NAVPOINT = 50;
39  
40  	public static final double MAX_ANGLE = 45 * Math.PI / 180;
41  	
42  	protected UT2004Bot bot;
43  	
44  	protected AgentInfo info;
45  	
46  	protected IUT2004PathRunner runner;
47  	
48  	protected boolean executing;
49  	
50  	protected LogCategory log;
51  
52  	protected IWorldEventListener<EndMessage> endListener = new IWorldEventListener<EndMessage>() {
53  		
54  		@Override
55  		public void notify(EndMessage event) {
56  			runStraight();
57  		}
58  		
59  	};
60  	
61  	protected List<IStuckDetector> stuckDetectors = new ArrayList<IStuckDetector>();
62  
63  	public UT2004RunStraight(UT2004Bot bot, AgentInfo info, AdvancedLocomotion move) {
64  		this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
65  		this.bot = bot;
66  		this.info = info;		
67  		this.runner = new KefikRunner(bot, info, move, log);
68  		
69  		stuckDetectors.add(new UT2004TimeStuckDetector(bot, 3000, 10000));
70  		stuckDetectors.add(new UT2004PositionStuckDetector(bot));
71  		stuckDetectors.add(new UT2004DistanceStuckDetector(bot));
72  		
73  		bot.getWorldView().addEventListener(EndMessage.class, endListener);
74  	}
75  	
76  	@Override
77  	public Logger getLog() {
78  		return log;
79  	}
80  	
81  	@Override
82  	public void addStuckDetector(IStuckDetector stuckDetector) {
83  		stuckDetectors.add(stuckDetector);
84  	}
85  	
86  	@Override
87  	public void removeStuckDetector(IStuckDetector stuckDetector) {
88  		stuckDetectors.remove(stuckDetector);
89  	}
90  	
91  	@Override
92  	public void clearStuckDetectors() {
93  		stuckDetectors.clear();
94  	}
95  		
96  	@Override
97  	public boolean isExecuting() {
98  		return executing;
99  	}
100 	
101 	@Override
102 	public boolean isSuccess() {
103 		return success;
104 	}
105 	
106 	@Override
107 	public boolean isFailed() {
108 		return failed;
109 	}
110 	
111 	@Override
112 	public ILocated getLastTarget() {
113 		return lastTarget;
114 	}
115 	
116 	@Override
117 	public ILocated getCurrentTarget() {
118 		return currentTarget;
119 	}
120 	
121 	@Override
122 	public void setFocus(ILocated focus) {
123 		this.focus = focus;
124 	}
125 	
126 	@Override
127 	public void runStraight(ILocated target) {
128 		if (executing) {
129 			if (currentTarget != null && currentTarget.getLocation().equals(target.getLocation())) {
130 				// same target
131 				return;
132 			}
133 			// different target!
134 		}
135 		
136 		if (log != null && log.isLoggable(Level.INFO)) log.info("Run straight to: " + target);
137 		
138 		reset();
139 				
140 		initialLocation = info.getLocation();
141 		currentTarget = target;
142 		
143 		for (IStuckDetector stuckDetector : stuckDetectors) {
144 			stuckDetector.reset();
145 			stuckDetector.setEnabled(true);
146 			stuckDetector.setBotTarget(target);
147 		}
148 			
149 		executing = true;
150 		
151 		runStraight();
152 	}
153 	
154 	@Override
155 	public void stop(boolean stopMovement) {		
156 		if (!executing) return;
157 		if (log != null && log.isLoggable(Level.INFO)) log.info("STOPPED");
158 		
159 		reset();
160 		if (stopMovement) {
161 			bot.getAct().act(new Stop());
162 		}
163 		
164 		for (IStuckDetector stuckDetector : stuckDetectors) {
165 			stuckDetector.setEnabled(false);
166 		}
167 	}
168 	
169 	//
170 	// VARIABLES
171 	//
172 		
173 	protected Location initialLocation;
174 	
175 	protected ILocated currentTarget;
176 
177 	protected boolean success;
178 
179 	protected boolean failed;
180 
181 	protected ILocated lastTarget;
182 
183 	protected ILocated focus;
184 	
185 	//
186 	// RESET
187 	// 
188 	
189 	protected void reset() {
190 		if (log != null && log.isLoggable(Level.FINER)) log.finer("Reset");
191 		
192 		if (currentTarget != null) {
193 			lastTarget = currentTarget;
194 		}
195 				
196 		initialLocation = null;
197 		currentTarget = null;
198 		
199 		success = false;
200 		failed = false;
201 		executing = false;
202 		
203 		runner.reset();
204 	}
205 	
206 	//
207 	// EXECUTION
208 	//
209 
210 	protected void runStraight() {
211 		if (!executing) return;
212 		
213 		for (IStuckDetector stuckDetector : stuckDetectors) {
214 			if (stuckDetector.isStuck()) {
215 				stuck();
216 				return;
217 			}
218 		}
219 		
220 		double distance = bot.getLocation().getDistance(currentTarget.getLocation());
221 		if (currentTarget instanceof Player) {
222 			if (distance < AT_PLAYER) {
223 				success();
224 				return;
225 			}
226 		} else
227 		if (distance < AT_NAVPOINT) {
228 			success();
229 			return;
230 		}
231 		
232 		double hDistance = bot.getLocation().getDistance2D(currentTarget.getLocation());
233 		double vDistance = bot.getLocation().getDistanceZ(currentTarget.getLocation());
234 		
235 		double angle = Math.atan(Math.abs(vDistance) / hDistance);
236 		
237 		if (!runner.runToLocation(initialLocation, currentTarget.getLocation(), null, focus == null ? currentTarget : focus, null, angle < MAX_ANGLE)) {
238 			stuck();
239 			return;
240 		}
241 		
242 	}
243 
244 	protected void success() {
245 		if (log != null && log.isLoggable(Level.FINE)) log.fine("Target reached.");
246 		stop(false);
247 		success = true;		
248 	}
249 
250 	protected void stuck() {
251 		if (log != null && log.isLoggable(Level.INFO)) log.info("Running failed.");
252 		stop(true);
253 		failed = true;
254 	}
255 
256 }