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, 10000));
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
150
151
152 protected TabooSet<NavPoint> tried;
153
154 protected NavPoint tryingNav;
155
156 protected Location initialLocation;
157
158
159
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
176
177
178 protected void getBackOnNavGraph() {
179 if (!executing) return;
180
181 if (isOnNavGraph()) {
182
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
191 return;
192 }
193
194
195 while (tryingNav == null) {
196 tryingNav = DistanceUtils.getNearest(tried.filter(bot.getWorldView().getAll(NavPoint.class).values()), info.getLocation());
197
198
199 if (tryingNav == null || info.getLocation().getDistance(tryingNav.getLocation()) > 2000) {
200
201
202 if (tried.size() == 0) {
203
204 runSomewhere();
205
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
219 initialLocation = info.getLocation();
220 for (IStuckDetector stuckDetector : stuckDetectors) {
221 stuckDetector.reset();
222 stuckDetector.setBotTarget(tryingNav);
223 }
224 runner.reset();
225
226
227 }
228 }
229
230
231
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
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
264
265 for (IStuckDetector stuckDetector : stuckDetectors) {
266 if (stuckDetector.isStuck()) {
267
268 runFailed();
269 return false;
270 }
271 }
272
273 return true;
274 } else {
275
276 runFailed();
277 return false;
278 }
279 }
280
281 protected void runFailed() {
282
283
284 tried.add(tryingNav);
285 tryingNav = null;
286 }
287
288 }