mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-29 18:42:07 +08:00
firestar5683
458 lines
17 KiB
Python
458 lines
17 KiB
Python
from __future__ import annotations
|
|
|
|
import math
|
|
from dataclasses import dataclass
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
import requests
|
|
|
|
from cereal import log
|
|
from openpilot.common.constants import CV
|
|
|
|
DIRECTIONS = ("left", "right", "straight")
|
|
MODIFIABLE_DIRECTIONS = ("left", "right")
|
|
|
|
EARTH_MEAN_RADIUS = 6371007.2
|
|
SPEED_CONVERSIONS = {
|
|
"km/h": CV.KPH_TO_MS,
|
|
"mph": CV.MPH_TO_MS,
|
|
}
|
|
|
|
OFF_ROUTE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0, 20.0, 40.0]
|
|
OFF_ROUTE_DISTANCE_BREAKPOINTS = [40.0, 50.0, 60.0, 80.0, 100.0]
|
|
UPCOMING_TURN_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0, 15.0, 20.0, 25.0, 30.0, 35.0, 40.0]
|
|
UPCOMING_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0, 45.0, 60.0, 75.0, 90.0, 105.0, 120.0]
|
|
UTURN_MODIFIER = "uturn"
|
|
|
|
LANE_DIRECTIONS = frozenset({
|
|
"none",
|
|
"left",
|
|
"right",
|
|
"straight",
|
|
"slightLeft",
|
|
"slightRight",
|
|
})
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Coordinate:
|
|
latitude: float
|
|
longitude: float
|
|
|
|
def __sub__(self, other: "Coordinate") -> "Coordinate":
|
|
return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude)
|
|
|
|
def __add__(self, other: "Coordinate") -> "Coordinate":
|
|
return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude)
|
|
|
|
def __mul__(self, scale: float) -> "Coordinate":
|
|
return Coordinate(self.latitude * scale, self.longitude * scale)
|
|
|
|
def dot(self, other: "Coordinate") -> float:
|
|
return self.latitude * other.latitude + self.longitude * other.longitude
|
|
|
|
def distance_to(self, other: "Coordinate") -> float:
|
|
dlat = math.radians(other.latitude - self.latitude)
|
|
dlon = math.radians(other.longitude - self.longitude)
|
|
|
|
haversine_dlat = math.sin(dlat / 2.0) ** 2
|
|
haversine_dlon = math.sin(dlon / 2.0) ** 2
|
|
|
|
a = haversine_dlat + math.cos(math.radians(self.latitude)) * math.cos(math.radians(other.latitude)) * haversine_dlon
|
|
return 2.0 * math.asin(math.sqrt(a)) * EARTH_MEAN_RADIUS
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class RouteStep:
|
|
banner_instructions: list[dict[str, Any]]
|
|
distance: float
|
|
duration: float
|
|
maneuver: str
|
|
location: Coordinate
|
|
cumulative_distance: float
|
|
maxspeed_ms: float
|
|
modifier: str
|
|
instruction: str
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class RouteProgress:
|
|
closest_index: int
|
|
closest_segment_index: int
|
|
distance_from_route: float
|
|
current_step: RouteStep
|
|
next_step: RouteStep | None
|
|
current_step_index: int
|
|
distance_to_end_of_step: float
|
|
distance_remaining: float
|
|
time_remaining: float
|
|
current_speed_limit_ms: float
|
|
all_maneuvers: list[dict[str, Any]]
|
|
|
|
|
|
def bearing_between_two_points(point_one: Coordinate, point_two: Coordinate) -> float:
|
|
lat_one = math.radians(point_one.latitude)
|
|
lat_two = math.radians(point_two.latitude)
|
|
dlon = math.radians(point_two.longitude - point_one.longitude)
|
|
|
|
bearing_radians = math.atan2(
|
|
math.sin(dlon) * math.cos(lat_two),
|
|
math.cos(lat_one) * math.sin(lat_two) - math.sin(lat_one) * math.cos(lat_two) * math.cos(dlon),
|
|
)
|
|
return (math.degrees(bearing_radians) + 360.0) % 360.0
|
|
|
|
|
|
def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate) -> float:
|
|
if a.distance_to(b) < 0.01:
|
|
return a.distance_to(p)
|
|
|
|
ap = p - a
|
|
ab = b - a
|
|
t = np.clip(ap.dot(ab) / ab.dot(ab), 0.0, 1.0)
|
|
projection = a + ab * t
|
|
return projection.distance_to(p)
|
|
|
|
def project_onto_segment(a: Coordinate, b: Coordinate, p: Coordinate) -> tuple[float, float]:
|
|
lat_scale = EARTH_MEAN_RADIUS * math.pi / 180.0
|
|
ref_lat = math.radians((a.latitude + b.latitude + p.latitude) / 3.0)
|
|
lon_scale = lat_scale * math.cos(ref_lat)
|
|
|
|
ab_x = (b.longitude - a.longitude) * lon_scale
|
|
ab_y = (b.latitude - a.latitude) * lat_scale
|
|
ap_x = (p.longitude - a.longitude) * lon_scale
|
|
ap_y = (p.latitude - a.latitude) * lat_scale
|
|
|
|
segment_length_sq = ab_x * ab_x + ab_y * ab_y
|
|
if segment_length_sq <= 1e-6:
|
|
return 0.0, math.hypot(ap_x, ap_y)
|
|
|
|
t = float(np.clip((ap_x * ab_x + ap_y * ab_y) / segment_length_sq, 0.0, 1.0))
|
|
proj_x = ab_x * t
|
|
proj_y = ab_y * t
|
|
return t, math.hypot(ap_x - proj_x, ap_y - proj_y)
|
|
|
|
|
|
def string_to_direction(direction: str) -> str:
|
|
normalized = direction or ""
|
|
for direction_name in DIRECTIONS:
|
|
if direction_name not in normalized:
|
|
continue
|
|
if "slight" in normalized and direction_name in MODIFIABLE_DIRECTIONS:
|
|
return f"slight{direction_name.capitalize()}"
|
|
if "sharp" in normalized and direction_name in MODIFIABLE_DIRECTIONS:
|
|
return f"sharp{direction_name.capitalize()}"
|
|
return direction_name
|
|
|
|
if "uturn" in normalized or "u-turn" in normalized:
|
|
return UTURN_MODIFIER
|
|
|
|
return "none"
|
|
|
|
|
|
def normalize_lane_direction(direction: str) -> str:
|
|
normalized = string_to_direction(direction)
|
|
return normalized if normalized in LANE_DIRECTIONS else "none"
|
|
|
|
|
|
def maxspeed_to_ms(maxspeed: dict[str, str | float]) -> float:
|
|
unit = str(maxspeed["unit"])
|
|
speed = float(maxspeed["speed"])
|
|
return float(SPEED_CONVERSIONS[unit] * speed)
|
|
|
|
|
|
def field_valid(dat: dict[str, Any], field: str) -> bool:
|
|
return field in dat and dat[field] is not None
|
|
|
|
|
|
def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> dict[str, Any] | None:
|
|
if not banners:
|
|
return None
|
|
|
|
instruction: dict[str, Any] = {}
|
|
current_banner = banners[0]
|
|
for banner in banners:
|
|
if distance_to_maneuver < banner["distanceAlongGeometry"]:
|
|
current_banner = banner
|
|
|
|
instruction["showFull"] = distance_to_maneuver < current_banner["distanceAlongGeometry"]
|
|
|
|
primary = current_banner["primary"]
|
|
if field_valid(primary, "text"):
|
|
instruction["maneuverPrimaryText"] = primary["text"]
|
|
if field_valid(primary, "type"):
|
|
instruction["maneuverType"] = primary["type"]
|
|
if field_valid(primary, "modifier"):
|
|
instruction["maneuverModifier"] = string_to_direction(primary["modifier"])
|
|
|
|
if field_valid(current_banner, "secondary"):
|
|
instruction["maneuverSecondaryText"] = current_banner["secondary"]["text"]
|
|
|
|
if field_valid(current_banner, "sub"):
|
|
lanes = []
|
|
for component in current_banner["sub"]["components"]:
|
|
if component["type"] != "lane":
|
|
continue
|
|
|
|
lane = {
|
|
"active": component["active"],
|
|
"directions": [normalize_lane_direction(direction) for direction in component["directions"]],
|
|
}
|
|
if field_valid(component, "active_direction"):
|
|
lane["activeDirection"] = normalize_lane_direction(component["active_direction"])
|
|
lanes.append(lane)
|
|
instruction["lanes"] = lanes
|
|
|
|
return instruction
|
|
|
|
|
|
@dataclass
|
|
class NavigationRoute:
|
|
geometry: list[Coordinate]
|
|
geometry_cumulative_distances: list[float]
|
|
bearings: list[float]
|
|
steps: list[RouteStep]
|
|
total_distance: float
|
|
total_duration: float
|
|
|
|
@classmethod
|
|
def from_mapbox_route(cls, route_data: dict[str, Any]) -> "NavigationRoute" | None:
|
|
geometry_data = route_data.get("geometry") or []
|
|
steps_data = route_data.get("steps") or []
|
|
if not geometry_data or not steps_data:
|
|
return None
|
|
|
|
geometry = [Coordinate(float(coord["latitude"]), float(coord["longitude"])) for coord in geometry_data]
|
|
cumulative_distances = [0.0]
|
|
for index in range(1, len(geometry)):
|
|
cumulative_distances.append(cumulative_distances[-1] + geometry[index - 1].distance_to(geometry[index]))
|
|
|
|
maxspeeds = [maxspeed_to_ms(item) for item in route_data.get("maxspeed", []) if field_valid(item, "speed") and field_valid(item, "unit")]
|
|
|
|
steps: list[RouteStep] = []
|
|
for step in steps_data:
|
|
location = Coordinate(float(step["location"]["latitude"]), float(step["location"]["longitude"]))
|
|
closest_index = min(range(len(geometry)), key=lambda idx: location.distance_to(geometry[idx]))
|
|
maxspeed_ms = maxspeeds[min(closest_index, len(maxspeeds) - 1)] if maxspeeds else 0.0
|
|
steps.append(RouteStep(
|
|
banner_instructions=step.get("bannerInstructions", []),
|
|
distance=float(step["distance"]),
|
|
duration=float(step["duration"]),
|
|
maneuver=str(step["maneuver"]),
|
|
location=location,
|
|
cumulative_distance=cumulative_distances[closest_index],
|
|
maxspeed_ms=maxspeed_ms,
|
|
modifier=string_to_direction(str(step.get("modifier", "none"))),
|
|
instruction=str(step.get("instruction", "")),
|
|
))
|
|
|
|
bearings = [bearing_between_two_points(geometry[index], geometry[index + 1]) for index in range(len(geometry) - 1)]
|
|
return cls(
|
|
geometry=geometry,
|
|
geometry_cumulative_distances=cumulative_distances,
|
|
bearings=bearings,
|
|
steps=steps,
|
|
total_distance=float(route_data.get("totalDistance", 0.0)),
|
|
total_duration=float(route_data.get("totalDuration", 0.0)),
|
|
)
|
|
|
|
def route_bearing_misaligned(self, closest_segment_index: int, current_bearing: float | None, v_ego: float) -> bool:
|
|
if current_bearing is None or v_ego < 2.5 or closest_segment_index < 0 or closest_segment_index >= len(self.bearings):
|
|
return False
|
|
|
|
route_bearing = self.bearings[closest_segment_index]
|
|
normalized_bearing = (current_bearing + 360.0) % 360.0
|
|
bearing_difference = abs(normalized_bearing - route_bearing)
|
|
return min(bearing_difference, 360.0 - bearing_difference) > 75.0
|
|
|
|
def get_progress(self, position: Coordinate) -> RouteProgress | None:
|
|
if not self.geometry or not self.steps:
|
|
return None
|
|
if len(self.geometry) == 1:
|
|
closest_index = 0
|
|
closest_segment_index = 0
|
|
min_distance = position.distance_to(self.geometry[0])
|
|
closest_cumulative = 0.0
|
|
else:
|
|
best_segment_index = 0
|
|
best_distance = float("inf")
|
|
best_t = 0.0
|
|
|
|
for index in range(len(self.geometry) - 1):
|
|
t, segment_distance = project_onto_segment(self.geometry[index], self.geometry[index + 1], position)
|
|
if segment_distance < best_distance:
|
|
best_distance = segment_distance
|
|
best_segment_index = index
|
|
best_t = t
|
|
|
|
closest_segment_index = best_segment_index
|
|
segment_start = self.geometry_cumulative_distances[closest_segment_index]
|
|
segment_end = self.geometry_cumulative_distances[closest_segment_index + 1]
|
|
closest_cumulative = segment_start + (segment_end - segment_start) * best_t
|
|
min_distance = best_distance
|
|
closest_index = min(closest_segment_index + (1 if best_t >= 0.5 else 0), len(self.geometry) - 1)
|
|
|
|
current_step_index = max(
|
|
(idx for idx, step in enumerate(self.steps) if step.cumulative_distance <= (closest_cumulative + 1e-3)),
|
|
default=-1,
|
|
)
|
|
current_step = self.steps[current_step_index if current_step_index >= 0 else 0]
|
|
next_step_index = current_step_index + 1
|
|
next_step = self.steps[next_step_index] if 0 <= next_step_index < len(self.steps) else None
|
|
|
|
distance_to_end_of_step = max(0.0, current_step.distance - (closest_cumulative - current_step.cumulative_distance))
|
|
distance_remaining = max(0.0, self.total_distance - closest_cumulative)
|
|
|
|
current_step_distance = max(current_step.distance, 1.0)
|
|
remaining_current_duration = current_step.duration * min(distance_to_end_of_step / current_step_distance, 1.0)
|
|
later_duration = sum(step.duration for step in self.steps[next_step_index:])
|
|
time_remaining = max(0.0, remaining_current_duration + later_duration)
|
|
|
|
all_maneuvers: list[dict[str, Any]] = []
|
|
start_index = max(current_step_index, 0)
|
|
end_index = min(start_index + 3, len(self.steps))
|
|
for index in range(start_index, end_index):
|
|
step = self.steps[index]
|
|
maneuver_distance = distance_to_end_of_step if index == start_index else max(0.0, step.cumulative_distance - closest_cumulative)
|
|
all_maneuvers.append({
|
|
"distance": maneuver_distance,
|
|
"type": step.maneuver,
|
|
"modifier": step.modifier,
|
|
})
|
|
|
|
return RouteProgress(
|
|
closest_index=closest_index,
|
|
closest_segment_index=closest_segment_index,
|
|
distance_from_route=min_distance,
|
|
current_step=current_step,
|
|
next_step=next_step,
|
|
current_step_index=max(current_step_index, 0),
|
|
distance_to_end_of_step=distance_to_end_of_step,
|
|
distance_remaining=distance_remaining,
|
|
time_remaining=time_remaining,
|
|
current_speed_limit_ms=current_step.maxspeed_ms,
|
|
all_maneuvers=all_maneuvers,
|
|
)
|
|
|
|
def upcoming_turn_modifier(self, progress: RouteProgress, position: Coordinate, v_ego: float) -> str:
|
|
if progress.next_step is None:
|
|
return "none"
|
|
|
|
distance_threshold = float(np.interp(v_ego, UPCOMING_TURN_SPEED_BREAKPOINTS, UPCOMING_TURN_DISTANCE_BREAKPOINTS))
|
|
if position.distance_to(progress.next_step.location) <= distance_threshold:
|
|
return progress.next_step.modifier
|
|
return "none"
|
|
|
|
def off_route_distance_exceeded(self, progress: RouteProgress, v_ego: float) -> bool:
|
|
distance_threshold = float(np.interp(v_ego, OFF_ROUTE_SPEED_BREAKPOINTS, OFF_ROUTE_DISTANCE_BREAKPOINTS))
|
|
return progress.distance_from_route > distance_threshold
|
|
|
|
def arrived(self, progress: RouteProgress, v_ego: float) -> bool:
|
|
if v_ego >= 2.0 or not progress.all_maneuvers:
|
|
return False
|
|
current = progress.all_maneuvers[0]
|
|
destination_step = current["type"] == "arrive" or progress.current_step.maneuver == "arrive" or progress.current_step.instruction.startswith("Your destination")
|
|
if not destination_step and progress.next_step is not None:
|
|
destination_step = progress.next_step.maneuver == "arrive" and progress.distance_to_end_of_step <= max(15.0, v_ego * 8.0)
|
|
return destination_step and progress.distance_remaining <= 40.0
|
|
|
|
def build_instruction_payload(self, progress: RouteProgress, *, use_vienna_sign: bool = False) -> dict[str, Any]:
|
|
parsed = parse_banner_instructions(progress.current_step.banner_instructions, progress.distance_to_end_of_step) or {}
|
|
primary_text = parsed.get("maneuverPrimaryText") or progress.current_step.instruction
|
|
secondary_text = parsed.get("maneuverSecondaryText") or ""
|
|
maneuver_type = parsed.get("maneuverType") or progress.current_step.maneuver
|
|
maneuver_modifier = parsed.get("maneuverModifier") or progress.current_step.modifier
|
|
lanes = []
|
|
for lane in parsed.get("lanes", []):
|
|
directions = [direction for direction in lane.get("directions", []) if direction in LANE_DIRECTIONS]
|
|
active_direction = lane.get("activeDirection", "none")
|
|
lanes.append({
|
|
"directions": directions or ["none"],
|
|
"active": bool(lane.get("active", False)),
|
|
"activeDirection": active_direction if active_direction in LANE_DIRECTIONS else "none",
|
|
})
|
|
|
|
return {
|
|
"maneuverPrimaryText": primary_text,
|
|
"maneuverSecondaryText": secondary_text,
|
|
"maneuverDistance": progress.distance_to_end_of_step,
|
|
"maneuverType": maneuver_type,
|
|
"maneuverModifier": maneuver_modifier,
|
|
"distanceRemaining": progress.distance_remaining,
|
|
"timeRemaining": progress.time_remaining,
|
|
"timeRemainingTypical": progress.time_remaining,
|
|
"lanes": lanes,
|
|
"showFull": bool(parsed.get("showFull", True)),
|
|
"speedLimit": progress.current_speed_limit_ms,
|
|
"speedLimitSign": log.NavInstruction.SpeedLimitSign.vienna if use_vienna_sign else log.NavInstruction.SpeedLimitSign.mutcd,
|
|
"allManeuvers": progress.all_maneuvers,
|
|
}
|
|
|
|
|
|
class MapboxRouteEngine:
|
|
DIRECTIONS_URL = "https://api.mapbox.com/directions/v5/mapbox/driving"
|
|
|
|
def __init__(self, session: Any = requests):
|
|
self._session = session
|
|
|
|
def fetch_route(self, token: str, start: Coordinate, destination: dict[str, Any], bearing: float | None = None) -> NavigationRoute | None:
|
|
if not token:
|
|
return None
|
|
|
|
end = Coordinate(float(destination["latitude"]), float(destination["longitude"]))
|
|
params: dict[str, str] = {
|
|
"access_token": token,
|
|
"geometries": "geojson",
|
|
"steps": "true",
|
|
"overview": "full",
|
|
"annotations": "maxspeed",
|
|
"alternatives": "false",
|
|
"banner_instructions": "true",
|
|
}
|
|
if bearing is not None:
|
|
params["bearings"] = f"{int((bearing + 360.0) % 360.0)},90;"
|
|
|
|
url = f"{self.DIRECTIONS_URL}/{start.longitude},{start.latitude};{end.longitude},{end.latitude}"
|
|
try:
|
|
response = self._session.get(url, params=params, timeout=5)
|
|
data = response.json() if response.status_code == 200 else {}
|
|
except requests.RequestException:
|
|
return None
|
|
|
|
routes = data.get("routes") or []
|
|
route = routes[0] if routes else None
|
|
legs = route.get("legs") if route else None
|
|
leg = legs[0] if legs else None
|
|
if data.get("code") != "Ok" or route is None or leg is None:
|
|
return None
|
|
|
|
route_data = {
|
|
"steps": [
|
|
{
|
|
"maneuver": step["maneuver"]["type"],
|
|
"instruction": step["maneuver"]["instruction"],
|
|
"distance": step["distance"],
|
|
"duration": step["duration"],
|
|
"location": {
|
|
"longitude": step["maneuver"]["location"][0],
|
|
"latitude": step["maneuver"]["location"][1],
|
|
},
|
|
"modifier": step["maneuver"].get("modifier", "none"),
|
|
"bannerInstructions": step.get("bannerInstructions", []),
|
|
}
|
|
for step in leg.get("steps", [])
|
|
],
|
|
"totalDistance": route["distance"],
|
|
"totalDuration": route["duration"],
|
|
"geometry": [
|
|
{"longitude": coord[0], "latitude": coord[1]}
|
|
for coord in route["geometry"]["coordinates"]
|
|
],
|
|
"maxspeed": [
|
|
{"speed": item["speed"], "unit": item["unit"]}
|
|
for item in leg.get("annotation", {}).get("maxspeed", [])
|
|
if field_valid(item, "speed") and field_valid(item, "unit")
|
|
],
|
|
}
|
|
return NavigationRoute.from_mapbox_route(route_data)
|