def _manual_run(self): pthjoin = os.path.join (translation_model, translation_data, rotation_model, rotation_data, measurement_model, measurement_data, beacons) = models.load_data(pthjoin('..','Data','001')) # Kluge fix -- need to fix the .mat file measurement_model = measurement_model[0] sm = Sensor.SensorManager() # Create Beacons # Need to add draw beacon functions total_beacons = beacons.shape[0] for i in range(total_beacons): sm.add_sensor(Sensor.BeaconSensor(measurement_model[0], measurement_model[1], [0,10000], beacons[i][0], beacons[i][1])) sm.add_sensor(Sensor.CompassSensor(0, np.deg2rad(5), [0, np.deg2rad(359.9)])) sm.add_sensor(Sensor.Trilateration2DSensor(None, np.eye(2)*measurement_model[1], None)) # Move Straight: Vector based on Motion Model measurements #translation_vec = [dist_hypot, dist_hypot, translation_model[2,0]] translation_vec = translation_model[:,0] translation_cov = np.cov(translation_data.T) #translation_mov = Movement.Movement(translation_vec, translation_cov) # Turn: Vector based on Motion Model measurements left_rotation_vec = rotation_model[:,0] right_rotation_vec = rotation_model[:,0] * -1 # May want to change this. This is a guess to get it working. # Playing around with cov(rotation_data) may lead to better results. rotation_cov = np.zeros([3,3]) rotation_cov[2,2] = rotation_model[2,1] #left_rot_mov = Movement.Movement(left_rotation_vec, rotation_cov) #right_rot_mov = Movement.Movement(right_rotation_vec, rotation_cov) #turn_leniency = np.deg2rad(10) # Used as an error judgement for deciding when to turn print 'D.TF.284' explorer_pos = self.fm.get_explorer_pos_mean() me = MoveExplorer(explorer_pos, [12, 12, np.deg2rad(6)], debug=True, translation_moves=[Movement(translation_vec, translation_cov), Movement(np.array([10, 0.5, np.deg2rad(0.5)]), np.eye(3, dtype=np.float32) * 0.0001), Movement(np.array([50, 1, np.deg2rad(0.7)]), np.eye(3, dtype=np.float32) * 0.0003)], rotation_moves=[Movement(right_rotation_vec, rotation_cov), Movement(left_rotation_vec, rotation_cov), Movement(np.array([0, 0, np.deg2rad(5)]), rotation_cov), Movement(np.array([0, 0, np.deg2rad(-5)]), rotation_cov)]) def draw_all_waypoints(cr): #cr.select_font_face('arial') cr.set_font_size(15) #cr.set_font_matrix(cairo.Matrix(1, 0, 0, -1, 0, 0)) cr.new_path() def draw_waypoints(cr, i, waypoints, rgba_color): for pnt in waypoints: cr.set_source_rgba(*rgba_color) cr.arc(pnt[0], pnt[1], 6, 0, 2 * np.pi) cr.stroke() cr.move_to(pnt[0], pnt[1]) #cr.show_text(str(i)) #i += 1 waypoints = me.get_old_waypoints()[-2:] draw_waypoints(cr, -2, waypoints, (0.5, 0, 0, 0.8)) waypoints = me.get_current_waypoints() draw_waypoints(cr, 1, waypoints[0:1], (0.4, 0.9, 0.8, 0.8)) if len(waypoints) > 1: draw_waypoints(cr, 2, waypoints[1:], (0, 0, 0.5, 0.8)) cr.new_path() #======================================================================= # Add Drawing Methods #======================================================================= self.tg.add_draw_method(draw_all_waypoints) for beacon in sm.sensors_by_type['Beacon']: self.tg.add_draw_method(beacon.draw) accumulate_distance = np.array([0, 0, 0], np.float32) while not self.quit: # Poll for any new user input. waypoints = tg.get_click_positions() for pnt in waypoints: me.move_to(pnt[0], pnt[1]) transition_mov = me.get_next_move(explorer_pos) if transition_mov is not None: print 'D.TF.337' explorer_pos = self.fm.get_explorer_pos_mean() self.fm.move(transition_mov.vec, transition_mov.cov) accumulate_distance += np.abs(transition_mov.vec) time.sleep(0.5) # Only perform filter observation when enough translation has occurred. # Observations must be independent. if accumulate_distance[0] >= 20: accumulate_distance[0] = 0 # Testing Trilateration beacon_sensors = sm.sensors_by_type['Beacon'] print 'D.TF.351' prob_pos = self.fm.get_explorer_pos_mean() for j in range(total_beacons): #prob_pos = self.fm.get_explorer_pos_mean() obs_dis = np.sqrt((beacons[j][0] - prob_pos[0])**2 + (beacons[j,1] - prob_pos[1])**2) + (randn() * measurement_model[1]) + measurement_model[0] #self.fm.observation(obs_dis, sm.sensors_by_type['Beacon'][j]) beacon_sensors[j].obs = obs_dis self.fm.observation(beacon_sensors, sm.sensors_by_type['Trilateration2D'][0]) if accumulate_distance[2] >= np.deg2rad(10): print 'D.TF.364' explorer_pos = self.fm.get_explorer_pos_mean() #self.fm.observation(explorer_pos[2] + (randn() * sm.sensors_by_type['Compass'][0].variance), sm.sensors_by_type['Compass'][0]) accumulate_distance[2] = 0 time.sleep(0.1)
def __init__(self, serial_port, baud_rate, map_obj, origin_pos, origin_cov, gui=None, run_pf=True, run_kf=True): ''' @param serial_port: @type serial_port: @param baud_rate: @type baud_rate: @param map_obj: @type map_obj: @param origin_pos: @type origin_pos: @param origin_cov: @type origin_cov: @param gui: @type gui: @param run_pf: @type run_pf: @param run_kf: @type run_kf: ''' super(FilterSystemRunner, self).__init__() self.quit = False self.allow_move_timer = 0 #======================================================================= # UART #======================================================================= self.cli = FilterCLI.FilterCLI() self.ua = UARTSystem.UART(serial_port, baud_rate, uart_input=self.cli, approve=True, debug=True) self.uin = [] # uart input from user self.uout = [] # uart output from base/roomba #======================================================================= # Filter #======================================================================= self.fm = FilterManager.FilterManager() if run_kf: self.fm.add_filter( KalmanFilter.KalmanFilter(origin_pos, origin_cov)) if run_pf: self.fm.add_filter( ParticleFilter.ParticleFilter(origin_pos, origin_cov)) #======================================================================= # Data #======================================================================= pthjoin = os.path.join (self.translation_model, self.translation_data, self.rotation_model, self.rotation_data, self.measurement_model, self.measurement_data, self.beacons) = models.load_data(pthjoin('Data', '001')) #======================================================================= # Sensors #======================================================================= # Kluge fix -- need to fix the .mat file measurement_model = self.measurement_model[0] self.sm = Sensor.SensorManager() # Create Beacons # Need to add draw beacon functions total_beacons = self.beacons.shape[0] for i in range(total_beacons): self.sm.add_sensor( Sensor.BeaconSensor(measurement_model[0], measurement_model[1], [0, 10000], self.beacons[i][0], self.beacons[i][1])) # Trilateration will be used if 2 or more beacons yield readings. self.sm.add_sensor( Sensor.Trilateration2DSensor(measurement_model[0], np.eye(2) * 15, None)) #measurement_model[1], None)) #======================================================================= # Movement #======================================================================= self._new_moves = [] self._last_move = None # Move Straight: Vector based on Motion Model measurements translation_vec = self.translation_model[:, 0] translation_vec[:2] = np.array([100.4, 4]) translation_cov = np.cov(self.translation_data.T) translation_mov = Movement.Movement(translation_vec, translation_cov, id='0 100\n') # Bit hacky here, did not do real measurements, just based off eye measurements translation_moves = [ translation_mov, Movement.Movement(np.array([10.1, 0.5, np.deg2rad(0.2)]), np.array([[0.5, 0, 0], [0, 1, 0], [0, 0, np.deg2rad(0.6)]]), id='0 10\n'), #np.eye(3) * .0001, Movement.Movement(np.array([50.2, 2, np.deg2rad(0.6)]), np.array([[0.5, 0, 0], [0, 1, 0], [0, 0, np.deg2rad(0.6)]]), id='0 50\n') ] #np.array([[],[],[]]))] # Turn: Vector based on Motion Model measurements left_rotation_vec = self.rotation_model[:, 0] right_rotation_vec = self.rotation_model[:, 0] * -1 # May want to change this. This is a guess to get it working. # Playing around with cov(rotation_data) may lead to better results. rotation_cov = np.zeros([3, 3]) rotation_cov[2, 2] = self.rotation_model[2, 1] left_rot_mov = Movement.Movement(left_rotation_vec, rotation_cov, id='10 0\n') right_rot_mov = Movement.Movement(right_rotation_vec, rotation_cov, id='-10 0\n') rotation_moves = [ left_rot_mov, right_rot_mov, Movement.Movement(np.array([0, 0, np.deg2rad(-5)]), rotation_cov, id='-4 0\n'), Movement.Movement(np.array([0, 0, np.deg2rad(5)]), rotation_cov, id='4 0\n') ] explorer_pos = self.fm.get_explorer_pos_mean() self.me = Movement.MoveExplorer(explorer_pos, [11, 11, np.deg2rad(6)], translation_moves=translation_moves, rotation_moves=rotation_moves) self._accumulate_distance = np.array([0, 0, 0], np.float32) #======================================================================= # Map #======================================================================= self.map_obj = map_obj #======================================================================= # GUI #======================================================================= self.rg = None if gui is not None: self.rg = gui self.rg.add_filter_draw_method(self.fm.get_draw()) for beacon in self.sm.sensors_by_type['Beacon']: self.rg.add_draw_method(beacon.draw) self.rg.add_draw_method(self._draw_all_waypoints)