1 package cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector;
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.IWorldView;
10 import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
11 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectEvent;
12 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectListener;
13 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
14 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
15 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
16 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.EndMessage;
17 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Self;
18
19 public class UT2004PositionStuckDetector implements IStuckDetector {
20
21 private static double DEFAULT_MIN_DIAMETER = 20.0;
22
23 private static double DEFAULT_MIN_Z = 40.0;
24
25 private static int DEFAULT_HISTORY_LENGTH = 8;
26
27 private UT2004Bot bot;
28
29 private double minMovementDiameter;
30
31 private int historyLength;
32
33 private List<Location> locationHistory;
34
35 private boolean stuck = false;
36
37 private boolean botWaiting = false;
38
39 private boolean enabled = false;
40
41 private class SelfListener implements IWorldObjectListener<Self> {
42
43 public SelfListener(IWorldView worldView) {
44 worldView.addObjectListener(Self.class, this);
45 }
46
47 @Override
48 public void notify(IWorldObjectEvent<Self> event) {
49 eventSelf(event);
50 }
51
52 };
53
54
55
56
57
58
59
60 private class EndMessageListener implements IWorldEventListener<EndMessage> {
61
62 public EndMessageListener(IWorldView worldView) {
63 worldView.addEventListener(EndMessage.class, this);
64 }
65
66 @Override
67 public void notify(EndMessage event) {
68 eventEndMessage(event);
69 }
70
71 };
72
73 private SelfListener selfListener;
74
75 private EndMessageListener endMessageListener;
76
77 private Logger log;
78
79 private double minMovementZ;
80
81 public UT2004PositionStuckDetector(UT2004Bot bot) {
82 this(bot, DEFAULT_HISTORY_LENGTH, DEFAULT_MIN_DIAMETER, DEFAULT_MIN_Z);
83 }
84
85 public UT2004PositionStuckDetector(UT2004Bot bot, int historyLength, double minMovementDiameter, double minMovementZ) {
86 if (this.log == null) {
87 this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
88 }
89 this.bot = bot;
90 selfListener = new SelfListener(bot.getWorldView());
91 endMessageListener = new EndMessageListener(bot.getWorldView());
92
93 this.historyLength = historyLength;
94
95 if (this.historyLength < 0) throw new IllegalArgumentException("historyLength can't be < 0");
96
97 this.minMovementDiameter = minMovementDiameter;
98 this.minMovementZ = minMovementZ;
99
100 this.locationHistory = new ArrayList<Location>(this.historyLength);
101 }
102
103 @Override
104 public void setEnabled(boolean state) {
105 if (this.enabled == state) return;
106 this.enabled = state;
107 }
108
109 @Override
110 public void setBotWaiting(boolean state) {
111 this.botWaiting = state;
112 }
113
114 @Override
115 public void setBotTarget(ILocated target) {
116
117 }
118
119 public void eventSelf(IWorldObjectEvent<Self> event) {
120 if (!enabled) return;
121 if (botWaiting) return;
122
123 locationHistory.add(event.getObject().getLocation());
124 while (locationHistory.size() > historyLength) {
125 locationHistory.remove(0);
126 }
127 }
128
129 public void eventEndMessage(EndMessage event) {
130 if (!enabled) return;
131 if (botWaiting) return;
132
133 if (locationHistory.size() == historyLength) {
134 double maxDistance = Double.NEGATIVE_INFINITY;
135 double maxHeight = Double.NEGATIVE_INFINITY;
136 for (Location loc1 : locationHistory){
137 for (Location loc2: locationHistory) {
138 if (maxDistance < loc1.getDistance2D(loc2)) {
139 maxDistance = loc1.getDistance2D(loc2);
140 }
141 if (Math.abs(loc1.z - loc2.z) > maxHeight) {
142 maxHeight = Math.abs(loc1.z - loc2.z);
143 }
144 }
145 }
146 if (maxDistance < this.minMovementDiameter && maxHeight < this.minMovementZ) {
147 if (log != null && log.isLoggable(Level.WARNING)) log.warning("Bot stuck detected. Distance: " + maxDistance + " < " + this.minMovementDiameter + " && Height: " + maxHeight + " < " + minMovementZ);
148 stuck = true;
149 } else {
150 stuck = false;
151 }
152 }
153 }
154
155
156 @Override
157 public boolean isStuck() {
158 return stuck;
159 }
160
161 @Override
162 public void reset() {
163 if (log != null && log.isLoggable(Level.FINER)) log.finer("Reset.");
164 this.locationHistory.clear();
165 stuck = false;
166 }
167
168 }