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
32
33
34
35
36
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
155
156
157 protected TabooSet<NavPoint> tried;
158
159 protected NavPoint tryingNav;
160
161 protected Location initialLocation;
162
163
164
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
181
182
183 protected void getBackOnNavGraph() {
184 if (!executing) return;
185
186 if (isOnNavGraph()) {
187
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
196 return;
197 }
198
199
200 while (tryingNav == null) {
201 tryingNav = DistanceUtils.getNearest(tried.filter(bot.getWorldView().getAll(NavPoint.class).values()), info.getLocation());
202
203
204 if (tryingNav == null || info.getLocation().getDistance(tryingNav.getLocation()) > 2000) {
205
206
207 if (tried.size() == 0) {
208
209 runSomewhere();
210
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
224 initialLocation = info.getLocation();
225 for (IStuckDetector stuckDetector : stuckDetectors) {
226 stuckDetector.reset();
227 stuckDetector.setBotTarget(tryingNav);
228 }
229 runner.reset();
230
231
232 }
233 }
234
235
236
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
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
269
270 for (IStuckDetector stuckDetector : stuckDetectors) {
271 if (stuckDetector.isStuck()) {
272
273 runFailed();
274 return false;
275 }
276 }
277
278 return true;
279 } else {
280
281 runFailed();
282 return false;
283 }
284 }
285
286 protected void runFailed() {
287
288
289 tried.add(tryingNav);
290 tryingNav = null;
291 }
292
293 }