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
27
28
29
30
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
131 return;
132 }
133
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
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
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
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 }