def read_mapa(mapa_filename, mapa_id): with open(mapa_filename, 'r') as mapa_file: mapa_file = json.load(mapa_file) mapa = mapa_file[mapa_id] geo_home = GeoPoint(mapa['geo_home'][1], mapa['geo_home'][0], mapa['geo_home'][2]) areas_bonificadoras = [ Conversor.list_geo_to_cart(area['geo_points'], geo_home) for area in mapa['areas_bonificadoras'] ] areas_penalizadoras = [ Conversor.list_geo_to_cart(area['geo_points'], geo_home) for area in mapa['areas_penalizadoras'] ] areas_nao_navegaveis = [] for area in mapa['areas_nao_navegaveis']: geo_points = [] for geo_point in area['geo_points']: geo_points.append( Conversor.geo_to_cart( GeoPoint(geo_point[1], geo_point[0], geo_point[2]), geo_home)) areas_nao_navegaveis.append(geo_points) return geo_home, areas_bonificadoras, areas_penalizadoras, areas_nao_navegaveis
# Parâmetros recebidos (arquivo .srv) origin_lat = -22.002237 origin_long = -47.932546 origin_alt = 13 destination_lat = -22.002674 destination_long = -47.932608 destination_alt = 15 mapa_id = 0 # Leitura do arquvio em DATA geo_home, _, _, areas_n = read_mapa('/home/vannini/drone_arch/Data/mapa.json', mapa_id) print(geo_home) cart_origin = Conversor.geo_to_cart( GeoPoint(origin_lat, origin_long, origin_alt), geo_home) cart_destination = Conversor.geo_to_cart( GeoPoint(destination_lat, destination_long, destination_alt), geo_home) mapa = Mapa(cart_origin, cart_destination, areas_n, inflation_rate=0.1) ## EXECUÇÃO DO AG ag = Genetic(Subject, mapa, taxa_cross=1.0, population_size=80, C_d=1000, C_obs=10000, C_con=10, C_cur=0,
def run_genetic(req): ## ENTRADA # Parâmetros recebidos (arquivo .srv) origin_lat = req.origin_lat origin_long = req.origin_long origin_alt = req.origin_alt destination_lat = req.destination_lat destination_long = req.destination_long destination_alt = req.destination_alt map_id = req.map_id print(origin_lat) # Leitura do arquvio em DATA geo_home, _, _, areas_n = read_mapa('/home/vannini/Data/mapa.json', map_id) cart_origin = Conversor.geo_to_cart( GeoPoint(origin_lat, origin_long, origin_alt), geo_home) cart_destination = Conversor.geo_to_cart( GeoPoint(destination_lat, destination_long, destination_alt), geo_home) mapa = Mapa(cart_origin, cart_destination, areas_n, inflation_rate=0.1) ## EXECUÇÃO DO AG ag = Genetic(Subject, mapa, taxa_cross=1.0, population_size=80, C_d=1000, C_obs=1000000, C_con=10, C_cur=0, C_t=0, max_exec_time=180, T_max=20, px0=cart_origin.x, py0=cart_origin.y) best = ag.run(info=True) # Melhor rota encontrada : WPs em cartesiano cart_points = best.get_route() # Melhor rota encontrada : WPs em geográfico geo_points = [ Conversor.cart_to_geo(CartesianPoint(cart_point[0], cart_point[1]), geo_home) for cart_point in cart_points ] ## SAÍDA ## Interface Gráfica plot_map(mapa.areas_n_inf, ['n' for _ in range(len(mapa.areas_n_inf))], cart_origin, cart_destination, best.get_route(), None) ## /Interface Gráfica output_filename = '/home/vannini/Missions/path_from_ga_output.wp' write_mavros(output_filename, geo_points) return GA_PlannerResponse(output_filename)
def run_genetic(req): ## ENTRADA ## Parâmetros recebidos (arquivo .srv) origin_lat = req.origin_lat origin_long = req.origin_long origin_alt = req.origin_alt destination_lat = req.destination_lat destination_long = req.destination_long destination_alt = req.destination_alt map_id = req.map_id print(origin_lat) # Leitura do arquvio em DATA PATH = '/home/vannini/drone_arch/Data/mapa.json' #Ubuntu Veronica #PATH = '/home/gustavosouza/Documents/Per/path-planning/data/mapa.json' #Ubuntu Gustavo #PATH = r'../../data' #Windows geo_home, _, _, areas_n = read_mapa(PATH, map_id) #geo_home, _, _, areas_n = upload_mapa(mapa_file, mapa_id) cart_origin = Conversor.geo_to_cart(GeoPoint(origin_lat, origin_long, origin_alt), geo_home) cart_destination = Conversor.geo_to_cart(GeoPoint(destination_lat, destination_long, destination_alt), geo_home) mapa = Mapa(cart_origin, cart_destination, areas_n, inflation_rate=3) ## EXECUÇÃO DO AG ag_teste = Genetic(Subject, mapa, # Genetic taxa_cross=0.5, population_size=100, C_d=1000, C_obs=10000, C_con=100, C_cur=500, C_t=100, max_exec_time= 1440, #Subject, T_min=10, T_max=40, mutation_prob=0.7, px0=cart_origin.x, py0=cart_origin.y ) best = ag_teste.run(info=True) # Melhor rota encontrada : WPs em cartesiano cart_points = best.get_route() # Melhor rota encontrada : WPs em geográfico geo_points = [ Conversor.cart_to_geo(CartesianPoint(cart_point[0], cart_point[1]), geo_home) for cart_point in cart_points ] # Visualização do Mapa usado, com a rota do melhor de todos areas = [ area for area in itertools.chain(mapa.areas_n, mapa.areas_n_inf) ] tipos = [ 'n' for _ in range(len(areas))] plot_map( areas=areas, # Mapa usado labels=tipos, # Tipo do mapa {'n','p','b'} <- Não afeta o genético, só muda a visualização origem=mapa.origin, # waypoint de origem destino=mapa.destination, # waypoint de destino waypoints=best.get_route(), # rota do melhor de todos ) ## SAÍDA ## /Interface Gráfica # output_filename = '/mnt/c/Projetos/path-planning/algorithms/ros_genetic/path_from_ga_output.wp' # Ubuntu Gustavo output_filename = '/home/vannini/drone_arch/Missions/path_from_ga_output.waypoints' # Ubuntu Veronica write_mavros(output_filename, geo_points) return GA_PlannerResponse(output_filename)
import argparse import matplotlib.pyplot as plt from data_definitions import Mapa, CartesianPoint, Conversor, GeoPoint ## obstacle (ICMC) #-22.006045,-47.896007 #-22.006045,-47.895561 #-22.007557, -47.895561 #-22.007557,,-47.896007 icmc = [GeoPoint(-22.006045, -47.896673, 10), GeoPoint(-22.005787, -47.896007,10), GeoPoint(-22.007557, -47.893121,10), GeoPoint(-22.007861, -47.895561,10)] icmc1 = [GeoPoint(-22.006045,-47.896007, 10), GeoPoint(-22.006045,-47.895561,10), GeoPoint(-22.007557, -47.895561,10), GeoPoint(-22.007557,-47.896007,10)] point_in_obstacle = GeoPoint(-22.007252, -47.894711, 10) point_out_obstacle = GeoPoint(-22.006484, -47.896854, 10) geo_home = GeoPoint(-22.006218, -47.898374,0) cart_icmc = [] for point in icmc: cart_icmc.append(Conversor.geo_to_cart(point, geo_home)) print(cart_icmc)