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