221 lines
7.9 KiB
Python
221 lines
7.9 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
|
|
|
|
# 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()
|
|
|
|
|
|
|