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
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
# 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
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)
#-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)
cart_in = Conversor.geo_to_cart(point_in_obstacle, geo_home)
cart_out = Conversor.geo_to_cart(point_out_obstacle, geo_home)
# x axis values
x = [cart_icmc[0].x, cart_icmc[1].x, cart_icmc[2].x, cart_icmc[3].x,cart_icmc[0].x]
# corresponding y axis values
y = [cart_icmc[0].y, cart_icmc[1].y, cart_icmc[2].y, cart_icmc[3].y, cart_icmc[0].y]
print(x)
print(y)
# plotting the points