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)