def build_sample_map():
# Função para criar um mapa de teste
wp_ori = CartesianPoint(10.0, 10.0)
wp_des = CartesianPoint(40.0, 10.0)
verts = [
(20.0, 5.0), # left, bottom
(20.0, 15.5), # left, top
(30.0, 15.5), # right, top
(30.0, 5.0), # right, bottom
(20.0, 5.0), # ignored
]
verts = [CartesianPoint(v[0], v[1]) for v in verts]
mapa = Mapa(wp_ori, wp_des, [verts])
return mapa
def read_mission(missao_filename, missao_id, mapa_filename, mapa_id):
with open(missao_filename, 'r') as missao_file, open(mapa_filename, 'r') as mapa_file:
#missao_file = json.load(missao_file)[[mapa_id]mapa_id]
mapa_file = json.load(mapa_file)
mapa_id = missao_file[missao_id]['map']
geo_home = mapa[mapa_id]['geo_home']
origin = Conversor.geo_to_cart(missao_file[missao_id]['mission_execution'][0]['instructions']['geo_origin'], geo_home)
destination = Conversor.geo_to_cart(missao_file[missao_id]['mission_execution'][0]['instructions']['geo_destination'], geo_home)
areas_bonificadoras = [ Conversor.list_geo_to_cart(mapa_file[mapa_id]['areas_bonificadoras']['geo_points'], geo_home) ]
areas_penalizadoras = [ Conversor.list_geo_to_cart(mapa_file[mapa_id]['areas_penalizadoras']['geo_points'], geo_home) ]
areas_nao_navegaveis = [ Conversor.list_geo_to_cart(mapa_file[mapa_id]['areas_nao_navegaveis']['geo_points'], geo_home) ]
mapa = Mapa(origin, destination, areas_nao_navegaveis)
return mapa
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,
C_t=0,
max_exec_time=1,
T_max=20,
px0=cart_origin.x,
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)
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)