235 lines
8.9 KiB
Python
235 lines
8.9 KiB
Python
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
|