/
navigation_algorithm.py
312 lines (210 loc) · 10.9 KB
/
navigation_algorithm.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
#!/usr/bin/env python
#
# - - - - I A R - - - -
#
# s1311631 Angus Pearson
# s1346981 Jevgenij Zubovskij
#
from navigation_state import Navigation_State
import constants
import sys
import time
import math
class Navigation_Algorithm:
# check if we are stuck
def is_stuck(self, dist):
#check if we are scraping on the sides
# multiple of 1.2 as 1.0 is handled by following
stuck_cone_left = dist[1] > constants.CONST_WALL_DIST
# multiple of 1.2 as 1.0 is handled by following
stuck_cone_right = dist[4] > constants.CONST_WALL_DIST
#check if we are about to be stuck in the front
stuck_cone_front = dist[2] > constants.CONST_WALL_DIST*0.5 or dist[3] > constants.CONST_WALL_DIST*0.5
return stuck_cone_left or stuck_cone_right or stuck_cone_front
# check if we "see" the left wall and it is closer than the one on the right
def should_follow_left_wall(self, dist, system_state):
within_range = dist[0] > constants.CONST_WALL_DIST * 0.5
#to not switch wall if two walls nearby
followed_right = system_state == constants.STATE_RIGHT_FOLLOW
return within_range and dist[0] > dist[5] and not followed_right
# check if we "see" the right wall and it is closer than the one on the left
def should_follow_right_wall(self, dist, system_state):
within_range = dist[5] > constants.CONST_WALL_DIST * 0.5
followed_left = system_state == constants.STATE_LEFT_FOLLOW
return within_range and not(dist[0] > dist[5]) and not followed_left
# check if we are over the distance threshold w.r.t. object on the left
def too_close_to_left(self, dist):
distance_close = dist[0] > constants.CONST_WALL_DIST
return distance_close
# check if we are over the distance threshold w.r.t. object on the right
def too_close_to_right(self, dist):
distance_close = dist[5] > constants.CONST_WALL_DIST
return distance_close
# check if we are under the distance threshold w.r.t. object on the right
def is_away_from_right(self, dist):
wall_in_range = dist[5] < constants.CONST_WALL_DIST * 0.8
return wall_in_range
# check if we are under the distance threshold w.r.t. object on the left
def is_away_from_left(self, dist):
wall_in_range = dist[0] < constants.CONST_WALL_DIST * 0.8
return wall_in_range
# check if there is more space on the right of the robot than on the left
def is_more_space_on_right(self, dist):
values_on_right = dist[3] + dist[4] + dist[5]
values_on_left = dist[1] + dist[2] + dist[0]
return (values_on_left > values_on_right)
# check if the system is being unstuck
def is_being_unstuck(self, state):
return (state is constants.STATE_STUCK_LEFT) or (state is constants.STATE_STUCK_RIGHT)
# check if it is more beneficial to unstuck by turning to the right
def should_unstuck_right(self, dist, system_state):
stuck_on_left = system_state == constants.STATE_LEFT_FOLLOW
return stuck_on_left or self.is_more_space_on_right(dist)
# check if robot is "bored"
def bored(self, boredom_counter):
return boredom_counter >= constants.CONST_WALL_BORED_MAX
# check if we are handling boredom
def is_boredom_handled(self, state):
return state == constants.STATE_BOREDOM_ROTATE or state == constants.STATE_BOREDOM_DRIVE
#function to determine if algorithm permits exploration
def can_explore(self, result, pathing_state):
#key point - maximum exploration until we find the 1st food source
#determine if we are following a wall
wall_following = result.system_state == constants.STATE_RIGHT_FOLLOW or result.system_state == constants.STATE_LEFT_FOLLOW
#determine if have not explored this round yet and done with main part of algorithm
#TODO uncomment the food bit maybe
should_explore_pathing = pathing_state.exploration_not_done() and not pathing_state.has_uncollected_food()
#if pathing not activated, we do not care if we have completed the main portion of the algorithm
exploration_beneficial = should_explore_pathing or not pathing_state.algorithm_activated
#return result that if we are wall following and if we would benefit from exploration and are already not doing so
return wall_following and exploration_beneficial and not result.boredom
def new_state(self, nav_state, pathing_state, comms, ds, odo, odo_state, pf):
result = Navigation_State()
result = nav_state
############################
# HANDLE BOREDOM
############################
#TODO check if traiviling off in a random direction is more useful potentially (or make it collect only after it's done with food)
if self.can_explore(result, pathing_state):
#keep coubnt of round we followed a wall
result.boredom_counter += 1
if result.boredom_counter > constants.MAX_BOREDOM:
print "BORED"
#drive away from the wall we were hugging
if result.system_state == constants.STATE_LEFT_FOLLOW:
comms.drive(constants.CONST_SPEED, -constants.CONST_SPEED)
else:
comms.drive(-constants.CONST_SPEED, constants.CONST_SPEED)
t0 = time.time()
prior_odo = None
# do for number of rounds that fit in 1 second
for x in xrange(int(500 / constants.MEASUREMENT_PERIOD_MS)):
prior_odo = odo_state
odo_state = odo.new_state(odo_state, comms.get_odo())
nav_state.dist = comms.get_ir()
t1 = time.time()
dt = t1 - t0
t0 = t1
delta_s = pf.m._euclidean((prior_odo.x, prior_odo.y), (odo_state.x, odo_state.y))
dtheta = prior_odo.theta - odo_state.theta
pf.push_params(dt, delta_s, dtheta, nav_state.dist)
predict_state, varience = pf()
odo_state.x, odo_state.y, odo_state.theta = predict_state
ds.push(odo_state, nav_state.dist)
#sleep to let sensors detect stuff
#drive forwards until stuck
comms.drive(constants.CONST_SPEED, constants.CONST_SPEED)
#make the boredom driving forward happen, reset boredom counter
result.boredom = True
result.boredom_counter = 0
#variable to indicate if reactive avoidance is in control of the robot
result.yielding_control = False
############################
# IF STUCK
############################
if self.is_stuck(result.dist):
if result.boredom:
print "DONE BEING BORED"
#disable boredom
result.boredom = False
#make sure to denote exploration end on this round
pathing_state.complete_exploration()
# do not interrupt if already handle
if self.is_being_unstuck(result.system_state):
return result
# determine direction of where better to turn to unstuck
if self.should_unstuck_right(result.dist, result.system_state):
result.system_state = constants.STATE_STUCK_RIGHT
result.speed_l = constants.CONST_SPEED
result.speed_r = -constants.CONST_SPEED
else:
result.system_state = constants.STATE_STUCK_LEFT
result.speed_l = -constants.CONST_SPEED
result.speed_r = constants.CONST_SPEED
return result
############################
# IF BOREDOM
############################
#if boredom active, continue driving until we get stuck
if result.boredom:
return result
#####################
##WALL FOLLOWING LEFT
#####################
#if not stuck
result.yielding_control = True
if self.should_follow_left_wall(result.dist, result.system_state):
turn_least = constants.CONST_SPEED * constants.TURN_LESS
turn_most = constants.CONST_SPEED * constants.TURN_MORE
no_turn = constants.CONST_SPEED
speed_r = result.speed_r
speed_l = result.speed_l
# set state accordingly
result.system_state = constants.STATE_LEFT_FOLLOW
# keep the distance within the threshold range
if self.too_close_to_left(result.dist) and not (speed_l == turn_most and speed_r == turn_least):
speed_l = turn_most
speed_r = turn_least
elif self.is_away_from_left(result.dist) and not (speed_l == turn_least and speed_r == turn_most):
speed_l = turn_least
speed_r = turn_most
elif not (speed_l == no_turn and speed_r == no_turn):
speed_l = no_turn
speed_r = no_turn
result.speed_r = speed_r
result.speed_l = speed_l
#####################
##WALL FOLLOWING RIGHT
#####################
elif self.should_follow_right_wall(result.dist, result.system_state):
#print("following right")
turn_least = constants.CONST_SPEED * constants.TURN_LESS
turn_most = constants.CONST_SPEED * constants.TURN_MORE
no_turn = constants.CONST_SPEED
speed_r = result.speed_r
speed_l = result.speed_l
# set state accordingly
result.system_state = constants.STATE_RIGHT_FOLLOW
# keep the distance within the threshold range
if self.too_close_to_right(result.dist) and not (speed_l == turn_least and speed_r == turn_most):
speed_l = turn_least
speed_r = turn_most
elif self.is_away_from_right(result.dist) and not (speed_l == turn_most and speed_r == turn_least):
speed_l = turn_most
speed_r = turn_least
elif not (speed_l == no_turn and speed_r == no_turn):
speed_l = no_turn
speed_r = no_turn
result.speed_r = speed_r
result.speed_l = speed_l
######################
# IF NONE OF THE ABOVE
#####################
else:
# reset variables as not doing anything
result.boredom_counter = 0
# set state accordingly
result.system_state = constants.STATE_DRIVE_FORWARD
result.speed_l = constants.CONST_SPEED
result.speed_r = constants.CONST_SPEED
return result