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.ut2004.agent.module.sensor.AgentInfo;
14 import cz.cuni.amis.pogamut.ut2004.agent.module.utils.TabooSet;
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.gbinfomessages.EndMessage;
22 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
23
24 public class UT2004GetBackToNavGraph {
25
26 public static final int CLOSE_ENOUGH = 50;
27
28 public static final double MAX_ANGLE = UT2004RunStraight.MAX_ANGLE;
29
30 protected UT2004Bot bot;
31
32 protected AgentInfo info;
33
34 protected IUT2004PathRunner runner;
35
36 protected boolean executing;
37
38 protected LogCategory log;
39
40 protected IWorldEventListener<EndMessage> endListener = new IWorldEventListener<EndMessage>() {
41
42 @Override
43 public void notify(EndMessage event) {
44 getBackOnNavGraph();
45 }
46
47 };
48
49 protected List<IStuckDetector> stuckDetectors = new ArrayList<IStuckDetector>();
50
51 protected ILocated focus;
52
53 public UT2004GetBackToNavGraph(UT2004Bot bot, AgentInfo info, AdvancedLocomotion move) {
54 this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
55 this.bot = bot;
56 this.info = info;
57 this.runner = new KefikRunner(bot, info, move, log);
58
59 stuckDetectors.add(new UT2004TimeStuckDetector(bot, 3000, 10000));
60 stuckDetectors.add(new UT2004PositionStuckDetector(bot));
61 stuckDetectors.add(new UT2004DistanceStuckDetector(bot));
62
63 bot.getWorldView().addEventListener(EndMessage.class, endListener);
64 }
65
66 public NavPoint getNearestNavPoint() {
67 return info.getNearestNavPoint();
68 }
69
70 public boolean isOnNavGraph() {
71 return info.isOnNavGraph();
72 }
73
74 public boolean isExecuting() {
75 return executing;
76 }
77
78 public void execute() {
79 if (executing) return;
80
81 if (log != null && log.isLoggable(Level.INFO)) log.info("STARTED");
82
83 reset();
84
85 initialLocation = info.getLocation();
86
87 for (IStuckDetector stuckDetector : stuckDetectors) {
88 stuckDetector.reset();
89 stuckDetector.setEnabled(true);
90 }
91
92 executing = true;
93 }
94
95 public void stop() {
96 if (!executing) return;
97 if (log != null && log.isLoggable(Level.INFO)) log.info("STOPPED");
98
99 executing = false;
100 reset();
101
102 for (IStuckDetector stuckDetector : stuckDetectors) {
103 stuckDetector.setEnabled(false);
104 }
105 }
106
107
108
109
110
111 protected TabooSet<NavPoint> tried;
112
113 protected NavPoint tryingNav;
114
115 protected Location initialLocation;
116
117
118
119
120
121 protected void reset() {
122 if (log != null && log.isLoggable(Level.FINER)) log.finer("Reset");
123
124 if (tried == null) {
125 tried = new TabooSet<NavPoint>(bot);
126 } else {
127 tried.clear();
128 }
129
130 tryingNav = null;
131 }
132
133
134
135
136
137 protected void getBackOnNavGraph() {
138 if (!executing) return;
139
140 if (isOnNavGraph()) {
141
142 if (log != null && log.isLoggable(Level.INFO)) log.info("Got back to Navigation Graph.");
143 stop();
144 return;
145 }
146
147 while (true) {
148 if (runToNavPoint()) {
149
150 return;
151 }
152
153
154 while (tryingNav == null) {
155 tryingNav = DistanceUtils.getNearest(tried.filter(bot.getWorldView().getAll(NavPoint.class).values()), info.getLocation());
156
157
158 if (tryingNav == null || info.getLocation().getDistance(tryingNav.getLocation()) > 2000) {
159
160
161 if (tried.size() == 0) {
162
163 stop();
164 return;
165 }
166
167 tried.clear();
168 tryingNav = null;
169
170 continue;
171 }
172 }
173
174 if (log != null && log.isLoggable(Level.FINE)) log.fine("Trying to get to: " + tryingNav);
175
176
177 initialLocation = info.getLocation();
178 for (IStuckDetector stuckDetector : stuckDetectors) {
179 stuckDetector.reset();
180 stuckDetector.setBotTarget(tryingNav);
181 }
182 runner.reset();
183
184
185 }
186 }
187
188 protected boolean runToNavPoint() {
189 if (tryingNav == null) return false;
190
191
192 if (log != null && log.isLoggable(Level.FINE)) log.fine("Running to: " + tryingNav);
193
194 double hDistance = bot.getLocation().getDistance2D(tryingNav.getLocation());
195 double vDistance = bot.getLocation().getDistanceZ(tryingNav.getLocation());
196
197 double angle = Math.atan(Math.abs(vDistance) / hDistance);
198
199 if (runner.runToLocation(initialLocation, tryingNav.getLocation(), null, focus == null ? tryingNav.getLocation() : focus, null, angle < MAX_ANGLE)) {
200
201
202 for (IStuckDetector stuckDetector : stuckDetectors) {
203 if (stuckDetector.isStuck()) {
204
205 runFailed();
206 return false;
207 }
208 }
209
210 return true;
211 } else {
212
213 runFailed();
214 return false;
215 }
216 }
217
218 protected void runFailed() {
219
220
221 tried.add(tryingNav);
222 tryingNav = null;
223 }
224
225 public void setFocus(ILocated located) {
226 this.focus = located;
227 }
228
229
230 }