1 package cz.cuni.amis.pogamut.udk.agent.navigation;
2
3 import cz.cuni.amis.pogamut.base.agent.navigation.IPathExecutorState;
4 import java.util.ArrayList;
5 import java.util.List;
6 import java.util.logging.Level;
7 import java.util.logging.Logger;
8
9 import cz.cuni.amis.pogamut.base.agent.navigation.IPathExecutionEstimator;
10 import cz.cuni.amis.pogamut.base.agent.navigation.IStuckDetector;
11 import cz.cuni.amis.pogamut.base.agent.navigation.PathExecutorState;
12 import cz.cuni.amis.pogamut.base.agent.navigation.impl.BasePathExecutor;
13 import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
14 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectEventListener;
15 import cz.cuni.amis.pogamut.base.communication.worldview.object.event.WorldObjectFirstEncounteredEvent;
16 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
17 import cz.cuni.amis.pogamut.udk.agent.navigation.loquenavigator.LoqueNavigator;
18 import cz.cuni.amis.pogamut.udk.agent.navigation.timeoutestimator.UDKBasicTimeoutEstimator;
19 import cz.cuni.amis.pogamut.udk.bot.impl.UDKBot;
20 import cz.cuni.amis.pogamut.udk.communication.messages.gbcommands.SetRoute;
21 import cz.cuni.amis.pogamut.udk.communication.messages.gbinfomessages.EndMessage;
22 import cz.cuni.amis.pogamut.udk.communication.messages.gbinfomessages.Self;
23 import cz.cuni.amis.utils.NullCheck;
24
25 public class UDKPathExecutor<PATH_ELEMENT extends ILocated> extends BasePathExecutor<PATH_ELEMENT> {
26
27 private IUDKPathNavigator<PATH_ELEMENT> navigator;
28
29 private UDKBot bot;
30
31 private Self self;
32
33 private long pathExecutionStart = Long.MIN_VALUE;
34
35 private double pathExecutionTimeout = Double.POSITIVE_INFINITY;
36
37 private IWorldObjectEventListener<Self, WorldObjectFirstEncounteredEvent<Self>> selfListener = new IWorldObjectEventListener<Self, WorldObjectFirstEncounteredEvent<Self>>() {
38 @Override
39 public void notify(WorldObjectFirstEncounteredEvent<Self> event) {
40 self = event.getObject();
41 }
42 };
43
44 private IWorldEventListener<EndMessage> endMessageListener = new IWorldEventListener<EndMessage>() {
45 @Override
46 public void notify(EndMessage event) {
47 eventEndMessage();
48 }
49 };
50
51 private IPathExecutionEstimator<PATH_ELEMENT> timeoutEstimator;
52
53 public UDKPathExecutor(UDKBot bot) {
54 this(bot, null, null);
55 }
56
57 public UDKPathExecutor(UDKBot bot, IUDKPathNavigator<PATH_ELEMENT> navigator) {
58 this(bot, navigator, null);
59 }
60
61 public UDKPathExecutor(UDKBot bot, IUDKPathNavigator<PATH_ELEMENT> navigator, Logger log) {
62 super(log);
63 if (getLog() == null) {
64 setLog(bot.getLogger().getCategory(getClass().getSimpleName()));
65 }
66 NullCheck.check(bot, "bot");
67 this.bot = bot;
68 this.navigator = navigator;
69 if (this.navigator == null) {
70 this.navigator = new LoqueNavigator<PATH_ELEMENT>(bot, getLog());
71 }
72 this.navigator.setBot(bot);
73 this.navigator.setExecutor(this);
74 bot.getWorldView().addObjectListener(Self.class, WorldObjectFirstEncounteredEvent.class, selfListener);
75 bot.getWorldView().addEventListener(EndMessage.class, endMessageListener);
76 this.timeoutEstimator = new UDKBasicTimeoutEstimator<PATH_ELEMENT>();
77 }
78
79 public UDKPathExecutor<PATH_ELEMENT> setTimeoutEstimator(IPathExecutionEstimator<PATH_ELEMENT> timeoutEstimator) {
80 this.timeoutEstimator = timeoutEstimator;
81 return this;
82 }
83
84 @Override
85 protected void stopped() {
86 }
87
88 @Override
89 protected void followPathImpl() {
90 }
91
92
93
94
95
96 @Override
97 protected void pathComputedImpl() {
98 if (getPath().isEmpty()) {
99 if(self.getLocation().getDistance(pathFuture.getPathTo().getLocation()) > navigator.getPrecision()){
100 getLog().warning("Path was computed, but is empty and the bot is too far from goal. Invoking bot stuck.");
101 stuck();
102 } else {
103 targetReached();
104 }
105 } else {
106 bot.getAct().act(new SetRoute().setRoute(getPath()));
107 navigator.newPath(getPath());
108 pathExecutionStart = System.currentTimeMillis();
109 calculateTimeout();
110 }
111 }
112
113 @Override
114 protected void pathComputationFailedImpl() {
115 }
116
117 @Override
118 protected void stuckImpl() {
119 navigator.reset();
120 }
121
122
123
124
125
126 @Override
127 protected void switchToAnotherPathElementImpl() {
128 List<PATH_ELEMENT> path = getPath();
129 if (path.size() > 31 + getPathElementIndex()) {
130 List<PATH_ELEMENT> pathPart = new ArrayList<PATH_ELEMENT>(32);
131 for (int i = getPathElementIndex(); i < path.size() && i < getPathElementIndex() + 31; ++i) {
132 pathPart.add(path.get(i));
133 }
134 bot.getAct().act(new SetRoute().setRoute(pathPart));
135 }
136 }
137
138 protected void calculateTimeout() {
139 IPathExecutionEstimator<PATH_ELEMENT> estimator = timeoutEstimator;
140 if (estimator != null) {
141 pathExecutionTimeout = estimator.getTimeout(getPath());
142 } else {
143 pathExecutionTimeout = Long.MAX_VALUE;
144 }
145
146 }
147
148 @Override
149 protected void targetReachedImpl() {
150 navigator.reset();
151 }
152
153 protected void eventEndMessage() {
154 if (inState(PathExecutorState.PATH_COMPUTED) || inState(PathExecutorState.SWITCHED_TO_ANOTHER_PATH_ELEMENT)) {
155 navigate();
156 }
157 }
158
159 protected void navigate() {
160 if (log != null && log.isLoggable(Level.FINER)) log.finer("navigating");
161 double timeDelta = System.currentTimeMillis() - pathExecutionStart;
162 if (timeDelta > pathExecutionTimeout) {
163 if (log != null && log.isLoggable(Level.WARNING)) log.finer("TIMEOUT! (" + pathExecutionTimeout + "ms)");
164 stuck();
165 return;
166 }
167 IStuckDetector detector = checkStuckDetectors();
168 if (detector != null) {
169 if (log != null && log.isLoggable(Level.INFO)) log.info(detector.getClass().getSimpleName() + " has reported that the bot has stuck");
170 stuck();
171 } else {
172 navigator.navigate();
173 }
174 }
175
176 }