Implementation - Robot Navigation

 Block Diagram

Initialization of Pyro object 

robot = Pyro4.Proxy("PYRONAME:example.robot") # use name server object lookup uri shortcut con =connect() #############..............................

 Define particles 

defining number of particles,initiating particles array and assigning initial position

# Generate initial particles. Each particle is (x, y, theta).
 number_of_particles = 25
start_state = np.array([500.0, 0.0, 45.0 / 180.0 * pi])
 initial_particles = [copy.copy(Particle(start_state)) for _ in xrange(number_of_particles)]

Initiating SLAM object

fs = FastSLAM(initial_particles, robot_width, scanner_displacement, control_motion_factor, control_turn_factor, measurement_distance_stddev, measurement_angle_stddev, minimum_correspondence_likelihood)
 

Loop
check whether incoming data is valid

try :
     while True:
              if robot.is_avilable():
              print "new cycle......";
              data_encode = robot.get_DataEncode();
              data_scan = robot.get_DataScan();
              robot.false_avilable()

Formatting Data

        for i in range(len(encode)):
                encode_data.append(int(encode[i]))
        #...........................
        scan = spilted_scandata[2:]
        scan_data = []
        for i in range(len(scan)):
                scan_data.append(int(scan[i]))

Predict

#...................................................................................
control = map(lambda x: x * ticks_to_mm, fs.get_tiks(encode_data))

fs.predict(control)
#....................................................................................
       def get_tiks(self,motor_Data):

             ticks = (motor_Data[2],motor_Data[6])
             if self.first_motor_ticks :
                  self.first_motor_ticks = False
                  self.last_tiks = ticks
             self.motor_ticks = tuple([ticks[i]- self.last_tiks[i] for i in range(2)])           
            self.last_tiks = ticks
            return self.motor_ticks
 #...................................................................................
      def predict(self, control):
            left, right = control
            left_std = sqrt((self.control_motion_factor * left)**2 +\   (self.control_turn_factor * (left-right))**2)
            right_std = sqrt((self.control_motion_factor * right)**2 +\ (self.control_turn_factor * (left-right))**2)
           # Modify list of particles: for each particle, predict its new position.

           for p in self.particles:
                 l = random.gauss(left, left_std)
                 r = random.gauss(right, right_std)
                 p.move(l, r, self.robot_width)
 #....................................................................................

correct
  

cylinders=get_cylinders_from_scan(scan_data,depth_jump,minimum_valid_distance,
cylinder_offset)
fs.correct(cylinders)

#.....................................................................................

def get_cylinders_from_scan(scan, jump, min_dist, cylinder_offset):
           der = compute_derivative(scan, min_dist)
           cylinders = find_cylinders(scan, der, jump, min_dist)
           result = []
           for c in cylinders:          
               bearing = beam_index_to_angle(c[0])
               distance = c[1] + cylinder_offset
               x, y = distance*cos(bearing), distance*sin(bearing)
               result.append( (np.array([distance, bearing]), np.array([x, y])) )
         return result

#....................................................................................
def correct(self, cylinders):
       weights = self.update_and_compute_weights(cylinders)
       self.particles = self.resample(weights)