├── README.md
├── helper_func.py
├── particle_class.py
└── resampling.py


/README.md:
--------------------------------------------------------------------------------
1 | # Grid-based FastSLAM on Turtlebot
2 | This project aims to implement the Grid-based FastSLAM algorithm on a Turtlebot. FastSLAM is a probabilistic algorithm for solving the Simultaneous Localization And Mapping (SLAM) problem. FastSLAM uses particle filters where each particle represents its belief of the robot's pose. Each particle also has a map that it updates by taking laser scan measurements of its environment.
3 | 
4 | # References
5 | [1] Dr.Cyrill Stachniss, Robot Mapping course, University of Freiburg. https://www.youtube.com/watch?v=3Yl2aq28LFQ&index=15&list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_
6 | 
7 | [2] Thrun et.al., Probabilistic Robotics
8 | 


--------------------------------------------------------------------------------
/helper_func.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | 
  3 | def prob_to_logodds(prob):
  4 |     ## Assuming that prob is a scalar
  5 |     prob = np.matrix(prob,float)
  6 |     logOdds = prob/(1+prob)
  7 |     return np.asscalar(logOdds)
  8 | 
  9 | def logOdds_to_prob(logOdds):
 10 |     ## Assuming that logOdds is a matrix
 11 |     p = 1 - 1/(1 + np.exp(logOdds))
 12 |     return p
 13 | 
 14 | def swap(a,b):
 15 |     x,y = b,a
 16 |     return x,y
 17 | 
 18 | def bresenham2(mycoords):
 19 |     mycoords = np.array(mycoords)
 20 |     x = np.round(mycoords[:,0])
 21 |     y = np.round(mycoords[:,1])
 22 |     steep = (abs(y[1]-y[0]) > abs(x[1]-x[0]))
 23 |     if steep:
 24 | 	x,y = swap(x,y)
 25 |     if x[0]>x[1]:
 26 | 	x = np.array(swap(x[0],x[1]))
 27 | 	y = np.array(swap(y[0],y[1]))	
 28 |     delx = x[1] - x[0]
 29 |     dely = abs(y[1] - y[0])
 30 |     error = 0
 31 |     x_n,y_n = x[0],y[0]
 32 | 
 33 |     X = np.zeros(delx+1)
 34 |     Y = np.zeros(delx+1)
 35 | 
 36 |     if y[0] < y[1]:
 37 | 	ystep = 1
 38 |     else:
 39 | 	ystep = -1
 40 |     for n in range(delx+1):
 41 | 	if steep:
 42 | 	    X[n],Y[n] = x_n,y_n
 43 | 	else:
 44 | 	    X[n],Y[n] = y_n,x_n
 45 | 	x_n += 1
 46 | 	error += dely
 47 | 	if error<<1 >= delx:
 48 | 	    y_n += ystep
 49 | 	    error -= delx
 50 |     X,Y = swap(X,Y)
 51 |     return X,Y
 52 | 
 53 | def pose_world_to_map(pntsWorld,gridSize):
 54 |     pntsMap = [x/gridSize for x in pntsWorld]
 55 |     return pntsMap
 56 | 
 57 | def laser_world_to_map(laserEndPnts, gridSize):
 58 |     pntsMap = laserEndPnts/gridSize
 59 |     return pntsMap
 60 | 
 61 | def v2t(v):
 62 |     x = v[0]
 63 |     y = v[1]
 64 |     th = v[2]
 65 |     trans = np.matrix([[cos(th), -sin(th), x],[sin(th), cos(th), y],[0, 0, 1]])
 66 |     return trans
 67 | 
 68 | def laser_to_xy(rl):
 69 |     numBeams = len(rl.ranges)
 70 |     maxRange = rl.range_max
 71 |     # apply the max range
 72 |     idx = []
 73 |     for i in range(len(rl.ranges)):
 74 |         if isnan(rl.ranges[i]):
 75 | 	    idx.append(False)
 76 | 	else:
 77 |             if rl.ranges[i]>0 and rl.ranges[i]<maxRange:
 78 | 	        idx.append(True)
 79 |             else:
 80 |                 idx.append(False)
 81 | 
 82 |     if (subsample):
 83 |     	idx(2:2:end) = 0
 84 | 
 85 |     lins = np.linspace(rl.angle_min, rl.angle_max, numBeams)
 86 |     for i in range(len(idx)):
 87 | 	if idx[i]==True:
 88 | 	    angles.append(lins[i])
 89 |     
 90 |     ranges_tmp = np.array(rl.ranges)
 91 |     idx_tmp = np.array(idx)
 92 |     points = np.matrix([[ranges_tmp[idx_tmp] * np.cos(angles)], [ranges_tmp[idx_tmp] * np.sin(angles)], [np.ones(1, len(angles))]])
 93 |     
 94 | #   laser_offset to be defined as [x_diff, y_diff, th_diff] = position vector from laser to robot base
 95 |     transf = v2t(laser_offset)
 96 | 
 97 |     # apply the laser offset
 98 |     points = transf * points
 99 |     return points
