494 lines
19 KiB
Python
494 lines
19 KiB
Python
import json
|
|
import geopandas as gpd
|
|
import matplotlib.pyplot as plt
|
|
from shapely.geometry import LineString, Point
|
|
import numpy as np
|
|
import paho.mqtt.client as mqtt
|
|
from queue import Queue
|
|
from threading import Thread
|
|
from matplotlib.animation import FuncAnimation
|
|
import networkx as nx
|
|
import math
|
|
|
|
# Carregar o arquivo GeoJSON
|
|
arquivo_mapa = "mapa.json"
|
|
mapa = gpd.read_file(arquivo_mapa)
|
|
|
|
# Configurações do MQTT
|
|
broker_address = "localhost" # Substitua pelo endereço do broker MQTT
|
|
topic = "coordenadas_gps" # Tópico onde o robô envia a posição
|
|
|
|
# Fila para comunicação entre threads
|
|
fila_dados = Queue()
|
|
|
|
# Converter distância em metros para graus (aproximado)
|
|
def metros_para_graus(distancia_metros):
|
|
return distancia_metros / 111320 # Aproximadamente 111.32 km por grau na região equatorial
|
|
|
|
# Converter distância em graus para metros (aproximado)
|
|
def graus_para_metros(distancia_graus):
|
|
return distancia_graus * 111320 # Aproximadamente 111.32 km por grau na região equatorial
|
|
|
|
def calcular_angulo_entre_pontos(ponto1, ponto2):
|
|
"""
|
|
Calcula o ângulo (em graus) entre dois pontos geográficos.
|
|
:param ponto1: (longitude, latitude) do primeiro ponto.
|
|
:param ponto2: (longitude, latitude) do segundo ponto.
|
|
:return: Ângulo em graus (0° a 360°).
|
|
"""
|
|
delta_y = ponto2[1] - ponto1[1]
|
|
delta_x = ponto2[0] - ponto1[0]
|
|
angulo_radianos = math.atan2(delta_y, delta_x)
|
|
angulo_graus = math.degrees(angulo_radianos)
|
|
return (angulo_graus + 360) % 360 # Garantir que o ângulo esteja entre 0° e 360°
|
|
|
|
def calcular_media_angulos(angulo1, angulo2):
|
|
"""
|
|
Calcula a média de dois ângulos em graus, considerando a circularidade.
|
|
:param angulo1: Primeiro ângulo em graus.
|
|
:param angulo2: Segundo ângulo em graus.
|
|
:return: Média dos ângulos em graus.
|
|
"""
|
|
seno_medio = (math.sin(math.radians(angulo1)) + math.sin(math.radians(angulo2))) / 2
|
|
cosseno_medio = (math.cos(math.radians(angulo1)) + math.cos(math.radians(angulo2))) / 2
|
|
angulo_medio = math.degrees(math.atan2(seno_medio, cosseno_medio))
|
|
return (angulo_medio + 360) % 360 # Garantir que o ângulo esteja entre 0° e 360°
|
|
|
|
TOLERANCIA_DESVIO = metros_para_graus(2.0) # Ajuste conforme necessário (ex.: em graus ou metros)
|
|
trajetoria_global = []
|
|
pontos_visitados = set()
|
|
corredor_atual = 0
|
|
ultimo_corredor_atual = -1
|
|
transicoes = [] # Lista para armazenar as transições entre corredores
|
|
orientacao_atual = 0
|
|
|
|
|
|
# Função para lidar com mensagens recebidas
|
|
def on_message(client, userdata, msg):
|
|
global trajetoria_global
|
|
global orientacao_atual
|
|
|
|
try:
|
|
payload = json.loads(msg.payload.decode("utf-8"))
|
|
|
|
if "longitude" in payload and "latitude" in payload:
|
|
latitude = payload["latitude"]
|
|
longitude = payload["longitude"]
|
|
orientacao_atual = payload["orientacao"]
|
|
posicao_atual = Point(longitude, latitude)
|
|
print(f"Posição atual do robô: {posicao_atual}")
|
|
|
|
atualizar_dados_trajetoria_global(posicao_atual, True)
|
|
|
|
else:
|
|
print(f"Chaves 'longitude' e 'latitude' ausentes no payload: {payload}")
|
|
except json.JSONDecodeError as e:
|
|
print(f"Erro ao decodificar JSON: {e}")
|
|
|
|
def atualizar_dados_trajetoria_global(posicao_atual, recalcular_trajetoria):
|
|
global centros
|
|
global trajetoria_global
|
|
global transicoes
|
|
global corredor_atual
|
|
global ultimo_corredor_atual
|
|
|
|
if recalcular_trajetoria:
|
|
nova_trajetoria = planejar_trajetoria_global_com_a_star(posicao_atual, centros)
|
|
nova_trajetoria = atualizar_trajetoria(posicao_atual, nova_trajetoria)
|
|
|
|
# Selecionar o próximo ponto baseado na orientação
|
|
proximo_ponto = selecionar_proximo_ponto_com_base_na_rua(posicao_atual, nova_trajetoria)
|
|
# Atualizar a trajetória para partir da posição atual e seguir pelo próximo ponto
|
|
nova_trajetoria = [nova_trajetoria[0], proximo_ponto] + [
|
|
ponto for ponto in nova_trajetoria[2:]
|
|
]
|
|
|
|
|
|
|
|
# Forçar o robô a concluir os pontos não visitados do corredor atual
|
|
pontos_nao_visitados = verificar_pontos_nao_visitados(ultimo_corredor_atual, pontos_visitados)
|
|
|
|
print(f"Ultimo corredor valido: {ultimo_corredor_atual}, Corredor atual: {corredor_atual}, Pontos visitados: {pontos_visitados}")
|
|
|
|
if pontos_nao_visitados and corredor_atual != ultimo_corredor_atual:
|
|
print("Ainda há pontos não visitados no corredor atual. Forçando retorno.")
|
|
nova_trajetoria = pontos_nao_visitados # + nova_trajetoria
|
|
if len(pontos_nao_visitados) == 0:
|
|
print("Todos os pontos do corredor atual foram visitados.")
|
|
ultimo_corredor_atual = ultimo_corredor_atual + 1
|
|
|
|
|
|
|
|
trajetoria_global = nova_trajetoria
|
|
fila_dados.put((posicao_atual, trajetoria_global, transicoes))
|
|
|
|
# Função para calcular o ponto médio entre duas coordenadas
|
|
def ponto_medio(coord1, coord2):
|
|
return [(p1 + p2) / 2 for p1, p2 in zip(coord1, coord2)]
|
|
|
|
# Calcular o centro entre duas linhas
|
|
def calcular_centro_entre_linhas(linha1, linha2):
|
|
coords1 = np.array(linha1.coords)
|
|
coords2 = np.array(linha2.coords)
|
|
centro_coords = [ponto_medio(c1, c2) for c1, c2 in zip(coords1, coords2)]
|
|
return LineString(centro_coords)
|
|
|
|
# Gerar pontos uniformemente ao longo de uma rua
|
|
def gerar_pontos_rua(rua, passo=1):
|
|
passo_graus = metros_para_graus(passo)
|
|
pontos = []
|
|
comprimento = rua.length
|
|
for distancia in np.arange(0, comprimento, passo_graus):
|
|
ponto = rua.interpolate(distancia)
|
|
pontos.append((ponto.y, ponto.x)) # (latitude, longitude)
|
|
return pontos
|
|
|
|
# Conectar ruas adjacentes
|
|
def conectar_ruas(fim_rua_atual, inicio_rua_proxima):
|
|
return [(fim_rua_atual[0] + (inicio_rua_proxima[0] - fim_rua_atual[0]) * t,
|
|
fim_rua_atual[1] + (inicio_rua_proxima[1] - fim_rua_atual[1]) * t)
|
|
for t in np.linspace(0, 1, num=5)]
|
|
|
|
# Planejar a trajetória global
|
|
def planejar_trajetoria_global(posicao_inicial, centros, passo=1):
|
|
trajetoria = []
|
|
|
|
for i, rua in enumerate(centros):
|
|
pontos_rua = gerar_pontos_rua(rua, passo)
|
|
|
|
# Inverter direção das ruas ímpares para otimizar
|
|
if i % 2 == 1:
|
|
pontos_rua = pontos_rua[::-1]
|
|
|
|
trajetoria.extend(pontos_rua)
|
|
|
|
if i < len(centros) - 1:
|
|
fim_rua_atual = pontos_rua[-1]
|
|
inicio_proxima_rua = gerar_pontos_rua(centros[i + 1], passo)[0]
|
|
conexao = conectar_ruas(fim_rua_atual, inicio_proxima_rua)
|
|
trajetoria.extend(conexao)
|
|
|
|
return trajetoria
|
|
|
|
# Atualizar o gráfico dinamicamente
|
|
def atualizar_grafico(frame):
|
|
while not fila_dados.empty():
|
|
posicao_atual, trajetoria, transicoes = fila_dados.get()
|
|
|
|
# Atualizar posição atual do robô
|
|
posicao_atual_plot.set_data([posicao_atual.x], [posicao_atual.y])
|
|
|
|
# Atualizar trajetória global
|
|
trajetoria_lat, trajetoria_long = zip(*trajetoria)
|
|
trajetoria_plot.set_data(trajetoria_long, trajetoria_lat)
|
|
|
|
# Atualizar transições
|
|
for transicao_plot in transicoes_plot:
|
|
transicao_plot.remove() # Remover os plots antigos
|
|
transicoes_plot.clear()
|
|
|
|
for transicao in transicoes:
|
|
transicao_lat, transicao_long = zip(*[(p[1], p[0]) for p in transicao]) # Converter (x, y) -> (lat, long)
|
|
plot, = ax.plot(transicao_long, transicao_lat, color="orange", label="Transições")
|
|
transicoes_plot.append(plot)
|
|
|
|
return posicao_atual_plot, trajetoria_plot
|
|
|
|
def calcular_distancia_geodésica(p1, p2):
|
|
"""
|
|
Calcula a distância geodésica entre dois pontos geográficos (latitude, longitude) usando Haversine.
|
|
:param p1: (longitude, latitude) do primeiro ponto
|
|
:param p2: (longitude, latitude) do segundo ponto
|
|
:return: Distância em metros
|
|
"""
|
|
R = 6371000 # Raio médio da Terra em metros
|
|
# Converter coordenadas de graus para radianos
|
|
lat1, lon1 = math.radians(p1[1]), math.radians(p1[0])
|
|
lat2, lon2 = math.radians(p2[1]), math.radians(p2[0])
|
|
# Diferença das coordenadas
|
|
dlat = lat2 - lat1
|
|
dlon = lon2 - lon1
|
|
# Fórmula de Haversine
|
|
a = math.sin(dlat / 2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2)**2
|
|
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
|
return R * c
|
|
|
|
|
|
def criar_grafo(centros):
|
|
grafo = nx.Graph()
|
|
for i, rua in enumerate(centros):
|
|
for j, ponto in enumerate(rua.coords):
|
|
grafo.add_node((i, j), pos=ponto) # Nó identificado por (rua, índice)
|
|
if j > 0:
|
|
grafo.add_edge((i, j - 1), (i, j)) # Conectar pontos consecutivos da mesma rua
|
|
if i < len(centros) - 1:
|
|
# Conectar o fim da rua atual ao início da próxima rua
|
|
grafo.add_edge((i, len(rua.coords) - 1), (i + 1, 0))
|
|
return grafo
|
|
|
|
def planejar_com_a_star(grafo, inicio, objetivo):
|
|
return nx.astar_path(
|
|
grafo,
|
|
inicio,
|
|
objetivo,
|
|
heuristic=lambda n1, n2: Point(grafo.nodes[n1]["pos"]).distance(Point(grafo.nodes[n2]["pos"]))
|
|
)
|
|
|
|
def gerar_curva_suave(ponto_inicial, ponto_final, num_pontos=10, angulo_deslocamento=0, distancia_deslocamento=2):
|
|
"""
|
|
Gera uma curva Bézier suave entre dois pontos, deslocando o ponto médio no ângulo e distância especificados.
|
|
:param ponto_inicial: Coordenadas (longitude, latitude) do ponto inicial.
|
|
:param ponto_final: Coordenadas (longitude, latitude) do ponto final.
|
|
:param num_pontos: Número de pontos na curva.
|
|
:param angulo_deslocamento: Ângulo de deslocamento do ponto médio (em graus, 0 para direção leste).
|
|
:param distancia_deslocamento: Distância do deslocamento (em metros).
|
|
:return: Lista de coordenadas (longitude, latitude) da curva.
|
|
"""
|
|
# Converter o deslocamento de metros para graus (aproximação)
|
|
deslocamento_graus = metros_para_graus(distancia_deslocamento)
|
|
|
|
# Calcular o ponto médio simples entre inicial e final
|
|
ponto_medio_basico = (
|
|
(ponto_inicial[0] + ponto_final[0]) / 2,
|
|
(ponto_inicial[1] + ponto_final[1]) / 2
|
|
)
|
|
|
|
# Converter o ângulo para radianos
|
|
angulo_radianos = math.radians(angulo_deslocamento)
|
|
|
|
# Calcular o deslocamento em longitude e latitude
|
|
deslocamento_longitude = deslocamento_graus * math.cos(angulo_radianos)
|
|
deslocamento_latitude = deslocamento_graus * math.sin(angulo_radianos)
|
|
|
|
# Aplicar o deslocamento ao ponto médio
|
|
ponto_medio_deslocado = (
|
|
ponto_medio_basico[0] + deslocamento_latitude,
|
|
ponto_medio_basico[1] + deslocamento_longitude
|
|
)
|
|
|
|
# Função de interpolação para Bézier cúbica
|
|
def bezier(t, p0, p1, p2):
|
|
x = (1 - t)**2 * p0[0] + 2 * (1 - t) * t * p1[0] + t**2 * p2[0]
|
|
y = (1 - t)**2 * p0[1] + 2 * (1 - t) * t * p1[1] + t**2 * p2[1]
|
|
return x, y
|
|
|
|
# Gerar os pontos da curva
|
|
curva = [bezier(t, ponto_inicial, ponto_medio_deslocado, ponto_final) for t in np.linspace(0, 1, num_pontos)]
|
|
return curva
|
|
|
|
|
|
def encontrar_melhor_no(grafo, no_atual, proximo_corredor):
|
|
"""
|
|
Encontra o nó mais próximo no próximo corredor.
|
|
:param grafo: Grafo contendo os nós e posições.
|
|
:param no_atual: Nó atual (longitude, latitude).
|
|
:param proximo_corredor: Coordenadas do próximo corredor.
|
|
:return: Melhor nó (coordenadas longitude, latitude).
|
|
"""
|
|
menor_distancia = float("inf")
|
|
melhor_no = None
|
|
|
|
for i, ponto in enumerate(proximo_corredor.coords):
|
|
distancia = calcular_distancia_geodésica(grafo.nodes[no_atual]["pos"], ponto)
|
|
if distancia < menor_distancia:
|
|
menor_distancia = distancia
|
|
melhor_no = ponto
|
|
return melhor_no
|
|
|
|
def planejar_trajetoria_global_com_a_star(posicao_atual, centros, passo_metros=1):
|
|
global grafo
|
|
global corredor_atual
|
|
global ultimo_corredor_atual
|
|
"""
|
|
Planeja a trajetória global considerando a posição atual do robô e a melhor conexão entre corredores.
|
|
"""
|
|
|
|
trajetoria = []
|
|
|
|
# Encontrar o nó mais próximo da posição atual no grafo
|
|
no_inicial = min(
|
|
grafo.nodes,
|
|
key=lambda n: Point(grafo.nodes[n]["pos"]).distance(posicao_atual),
|
|
)
|
|
print(f"Nó inicial mais próximo da posição atual: {no_inicial}")
|
|
corredor_atual = no_inicial[0]
|
|
ultimo_corredor_atual = corredor_atual if ultimo_corredor_atual == -1 else 0
|
|
ponto_atual = no_inicial[1]
|
|
# Verificar se o ponto já foi visitado e adicioná-lo, caso contrário
|
|
if ponto_atual not in pontos_visitados:
|
|
idx = 0
|
|
pontos_visitados.clear()
|
|
while (idx <= ponto_atual):
|
|
pontos_visitados.add(idx)
|
|
idx = idx + 1
|
|
|
|
# Planejar trajetória dentro de cada corredor
|
|
for i, rua in enumerate(centros):
|
|
pontos_rua = gerar_pontos_rua(rua, passo_metros)
|
|
|
|
inverter = False
|
|
# Inverter a direção de ruas ímpares para alternar o percurso
|
|
if i % 2 == 1:
|
|
pontos_rua = pontos_rua[::-1]
|
|
inverter = True
|
|
|
|
# Adicionar os pontos do corredor à trajetória
|
|
trajetoria.extend(pontos_rua)
|
|
|
|
# Conectar ao próximo corredor, se não for o último
|
|
if i < len(centros) - 1:
|
|
# Nós possíveis para conexão
|
|
no_fim_atual = (i, len(rua.coords) - 1)
|
|
no_inicio_proxima = (i + 1, 0)
|
|
no_fim_proxima = (i + 1, len(centros[i + 1].coords) - 1)
|
|
|
|
# Avaliar todas as conexões possíveis
|
|
opcoes_conexao = [
|
|
(no_fim_atual, no_inicio_proxima),
|
|
(no_fim_atual, no_fim_proxima),
|
|
]
|
|
|
|
# Escolher a melhor conexão (menor caminho com A*)
|
|
curva = []
|
|
menor_distancia = float("inf")
|
|
for inicio, destino in opcoes_conexao:
|
|
pos_inicio = (pontos_rua[-1][1], pontos_rua[-1][0]) if inverter else grafo.nodes[inicio]['pos']
|
|
pos_destino = grafo.nodes[destino]['pos']
|
|
|
|
distancia = calcular_distancia_geodésica(pos_inicio, pos_destino)
|
|
#print(f"Distância calculada ({inicio} -> {destino}): {distancia:.2f} metros")
|
|
if distancia < menor_distancia:
|
|
menor_distancia = distancia
|
|
|
|
# Calcular o ângulo do corredor atual
|
|
angulo_atual = calcular_angulo_entre_pontos(pontos_rua[-2], pontos_rua[-1])
|
|
|
|
#print(f"angulo rua {i}: {angulo_atual}")
|
|
|
|
curva = gerar_curva_suave(pos_inicio, pos_destino, num_pontos=7, angulo_deslocamento=angulo_atual, distancia_deslocamento=2)
|
|
|
|
# Adicionar a curva à trajetória
|
|
trajetoria.extend([(p[1], p[0]) for p in curva]) # Converter (x, y) -> (lat, long)
|
|
transicoes.append(curva) # Armazenar a curva como uma transição
|
|
|
|
return trajetoria
|
|
|
|
def atualizar_trajetoria(posicao_atual, trajetoria_global):
|
|
"""
|
|
Atualiza a trajetória global para começar a partir da posição atual do robô.
|
|
Remove pontos já percorridos na trajetória.
|
|
:param posicao_atual: Posição atual do robô (shapely.geometry.Point).
|
|
:param trajetoria_global: Lista de pontos (latitude, longitude) da trajetória global.
|
|
:return: Nova trajetória global começando da posição atual.
|
|
"""
|
|
# Converter a posição atual para o formato (latitude, longitude)
|
|
posicao_atual_coords = (posicao_atual.y, posicao_atual.x)
|
|
|
|
# Encontrar o ponto mais próximo na trajetória global
|
|
menor_distancia = float("inf")
|
|
indice_mais_proximo = 0
|
|
for i, ponto in enumerate(trajetoria_global):
|
|
distancia = calcular_distancia_geodésica(posicao_atual_coords, ponto)
|
|
if distancia < menor_distancia:
|
|
menor_distancia = distancia
|
|
indice_mais_proximo = i
|
|
|
|
# Retornar a trajetória a partir do ponto mais próximo
|
|
nova_trajetoria = trajetoria_global[indice_mais_proximo:]
|
|
|
|
# Adicionar a posição atual como o primeiro ponto da nova trajetória
|
|
nova_trajetoria.insert(0, posicao_atual_coords)
|
|
|
|
return nova_trajetoria
|
|
|
|
def selecionar_proximo_ponto_com_base_na_rua(posicao_atual, pontos_rua):
|
|
"""
|
|
Seleciona o próximo ponto da rua com base na posição do robô e no ângulo da rua.
|
|
:param posicao_atual: Posição atual do robô (shapely.geometry.Point).
|
|
:param pontos_rua: Lista de pontos (latitude, longitude) da rua.
|
|
:return: Coordenadas do próximo ponto (latitude, longitude).
|
|
"""
|
|
if len(pontos_rua) < 3:
|
|
# Se houver menos de três pontos, não há como calcular o ângulo da rua adequadamente
|
|
return None
|
|
|
|
# Calcular o ângulo de referência da rua (entre os dois primeiros pontos da rua)
|
|
angulo_rua = calcular_angulo_entre_pontos(
|
|
(pontos_rua[0][0], pontos_rua[0][1]), # Inverter para (longitude, latitude)
|
|
(pontos_rua[1][0], pontos_rua[1][1]) # Inverter para (longitude, latitude)
|
|
)
|
|
|
|
menor_diferenca_angular = float("inf")
|
|
proximo_ponto = None
|
|
|
|
# Analisar os dois próximos pontos da rua
|
|
for ponto in pontos_rua[1:3]:
|
|
# Calcular o ângulo do robô até o ponto
|
|
angulo_ponto = calcular_angulo_entre_pontos(
|
|
(posicao_atual.x, posicao_atual.y), # Inverter para (longitude, latitude)
|
|
(ponto[0], ponto[1]) # Inverter para (longitude, latitude)
|
|
)
|
|
|
|
# Calcular a diferença angular em relação ao ângulo da rua
|
|
diferenca_angular = abs(angulo_rua - angulo_ponto)
|
|
if diferenca_angular > 180:
|
|
diferenca_angular = 360 - diferenca_angular
|
|
|
|
# Escolher o ponto com a menor diferença angular
|
|
if diferenca_angular < menor_diferenca_angular:
|
|
menor_diferenca_angular = diferenca_angular
|
|
proximo_ponto = ponto
|
|
|
|
return proximo_ponto
|
|
|
|
def verificar_pontos_nao_visitados(idx_corredor, pontos_visitados):
|
|
"""
|
|
Retorna uma lista de pontos no corredor que ainda não foram visitados, com base nos índices.
|
|
:param idx_corredor: Índice do corredor no conjunto de corredores (centros).
|
|
:param pontos_visitados: Conjunto de índices dos pontos já visitados.
|
|
:return: Lista de pontos (latitude, longitude) que ainda não foram visitados.
|
|
"""
|
|
|
|
# Filtrar os pontos cujos índices não estão nos pontos visitados
|
|
pontos_nao_visitados = [
|
|
ponto for idx, ponto in enumerate(centros[idx_corredor].coords) if idx not in pontos_visitados
|
|
]
|
|
|
|
return pontos_nao_visitados
|
|
|
|
|
|
# Processar o mapa para calcular os centros das ruas
|
|
linhas = list(mapa.geometry)
|
|
centros = [calcular_centro_entre_linhas(linhas[i], linhas[i + 1]) for i in range(len(linhas) - 1)]
|
|
grafo = criar_grafo(centros) # Criar o grafo do mapa
|
|
|
|
# Configurar o gráfico
|
|
fig, ax = plt.subplots()
|
|
mapa.plot(ax=ax, color="blue", label="Ruas")
|
|
gpd.GeoSeries(centros).plot(ax=ax, color="red", linestyle="--", label="Centros")
|
|
posicao_atual_plot, = ax.plot([], [], 'go', label="Posição Atual")
|
|
trajetoria_plot, = ax.plot([], [], color="green", label="Trajetória Global")
|
|
transicoes_plot = ax.plot([], [], color="orange", label="Transições")
|
|
plt.legend()
|
|
plt.title("Trajetória Dinâmica")
|
|
plt.xlabel("Longitude")
|
|
plt.ylabel("Latitude")
|
|
|
|
# Configurar animação
|
|
ani = FuncAnimation(fig, atualizar_grafico, interval=100)
|
|
|
|
# Inicializar o cliente MQTT
|
|
client = mqtt.Client()
|
|
client.on_message = on_message
|
|
client.connect(broker_address)
|
|
client.subscribe(topic)
|
|
|
|
# Iniciar o loop MQTT em uma thread separada
|
|
thread_mqtt = Thread(target=client.loop_forever)
|
|
thread_mqtt.daemon = True
|
|
thread_mqtt.start()
|
|
|
|
|
|
# Exibir o gráfico
|
|
plt.show()
|