153 lines
6.0 KiB
Python
153 lines
6.0 KiB
Python
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()
|