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()