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 # Carregar o arquivo GeoJSON # Substitua o caminho 'mapa.json' pelo caminho do seu arquivo 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() # Função para lidar com mensagens recebidas def on_message(client, userdata, msg): try: # Verifique e exiba o payload recebido print(f"Payload bruto recebido: {msg.payload}") # Decodifique o payload payload = json.loads(msg.payload.decode("utf-8")) if "longitude" in payload and "latitude" in payload: latitude = payload["latitude"] longitude = payload["longitude"] posicao_atual = Point(longitude, latitude) print(f"Posição atual do robô: {posicao_atual}") calcular_trajetoria_dinamica(posicao_atual, centros) else: print(f"Chaves 'longitude' e 'latitude' ausentes no payload: {payload}") except json.JSONDecodeError as e: print(f"Erro ao decodificar JSON: {e}") print(f"Payload recebido (não formatado): {msg.payload.decode('utf-8')}") # 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): # Obter as coordenadas das duas linhas coords1 = np.array(linha1.coords) coords2 = np.array(linha2.coords) # Garantir que as linhas têm o mesmo número de pontos if len(coords1) != len(coords2): raise ValueError("As linhas devem ter o mesmo número de coordenadas!") # Calcular os pontos médios centro_coords = [ponto_medio(c1, c2) for c1, c2 in zip(coords1, coords2)] return LineString(centro_coords) # Função para encontrar o ponto mais próximo no centro def ponto_mais_proximo(posicao, centros): menor_distancia = float("inf") ponto_central_mais_proximo = None # Iterar sobre todas as linhas centrais for linha_central in centros: for ponto in linha_central.coords: ponto_central = Point(ponto) distancia = posicao.distance(ponto_central) if distancia < menor_distancia: menor_distancia = distancia ponto_central_mais_proximo = ponto_central return ponto_central_mais_proximo # Calcular a trajetória e enviar para a fila def calcular_trajetoria_dinamica(posicao_atual, centros): ponto_inicial = ponto_mais_proximo(posicao_atual, centros) tolerancia = 1e-5 for linha_central in centros: if linha_central.distance(ponto_inicial) <= tolerancia: print(f"Trajetória encontrada: {linha_central}") fila_dados.put((posicao_atual, linha_central)) # Enviar para a fila return print("Nenhuma linha central encontrada para o ponto inicial.") def gerar_pontos_rua(rua, passo=1): """ Gera pontos uniformemente espaçados ao longo de uma rua. :param rua: Linha central (LineString) da rua. :param passo: Distância entre os pontos gerados (em coordenadas). :return: Lista de pontos (latitude, longitude). """ pontos = [] comprimento = rua.length for distancia in np.arange(0, comprimento, passo): ponto = rua.interpolate(distancia) # Obter ponto ao longo da linha pontos.append((ponto.y, ponto.x)) # Inverter para (lat, long) return pontos def conectar_ruas(fim_rua_atual, inicio_rua_proxima): """ Conecta o ponto final de uma rua ao ponto inicial de outra rua. :param fim_rua_atual: Último ponto da rua atual (latitude, longitude). :param inicio_rua_proxima: Primeiro ponto da próxima rua (latitude, longitude). :return: Lista de pontos representando a conexão. """ # Uma conexão simples em linha reta 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)] # Dividir a conexão em 5 pontos def planejar_trajetoria_global(posicao_inicial, centros, passo=1): """ Planeja a trajetória global a partir da posição inicial, conectando todas as ruas. :param posicao_inicial: Ponto inicial (shapely.geometry.Point). :param centros: Lista de linhas centrais (ruas). :param passo: Distância entre os pontos gerados. :return: Lista de pontos (latitude, longitude) representando a trajetória. """ trajetoria = [] posicao_atual = posicao_inicial for i, rua in enumerate(centros): # Gerar pontos ao longo da rua atual pontos_rua = gerar_pontos_rua(rua, passo) # Verificar o sentido (ir ou voltar na rua) if i % 2 == 1: pontos_rua = pontos_rua[::-1] # Inverter a direção em ruas ímpares # Adicionar os pontos da rua à trajetória trajetoria.extend(pontos_rua) # Conectar ao próximo corredor (se não for a última rua) if i < len(centros) - 1: fim_rua_atual = pontos_rua[-1] inicio_proxima_rua = gerar_pontos_rua(centros[i + 1], passo=passo)[0] conexao = conectar_ruas(fim_rua_atual, inicio_proxima_rua) trajetoria.extend(conexao) return trajetoria def visualizar_trajetoria_global(trajetoria): fig, ax = plt.subplots() mapa.plot(ax=ax, color="blue", label="Ruas") gpd.GeoSeries(centros).plot(ax=ax, color="red", linestyle="--", label="Centros") # Adicionar a trajetória global trajetoria_lat, trajetoria_long = zip(*trajetoria) ax.plot(trajetoria_long, trajetoria_lat, color="green", label="Trajetória Global") plt.legend() plt.title("Planejamento de Trajetória Global") plt.xlabel("Longitude") plt.ylabel("Latitude") plt.show() # Iterar sobre pares de linhas para calcular os centros linhas = list(mapa.geometry) centros = [] for i in range(len(linhas) - 1): centro = calcular_centro_entre_linhas(linhas[i], linhas[i + 1]) centros.append(centro) # Criar a figura para exibir 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") plt.legend() plt.title("Trajetória Dinâmica") plt.xlabel("Longitude") plt.ylabel("Latitude") # Função para atualizar o gráfico dinamicamente def atualizar_grafico(frame): while not fila_dados.empty(): posicao_atual, linha_trajetoria = fila_dados.get() # Atualizar a posição do robô posicao_atual_plot.set_data([posicao_atual.x], [posicao_atual.y]) # Atualizar a linha da trajetória x, y = zip(*linha_trajetoria.coords) trajetoria_plot.set_data(x, y) return posicao_atual_plot, trajetoria_plot posicao_inicial = Point(-47.372243, -22.178959) # Exemplo trajetoria_global = planejar_trajetoria_global(posicao_inicial, centros, passo=0.0001) #visualizar_trajetoria_global(trajetoria_global) # 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 from threading import Thread thread_mqtt = Thread(target=client.loop_forever) thread_mqtt.daemon = True thread_mqtt.start() # Exibir o gráfico plt.show()