diff --git a/.gitignore b/.gitignore index 4ed1d53..037a765 100644 --- a/.gitignore +++ b/.gitignore @@ -13,3 +13,6 @@ robot_localization.xcodeproj/* # Ignore vscode project .vscode/* + +# Ignore python source code +code/* diff --git a/code/scripts/MapReader.py b/code/scripts/MapReader.py deleted file mode 100644 index d20cb68..0000000 --- a/code/scripts/MapReader.py +++ /dev/null @@ -1,47 +0,0 @@ -import numpy as np - -from matplotlib import pyplot as plt -from matplotlib import figure as fig - -class MapReader: - - def __init__(self, src_path_map): - - self._occupancy_map = np.genfromtxt(src_path_map, skip_header=7) - print(self._occupancy_map) - exit() - self._occupancy_map[self._occupancy_map < 0] = -1 - self._occupancy_map[self._occupancy_map > 0] = 1 - self._occupancy_map[self._occupancy_map > 0] - self._occupancy_map = np.flipud(self._occupancy_map) - - self._resolution = 10 # each cell has a 10cm resolution in x,y axes - self._size_x = self._occupancy_map.shape[0] * self._resolution - self._size_y = self._occupancy_map.shape[1] * self._resolution - - print 'Finished reading 2D map of size : ' + '(' + str(self._size_x) + ',' + str(self._size_y) + ')' - - def visualize_map(self): - fig = plt.figure() - # plt.switch_backend('TkAgg') - # mng = plt.get_current_fig_manager() - # mng.resize(*mng.window.maxsize()) - plt.ion() - plt.imshow(self._occupancy_map, cmap='Greys') - plt.axis([0, 800, 0, 800]) - plt.draw() - plt.pause(0) - - def get_map(self): - return self._occupancy_map - - def get_map_size_x(self): # in cm - return self._size_x - - def get_map_size_y(self): # in cm - return self._size_y - -if __name__=="__main__": - - src_path_map = '../data/map/wean.dat' - map1 = MapReader(src_path_map) - map1.visualize_map() diff --git a/code/scripts/MotionModel.py b/code/scripts/MotionModel.py deleted file mode 100644 index 505c7ca..0000000 --- a/code/scripts/MotionModel.py +++ /dev/null @@ -1,34 +0,0 @@ -import sys -import numpy as np -import math - -class MotionModel: - - """ - References: Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005. - [Chapter 5] - """ - - def __init__(self): - - """ - TODO : Initialize Motion Model parameters here - """ - - - def update(self, u_t0, u_t1, x_t0): - """ - param[in] u_t0 : particle state odometry reading [x, y, theta] at time (t-1) [odometry_frame] - param[in] u_t1 : particle state odometry reading [x, y, theta] at time t [odometry_frame] - param[in] x_t0 : particle state belief [x, y, theta] at time (t-1) [world_frame] - param[out] x_t1 : particle state belief [x, y, theta] at time t [world_frame] - """ - - """ - TODO : Add your code here - """ - - return x_t1 - -if __name__=="__main__": - pass \ No newline at end of file diff --git a/code/scripts/Resampling.py b/code/scripts/Resampling.py deleted file mode 100644 index b408460..0000000 --- a/code/scripts/Resampling.py +++ /dev/null @@ -1,43 +0,0 @@ -import numpy as np -import pdb - -class Resampling: - - """ - References: Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005. - [Chapter 4.3] - """ - - def __init__(self): - """ - TODO : Initialize resampling process parameters here - """ - - def multinomial_sampler(self, X_bar): - - """ - param[in] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles - param[out] X_bar_resampled : [num_particles x 4] sized array containing [x, y, theta, wt] values for resampled set of particles - """ - - """ - TODO : Add your code here - """ - - return X_bar_resampled - - def low_variance_sampler(self, X_bar): - - """ - param[in] X_bar : [num_particles x 4] sized array containing [x, y, theta, wt] values for all particles - param[out] X_bar_resampled : [num_particles x 4] sized array containing [x, y, theta, wt] values for resampled set of particles - """ - - """ - TODO : Add your code here - """ - - return X_bar_resampled - -if __name__ == "__main__": - pass \ No newline at end of file diff --git a/code/scripts/SensorModel.py b/code/scripts/SensorModel.py deleted file mode 100644 index 9f58aca..0000000 --- a/code/scripts/SensorModel.py +++ /dev/null @@ -1,37 +0,0 @@ -import numpy as np -import math -import time -from matplotlib import pyplot as plt -from scipy.stats import norm -import pdb - -from MapReader import MapReader - -class SensorModel: - - """ - References: Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005. - [Chapter 6.3] - """ - - def __init__(self, occupancy_map): - - """ - TODO : Initialize Sensor Model parameters here - """ - - def beam_range_finder_model(self, z_t1_arr, x_t1): - """ - param[in] z_t1_arr : laser range readings [array of 180 values] at time t - param[in] x_t1 : particle state belief [x, y, theta] at time t [world_frame] - param[out] prob_zt1 : likelihood of a range scan zt1 at time t - """ - - """ - TODO : Add your code here - """ - - return q - -if __name__=='__main__': - pass \ No newline at end of file diff --git a/code/scripts/main.py b/code/scripts/main.py deleted file mode 100644 index b6285c1..0000000 --- a/code/scripts/main.py +++ /dev/null @@ -1,147 +0,0 @@ -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 - 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 - - """ - TODO : Add your code here - """ - - 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 = 500 - X_bar = init_particles_random(num_particles, occupancy_map) - - vis_flag = 1 - - """ - 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) - - """ - 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()