100 | 


--------------------------------------------------------------------------------
/particle_class.py:
--------------------------------------------------------------------------------
  1 | from numpy.random import normal
  2 | import numpy as np
  3 | from math import sin,cos
  4 | from helper_func import *
  5 | from resampling import resampling
  6 | 
  7 | class particle:
  8 |     pose = np.array([0,0,0])
  9 |     grid_map = np.matrix(np.ones((200,200)))
 10 |     weight = 0.0
 11 |     probOcc = 0.9
 12 |     probFree = 0.35
 13 |     gridSize = 0.05
 14 |     logOddsPrior = prob_to_logOdds(prior)
 15 | 
 16 |     def __init__(self,num_particles):
 17 | 	self.weight = 1.0/num_particles	
 18 | 	prior = 0.5
 19 |         self.grid_map = self.logOddsPrior*self.grid_map
 20 | 
 21 |     def sample_motion_model(self,ut):
 22 |         # Inputs : ut = [v w]'
 23 |         #          x_pre = [x y theta]'
 24 |         # v = 1
 25 |         # w = 0
 26 |         [v,w] = ut
 27 |         [x,y,theta] = self.pose
 28 |         a1 = 0.01
 29 |         a2 = 0.01
 30 |         a3 = 0.01
 31 |         a4 = 0.01
 32 |         a5 = 0.01
 33 |         a6 = 0.01
 34 |         del_t = 1
 35 |         v_cap = v + sample(a1*v**2 + a2*w**2)
 36 |         w_cap = w + sample(a3*v**2 + a4*w**2)
 37 |         gamma_cap = sample(a5*v**2 + a6*w**2)
 38 |         x_prime = x - (v_cap/w_cap)*sin(theta) + v_cap/w_cap*sin(theta + w_cap*del_t)
 39 |         y_prime = y + (v_cap/w_cap)*cos(theta) - v_cap/w_cap*cos(theta + w_cap*del_t)
 40 |         theta_prime = theta + w_cap*del_t + gamma_cap*del_t
 41 |         xt = [x_prime, y_prime, theta_prime]
 42 |         return xt
 43 | 
 44 |     def inv_sensor_model(self,scan):
 45 |         ### "scan" has to be of type sensor_msgs/LaserScan message
 46 |         ### Assume that scan is a python list
 47 |         mapUpdate = np.zeros(np.shape(self.grid_map))
 48 |         robMapPose = pose_world_to_map(self.pose[2],self.gridSize)
 49 |         robTrans = v2t(self.pose)
 50 |         laserEndPnts = laser_to_xy(scan)
 51 |         laserEndPnts = robTrans*laserEndPnts # convert from laser – robot coords
 52 |         laserEndPntsMapFrame = laser_world_to_map(laserEndPnts[:2],gridSize)
 53 |     	
 54 |         freeCells = []
 55 |         for col in range(len(laserEndPntsMapFrame[0])):
 56 |             [X,Y] = bresenham2(robMapPose[0], robMapPose[1],laserEndPointsMapFrame[0][col],laserEndPointsMapFrame[1][col])
 57 |             freeCells.append([X,Y])
 58 | 
 59 |         for freeCell in freeCells:
 60 | 	    mapUpdate[freeCell[0]][freeCell[1]] = prob_to_logodds(self.probFree)
 61 |         for endPnt in LaserEndPntsMapFrame.transpose():
 62 | 	    mapUpdate[endPnt[0]][endPnt[1]] = prob_to_logodds(self.probOcc)
 63 | 	return mapUpdate, robMapPose, laserEndPntsMapFrame
 64 | 
 65 |     def observation_model(self,scan):
 66 |         zt = scan.ranges
 67 |         d = raytrace(scan,self.pose,self.grid_map)
 68 |         weight = 1.0/(np.linalg.norm(zt-d) + 1)
 69 |         return weight
 70 | 
 71 |     def raytrace(self,scan,pos,particle_map):
 72 | 	thresh_occ = 0.75
 73 |         x,y,theta = pos[0],pos[1],pos[2]
 74 |         ang_min = theta + scan.angle_min
 75 |         ang_max = theta + scan.angle_max
 76 |         angle_increment = scan.angle_increment
 77 |         distances = []
 78 |         for i in np.arange(ang_min,ang_max,angle_increment):
 79 |             for n in range(1,int(scan.range_max/self.grid_size)):
 80 |                 x_pos = int(np.round(x + n*self.grid_size*np.cos(i)))
 81 |                 y_pos = int(np.round(y + n*self.grid_size*np.sin(i)))
 82 |                 if particle_map[x_pos][y_pos] > thresh_occ:
 83 |     	  	    distances.append(np.linalg.norm([x_pos-x,y_pos-y]))
 84 |                     break
 85 |                 elif n == int(scan.range_max/self.grid_size):
 86 |                     distances.append('OOR')
 87 |         return distances
 88 | 
 89 | if __name__ == '__main__':
 90 |     num_particles = 30
 91 |     particles = [particle(num_particles) for i in range(num_particles)]
 92 |     while(not ut.end):
 93 |     ### ut, scan = Subscribe to topics to get twist and laser scan after delta_t
 94 |         for i in range(num_particles):
 95 | 	    particles[i].pose = particles[i].sample_motion_model(ut)
 96 |     	    particles[i].weight = observation_model(scan)
 97 |             [mapUpdate, robPoseMap, laserEndPntsMapFrame] = inverse_sensor_model(scan)
 98 | 	    particles[i].grid_map -= logOddsPrior	
 99 | 	    particles[i].grid_map += mapUpdate
