import json import matplotlib.pyplot as plt from shapely.geometry import shape, Point, LineString import numpy as np import paho.mqtt.client as mqtt import queue update_queue = queue.Queue() # Substitua este caminho pelo local onde o seu arquivo GeoJSON está armazenado geojson_path = "mapa.json" # Carregar o arquivo GeoJSON with open(geojson_path, 'r') as file: geojson_data = json.load(file) # Extrair as ruas como objetos LineString ruas = [shape(feature['geometry']) for feature in geojson_data['features']] # Posição atual do robô (latitude, longitude) posicao_atual = Point(-47.372138166667, -22.1789853333) # 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 # Função para deslocar o ponto para o espaço central entre duas ruas def deslocar_para_centro(rua1, rua2, ponto): ponto_rua1 = rua1.interpolate(rua1.project(ponto)) ponto_rua2 = rua2.interpolate(rua2.project(ponto)) x_central = (ponto_rua1.x + ponto_rua2.x) / 2 y_central = (ponto_rua1.y + ponto_rua2.y) / 2 return Point(x_central, y_central) # Função para prolongar uma rua adicionando pontos antes e depois def prolongar_rua(rua, distancia_graus): # Coordenadas da rua coords = list(rua.coords) # Calcular vetor direção no início da rua inicio = Point(coords[0]) segundo = Point(coords[1]) vetor_inicio = np.array([inicio.x - segundo.x, inicio.y - segundo.y]) vetor_inicio = vetor_inicio / np.linalg.norm(vetor_inicio) # Calcular vetor direção no final da rua fim = Point(coords[-1]) penultimo = Point(coords[-2]) vetor_fim = np.array([fim.x - penultimo.x, fim.y - penultimo.y]) vetor_fim = vetor_fim / np.linalg.norm(vetor_fim) # Criar novos pontos prolongados novo_inicio = Point(inicio.x + vetor_inicio[0] * distancia_graus, inicio.y + vetor_inicio[1] * distancia_graus) novo_fim = Point(fim.x + vetor_fim[0] * distancia_graus, fim.y + vetor_fim[1] * distancia_graus) # Retornar nova linha com os pontos prolongados nova_coords = [novo_inicio] + coords + [novo_fim] return LineString(nova_coords) # Função principal para calcular a trajetória dinâmica def calcular_trajetoria_dinamica(ruas, posicao_atual, distancia_prolongamento_metros): distancia_prolongamento_graus = metros_para_graus(distancia_prolongamento_metros) trajetoria = [] # Função para encontrar o ponto mais próximo de uma posição em uma linha def ponto_mais_proximo(rua, posicao): menor_distancia = float("inf") ponto_mais_proximo = None for ponto in rua.coords: p = Point(ponto) distancia = posicao.distance(p) if distancia < menor_distancia: menor_distancia = distancia ponto_mais_proximo = p return ponto_mais_proximo # Iniciar no espaço central entre a posição atual e a primeira rua if len(ruas) > 1: ponto_inicial = deslocar_para_centro(ruas[0], ruas[1], posicao_atual) trajetoria.append(posicao_atual) trajetoria.append(ponto_inicial) # Percorrer todas as ruas for idx in range(len(ruas) - 1): rua_atual = ruas[idx] proxima_rua = ruas[idx + 1] # Prolongar as ruas antes de calcular a trajetória rua_atual_prolongada = prolongar_rua(rua_atual, distancia_prolongamento_graus) proxima_rua_prolongada = prolongar_rua(proxima_rua, distancia_prolongamento_graus) # Usar o último ponto da trajetória dinâmica como referência ultimo_ponto_trajetoria = trajetoria[-1] # Encontrar o ponto mais próximo da posição atual no início da rua ponto_inicio_atual = ponto_mais_proximo(rua_atual_prolongada, ultimo_ponto_trajetoria) # Filtrar os pontos a partir do ponto mais próximo indice_inicio = list(rua_atual_prolongada.coords).index((ponto_inicio_atual.x, ponto_inicio_atual.y)) pontos_filtrados = list(rua_atual_prolongada.coords)[indice_inicio + 1:] if (idx == 0): # Adicionar os pontos deslocados para o centro na trajetória for ponto in pontos_filtrados: ponto_atual = Point(ponto) deslocado = deslocar_para_centro(rua_atual_prolongada, proxima_rua_prolongada, ponto_atual) trajetoria.append(deslocado) # Cálculo dos extremos da próxima rua ponto_inicio_proxima = Point(proxima_rua.coords[0]) # Primeiro ponto da próxima rua ponto_fim_proxima = Point(proxima_rua.coords[-1]) # Último ponto da próxima rua # Calcular as distâncias do último ponto da trajetória aos extremos da próxima rua dist_inicio = ultimo_ponto_trajetoria.distance(ponto_inicio_proxima) dist_final = ultimo_ponto_trajetoria.distance(ponto_fim_proxima) # Verificar qual ponto extremo da próxima rua está mais próximo if dist_inicio < dist_final: pontos_proxima_rua = list(proxima_rua_prolongada.coords) else: pontos_proxima_rua = list(proxima_rua_prolongada.coords[::-1]) if idx > 0: # Adicionar os pontos da próxima rua na trajetória dinâmica for ponto in pontos_proxima_rua: ponto_atual = Point(ponto) deslocado = deslocar_para_centro(rua_atual_prolongada, proxima_rua_prolongada, ponto_atual) trajetoria.append(deslocado) return trajetoria # Função para processar mensagens MQTT def on_message(client, userdata, msg): global posicao_atual 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: posicao_atual = Point(payload["longitude"], payload["latitude"]) print(f"Posição atual atualizada: {posicao_atual}") trajetoria = calcular_trajetoria_dinamica(ruas, posicao_atual, 1.5) # 1.5 metros print("Trajetória recalculada.") # Colocar dados na fila para a thread principal processar update_queue.put((posicao_atual, trajetoria)) 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')}") def atualizar_grafico(trajetoria): global posicao_atual, ruas # Limpar o gráfico atual plt.clf() # Plotar as ruas originais for idx, rua in enumerate(ruas): x, y = zip(*rua.coords) plt.plot(x, y, label=f"Rua {idx + 1}", linewidth=2) # Plotar a trajetória dinâmica recalculada if trajetoria: x_traj, y_traj = zip(*[(p.x, p.y) for p in trajetoria]) plt.plot(x_traj, y_traj, 'y-', label="Trajetória Dinâmica Prolongada", linewidth=2) # Plotar a posição atual do robô plt.plot(posicao_atual.x, posicao_atual.y, 'ro', label="Posição Atual do Robô") # Configurações de exibição plt.title("Mapa com Trajetória Dinâmica Prolongada", fontsize=14) plt.xlabel("Longitude") plt.ylabel("Latitude") plt.legend() plt.grid(True) plt.tight_layout() # Atualizar o gráfico plt.pause(0.1) # Pequena pausa para atualizar a interface # Configuração do MQTT client = mqtt.Client() client.on_message = on_message client.connect("localhost", 1883, 60) client.subscribe("coordenadas_gps") # Loop do MQTT client.loop_start() # Criar a plotagem inicial fig, ax = plt.subplots(figsize=(10, 8)) atualizar_grafico([]) # Mantenha o script ativo while True: try: # Processar dados da fila, se houver if not update_queue.empty(): posicao_atual, nova_trajetoria = update_queue.get() # Atualizar o gráfico plt.clf() # Plotar as ruas originais for idx, rua in enumerate(ruas): x, y = zip(*rua.coords) plt.plot(x, y, label=f"Rua {idx + 1}", linewidth=2) # Plotar a nova trajetória dinâmica if nova_trajetoria: x_traj, y_traj = zip(*[(p.x, p.y) for p in nova_trajetoria]) plt.plot(x_traj, y_traj, 'y-', label="Trajetória Dinâmica Prolongada", linewidth=2) # Plotar a posição atual do robô plt.plot(posicao_atual.x, posicao_atual.y, 'ro', label="Posição Atual do Robô") # Configurações de exibição plt.title("Mapa com Trajetória Dinâmica Prolongada", fontsize=14) plt.xlabel("Longitude") plt.ylabel("Latitude") plt.legend() plt.grid(True) plt.tight_layout() plt.pause(0.1) # Pequena pausa para atualizar o gráfico except KeyboardInterrupt: print("Encerrando...") break