-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathTestMotionModel.py
183 lines (141 loc) · 6.09 KB
/
TestMotionModel.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
import numpy as np
import sys
import pdb
from MapReader import MapReader
from MotionModel import MotionModel
from SensorModel import SensorModel
from Resampling import Resampling
from matplotlib import pyplot as plt
from matplotlib import figure as fig
import time
def visualize_map(occupancy_map):
fig = plt.figure()
plt.switch_backend('TkAgg')
mng = plt.get_current_fig_manager(); # mng.resize(*mng.window.maxsize())
plt.ion(); plt.imshow(occupancy_map, cmap='Greys'); plt.axis([0, 800, 0, 800]);
def visualize_timestep(X_bar, tstep):
x_locs = X_bar[:,0]/10.0
y_locs = X_bar[:,1]/10.0
scat = plt.scatter(x_locs, y_locs, c='r', marker='o')
plt.pause(0.00001)
scat.remove()
def init_particles_random(num_particles, occupancy_map):
# initialize [x, y, theta] positions in world_frame for all particles
# (randomly across the map)
y0_vals = np.random.uniform( 0, 7000, (num_particles, 1) )
x0_vals = np.random.uniform( 3000, 7000, (num_particles, 1) )
theta0_vals = np.random.uniform( -3.14, 3.14, (num_particles, 1) )
# initialize weights for all particles
w0_vals = np.ones( (num_particles,1), dtype=np.float64)
w0_vals = w0_vals / num_particles
X_bar_init = np.hstack((x0_vals,y0_vals,theta0_vals,w0_vals))
return X_bar_init
def init_particles_freespace(num_particles, occupancy_map):
# initialize [x, y, theta] positions in world_frame for all particles
# (in free space areas of the map)
"""
TODO : Add your code here
"""
# Initializes empty np arrays
y0_vals = np.empty([num_particles,1])
x0_vals = np.empty([num_particles,1])
# Intializes all angles theta
theta0_vals = np.random.uniform( -3.14, 3.14, (num_particles, 1) )
for i in range(0,num_particles):
# Creates x0 and y0 doubles
y0_vals[i] = np.random.uniform( 0, 7000)
x0_vals[i] = np.random.uniform( 3000, 7000)
# If the particles are not in the hallways, generate new values until they are.
while occupancy_map[int(y0_vals[i]/10)][int(x0_vals[i]/10)] < 0 or occupancy_map[int(y0_vals[i]/10)][int(x0_vals[i]/10)]> 0.05:
y0_vals[i] = np.random.uniform( 0, 7000)
x0_vals[i] = np.random.uniform( 3000, 7000)
w0_vals = np.ones( (num_particles,1), dtype=np.float64)
w0_vals = w0_vals / num_particles
X_bar_init = np.hstack((x0_vals,y0_vals,theta0_vals,w0_vals))
return X_bar_init
def main():
"""
Description of variables used
u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame]
u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame]
x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame]
x_t1 : particle state belief [x, y, theta] at time t [world_frame]
X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles
z_t : array of 180 range measurements for each laser scan
"""
"""
Initialize Parameters
"""
src_path_map = '../data/map/wean.dat'
src_path_log = '../data/log/robotdata1.log'
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
logfile = open(src_path_log, 'r')
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
num_particles = 1
X_bar = init_particles_freespace(num_particles, occupancy_map)
vis_flag = 1
# ---------------------------------------------------
# Weights are dummy weights for testing motion model
w0_vals = np.ones( (num_particles,1), dtype=np.float64)
w_t = w0_vals / num_particles
#----------------------------------------------------
"""
Monte Carlo Localization Algorithm : Main Loop
"""
if vis_flag:
visualize_map(occupancy_map)
first_time_idx = True
for time_idx, line in enumerate(logfile):
# Read a single 'line' from the log file (can be either odometry or laser measurement)
meas_type = line[0] # L : laser scan measurement, O : odometry measurement
meas_vals = np.fromstring(line[2:], dtype=np.float64, sep=' ') # convert measurement values from string to double
odometry_robot = meas_vals[0:3] # odometry reading [x, y, theta] in odometry frame
time_stamp = meas_vals[-1]
# if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure odometry measurements for now (faster debugging)
# continue
if (meas_type == "L"):
odometry_laser = meas_vals[3:6] # [x, y, theta] coordinates of laser in odometry frame
ranges = meas_vals[6:-1] # 180 range measurement values from single laser scan
print "Processing time step " + str(time_idx) + " at time " + str(time_stamp) + "s"
if (first_time_idx):
u_t0 = odometry_robot
first_time_idx = False
continue
X_bar_new = np.zeros( (num_particles,4), dtype=np.float64)
u_t1 = odometry_robot
for m in range(0, num_particles):
"""
MOTION MODEL
"""
x_t0 = X_bar[m, 0:3]
x_t1 = motion_model.update(u_t0, u_t1, x_t0)
print x_t1
# ---------------------------------------------------
# For testing Motion Model
X_bar_new[m,:] = np.hstack((x_t1, w_t))
# ---------------------------------------------------
"""
SENSOR MODEL
"""
"""
if (meas_type == "L"):
z_t = ranges
w_t = sensor_model.beam_range_finder_model(z_t, x_t1)
# w_t = 1/num_particles
X_bar_new[m,:] = np.hstack((x_t1, w_t))
else:
X_bar_new[m,:] = np.hstack((x_t1, X_bar[m,3]))
"""
X_bar = X_bar_new
u_t0 = u_t1
"""
RESAMPLING
"""
#X_bar = resampler.low_variance_sampler(X_bar)
if vis_flag:
visualize_timestep(X_bar, time_idx)
if __name__=="__main__":
main()