100 | 	    plot_map(particles[i].grid_map,robPoseMap, laserEndPntsMapFrame, particles[i].gridSize)
101 | 	particles = resampling(particles)
102 | 


--------------------------------------------------------------------------------
/resampling.py:
--------------------------------------------------------------------------------
 1 | # This code can be worked on for the resampling part of the algorithm
 2 | # Assuming the input is the particles i.e [p1,p2,p3]
 3 | 
 4 | import random
 5 | 
 6 | def ssum(k,particles):
 7 |     sum = 0
 8 |     for i in range(k+1):
 9 |         sum = sum + particle_weights[i]
10 |     return sum
11 | 
12 | def sus(particles):
13 |     f = 360
14 |     n = len(particles)
15 |     p = f/n
16 |     start = random.randint(0,p)
17 |     pointers = []
18 |     for i in range(n):
19 |         pointers.append(start + i*p)
20 |     return rws(pointers,particles)
21 | 
22 | def rws(pointers,particles):
23 |     keep = []
24 |     for pointer in pointers:
25 |         k = 0
26 |         while (ssum(k,particles)) < pointer:
27 |             k += 1
28 |         keep.append(particles[k][4])
29 |     return keep
30 | 
31 | #INPUT
32 | p1 = [0,0,0,1,1]
33 | p2 = [0,0,0,0,2]
34 | p3 = [0,0,0,0,3]
35 | particles = [p1,p2,p3]
36 | 
37 | #Extracting Weights
38 | particle_weights = []
39 | for i in range(len(particles)):
40 |     particle_weights.append(particles[i][3]*360)
41 | 
42 | #Resampling
43 | print sus(particles)
44 | 


--------------------------------------------------------------------------------