mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-07-29 13:52:11 +08:00
FrogAi
81 lines
2.7 KiB
Python
81 lines
2.7 KiB
Python
#!/usr/bin/env python3
|
|
# PFEIFER - MTSC - Modified by FrogAi for FrogPilot
|
|
import json
|
|
import math
|
|
|
|
from openpilot.frogpilot.common.frogpilot_utilities import calculate_distance_to_point
|
|
from openpilot.frogpilot.common.frogpilot_variables import PLANNER_TIME, TO_RADIANS, params_memory
|
|
|
|
def calculate_curvature(p1, p2, p3):
|
|
lat1, lon1 = p1
|
|
lat2, lon2 = p2
|
|
lat3, lon3 = p3
|
|
|
|
lat1_rad, lon1_rad = lat1 * TO_RADIANS, lon1 * TO_RADIANS
|
|
lat2_rad, lon2_rad = lat2 * TO_RADIANS, lon2 * TO_RADIANS
|
|
lat3_rad, lon3_rad = lat3 * TO_RADIANS, lon3 * TO_RADIANS
|
|
|
|
side_a = calculate_distance_to_point(lat2_rad, lon2_rad, lat3_rad, lon3_rad)
|
|
side_b = calculate_distance_to_point(lat1_rad, lon1_rad, lat3_rad, lon3_rad)
|
|
side_c = calculate_distance_to_point(lat1_rad, lon1_rad, lat2_rad, lon2_rad)
|
|
|
|
s = (side_a + side_b + side_c) / 2
|
|
|
|
area_squared = s * (s - side_a) * (s - side_b) * (s - side_c)
|
|
if area_squared <= 0:
|
|
return 0
|
|
|
|
area = math.sqrt(area_squared)
|
|
|
|
radius = (side_a * side_b * side_c) / (4 * area)
|
|
if radius == 0:
|
|
return 0
|
|
|
|
curvature = 1 / radius
|
|
return curvature
|
|
|
|
class MapTurnSpeedController:
|
|
def get_map_curvature(self, gps_position, v_ego):
|
|
if not gps_position:
|
|
return 1e-6
|
|
current_latitude = gps_position["latitude"]
|
|
current_longitude = gps_position["longitude"]
|
|
|
|
distances = []
|
|
minimum_idx = 0
|
|
minimum_distance = 1000.0
|
|
|
|
target_velocities = json.loads(params_memory.get("MapTargetVelocities") or "[]")
|
|
for i, target_velocity in enumerate(target_velocities):
|
|
target_latitude = target_velocity["latitude"]
|
|
target_longitude = target_velocity["longitude"]
|
|
|
|
distance = calculate_distance_to_point(current_latitude * TO_RADIANS, current_longitude * TO_RADIANS, target_latitude * TO_RADIANS, target_longitude * TO_RADIANS)
|
|
distances.append(distance)
|
|
|
|
if distance < minimum_distance:
|
|
minimum_distance = distance
|
|
minimum_idx = i
|
|
|
|
forward_distances = distances[minimum_idx:]
|
|
|
|
cumulative_distance = 0.0
|
|
target_idx = None
|
|
|
|
for i, distance in enumerate(forward_distances):
|
|
cumulative_distance += distance
|
|
if cumulative_distance >= PLANNER_TIME * v_ego:
|
|
target_idx = i
|
|
break
|
|
|
|
forward_points = target_velocities[minimum_idx:]
|
|
|
|
if target_idx is None or target_idx == 0 or target_idx >= len(forward_points) - 1:
|
|
return 1e-6
|
|
|
|
p1 = (forward_points[target_idx - 1]["latitude"], forward_points[target_idx - 1]["longitude"])
|
|
p2 = (forward_points[target_idx]["latitude"], forward_points[target_idx]["longitude"])
|
|
p3 = (forward_points[target_idx + 1]["latitude"], forward_points[target_idx + 1]["longitude"])
|
|
|
|
return max(calculate_curvature(p1, p2, p3), 1e-6)
|