/
mcl.py
350 lines (287 loc) · 9.7 KB
/
mcl.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
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
#!/usr/bin/env python
import sys, os
import roslib; roslib.load_manifest('uml_mcl')
import rospy
rospy.init_node('mcl_localizer')
try:
sys.path.append('/home/cowley/project-files/ros_workspace/sample_hw7/src')
sys.path.append('/home/cowley/project-files/ros_workspace/sample_hw7/src/raycaster')
import mcl_tools
except ImportError:
print "error importing mcl tools"
sys.exit(1)
import numpy as np
from scipy.cluster.vq import vq, kmeans, kmeans2, whiten
from scipy.spatial.distance import cdist, pdist
from scipy.sparse import coo_matrix
import random
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from time import *
import random
from math import *
import angles
import threading
PAR_COUNT = 1200
NOISE = int(PAR_COUNT*.10)
cmd_vel = None
dt = 0.1
control = (0.0, 0.0)
# State
parset = [mcl_tools.random_particle() for ii in range(PAR_COUNT)]
# range thetas, or angles
range_ts = [radians(t) for t in xrange(-90, 91, 45)]
centroid = (0, 0, 0)
var_threshold = 0.70
threshold_cnt = 0
mode = 'wander'
gaussian_base = 1./(sqrt(2*pi)*0.5)
Wait, Found_Goal = False, False
# Gaussian probability
def gaussian_p(s_reading, p_reading):
global gaussian_base
mu = p_reading - s_reading
exp = -(mu*mu)/0.5
return gaussian_base * e**exp
# Particle weight assignment
def particle_weight(particle, scan):
global range_ts
return sum([gaussian_p(scan[i], mcl_tools.map_range(particle, range_ts[i]))
for i in xrange(5)])
# Low variance sampling algorithm from the book.
def low_var_sample(ps, sample_count, ws):
p_prime = []
sc_inverse = 1./len(ps)
r = random.random()*sc_inverse
c = ws[0]
i = 0
for m in xrange(sample_count):
threshold = r + i*sc_inverse
while threshold > c:
i += 1
c += ws[i]
p_prime.append(ps[i])
return p_prime
weights = np.array([1.0 for ii in xrange(PAR_COUNT)])
def particle_filter(ps, control, scan):
global weights
v, w = control
v_dt = v*dt
w_dt = w*dt
sigma = sqrt(v*v + w*w)/6.0 * dt
def motion_update(p):
x, y, t = p
new_p = (x + random.gauss(v_dt*cos(t), sigma),
y + random.gauss(v_dt*sin(t), sigma),
t + random.gauss(w_dt, sigma))
if not mcl_tools.map_hit(new_p[0], new_p[1]):
return new_p
else:
return mcl_tools.random_particle()
'''
p_prime = [motion_update(p) if not mcl_tools.map_hit(p[0], p[1]) else
mcl_tools.random_particle()
for p in ps]
'''
new_weights = np.array([particle_weight(p, scan) for p in ps])
weights *= new_weights
weights /= weights.sum()
wvar = 1./sum([w*w for w in weights])
if wvar < random.gauss(PAR_COUNT*.81, 60):
ps = mcl_tools.random_sample(ps, PAR_COUNT - NOISE, weights) + [mcl_tools.random_particle() for ii in xrange(NOISE)]
weights = [1.0 for ii in xrange(PAR_COUNT)]
else:
pass
return [motion_update(p) for p in ps]
def got_scan(msg):
global Wait
global Found_Goal
if Wait or Found_Goal:
return
global parset
global control
global centroid
laserscan = msg.ranges
'''
enum_ls = [(s, i) for i, s in enumerate(laserscan)]
max_ls_index = max(enum_ls)[1]
min_ls_val, min_ls_index = min(enum_ls)
print max_ls_index
control = (min_ls_val*0.9 - 1, (max_ls_index-(min_ls_index*0.55))*.85)
'''
# Calculate command to robot.
# if front sensor reading is greater than 3, take the difference between
# left and right diagnal sensors.
# Otherwise, slow down and go in whichever direction has more room.
parset = particle_filter(parset, control, laserscan)
mcl_tools.show_particles(parset)
mcl_tools.show_best_pose(centroid)
return
# find a path to goal tuple(x, y) from current(x, y, theta)
# specific to this scenario. Go to the middle of the hallway, go to correct x
# coordinate, go to correct y coordinate
def pathfind(goal, current):
x, y, t = current
gx, gy = goal
dx = x - gx
dy = y - 6.0 # middle of the hallway
def set_heading(new_t):
new_t = angles.r2r(new_t)
clkwise = new_t - t
ct_clkwise = 2*pi + t - new_t
return clkwise if abs(clkwise) < abs(ct_clkwise) else ct_clkwise
# are we at the right place on the x-axis?
if abs(dx) < 0.4:
print "correct longitude"
# are we at the goal??
dy = y - gy
if abs(dy) < 0.3:
# We're there! No movement signals success.
return (0.0, 0.0)
# are we facing the right direction?
dt = set_heading(3*pi/2 - dx)
v = 0.9 if abs(dt) < pi/8 else 0.0
control = (v, dt/3)
# if not, are we at the right place on the y-axis?
elif abs(dy) < 0.5:
# are we facing the right difference?
print "correct latitude"
dt = set_heading(0.0 if dx > 0 else pi)
v = 1.5 if abs(dt) < pi/8 else 0.0
control = (v, dt/3)
# if neither, get to the right latitude
else:
print "Neither latitude nor longitude are correct"
# are we facing the right difference?
dt = set_heading(3*pi/2 if dy > 0 else pi/2)
v = 1.0 if abs(dt) < pi/8 else 0.0
control = (v, dt/3)
return control
def controller(msg):
global Wait
global Found_Goal
if Wait or Found_Goal:
control = (0.0, 0.0)
cmd = Twist()
(cmd.linear.x, cmd.angular.z) = control
cmd_vel.publish(cmd)
return
global cmd_vel
global mode
global centroid
global control
global NOISE
x, y, t = centroid
laserscan = msg.ranges
# If mode is wander, just avoid hitting the walls.
if mode == 'wander' or abs(x-19.5) > 0.4:
# Is there a wall in front of us? If so, slow down and spin.
if min(laserscan[1:4]) < 1.9:
t = ((laserscan[4]+laserscan[3]) - (laserscan[1]+laserscan[0]))
if abs(t) < 1.00:
t = copysign(0.4, t)
else:
t *= 0.20
control = ((min(laserscan) - 1.0)*0.6, t)
# Otherwise, move forward and try to align with the nearest wall.
else:
l, d, t = (0, 1, 1) if laserscan[1] < laserscan[3] else (4, 3, -1)
w = sqrt(l**2 + d**2 - 2*l*d*cos(pi/4))
t = (l-w)*t if l > 0.5 else (w-l)*t
control = (1.5, t*.5)
# If mode is goal, try to get to the goal using pathfind()
else: # mode == 'goal'
v, t = pathfind((19.5, 1.5), centroid)
if (v, t) == (0.0, 0.0):
print ''
print 'Great Success!'
print ''
Found_Goal = True
return
if min(laserscan[1:4]) < 0.75:
t += (laserscan[3] - laserscan[1])*0.35
control = (v, t)
cmd = Twist()
(cmd.linear.x, cmd.angular.z) = control
cmd_vel.publish(cmd)
return
def dist(p0, p1):
return sqrt((p0[0]-p1[0])**2 + (p0[1]-p1[1])**2)
def clustering():
global parset
global centroid
global var_threshold
global threshold_cnt
global mode
global control
global Wait
def motion_update(p):
v, w = control
v_dt = v*dt
w_dt = w*dt
x, y, t = p
return (x + v_dt*cos(t),
y + v_dt*sin(t),
angles.r2r(t))
# Clustering algorithm: DBSCAN
# Not important for the primary purpose of this assignment.
while True:
#ps = np.array([(p[0], p[1]) for p in parset])
#whitened = whiten(ps)
Wait = True
ps = parset
ps.sort()
visited = set()
clusters = {}
cluster_cnt = 0
for i in xrange(PAR_COUNT):
if i in visited:
continue
frontier = [i]
cluster_cnt += 1
clusters[cluster_cnt] = []
while len(frontier):
ci = frontier.pop()
current = ps[ci]
clusters[cluster_cnt].append(current)
end_i = ci + 100
if end_i > PAR_COUNT:
end_i = PAR_COUNT
region_query = [(dist(current, ps[j]), j) for j in xrange(ci, end_i)
if j not in visited]
region_query.sort(lambda a, b: cmp(a[0], b[0]))
for d, qi in region_query:
if d > 0.2:
break
else:
visited.add(qi)
frontier.append(qi)
sz, best_cluster = max([(len(c), c) for c in clusters.values()])
centroids, var = kmeans(np.array([(x, y) for x, y, t in best_cluster]), 1)
x, y = centroids[0]
centroid = (x, y, best_cluster[random.randint(0, sz-1)][2])
mode = 'wander' if sz < 800 else 'goal'
print mode, centroid
Wait = False
rospy.sleep(0.1)
for i in xrange(20):
centroid = motion_update(centroid)
rospy.sleep(0.1)
if __name__ == '__main__':
# Uncomment for debugging if nesssary, recomment before turning in.
#rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
try:
import psyco
psyco.full()
except IOError:
pass
cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist)
rospy.Subscriber('/robot/base_scan', LaserScan, callback=got_scan,
queue_size = 10, tcp_nodelay = True)
rospy.Subscriber('/robot/base_scan', LaserScan, callback=controller,
queue_size = 10, tcp_nodelay = True)
clustering_thread = threading.Thread(target=clustering)
clustering_thread.start()
mcl_tools.mcl_init('sample_hw7')
mcl_tools.mcl_run_viz()