import json import matplotlib.pyplot as plt from shapely.geometry import shape, Point, LineString import numpy as np # 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) # 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): # 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, inicio.y + vetor_inicio[1] * distancia) novo_fim = Point(fim.x + vetor_fim[0] * distancia, fim.y + vetor_fim[1] * distancia) # 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): 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) proxima_rua_prolongada = prolongar_rua(proxima_rua, distancia_prolongamento) # 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 # Parâmetros distancia_prolongamento = 0.00003 # Distância para prolongar as ruas em graus # Calcular a trajetória dinâmica trajetoria = calcular_trajetoria_dinamica(ruas, posicao_atual, distancia_prolongamento) # Criar a plotagem fig, ax = plt.subplots(figsize=(10, 8)) # Plotar as ruas originais for idx, rua in enumerate(ruas): x, y = zip(*rua.coords) ax.plot(x, y, label=f"Rua {idx + 1}", linewidth=2) # Plotar a trajetória dinâmica x_traj, y_traj = zip(*[(p.x, p.y) for p in trajetoria]) ax.plot(x_traj, y_traj, 'y-', label="Trajetória Dinâmica Prolongada", linewidth=2) # Plotar a posição inicial do robô ax.plot(posicao_atual.x, posicao_atual.y, 'ro', label="Posição Atual do Robô") # Configurações de exibição ax.set_title("Mapa com Trajetória Dinâmica Prolongada", fontsize=14) ax.set_xlabel("Longitude") ax.set_ylabel("Latitude") ax.legend() plt.grid(True) plt.tight_layout() plt.show()