agrobot_base/Python/trajetoria-dinamica/test2.py

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