1 package cz.cuni.amis.pogamut.udk.agent.navigation.stuckdetector;
2
3 import java.util.logging.Level;
4 import java.util.logging.Logger;
5
6 import cz.cuni.amis.pogamut.base.agent.navigation.IStuckDetector;
7 import cz.cuni.amis.pogamut.base.communication.worldview.IWorldView;
8 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectEvent;
9 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectListener;
10 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
11 import cz.cuni.amis.pogamut.udk.bot.impl.UDKBot;
12 import cz.cuni.amis.pogamut.udk.communication.messages.gbinfomessages.Self;
13
14
15
16
17
18
19
20
21
22
23
24 public class UDKTimeStuckDetector implements IStuckDetector {
25
26
27
28
29 private static final double NO_MOVEMENT_SIZE = 10;
30
31
32
33
34
35 private static int DEFAULT_TIMEOUT = 3000;
36
37
38
39
40 private static int DEFAULT_WAITING_TIMEOUT = 10000;
41
42
43
44
45 private UDKBot bot;
46
47
48
49
50
51 private double timeout;
52
53
54
55
56
57 private double waitingTimeout;
58
59 private boolean botWaiting = false;
60
61
62
63
64
65 private boolean bWasIsStuckCalled = false;
66
67 private long currentTime;
68
69
70
71
72
73
74
75 private class SelfListener implements IWorldObjectListener<Self> {
76
77 public SelfListener(IWorldView worldView) {
78 worldView.addObjectListener(Self.class, this);
79 }
80
81 @Override
82 public void notify(IWorldObjectEvent<Self> event) {
83 eventSelf(event);
84 }
85
86 };
87
88
89
90
91
92 private SelfListener selfListener;
93
94
95
96
97
98 private Double lastMovementTime = null;
99
100
101
102
103 private boolean stuck = false;
104
105 private boolean enabled;
106
107 private Logger log;
108
109 public UDKTimeStuckDetector(UDKBot bot) {
110 this(bot, DEFAULT_TIMEOUT, DEFAULT_WAITING_TIMEOUT);
111 }
112
113 public UDKTimeStuckDetector(UDKBot bot, double timeoutMillis,
114 double waitingTimeoutMillis) {
115 if (this.log == null) {
116 this.log = bot.getLogger().getCategory(
117 this.getClass().getSimpleName());
118 }
119 this.bot = bot;
120 this.timeout = timeoutMillis;
121 this.waitingTimeout = waitingTimeoutMillis;
122 selfListener = new SelfListener(bot.getWorldView());
123 }
124
125 public void eventSelf(IWorldObjectEvent<Self> event) {
126 if (!enabled) return;
127
128 currentTime = event.getObject().getSimTime();
129
130 if (!bWasIsStuckCalled) {
131 return;
132 }
133 if (event.getObject().getVelocity().size() > NO_MOVEMENT_SIZE || lastMovementTime == null) {
134 lastMovementTime = (double) event.getObject().getSimTime();
135 }
136 if (botWaiting) {
137 if (event.getObject().getSimTime() - lastMovementTime >= waitingTimeout) {
138 stuck = true;
139 if (log != null && log.isLoggable(Level.INFO)) log.info("Bot is WAITING for more than " + waitingTimeout + " ms, considering that it has stuck.");
140 }
141 } else {
142 if (event.getObject().getSimTime() - lastMovementTime >= timeout) {
143 stuck = true;
144 if (log != null && log.isLoggable(Level.INFO)) log.info("Bot should be moving but it is standing still for more than " + timeout + " ms, considering that it has stuck.");
145 }
146 }
147 }
148
149 @Override
150 public void setEnabled(boolean state) {
151 if (this.enabled == state) return;
152 this.enabled = state;
153 }
154
155 @Override
156 public void setBotWaiting(boolean state) {
157 botWaiting = state;
158 lastMovementTime = null;
159 }
160
161 @Override
162 public boolean isStuck() {
163 if (!bWasIsStuckCalled) {
164
165
166 lastMovementTime = (double) currentTime;
167 bWasIsStuckCalled = true;
168 return false;
169 }
170 return stuck;
171 }
172
173 @Override
174 public void reset() {
175 if (log != null && log.isLoggable(Level.FINER)) log.finer("Reset.");
176 lastMovementTime = Double.NEGATIVE_INFINITY;
177 bWasIsStuckCalled = false;
178 stuck = false;
179 }
180
181 @Override
182 public void setBotTarget(ILocated target) {
183
184 }
185
186 }