def create_grid_with_no_points(self):
self.grid = []
backtrack = False
for x_counter in range(self.no_x):
if backtrack:
for y_counter in range(self.no_y):
self.grid.append(self.origin +
UE4Coord(x_counter *
self.lng_spacing, y_counter *
self.lat_spacing))
backtrack = not backtrack
else:
for y_counter in range(self.no_y - 1, -1, -1):
self.grid.append(self.origin +
UE4Coord(x_counter *
self.lng_spacing, y_counter *
self.lat_spacing))
backtrack = not backtrack
#ToDo:
#Currently observations must be made at grid locations - instead compute which observations are made
#in each grid location and then compute the belief map
return_belief_map = create_belief_map(grid, self.agent_name)
return_belief_map.update_from_observations(self.get_all_observations())
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()
self.update_observations_file(self.observations_file_loc,
newest_observation)
logger(str(newest_observation), "info", "observations")
#if agent is in range, communicate
def explore_t_timesteps(self, t: int):
for i in range(t):
self.explore_timestep()
self.timestep += 1
#print("current belief map: {}".format(self.current_belief_map))
return self.current_belief_map
if __name__ == "__main__":
from Utils.ClientMock import KinematicsState, MockRavForTesting, ImageType, Vector3r
grid = UE4Grid(15, 20, UE4Coord(0, 0), 60, 45)
#grid, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, performance_csv_path: "file path that agent can write performance to", prior = []
#grid, initial_pos, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, prior = {}
occupancy_grid_agent = OccupancyGridAgent(
grid, UE4Coord(0, 0), get_move_from_belief_map_epsilon_greedy, -12,
0.2, MockRavForTesting(), 'agent1')
#write some tests for agent here
occupancy_grid_agent.current_pos_intended = UE4Coord(0, 0)
occupancy_grid_agent.current_pos_measured = None
occupancy_grid_agent.current_reading = 0.1
occupancy_grid_agent.get_agent_state_for_analysis()
occupancy_grid_agent.explore_timestep()
#belief_map: BeliefMap, current_grid_loc: UE4Coord, epsilon: float, eff_radius = None) -> UE4Coord:
dont_move = lambda belief_map, current_grid_loc, epsilon: UE4Coord(15, 20)
print(grid.get_grid_points())
#%%
def calc_likelihood(observations: typing.List[float]):
return functools.reduce(lambda x, y: x * y, observations)
assert calc_likelihood([0.1, 0.1, 0.2, 0.4]) == 0.1 * 0.1 * 0.2 * 0.4
#grid, agent_name, prior = {}
#update_bel_map(update_bel_map(test_map, 0.5, 3), 0.5,3)
if __name__ != '__main__':
from ClientMock import Vector3r
grid = UE4Grid(20, 15, UE4Coord(0, 0), 120, 150)
#grid, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, performance_csv_path: "file path that agent can write performance to", prior = []
occupancy_grid_agent = OccupancyGridAgent(
grid, get_move_from_belief_map_epsilon_greedy, -12, 0.2,
MockRavForTesting(), 'agent1')
#write some tests for agent here
occupancy_grid_agent.current_pos_intended = UE4Coord(0, 0)
occupancy_grid_agent.current_pos_measured = None
occupancy_grid_agent.current_reading = 0.1
occupancy_grid_agent.get_agent_state_for_analysis()
occupancy_grid_agent.explore_timestep()
##################### Functions that can deal with the initialization of RAVs ####################
#%%
return_bel_map = create_belief_map(grid, agent_name, agent_belief_map_prior)
#update belief map based on all observations...
return_bel_map.update_from_observations(agent_observations)
return return_bel_map
if __name__ == "__main__":
#tests for calc_posterior
assert abs(calc_posterior(0.5, 0.2) - 0.2) <= 0.001
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)
#return str(agent_analysis_state._fields).replace(')','').replace('(','').replace("'", '')
return _get_agent_observation_csv(**agent_observation._asdict())
#AgentObservation = namedtuple('obs_location', ['grid_loc','probability','timestep', 'timestamp', 'observer_name'])
def _init_observations_file(file_path):
with open(file_path, 'w+') as f:
f.write(','.join(AgentObservation._fields))
def _write_to_obserations_file(file_path, agent_observation):
with open(file_path, 'a') as f:
f.write('\n' + get_agent_observation_csv(agent_observation))
if __name__ == "__main__":
test_grid = UE4Grid(1, 1, UE4Coord(0, 0), 10, 6)
test_agent_observations = AgentObservations(test_grid)
obs1 = AgentObservation(UE4Coord(0, 0), 0.5, 1, 1234, 'agent1')
obs2 = AgentObservation(UE4Coord(0, 0), 0.7, 2, 1235, 'agent1')
obs3 = AgentObservation(UE4Coord(0, 1), 0.9, 3, 1237, 'agent1')
test_agent_observations.record_agent_observation(obs1)
test_agent_observations.record_agent_observation(obs2)
test_agent_observations.record_agent_observation(obs3)
assert test_agent_observations.get_most_recent_observation() == obs3
assert test_agent_observations.get_most_recent_observation_at_position(
UE4Coord(0, 0)) == obs2
assert test_agent_observations.get_all_observations_at_position(
UE4Coord(0, 1)) == [obs3]
nargs="+",
help=
'The uniquely identifiable names of other agents active in the environment',
action='store',
default=[])
return parser.parse_args(args)
if __name__ == "__main__":
args = parse_args(sys.argv[1:])
agent_name = args.agent_name
print('args: ', args)
if "names_list" in args:
other_active_agents = args.names_list
else:
other_active_agents = []
#hard code grid for now
grid = UE4Grid(20, 15, UE4Coord(0, 0), 120, 150)
client = airsim.MultirotorClient()
create_rav(client, agent_name)
print('Running with other active agents: {}'.format(other_active_agents))
#grid, initial_pos, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, prior = {}
OccupancyGridAgent(
grid, UE4Coord(0, 0), get_move_from_belief_map_epsilon_greedy, -12,
0.3, client, agent_name,
other_active_agents).explore_t_timesteps(args.no_timesteps)
destroy_rav(client, agent_name)
def get_lng_spacing(self):
return self.lng_spacing
def get_no_points_x(self):
return self.no_x
def get_no_points_y(self):
return self.no_y
def get_neighbors(self, grid_loc, radius):
'''Gets neighbors of grid_loc within radius.'''
return list(
filter(
lambda alt_grid_loc: alt_grid_loc.get_dist_to_other(
grid_loc) <= radius and alt_grid_loc != grid_loc,
self.get_grid_points()))
if __name__ == "__main__":
test_grid = UE4Grid(1, 1, UE4Coord(0, 0), 10, 6)
assert set(test_grid.get_neighbors(UE4Coord(2, 2), 1.9)) == set([
UE4Coord(1, 2),
UE4Coord(2, 1),
UE4Coord(2, 3),
UE4Coord(3, 2),
UE4Coord(3, 3),
UE4Coord(1, 3),
UE4Coord(1, 1),
UE4Coord(3, 1)
])
#a list of UE4Coord
neighbors = belief_map.get_grid().get_neighbors(current_grid_loc, eff_radius)
#don't move to new position if can't find any neighbors to move to
if not neighbors:
return current_grid_loc
#neighbors = list(filter(lambda grid_loc: grid_loc.get_dist_to_other(current_grid_loc) <= eff_radius and grid_loc!=current_grid_loc, bel_map.keys()))
if random.random() < epsilon:
#epsilon random
return_move = random.choice(neighbors)
else:
#otherwise choose move that has highest value
max_move_value = 0
for neighbor in neighbors:
if belief_map.get_belief_map_component(neighbor).likelihood > max_move_value:
max_move_value = belief_map.get_belief_map_component(neighbor).likelihood
return_move = neighbor
# move = max(map(lambda neighbor: bel_map[neighbor].likelihood, neighbors))
return return_move
if __name__ == "__main__":
test_grid = UE4Grid(1, 1, UE4Coord(0,0), 6, 5)
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')
#(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)
assert get_move_from_belief_map_epsilon_greedy(belief_map, UE4Coord(1,1), 0.0, 1.8) == UE4Coord(0,1)