# measurement update if(action != 'Rotate'): #update only when translate particleFilter.measurementUpdate(z) particleFilter.normalizeWeights() # get predict state robotState = particleFilter.getPredictState() # print state #print "State : ", robotState #print "Goal : ", currentPointIndex, wayPoints[currentPointIndex] # set control signal #leftVel, rightVel, action = navigator.navigateToWayPoint(robotState, wayPoints[currentPointIndex]) # leftVel, rightVel, action = navigator.navigateToWayPointStateFul(robotState, wayPoints[currentPointIndex]) leftVel, rightVel, action = navigator.navigateToWayPointStateFul2(robotState, enc_distL, enc_distR, wayPoints[currentPointIndex]) #if action is not lastAction: # robot.motors.reset() #lastAction = action robot.motors.setVel(leftVel, rightVel, enc_velL, enc_velR) # set waypoint index if(action == 'Complete'): currentPointIndex += 1 if(currentPointIndex >= len(wayPoints)): break # resampling if(timeStep%RESAMPLING_PERIOD == 0): particleFilter.resample()
