Пример #1
0
 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()
    
Пример #3
0
        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)
Пример #6
0
    #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)
Пример #8
0
    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)
    ])
Пример #9
0
    #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)