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