def explore_timestep(self): '''Gets rav to explore next timestep''' next_pos = self.move_from_bel_map_callable(self.current_belief_map, self.current_pos_intended, self.epsilon) print("self.current_pos_intended: {}".format( self.current_pos_intended)) self.move_agent(next_pos) self.current_pos_intended = next_pos self.current_pos_measured = self.rav.getMultirotorState( vehicle_name=self.agent_name).kinematics_estimated.position self.update_agent_pos_measured() #record image at location self.record_image() #get sensor reading, can be done on separate thread self.current_reading = float( sensor_reading(OccupancyGridAgent.ImageDir + "/photo_" + str(self.timestep) + '.png')[0]) self.current_belief_map.update_from_prob(self.current_pos_intended, self.current_reading) newest_observation = AgentObservation(self.current_pos_intended, self.current_reading, self.timestep, time.time(), self.agent_name) self.observation_manager.update_rav_obs_set(self.agent_name, [ AgentObservation(self.current_pos_intended, self.current_reading, self.timestep, time.time(), self.agent_name) ]) #coordinate with other agents if possible for other_active_agent in self.other_active_agents: if self.can_coord_with_other(other_active_agent, self.comms_radius): self.coord_with_other(other_active_agent) #self._write_observations(self.observations_file_loc) self.update_state_for_analysis_file( self.agent_state_file_loc, self.get_agent_state_for_analysis()) logger(str(self.get_agent_state_for_analysis()), "info", "state") self.update_observations_file(self.observations_file_loc, newest_observation) logger(str(newest_observation), "info", "observations")
def _read_observations(self, file_loc): with open(file_loc, 'r') as f: lines = f.read().split('\n') observations = set([ AgentObservation(eval(line.split(', ')[0]), float(line.split(', ')[1]), int(line.split(', ')[2]), float(line.split(', ')[3]), str(line.split(', ')[4])) for line in lines[1:] ]) return observations
def get_observations_from(self, other_agent_name, timestep = None): ''' Returns a list of agent observations from another agent given the other agents name. ''' #print("making get request to: ", self.create_request_url(other_agent_name, timestamp)) response_json = requests.get(self.create_request_url(other_agent_name, timestep)).json() return_observations = [] for row in response_json[1:]: row = row.split(',') # try: return_observations.append(AgentObservation(Vector3r(row[1], row[0], row[2]), *row[3:])) # except Exception as e: # print("found an exception: ", row) return return_observations
def read_all_observations_from_file(self): ''' Returns all observations from file associated with agent as AgentObservations ''' try: self.file_handle.seek(0) reader = csv.reader(self.file_handle) #header = next(reader) #read the header and then throw it away next(reader) #reset the file handle to it's start position for reading again return [ AgentObservation(Vector3r(row[1], row[0], row[2]), *row[3:]) for row in reader ] #not sure how this could be handled for now except Exception as e: raise e
pass def get_move(self, belief_map, current_grid_loc: Vector3r, explored_grid_locs: 'list of Vector3r') -> (bool, Vector3r): return self.get_move_from_belief_map_epsilon_greedy(belief_map, current_grid_loc, self.epsilon, self.eff_radius) #%% if __name__ == "__main__": test_grid = UE4Grid(1, 1, Vector3r(0,0), 6, 5) tsp = TSPActionSelection(test_grid, Vector3r(0,3)) print(tsp.get_moves()) obs1 = AgentObservation(Vector3r(0,0),0.5, 1, 1234, 'agent2') obs2 = AgentObservation(Vector3r(0,0),0.7, 2, 1235, 'agent2') obs3 = AgentObservation(Vector3r(0,1),0.95, 3, 1237, 'agent2') #(grid, agent_name, prior = {}) obs_man = ObservationSetManager("agent1") obs_man.update_rav_obs_set('agent2', [obs1, obs2, obs3]) belief_map = obs_man.get_discrete_belief_map_from_observations(test_grid) epsilon_greedy = EpsilonGreedyActionSelection(0.0, 1.8) assert epsilon_greedy.get_move(belief_map, Vector3r(1,1), []) == (False, Vector3r(0,1)) #test that agent will travel further for higher prediction test_grid1 = UE4Grid(2, 1, Vector3r(0,0), 6, 5) obs_man1 = ObservationSetManager("agent1") epsilon_greedy1 = EpsilonGreedyActionSelection(0.0, 2.8)
return return_belief_map def get_continuous_belief_map_from_observations(self, grid_bounds): '''Given grid bounds, returns a function which returns the likelihood given the continuous position of the RAV. I.E. transform the discrete PDF as above to a continuous one.''' pass if __name__ == "__main__": test_grid = UE4Grid(1, 1, UE4Coord(0,0), 6, 5) test_ObservationSetManager = ObservationSetManager('agent1') test_ObservationSetManager.observation_sets obs1 = AgentObservation(UE4Coord(0,0),0.5, 1, 1234, 'agent2') obs2 = AgentObservation(UE4Coord(0,0),0.7, 2, 1235, 'agent2') obs3 = AgentObservation(UE4Coord(0,1),0.95, 3, 1237, 'agent2') obs4 = AgentObservation(UE4Coord(0,1),0.9, 3, 1238, 'agent1') test_ObservationSetManager.init_rav_observation_set('agent2', set([obs1, obs2])) test_ObservationSetManager.observation_sets test_ObservationSetManager.update_rav_obs_set('agent2', set([obs3])) test_ObservationSetManager.get_all_observations() assert test_ObservationSetManager.get_observation_set('agent2') == set([obs1, obs2, obs3]), "agent2 observations should have been added to set" assert test_ObservationSetManager.get_observation_set('agent1') == set([]), "agent1 observations should be empty" test_ObservationSetManager.update_with_obseravation(obs4)
continuous position of the RAV. I.E. transform the discrete PDF as above to a continuous one.''' pass #%% #%% if __name__ == "__main__": test_grid = UE4Grid(1, 1, Vector3r(0, 0), 6, 5) test_ObservationSetManager = ObservationSetManager('agent1') test_ObservationSetManager.observation_sets obs1 = AgentObservation(Vector3r(0, 0), 0.5, 1, 1234, 'agent2') obs2 = AgentObservation(Vector3r(0, 0), 0.7, 2, 1235, 'agent2') obs3 = AgentObservation(Vector3r(0, 1), 0.95, 3, 1237, 'agent2') obs4 = AgentObservation(Vector3r(0, 1), 0.9, 3, 1238, 'agent1') test_ObservationSetManager.init_rav_observation_set( 'agent2', set([obs1, obs2])) assert test_ObservationSetManager.get_observation_set('agent2') == set( [obs1, obs2]) test_ObservationSetManager.observation_sets test_ObservationSetManager.update_rav_obs_set('agent2', set([obs3])) test_ObservationSetManager.get_all_observations() assert test_ObservationSetManager.get_observation_set(
assert abs(calc_posterior(0.8, 0.2) - 0.5) <= 0.001 #tests for get_posterior_given_obs assert abs(get_posterior_given_obs([0.5,0.2,0.8], 0.5) - 0.5) <= 0.001 #tests for belief map test_grid = UE4Grid(1, 1, UE4Coord(0,0), 10, 6) test_map = create_belief_map(test_grid, "agent1") assert test_map.get_belief_map_component(UE4Coord(0,0)) == BeliefMapComponent(UE4Coord(0,0), 1/len(test_grid.get_grid_points())) assert test_map._get_observation_grid_index(UE4Coord(0,0)) == 5 test_map.update_from_prob(UE4Coord(0,0), 0.9) assert 0.132<test_map.get_belief_map_component(UE4Coord(0,0)).likelihood < 0.133 #prove order in which observations come in doesn't matter obs1 = AgentObservation(UE4Coord(0,0),0.4, 1, 1234, 'agent1') obs2 = AgentObservation(UE4Coord(0,0),0.7, 2, 1235, 'agent1') obs3 = AgentObservation(UE4Coord(0,0),0.93, 3, 1237, 'agent1') test_map = create_belief_map(test_grid, "agent1") test_map.update_from_observation(obs1) assert 0.0111 < test_map.get_belief_map_component(UE4Coord(0,0)).likelihood < 0.0112 test_map.update_from_observation(obs2) assert 0.025688 < test_map.get_belief_map_component(UE4Coord(0,0)).likelihood < 0.0256881 test_map.update_from_observation(obs3) assert 0.2594 < test_map.get_belief_map_component(UE4Coord(0,0)).likelihood < 0.2595 #now check observing in a different order gives same result test_map = create_belief_map(test_grid, "agent1") test_map.update_from_observation(obs2) test_map.update_from_observation(obs1) test_map.update_from_observation(obs3)