Compare commits
7 Commits
pre-build
...
0.10.3-car
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
401a9ea75a | ||
|
|
857d58fcf8 | ||
|
|
dab1c5b7e0 | ||
|
|
e975fdcd6c | ||
|
|
29beffdd30 | ||
|
|
ef7cd06332 | ||
|
|
7950dee9a1 |
91
BRANCHES.md
@@ -1,91 +0,0 @@
|
||||
```mermaid
|
||||
flowchart TD
|
||||
B000["devel-staging"] ---> CORE["core"]
|
||||
CORE ---> CORE_001["core-feat/params"]
|
||||
CORE_001 ---> CORE_002["core-feat/panel"]
|
||||
CORE_002 ---> CORE_003["core-feat/safety-ext"]
|
||||
CORE_003 ---> MIN["min"]
|
||||
MIN ---> MIN_001["min-feat/ui/display-mode"]
|
||||
MIN ---> MIN_002["min-feat/dev/model-selector"]
|
||||
MIN ---> MIN_003["min-feat/lat/lca"]
|
||||
MIN ---> MIN_004["min-feat/dev/on-off-road"]
|
||||
MIN ---> MIN_005["min-feat/ui/hide-hud"]
|
||||
MIN ---> MIN_006["min-feat/lon/ext-radar"]
|
||||
MIN ---> MIN_007["min-feat/lat/road-edge-detection"]
|
||||
MIN ---> MIN_008["min-feat/ui/rainbow-path"]
|
||||
MIN ---> MIN_009["min-feat/lon/acm"]
|
||||
MIN ---> MIN_010["min-feat/lon/aem"]
|
||||
MIN ---> MIN_011["min-feat/lon/dtsc"]
|
||||
MIN ---> MIN_012["min-feat/lon/apm"]
|
||||
MIN ---> MIN_013["min-feat/lon/dasr"]
|
||||
MIN ---> MIN_014["min-feat/dev/alert-mode"]
|
||||
MIN ---> MIN_015["min-feat/dev/auto-shutdown"]
|
||||
MIN ---> MIN_016["min-feat/ui/lead-stats"]
|
||||
MIN ---> MIN_017["min-feat/ui/border-indicator"]
|
||||
MIN ---> MIN_018["min-feat/dev/delay-loggerd"]
|
||||
MIN ---> MIN_019["min-feat/dev/disable-connect"]
|
||||
MIN ---> MIN_020["min-feat/dev/tether-on-boot"]
|
||||
MIN ---> MIN_021["min-feat/ui/torque-bar"]
|
||||
MIN ---> MIN_022["min-feat/ui/mici-ui-mode"]
|
||||
MIN ---> MIN_023["min-feat/lat/lat-offset"]
|
||||
MIN_001 ---> FULL["full"]
|
||||
MIN_002 ---> FULL
|
||||
MIN_003 ---> FULL
|
||||
MIN_004 ---> FULL
|
||||
MIN_005 ---> FULL
|
||||
MIN_006 ---> FULL
|
||||
MIN_007 ---> FULL
|
||||
MIN_008 ---> FULL
|
||||
MIN_009 ---> FULL
|
||||
MIN_010 ---> FULL
|
||||
MIN_011 ---> FULL
|
||||
MIN_012 ---> FULL
|
||||
MIN_013 ---> FULL
|
||||
MIN_014 ---> FULL
|
||||
MIN_015 ---> FULL
|
||||
MIN_016 ---> FULL
|
||||
MIN_017 ---> FULL
|
||||
MIN_018 ---> FULL
|
||||
MIN_019 ---> FULL
|
||||
MIN_020 ---> FULL
|
||||
MIN_021 ---> FULL
|
||||
FULL ---> TOYOTA_001[brand/toyota/safety-common]
|
||||
FULL ---> TOYOTA_002[brand/toyota/door-auto-lock-unlock]
|
||||
FULL ---> TOYOTA_003[brand/toyota/tss1-sng]
|
||||
FULL ---> TOYOTA_004[brand/toyota/radar-filter]
|
||||
FULL ---> TOYOTA_005[brand/toyota/sdsu]
|
||||
FULL ---> TOYOTA_006[brand/toyota/dsu-bypass]
|
||||
FULL ---> TOYOTA_007[brand/toyota/zss]
|
||||
FULL ---> TOYOTA_008[brand/toyota/stock-lon]
|
||||
FULL ---> VAG_001[brand/vag/a0-sng]
|
||||
FULL ---> VAG_002[brand/vag/pq-steering-patch]
|
||||
FULL ---> VAG_003[brand/vag/pq-no-dashcam]
|
||||
FULL ---> VAG_004[brand/vag/avoid-eps-lockout]
|
||||
FULL ---> HKG_001[brand/hkg/smdps]
|
||||
FULL ---> HONDA_001[brand/honda/eps-mod]
|
||||
FULL ---> HONDA_002[brand/honda/nidec-stock-long]
|
||||
FULL ---> SUBARU_001[brand/subaru/torque-3071]
|
||||
TOYOTA_001 ---> TOYOTA[pre-toyota]
|
||||
TOYOTA_002 ---> TOYOTA
|
||||
TOYOTA_003 ---> TOYOTA
|
||||
TOYOTA_004 ---> TOYOTA
|
||||
TOYOTA_005 ---> TOYOTA
|
||||
TOYOTA_006 ---> TOYOTA
|
||||
TOYOTA_007 ---> TOYOTA
|
||||
TOYOTA_008 ---> TOYOTA
|
||||
VAG_001 ---> VAG[pre-vag]
|
||||
VAG_002 ---> VAG
|
||||
VAG_003 ---> VAG
|
||||
VAG_004 ---> VAG
|
||||
HKG_001 ---> HKG[pre-hkg]
|
||||
HONDA_001 ---> HONDA[pre-honda]
|
||||
SUBARU_001 ---> SUBARU[pre-subaru]
|
||||
TOYOTA ---> PRE[pre]
|
||||
VAG ---> PRE
|
||||
HKG ---> PRE
|
||||
HONDA ---> PRE
|
||||
SUBARU ---> PRE
|
||||
PRE ---> PRE_PATCH[pre-patch]
|
||||
PRE_PATCH ---> PREBUILD[pre-build]
|
||||
PREBUILD ---> VERSION[x.x.x]
|
||||
```
|
||||
1877
CHANGELOGS.md
26
SPONSORS.md
@@ -1,26 +0,0 @@
|
||||
# Sponsors 贊助者
|
||||
|
||||
我們誠摯感謝以下贊助者提供的硬體資源,讓專案能夠在多種平台上進行測試與驗證。
|
||||
|
||||
We sincerely thank the following sponsors for providing hardware resources, which enable the project to be tested and validated across multiple platforms.
|
||||
|
||||
---
|
||||
|
||||
## 贊助者列表 Sponsors
|
||||
|
||||
| 贊助者 Sponsor | 設備 Deices | 備註 Notes |
|
||||
| -------------- | ----------- | ---------- |
|
||||
| BlueGood | <ul><li>Radar Filter x 2</li><li>sDSU x1</li></ul> | - |
|
||||
| Chia Chun Lee | <ul><li>C3 Quick Mount x1</li></ul> | - |
|
||||
| CloudJ | <ul><li>C3X x1</li></ul> | - |
|
||||
| FareWay | <ul><li>EON Quick Mount x1</li><li>C2/C3 Quick Mount x1</li></ul> | Special thanks for fixing my good old EON |
|
||||
| Fred Wang | <ul><li>Oneplus 3t x1</li></ul> | - |
|
||||
| Saber Huang | <ul><li>O3L x1</li></ul> | - |
|
||||
| [馬威 Mr. One](https://shop61532546.taobao.com/) | <ul><li>O3 x 1</li><li>O3 (Dev) x1</li><li>O3XL x1</li><li>Red Panda x1</li><li>Panda Jungle v1 x1</li></ul> | - |
|
||||
| 門文梁 | <ul><li>C1.5 x1</li></ul> | - |
|
||||
|
||||
---
|
||||
|
||||
🙏 沒有你們的支持,我們無法讓專案在這麼多硬體平台上持續成長與驗證。
|
||||
|
||||
Without your support, this project could not continue to grow and be validated across so many hardware platforms.
|
||||
@@ -43,68 +43,26 @@ struct LiveGPS @0xda96579883444c35 {
|
||||
status @8 :Status;
|
||||
|
||||
enum Status {
|
||||
uninitialized @0; # no GPS data yet
|
||||
uncalibrated @1; # has GPS but fusion not ready (raw passthrough)
|
||||
valid @2; # fusion active with calibrated bearing
|
||||
noGps @0;
|
||||
initializing @1;
|
||||
calibrating @2;
|
||||
valid @3;
|
||||
recalibrating @4;
|
||||
gpsStale @5;
|
||||
}
|
||||
|
||||
# Metadata
|
||||
unixTimestampMillis @9 :Int64;
|
||||
lastGpsTimestamp @10 :UInt64; # logMonoTime of last GPS
|
||||
|
||||
# livePose health (for debugging fusion issues)
|
||||
livePoseOk @11 :Bool; # livePose valid and providing orientation/velocity
|
||||
}
|
||||
|
||||
struct MaaControl @0x80ae746ee2596b11 {
|
||||
# Map-Aware Assist control signals
|
||||
|
||||
# Curvature data (for lateral control)
|
||||
curvature @0 :Float32; # current nav curvature (1/m)
|
||||
curvatureValid @1 :Bool; # curvature data is valid
|
||||
|
||||
# Turn speed data (for longitudinal control)
|
||||
turnSpeedLimit @2 :Float32; # target speed for turn (m/s)
|
||||
turnDistance @3 :Float32; # distance to turn (m)
|
||||
turnDirection @4 :TurnDirection;
|
||||
turnValid @5 :Bool; # turn data is valid
|
||||
maneuverType @6 :ManeuverType; # type of maneuver (turn vs lane change)
|
||||
turnAngle @7 :Float32; # expected turn angle in degrees (positive=left, negative=right)
|
||||
turnCurvature @8 :Float32; # curvature at turn point (1/m), used for speed limit calc
|
||||
|
||||
# Turn execution (heading-based tracking)
|
||||
desireActive @9 :Bool; # true when turn desire should be sent to model
|
||||
turnState @10 :UInt8; # TurnState enum: 0=none, 1=approaching, 2=executing, 3=complete
|
||||
turnProgress @11 :Float32; # accumulated heading change during turn (degrees)
|
||||
|
||||
# Driver acknowledgment (blinker = commit to turn)
|
||||
driverAcknowledged @12 :Bool; # driver turned on blinker matching turn direction
|
||||
speedLimitActive @13 :Bool; # turn speed limit should be enforced (blinker on)
|
||||
blockLaneChange @14 :Bool; # within commit distance, block lane change desire
|
||||
|
||||
enum TurnDirection {
|
||||
none @0;
|
||||
left @1;
|
||||
right @2;
|
||||
}
|
||||
|
||||
enum ManeuverType {
|
||||
none @0;
|
||||
turn @1; # intersection turn - use turnLeft/Right desire
|
||||
laneChange @2; # highway exit/fork - use laneChangeLeft/Right desire
|
||||
}
|
||||
struct CustomReserved4 @0x80ae746ee2596b11 {
|
||||
}
|
||||
|
||||
struct DashyState @0xa5cd762cd951a455 {
|
||||
# Pre-serialized JSON bytes for dashy UI
|
||||
# Aggregates all topics needed by dashy into single message
|
||||
json @0 :Data;
|
||||
struct CustomReserved5 @0xa5cd762cd951a455 {
|
||||
}
|
||||
|
||||
struct NavInstructionExt @0xf98d843bfd7004a3 {
|
||||
# Extension fields for NavInstruction (not in upstream)
|
||||
turnAngle @0 :Float32; # degrees, positive=left, negative=right
|
||||
turnCurvature @1 :Float32; # 1/m, positive=left, negative=right
|
||||
struct CustomReserved6 @0xf98d843bfd7004a3 {
|
||||
}
|
||||
|
||||
struct CustomReserved7 @0xb86e6369214c01c8 {
|
||||
|
||||
@@ -2629,9 +2629,9 @@ struct Event {
|
||||
carStateExt @108 :Custom.CarStateExt;
|
||||
modelExt @109 :Custom.ModelExt;
|
||||
liveGPS @110 :Custom.LiveGPS;
|
||||
maaControl @111 :Custom.MaaControl;
|
||||
dashyState @112 :Custom.DashyState;
|
||||
navInstructionExt @113 :Custom.NavInstructionExt;
|
||||
customReserved4 @111 :Custom.CustomReserved4;
|
||||
customReserved5 @112 :Custom.CustomReserved5;
|
||||
customReserved6 @113 :Custom.CustomReserved6;
|
||||
customReserved7 @114 :Custom.CustomReserved7;
|
||||
customReserved8 @115 :Custom.CustomReserved8;
|
||||
customReserved9 @116 :Custom.CustomReserved9;
|
||||
|
||||
@@ -75,7 +75,7 @@ _services: dict[str, tuple] = {
|
||||
"modelV2": (True, 20., None, QueueSize.BIG),
|
||||
"managerState": (True, 2., 1),
|
||||
"uploaderState": (True, 0., 1),
|
||||
# "navInstruction": (True, 1., 10), # dp - make it 0 hz
|
||||
"navInstruction": (True, 1., 10),
|
||||
"navRoute": (True, 0.),
|
||||
"navThumbnail": (True, 0.),
|
||||
"qRoadEncodeIdx": (False, 20.),
|
||||
@@ -105,12 +105,7 @@ _services: dict[str, tuple] = {
|
||||
"controlsStateExt": (True, 100.),
|
||||
"carStateExt": (True, 100.),
|
||||
"modelExt": (True, 20.),
|
||||
# dashy
|
||||
"navInstruction": (True, 0.),
|
||||
"navInstructionExt": (True, 0.),
|
||||
"liveGPS": (True, 0.), # GPS fusion from gpsd (optional)
|
||||
"maaControl": (True, 0.), # Map-Aware Assist control signals (optional)
|
||||
"dashyState": (True, 0.), # Aggregated dashy UI state (optional)
|
||||
"liveGPS": (True, 20.),
|
||||
}
|
||||
SERVICE_LIST = {name: Service(*vals) for
|
||||
idx, (name, vals) in enumerate(_services.items())}
|
||||
|
||||
@@ -147,16 +147,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"dp_lon_acm", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_lon_aem", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_lon_dtsc", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_lon_apm", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_lon_dasr", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_dev_audible_alert_mode", {PERSISTENT, INT, "0"}},
|
||||
{"dp_dev_auto_shutdown_in", {PERSISTENT, INT, "-5"}},
|
||||
{"dp_ui_lead", {PERSISTENT, INT, "0"}},
|
||||
{"dp_dev_opview", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_dev_dashy", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_maa_route", {CLEAR_ON_MANAGER_START, JSON}},
|
||||
{"dp_maa_destination", {PERSISTENT, JSON}},
|
||||
{"dp_maa_places", {PERSISTENT, JSON}},
|
||||
{"dp_dev_delay_loggerd", {PERSISTENT, INT, "0"}},
|
||||
{"dp_dev_disable_connect", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_dev_tethering", {PERSISTENT, BOOL, "0"}},
|
||||
@@ -168,5 +162,4 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"dp_vag_a0_sng", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_vag_pq_steering_patch", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_vag_avoid_eps_lockout", {PERSISTENT, BOOL, "0"}},
|
||||
{"dp_honda_nidec_stock_long", {PERSISTENT, BOOL, "0"}},
|
||||
};
|
||||
|
||||
0
dragonpilot/dashy/.nojekyll
Normal file
15
dragonpilot/dashy/LICENSE.md
Normal file
@@ -0,0 +1,15 @@
|
||||
Copyright (c) 2025, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
@@ -1,41 +1,46 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
# Dashy Release Branch
|
||||
|
||||
This is the production-ready release branch of Dashy - Dragonpilot's All-in-one System Hub for You.
|
||||
|
||||
## 🚀 Quick Installation
|
||||
|
||||
```bash
|
||||
git clone -b release https://github.com/efinilan/dashy
|
||||
cd dashy
|
||||
python3 backend/server.py
|
||||
```
|
||||
|
||||
## 📁 What's Included
|
||||
|
||||
- `backend/` - Python server with all dependencies included
|
||||
- `web/` - Pre-built web interface (minified and optimized)
|
||||
|
||||
## 🌐 Access
|
||||
|
||||
After starting the server, open Chrome browser and navigate to:
|
||||
```
|
||||
http://<device-ip>:5088
|
||||
```
|
||||
|
||||
## 🔧 Requirements
|
||||
|
||||
- Network connection
|
||||
- Port 5088 available
|
||||
|
||||
## 📄 License
|
||||
|
||||
Copyright (c) 2025, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
"""
|
||||
|
||||
from cereal import log
|
||||
|
||||
# Hysteresis thresholds (km/h -> m/s)
|
||||
APM_ACTIVATE_SPEED = 30 * 1000 / 3600 # 30 km/h ~8.33 m/s — switch to aggressive below this
|
||||
APM_DEACTIVATE_SPEED = 40 * 1000 / 3600 # 40 km/h ~11.11 m/s — restore user personality above this
|
||||
|
||||
|
||||
class APM:
|
||||
|
||||
def __init__(self):
|
||||
self._active = False
|
||||
|
||||
def get_personality(self, v_ego, personality):
|
||||
if self._active:
|
||||
if v_ego > APM_DEACTIVATE_SPEED:
|
||||
self._active = False
|
||||
else:
|
||||
if v_ego < APM_ACTIVATE_SPEED:
|
||||
self._active = True
|
||||
|
||||
if self._active:
|
||||
return log.LongitudinalPersonality.aggressive
|
||||
return personality
|
||||
1
dragonpilot/dashy/backend/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
__pycache__
|
||||
@@ -1,7 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
Copyright (c) 2025, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
@@ -15,25 +15,10 @@ for non-commercial purposes only, subject to the following conditions:
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Dashy HTTP Server
|
||||
|
||||
Provides REST API and static file serving for the dashy web UI.
|
||||
- Settings management (read/write params)
|
||||
- Navigation API (destination, search, places, tiles)
|
||||
- File browser for drive logs
|
||||
- WebRTC stream proxy
|
||||
- Static file serving for web UI
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import asyncio
|
||||
import json
|
||||
import os
|
||||
import logging
|
||||
@@ -42,37 +27,22 @@ from datetime import datetime
|
||||
from functools import wraps
|
||||
from urllib.parse import quote
|
||||
|
||||
from aiohttp import web, ClientSession, ClientTimeout, ClientConnectorError
|
||||
|
||||
from cereal import messaging
|
||||
from aiohttp import web, ClientSession, ClientTimeout
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.hardware import PC, HARDWARE
|
||||
from openpilot.system.ui.lib.multilang import multilang as base_multilang
|
||||
from dragonpilot.settings import SETTINGS
|
||||
from dragonpilot.dashy.maa.providers import MapService
|
||||
from dragonpilot.dashy.maa.providers.models import Coordinate
|
||||
|
||||
# --- Configuration ---
|
||||
DEFAULT_DIR = os.path.realpath(os.path.join(os.path.dirname(__file__), '..') if PC else '/data/media/0/realdata')
|
||||
WEB_DIST_PATH = os.path.join(os.path.dirname(__file__), "web", "dist")
|
||||
WEB_DIST_PATH = os.path.join(os.path.dirname(__file__), "..", "web", "dist")
|
||||
WEBRTC_TIMEOUT = ClientTimeout(total=10)
|
||||
CAR_PARAMS_CACHE_TTL = 30 # seconds
|
||||
|
||||
logger = logging.getLogger("dashy")
|
||||
|
||||
|
||||
class MockParams:
|
||||
"""In-memory params mock for dev mode."""
|
||||
_store = {}
|
||||
def get(self, key, default=None): return self._store.get(key, default)
|
||||
def get_bool(self, key, default=False): return bool(self._store.get(key)) if key in self._store else default
|
||||
def put(self, key, value): self._store[key] = value
|
||||
def put_bool(self, key, value): self._store[key] = value
|
||||
def remove(self, key): self._store.pop(key, None)
|
||||
def check_key(self, key): return True
|
||||
|
||||
|
||||
# --- Caching Layer ---
|
||||
class AppCache:
|
||||
"""Centralized cache for expensive operations."""
|
||||
@@ -83,18 +53,12 @@ class AppCache:
|
||||
self._car_params_time = 0
|
||||
self._context = None
|
||||
self._context_time = 0
|
||||
self._settings_cache = None
|
||||
self._settings_cache_time = 0
|
||||
|
||||
@property
|
||||
def params(self):
|
||||
"""Get shared Params instance (or mock if unavailable)."""
|
||||
def params(self) -> Params:
|
||||
"""Get shared Params instance."""
|
||||
if self._params is None:
|
||||
try:
|
||||
self._params = Params()
|
||||
except Exception as e:
|
||||
logger.warning(f"Params unavailable, using mock: {e}")
|
||||
self._params = MockParams()
|
||||
self._params = Params()
|
||||
return self._params
|
||||
|
||||
def get_car_params(self):
|
||||
@@ -151,7 +115,6 @@ class AppCache:
|
||||
"""Invalidate all caches."""
|
||||
self._car_params = None
|
||||
self._context = None
|
||||
self._settings_cache = None
|
||||
|
||||
|
||||
# --- Helper Functions ---
|
||||
@@ -199,8 +162,14 @@ def resolve_value(value):
|
||||
async def init_api(request):
|
||||
"""Provide initial data to the client."""
|
||||
cache: AppCache = request.app['cache']
|
||||
car_params = cache.get_car_params()
|
||||
|
||||
return web.json_response({
|
||||
'is_metric': cache.get_bool_safe("IsMetric"),
|
||||
'dp_dev_dashy': cache.get_bool_safe("dp_dev_dashy", True),
|
||||
'openpilot_longitudinal_control': car_params['openpilot_longitudinal_control'],
|
||||
'ublox_available': cache.get_bool_safe("UbloxAvailable", True),
|
||||
'dp_lat_alka': cache.get_bool_safe("dp_lat_alka", False),
|
||||
})
|
||||
|
||||
|
||||
@@ -273,12 +242,6 @@ async def serve_manifest_api(request):
|
||||
async def get_settings_config_api(request):
|
||||
"""Get the settings configuration from settings.py."""
|
||||
cache: AppCache = request.app['cache']
|
||||
|
||||
# Return cached settings if fresh (2 second TTL)
|
||||
now = time.time()
|
||||
if cache._settings_cache is not None and (now - cache._settings_cache_time) < 2:
|
||||
return web.json_response(cache._settings_cache)
|
||||
|
||||
params = cache.params
|
||||
|
||||
# Update language if changed
|
||||
@@ -322,10 +285,7 @@ async def get_settings_config_api(request):
|
||||
section_copy['settings'] = settings_list
|
||||
settings_with_values.append(section_copy)
|
||||
|
||||
response_data = {'settings': settings_with_values}
|
||||
cache._settings_cache = response_data
|
||||
cache._settings_cache_time = now
|
||||
return web.json_response(response_data)
|
||||
return web.json_response({'settings': settings_with_values})
|
||||
|
||||
|
||||
def _get_setting_value(params, setting):
|
||||
@@ -371,7 +331,6 @@ async def save_param_api(request):
|
||||
return web.json_response({'error': 'value is required in body'}, status=400)
|
||||
|
||||
_save_param(params, param_name, data['value'])
|
||||
cache.invalidate()
|
||||
logger.info(f"Param saved: {param_name}={data['value']}")
|
||||
|
||||
return web.json_response({'status': 'success', 'key': param_name, 'value': data['value']})
|
||||
@@ -402,20 +361,14 @@ def _save_param(params, key, value):
|
||||
def _get_param_value(params, key):
|
||||
"""Get a single param value with proper type handling."""
|
||||
try:
|
||||
# Try get_bool first for boolean params
|
||||
return params.get_bool(key)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
try:
|
||||
raw_value = params.get(key)
|
||||
if raw_value is None:
|
||||
return None
|
||||
elif isinstance(raw_value, bytes):
|
||||
return raw_value.decode('utf-8')
|
||||
return raw_value
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
@api_handler
|
||||
@@ -426,10 +379,8 @@ async def get_param_api(request):
|
||||
return web.json_response({'error': 'param_name is required'}, status=400)
|
||||
|
||||
cache: AppCache = request.app['cache']
|
||||
try:
|
||||
value = _get_param_value(cache.params, param_name)
|
||||
except Exception:
|
||||
value = None
|
||||
params = cache.params
|
||||
value = _get_param_value(params, param_name)
|
||||
|
||||
return web.json_response({'key': param_name, 'value': value})
|
||||
|
||||
@@ -490,406 +441,18 @@ async def webrtc_stream_proxy(request):
|
||||
body = await request.read()
|
||||
session: ClientSession = request.app['http_session']
|
||||
|
||||
try:
|
||||
async with session.post(
|
||||
f'http://{host}:5001/stream',
|
||||
data=body,
|
||||
headers={'Content-Type': 'application/json'}
|
||||
) as resp:
|
||||
response_body = await resp.read()
|
||||
return web.Response(
|
||||
body=response_body,
|
||||
status=resp.status,
|
||||
content_type=resp.content_type
|
||||
)
|
||||
except ClientConnectorError:
|
||||
# webrtcd not running - return 503 Service Unavailable
|
||||
return web.json_response(
|
||||
{'error': 'Stream service unavailable', 'code': 'WEBRTCD_UNAVAILABLE'},
|
||||
status=503
|
||||
async with session.post(
|
||||
f'http://{host}:5001/stream',
|
||||
data=body,
|
||||
headers={'Content-Type': 'application/json'}
|
||||
) as resp:
|
||||
response_body = await resp.read()
|
||||
return web.Response(
|
||||
body=response_body,
|
||||
status=resp.status,
|
||||
content_type=resp.content_type
|
||||
)
|
||||
|
||||
# --- Navigation API Endpoints ---
|
||||
@api_handler
|
||||
async def nav_get_destination_api(request):
|
||||
"""Get current navigation destination.
|
||||
|
||||
GET /api/nav/destination
|
||||
Returns: { latitude, longitude, name } or {}
|
||||
"""
|
||||
cache: AppCache = request.app['cache']
|
||||
params = cache.params
|
||||
|
||||
destination = {}
|
||||
try:
|
||||
# JSON type params return dict directly
|
||||
dest = params.get("dp_maa_destination")
|
||||
if dest:
|
||||
destination = dest
|
||||
except Exception as e:
|
||||
logger.debug(f"Could not get dp_maa_destination: {e}")
|
||||
|
||||
return web.json_response(destination)
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_set_destination_api(request):
|
||||
"""Set navigation destination.
|
||||
|
||||
POST /api/nav/destination
|
||||
Body: { "latitude": float, "longitude": float, "name": string (optional) }
|
||||
"""
|
||||
cache: AppCache = request.app['cache']
|
||||
params = cache.params
|
||||
data = await request.json()
|
||||
|
||||
if 'latitude' not in data or 'longitude' not in data:
|
||||
return web.json_response({'error': 'latitude and longitude required'}, status=400)
|
||||
|
||||
destination = {
|
||||
'latitude': float(data['latitude']),
|
||||
'longitude': float(data['longitude']),
|
||||
'name': data.get('name', '')
|
||||
}
|
||||
|
||||
try:
|
||||
# Use native dict with put_nonblocking for JSON types
|
||||
params.put_nonblocking("dp_maa_destination", destination)
|
||||
logger.info(f"Nav destination set: {destination['latitude']:.6f}, {destination['longitude']:.6f}")
|
||||
except Exception as e:
|
||||
logger.warning(f"Could not save NavDestination to params: {e}")
|
||||
|
||||
return web.json_response({'status': 'success', 'destination': destination})
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_clear_destination_api(request):
|
||||
"""Clear navigation destination.
|
||||
|
||||
DELETE /api/nav/destination
|
||||
"""
|
||||
cache: AppCache = request.app['cache']
|
||||
params = cache.params
|
||||
try:
|
||||
params.remove("dp_maa_destination")
|
||||
logger.info("Nav destination cleared")
|
||||
except Exception as e:
|
||||
logger.warning(f"Could not remove NavDestination from params: {e}")
|
||||
return web.json_response({'status': 'success'})
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_search_api(request):
|
||||
"""Search for places/addresses.
|
||||
|
||||
GET /api/nav/search?q=<query>&lat=<lat>&lon=<lon>&limit=<limit>
|
||||
Returns: [{ name, address, latitude, longitude, distance }, ...]
|
||||
"""
|
||||
query = request.query.get('q', '').strip()
|
||||
if not query or len(query) < 2:
|
||||
return web.json_response([])
|
||||
|
||||
# Parse optional proximity
|
||||
proximity = None
|
||||
lat_str = request.query.get('lat')
|
||||
lon_str = request.query.get('lon')
|
||||
if lat_str and lon_str:
|
||||
try:
|
||||
proximity = Coordinate(float(lat_str), float(lon_str))
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
limit = min(int(request.query.get('limit', 10)), 20)
|
||||
|
||||
# Get map service from app cache
|
||||
map_service: MapService = request.app.get('map_service')
|
||||
if not map_service:
|
||||
cache: AppCache = request.app['cache']
|
||||
map_service = MapService(cache.params)
|
||||
request.app['map_service'] = map_service
|
||||
|
||||
try:
|
||||
results = await map_service.search_provider.search(query, proximity, limit)
|
||||
# Log which provider was used
|
||||
if results:
|
||||
provider = results[0].provider if hasattr(results[0], 'provider') else 'unknown'
|
||||
logger.info(f"Search '{query}' returned {len(results)} results via {provider}")
|
||||
return web.json_response([
|
||||
{
|
||||
'name': r.name,
|
||||
'address': r.address,
|
||||
'latitude': r.coordinate.latitude,
|
||||
'longitude': r.coordinate.longitude,
|
||||
'distance': r.distance,
|
||||
}
|
||||
for r in results
|
||||
])
|
||||
except Exception as e:
|
||||
logger.error(f"Search error: {e}")
|
||||
return web.json_response([])
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_route_api(request):
|
||||
"""Calculate route between two points.
|
||||
|
||||
POST /api/nav/route
|
||||
Body: { "start": {"lat": float, "lon": float}, "end": {"lat": float, "lon": float} }
|
||||
Returns: { distance_m, duration_s, polyline, maneuvers, has_traffic }
|
||||
"""
|
||||
data = await request.json()
|
||||
|
||||
start = data.get('start', {})
|
||||
end = data.get('end', {})
|
||||
|
||||
if not all([start.get('lat'), start.get('lon'), end.get('lat'), end.get('lon')]):
|
||||
return web.json_response({'error': 'start and end coordinates required'}, status=400)
|
||||
|
||||
origin = Coordinate(float(start['lat']), float(start['lon']))
|
||||
destination = Coordinate(float(end['lat']), float(end['lon']))
|
||||
|
||||
# Get map service
|
||||
map_service: MapService = request.app.get('map_service')
|
||||
if not map_service:
|
||||
cache: AppCache = request.app['cache']
|
||||
map_service = MapService(cache.params)
|
||||
request.app['map_service'] = map_service
|
||||
|
||||
try:
|
||||
route = await map_service.route_provider.get_route(origin, destination)
|
||||
if not route:
|
||||
return web.json_response({'error': 'No route found'}, status=404)
|
||||
|
||||
logger.info(f"Route calculated: {route.distance/1000:.1f}km via {route.provider}")
|
||||
return web.json_response({
|
||||
'distance_m': route.distance,
|
||||
'duration_s': route.duration,
|
||||
'polyline': _encode_polyline(route.geometry) if route.geometry else '',
|
||||
'geometry': [[c.latitude, c.longitude] for c in route.geometry] if route.geometry else [],
|
||||
'maneuvers': [
|
||||
{
|
||||
'instruction': step.name or '',
|
||||
'distance_m': step.distance,
|
||||
'duration_s': step.duration,
|
||||
'position': {
|
||||
'lat': step.maneuver_point.latitude,
|
||||
'lon': step.maneuver_point.longitude
|
||||
} if step.maneuver_point else None,
|
||||
'type': step.maneuver_type,
|
||||
'modifier': step.maneuver_modifier,
|
||||
}
|
||||
for step in route.steps
|
||||
],
|
||||
'has_traffic': route.has_traffic,
|
||||
'provider': route.provider,
|
||||
})
|
||||
except Exception as e:
|
||||
logger.error(f"Route error: {e}")
|
||||
return web.json_response({'error': str(e)}, status=500)
|
||||
|
||||
|
||||
def _encode_polyline(coordinates: list) -> str:
|
||||
"""Encode coordinates to Google polyline format."""
|
||||
if not coordinates:
|
||||
return ''
|
||||
|
||||
result = []
|
||||
prev_lat = 0
|
||||
prev_lon = 0
|
||||
|
||||
for coord in coordinates:
|
||||
lat = int(round(coord.latitude * 1e5))
|
||||
lon = int(round(coord.longitude * 1e5))
|
||||
|
||||
d_lat = lat - prev_lat
|
||||
d_lon = lon - prev_lon
|
||||
|
||||
for val in [d_lat, d_lon]:
|
||||
val = ~(val << 1) if val < 0 else (val << 1)
|
||||
while val >= 0x20:
|
||||
result.append(chr((0x20 | (val & 0x1f)) + 63))
|
||||
val >>= 5
|
||||
result.append(chr(val + 63))
|
||||
|
||||
prev_lat = lat
|
||||
prev_lon = lon
|
||||
|
||||
return ''.join(result)
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_tiles_config_api(request):
|
||||
"""Get tile provider configuration.
|
||||
|
||||
GET /api/nav/tiles/config
|
||||
Returns: { url_template, style_url, attribution, min_zoom, max_zoom }
|
||||
"""
|
||||
map_service: MapService = request.app.get('map_service')
|
||||
if not map_service:
|
||||
cache: AppCache = request.app['cache']
|
||||
map_service = MapService(cache.params)
|
||||
request.app['map_service'] = map_service
|
||||
|
||||
try:
|
||||
config = map_service.tile_provider.get_tile_config()
|
||||
return web.json_response({
|
||||
'url_template': config.url_template,
|
||||
'style_url': config.style_url,
|
||||
'attribution': config.attribution,
|
||||
'min_zoom': config.min_zoom,
|
||||
'max_zoom': config.max_zoom,
|
||||
})
|
||||
except Exception as e:
|
||||
logger.error(f"Tile config error: {e}")
|
||||
return web.json_response({'error': str(e)}, status=500)
|
||||
|
||||
|
||||
# --- Places API (Favorites + Recent) ---
|
||||
# In-memory cache (persists for server session even if params fails)
|
||||
_places_cache = {"home": None, "work": None, "recent": []}
|
||||
|
||||
def _get_places(params) -> dict:
|
||||
"""Get places data from dp_maa_places param or memory cache."""
|
||||
global _places_cache
|
||||
try:
|
||||
# JSON type params return dict/list directly
|
||||
data = params.get("dp_maa_places")
|
||||
if data:
|
||||
_places_cache = data # sync to memory
|
||||
return data
|
||||
except Exception as e:
|
||||
logger.debug(f"Could not parse dp_maa_places: {e}")
|
||||
return _places_cache
|
||||
|
||||
|
||||
def _save_places(params, places: dict):
|
||||
"""Save places data to dp_maa_places param and memory cache."""
|
||||
global _places_cache
|
||||
_places_cache = places # always save to memory first
|
||||
try:
|
||||
# JSON type params accept dict/list directly
|
||||
params.put("dp_maa_places", places)
|
||||
except Exception as e:
|
||||
logger.warning(f"Failed to save places to params: {e}")
|
||||
|
||||
|
||||
def _haversine_distance(lat1, lon1, lat2, lon2) -> float:
|
||||
"""Calculate distance between two points in meters."""
|
||||
import math
|
||||
R = 6371000
|
||||
phi1, phi2 = math.radians(lat1), math.radians(lat2)
|
||||
d_phi = math.radians(lat2 - lat1)
|
||||
d_lambda = math.radians(lon2 - lon1)
|
||||
a = math.sin(d_phi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(d_lambda / 2) ** 2
|
||||
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
||||
|
||||
|
||||
def _add_to_recent(places: dict, place: dict) -> dict:
|
||||
"""Add a place to recent list with deduplication."""
|
||||
recent = [r for r in places.get("recent", [])
|
||||
if _haversine_distance(r["lat"], r["lon"], place["lat"], place["lon"]) > 100]
|
||||
recent.insert(0, place)
|
||||
places["recent"] = recent[:5]
|
||||
return places
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_get_places_api(request):
|
||||
"""GET /api/nav/places - Get all places."""
|
||||
cache: AppCache = request.app['cache']
|
||||
return web.json_response(_get_places(cache.params))
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_set_place_api(request):
|
||||
"""POST /api/nav/places/{place_type} - Set home or work."""
|
||||
place_type = request.match_info.get('place_type')
|
||||
if place_type not in ('home', 'work'):
|
||||
return web.json_response({'error': 'Invalid place type'}, status=400)
|
||||
|
||||
cache: AppCache = request.app['cache']
|
||||
data = await request.json()
|
||||
if 'lat' not in data or 'lon' not in data:
|
||||
return web.json_response({'error': 'lat and lon required'}, status=400)
|
||||
|
||||
places = _get_places(cache.params)
|
||||
places[place_type] = {
|
||||
'name': data.get('name', place_type.capitalize()),
|
||||
'address': data.get('address', ''),
|
||||
'lat': float(data['lat']),
|
||||
'lon': float(data['lon'])
|
||||
}
|
||||
_save_places(cache.params, places)
|
||||
return web.json_response({'success': True, place_type: places[place_type]})
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_delete_place_api(request):
|
||||
"""DELETE /api/nav/places/{place_type} - Delete home or work."""
|
||||
place_type = request.match_info.get('place_type')
|
||||
if place_type not in ('home', 'work'):
|
||||
return web.json_response({'error': 'Invalid place type'}, status=400)
|
||||
|
||||
cache: AppCache = request.app['cache']
|
||||
places = _get_places(cache.params)
|
||||
places[place_type] = None
|
||||
_save_places(cache.params, places)
|
||||
return web.json_response({'success': True})
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_add_recent_api(request):
|
||||
"""POST /api/nav/places/recent - Add to recent."""
|
||||
cache: AppCache = request.app['cache']
|
||||
data = await request.json()
|
||||
if 'lat' not in data or 'lon' not in data:
|
||||
return web.json_response({'error': 'lat and lon required'}, status=400)
|
||||
|
||||
places = _get_places(cache.params)
|
||||
place = {'name': data.get('name', 'Unknown'), 'address': data.get('address', ''),
|
||||
'lat': float(data['lat']), 'lon': float(data['lon'])}
|
||||
places = _add_to_recent(places, place)
|
||||
_save_places(cache.params, places)
|
||||
return web.json_response({'success': True, 'recent': places['recent']})
|
||||
|
||||
|
||||
@api_handler
|
||||
async def nav_clear_recent_api(request):
|
||||
"""DELETE /api/nav/places/recent - Clear recent."""
|
||||
cache: AppCache = request.app['cache']
|
||||
places = _get_places(cache.params)
|
||||
places['recent'] = []
|
||||
_save_places(cache.params, places)
|
||||
return web.json_response({'success': True})
|
||||
|
||||
|
||||
# --- WebSocket endpoint for data streaming ---
|
||||
async def websocket_handler(request):
|
||||
"""WebSocket endpoint for data-only connections - streams dashyState directly."""
|
||||
ws = web.WebSocketResponse()
|
||||
await ws.prepare(request)
|
||||
|
||||
logger.info("WebSocket client connected")
|
||||
|
||||
# Create a SubMaster for this connection
|
||||
sm = messaging.SubMaster(['dashyState'])
|
||||
|
||||
try:
|
||||
while not ws.closed:
|
||||
sm.update(0)
|
||||
if sm.updated['dashyState']:
|
||||
json_data = sm['dashyState'].json
|
||||
if isinstance(json_data, bytes):
|
||||
json_data = json_data.decode('utf-8')
|
||||
await ws.send_str(json_data)
|
||||
await asyncio.sleep(0.01)
|
||||
except Exception as e:
|
||||
logger.warning(f"WebSocket error: {e}")
|
||||
finally:
|
||||
logger.info("WebSocket client disconnected")
|
||||
|
||||
return ws
|
||||
|
||||
|
||||
# --- CORS Middleware ---
|
||||
@web.middleware
|
||||
@@ -952,23 +515,6 @@ def setup_aiohttp_app(host: str, port: int, debug: bool):
|
||||
app.router.add_get("/api/models", get_model_list_api)
|
||||
app.router.add_post("/api/models/select", save_model_selection_api)
|
||||
app.router.add_post("/api/stream", webrtc_stream_proxy)
|
||||
app.router.add_get("/api/ws", websocket_handler) # WebSocket for data streaming
|
||||
|
||||
# Navigation routes
|
||||
app.router.add_get("/api/nav/destination", nav_get_destination_api)
|
||||
app.router.add_post("/api/nav/destination", nav_set_destination_api)
|
||||
app.router.add_delete("/api/nav/destination", nav_clear_destination_api)
|
||||
app.router.add_get("/api/nav/search", nav_search_api)
|
||||
app.router.add_post("/api/nav/route", nav_route_api)
|
||||
app.router.add_get("/api/nav/tiles/config", nav_tiles_config_api)
|
||||
|
||||
# Places routes (favorites + recent) - specific routes before parametrized
|
||||
app.router.add_get("/api/nav/places", nav_get_places_api)
|
||||
app.router.add_post("/api/nav/places/recent", nav_add_recent_api)
|
||||
app.router.add_delete("/api/nav/places/recent", nav_clear_recent_api)
|
||||
app.router.add_post("/api/nav/places/{place_type}", nav_set_place_api)
|
||||
app.router.add_delete("/api/nav/places/{place_type}", nav_delete_place_api)
|
||||
|
||||
app.router.add_route('OPTIONS', '/{tail:.*}', handle_cors_preflight)
|
||||
|
||||
# Static files
|
||||
@@ -1,598 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Dashy State Aggregation Daemon
|
||||
|
||||
Aggregates all cereal topics needed by dashy UI into a single dashyState message.
|
||||
This reduces CPU overhead in webrtcd by doing JSON serialization once here
|
||||
instead of serializing 14+ topics separately.
|
||||
|
||||
All display formatting (units, distances, times) is done here so the frontend
|
||||
can be a pure display layer with no conversion logic.
|
||||
|
||||
Publishes: dashyState (pre-serialized JSON at 15Hz)
|
||||
"""
|
||||
import json
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from opendbc.car.common.conversions import Conversions
|
||||
|
||||
# Main loop rate
|
||||
LOOP_RATE = 15 # Hz
|
||||
|
||||
# Downsample factor for modelV2 arrays (33 points -> 17 points)
|
||||
DOWNSAMPLE_FACTOR = 2
|
||||
|
||||
# Unit conversion constants
|
||||
M_TO_KM = 0.001
|
||||
M_TO_MI = 0.000621371
|
||||
M_TO_FT = 3.28084
|
||||
|
||||
# Global state (refreshed periodically)
|
||||
_is_metric = True
|
||||
_params = None
|
||||
_car_params_cache = None
|
||||
|
||||
|
||||
def _ensure_params():
|
||||
"""Ensure Params instance exists."""
|
||||
global _params
|
||||
if _params is None:
|
||||
_params = Params()
|
||||
return _params
|
||||
|
||||
|
||||
def refresh_metric_preference():
|
||||
"""Refresh metric preference from params (called periodically)."""
|
||||
global _is_metric
|
||||
try:
|
||||
_is_metric = _ensure_params().get_bool("IsMetric")
|
||||
except Exception:
|
||||
_is_metric = True
|
||||
|
||||
|
||||
def get_car_params_from_params():
|
||||
"""Read carParams from Params storage (for immediate availability at startup)."""
|
||||
global _car_params_cache
|
||||
if _car_params_cache is not None:
|
||||
return _car_params_cache
|
||||
try:
|
||||
from cereal import car
|
||||
cp_bytes = _ensure_params().get("CarParams")
|
||||
if cp_bytes:
|
||||
with car.CarParams.from_bytes(cp_bytes) as cp:
|
||||
_car_params_cache = {
|
||||
'openpilotLongitudinalControl': bool(cp.openpilotLongitudinalControl),
|
||||
}
|
||||
return _car_params_cache
|
||||
except Exception:
|
||||
pass
|
||||
return {'openpilotLongitudinalControl': False}
|
||||
|
||||
|
||||
def format_speed(speed_ms: float) -> str:
|
||||
"""Format speed for display (m/s -> km/h or mph)."""
|
||||
if _is_metric:
|
||||
return f"{max(0, speed_ms * Conversions.MS_TO_KPH):.0f}"
|
||||
return f"{max(0, speed_ms * Conversions.MS_TO_MPH):.0f}"
|
||||
|
||||
|
||||
def format_speed_value(speed_ms: float) -> float:
|
||||
"""Convert speed to display units (m/s -> km/h or mph)."""
|
||||
if _is_metric:
|
||||
return max(0, speed_ms * Conversions.MS_TO_KPH)
|
||||
return max(0, speed_ms * Conversions.MS_TO_MPH)
|
||||
|
||||
|
||||
def format_distance(meters: float) -> str:
|
||||
"""Format distance for display."""
|
||||
if meters <= 0:
|
||||
return ""
|
||||
if _is_metric:
|
||||
if meters >= 1000:
|
||||
return f"{meters * M_TO_KM:.1f} km"
|
||||
return f"{meters:.0f} m"
|
||||
else:
|
||||
miles = meters * M_TO_MI
|
||||
if miles >= 0.1:
|
||||
return f"{miles:.1f} mi"
|
||||
return f"{meters * M_TO_FT:.0f} ft"
|
||||
|
||||
|
||||
def format_time(seconds: float) -> str:
|
||||
"""Format time duration for display."""
|
||||
if seconds <= 0:
|
||||
return ""
|
||||
minutes = int(seconds / 60)
|
||||
if minutes < 60:
|
||||
return f"{minutes} min"
|
||||
hours = minutes // 60
|
||||
mins = minutes % 60
|
||||
if mins == 0:
|
||||
return f"{hours} hr"
|
||||
return f"{hours} hr {mins} min"
|
||||
|
||||
|
||||
def get_speed_unit() -> str:
|
||||
"""Get current speed unit string."""
|
||||
return "km/h" if _is_metric else "mph"
|
||||
|
||||
|
||||
def get_distance_unit() -> str:
|
||||
"""Get current distance unit string."""
|
||||
return "km" if _is_metric else "mi"
|
||||
|
||||
|
||||
SET_SPEED_NA = 255
|
||||
|
||||
|
||||
def get_cruise_speed(v_cruise_cluster: float) -> int:
|
||||
"""Get cruise speed value for display.
|
||||
|
||||
Returns the set speed in display units (km/h or mph), or 255 if not set.
|
||||
"""
|
||||
if not (0 < v_cruise_cluster < SET_SPEED_NA):
|
||||
return SET_SPEED_NA
|
||||
|
||||
set_speed = v_cruise_cluster
|
||||
if not _is_metric:
|
||||
set_speed *= Conversions.KPH_TO_MPH
|
||||
|
||||
return round(set_speed)
|
||||
|
||||
|
||||
def downsample(arr):
|
||||
"""Downsample list by factor."""
|
||||
if not arr:
|
||||
return []
|
||||
return list(arr[::DOWNSAMPLE_FACTOR])
|
||||
|
||||
|
||||
def safe_get(obj, attr, default=None):
|
||||
"""Safely get attribute from object."""
|
||||
try:
|
||||
return getattr(obj, attr, default)
|
||||
except Exception:
|
||||
return default
|
||||
|
||||
|
||||
def extract_car_state(sm):
|
||||
"""Extract carState fields used by dashy."""
|
||||
cs = sm['carState']
|
||||
v_ego = float(cs.vEgo)
|
||||
v_ego_cluster = float(cs.vEgoCluster)
|
||||
|
||||
# Get set speed from controlsState.vCruiseDEPRECATED (fallback to carState.vCruiseCluster)
|
||||
v_cruise = float(cs.vCruiseCluster)
|
||||
if 'controlsState' in sm.updated and sm.updated['controlsState']:
|
||||
v_cruise = float(sm['controlsState'].vCruiseDEPRECATED)
|
||||
set_speed = get_cruise_speed(v_cruise)
|
||||
|
||||
return {
|
||||
'vEgo': v_ego,
|
||||
'vEgoCluster': v_ego_cluster,
|
||||
'gearShifter': str(cs.gearShifter),
|
||||
'aEgo': float(cs.aEgo),
|
||||
'steeringAngleDeg': float(cs.steeringAngleDeg),
|
||||
'leftBlinker': bool(cs.leftBlinker),
|
||||
'rightBlinker': bool(cs.rightBlinker),
|
||||
'leftBlindspot': bool(cs.leftBlindspot),
|
||||
'rightBlindspot': bool(cs.rightBlindspot),
|
||||
# Pre-formatted display values
|
||||
'speedDisplay': format_speed(v_ego),
|
||||
'speedClusterDisplay': format_speed(v_ego_cluster) if v_ego_cluster > 0 else format_speed(v_ego),
|
||||
'setSpeed': set_speed, # 255 = not set, otherwise display value
|
||||
'speedUnit': get_speed_unit(),
|
||||
}
|
||||
|
||||
|
||||
def extract_selfdrive_state(sm):
|
||||
"""Extract selfdriveState fields used by dashy."""
|
||||
ss = sm['selfdriveState']
|
||||
return {
|
||||
'enabled': bool(ss.enabled),
|
||||
'activeOverride': int(safe_get(ss, 'activeOverride', 0)),
|
||||
'experimentalMode': bool(ss.experimentalMode),
|
||||
'alertText1': str(ss.alertText1),
|
||||
'alertText2': str(ss.alertText2),
|
||||
'alertSize': str(ss.alertSize),
|
||||
'alertStatus': str(ss.alertStatus),
|
||||
}
|
||||
|
||||
|
||||
def extract_device_state(sm):
|
||||
"""Extract deviceState fields used by dashy."""
|
||||
ds = sm['deviceState']
|
||||
temp_c = float(safe_get(ds, 'maxTempC', 0))
|
||||
# Pre-format temperature for display
|
||||
if _is_metric:
|
||||
temp_display = f"{temp_c:.0f}°" if temp_c > 0 else "--"
|
||||
else:
|
||||
temp_f = temp_c * 9 / 5 + 32
|
||||
temp_display = f"{temp_f:.0f}°" if temp_c > 0 else "--"
|
||||
return {
|
||||
'cpuUsagePercent': list(ds.cpuUsagePercent) if ds.cpuUsagePercent else [],
|
||||
'memoryUsagePercent': int(ds.memoryUsagePercent),
|
||||
'maxTempC': temp_c,
|
||||
'deviceType': str(ds.deviceType),
|
||||
'tempDisplay': temp_display,
|
||||
}
|
||||
|
||||
|
||||
def extract_lead(lead, sm):
|
||||
"""Extract lead vehicle data."""
|
||||
d_rel = float(lead.dRel)
|
||||
v_rel = float(lead.vRel)
|
||||
y_rel = float(lead.yRel)
|
||||
has_lead = bool(lead.status)
|
||||
|
||||
# Pre-format lead display values
|
||||
if has_lead:
|
||||
v_ego = float(sm['carState'].vEgo) if sm.valid['carState'] else 0
|
||||
lead_speed_ms = max(0, v_ego + v_rel)
|
||||
speed_display = format_speed(lead_speed_ms)
|
||||
distance_display = f"{d_rel:.1f}m" if _is_metric else f"{d_rel * M_TO_FT:.0f}ft"
|
||||
else:
|
||||
speed_display = "--"
|
||||
distance_display = "--"
|
||||
|
||||
return {
|
||||
'status': has_lead,
|
||||
'dRel': d_rel,
|
||||
'yRel': y_rel,
|
||||
'vRel': v_rel,
|
||||
'speedDisplay': speed_display,
|
||||
'distanceDisplay': distance_display,
|
||||
}
|
||||
|
||||
|
||||
def extract_radar_state(sm):
|
||||
"""Extract radarState fields used by dashy."""
|
||||
rs = sm['radarState']
|
||||
return {
|
||||
'leadOne': extract_lead(rs.leadOne, sm),
|
||||
'leadTwo': extract_lead(rs.leadTwo, sm),
|
||||
}
|
||||
|
||||
|
||||
def extract_live_tracks(sm):
|
||||
"""Extract liveTracks radar points for bird's eye view.
|
||||
|
||||
Filters out tracks that are already shown as leadOne or leadTwo.
|
||||
Uses radarTrackId matching: when radarState matches a liveTrack to a lead,
|
||||
radarTrackId changes from -1 (vision-only) to the track's ID.
|
||||
"""
|
||||
try:
|
||||
lt = sm['liveTracks']
|
||||
points = []
|
||||
|
||||
# Get lead vehicle radar track IDs to filter them out
|
||||
# radarTrackId = -1 means vision-only (no radar match)
|
||||
# radarTrackId >= 0 means matched to a radar track
|
||||
lead_track_ids = set()
|
||||
if sm.valid.get('radarState', False):
|
||||
rs = sm['radarState']
|
||||
if rs.leadOne.status and rs.leadOne.radarTrackId >= 0:
|
||||
lead_track_ids.add(rs.leadOne.radarTrackId)
|
||||
if rs.leadTwo.status and rs.leadTwo.radarTrackId >= 0:
|
||||
lead_track_ids.add(rs.leadTwo.radarTrackId)
|
||||
|
||||
if hasattr(lt, 'points'):
|
||||
for pt in lt.points:
|
||||
# Skip if this track is already shown as a lead vehicle
|
||||
if pt.trackId in lead_track_ids:
|
||||
continue
|
||||
|
||||
points.append({
|
||||
'd': float(pt.dRel),
|
||||
'y': float(pt.yRel),
|
||||
'v': float(pt.vRel),
|
||||
'm': bool(pt.measured),
|
||||
})
|
||||
return {'points': points}
|
||||
except Exception as e:
|
||||
cloudlog.warning(f"extract_live_tracks error: {e}")
|
||||
return {'points': []}
|
||||
|
||||
|
||||
def extract_live_gps(sm):
|
||||
"""Extract liveGPS fields used by dashy."""
|
||||
gps = sm['liveGPS']
|
||||
# Skip if no coordinates yet
|
||||
if gps.latitude == 0 and gps.longitude == 0:
|
||||
return None
|
||||
return {
|
||||
'latitude': float(gps.latitude),
|
||||
'longitude': float(gps.longitude),
|
||||
'bearingDeg': float(gps.bearingDeg),
|
||||
'speed': float(gps.speed),
|
||||
'gpsOK': bool(gps.gpsOK),
|
||||
'horizontalAccuracy': float(gps.horizontalAccuracy),
|
||||
'status': str(gps.status),
|
||||
}
|
||||
|
||||
|
||||
def extract_nav_instruction(sm):
|
||||
"""Extract navInstruction fields used by dashy."""
|
||||
nav = sm['navInstruction']
|
||||
maneuver_dist = float(safe_get(nav, 'maneuverDistance', 0))
|
||||
dist_remaining = float(safe_get(nav, 'distanceRemaining', 0))
|
||||
time_remaining = float(safe_get(nav, 'timeRemaining', 0))
|
||||
speed_limit_ms = float(safe_get(nav, 'speedLimit', 0))
|
||||
|
||||
return {
|
||||
'valid': sm.valid['navInstruction'],
|
||||
'maneuverDistance': maneuver_dist,
|
||||
'maneuverPrimaryText': str(safe_get(nav, 'maneuverPrimaryText', '')),
|
||||
'maneuverSecondaryText': str(safe_get(nav, 'maneuverSecondaryText', '')),
|
||||
'maneuverType': str(safe_get(nav, 'maneuverType', '')),
|
||||
'maneuverModifier': str(safe_get(nav, 'maneuverModifier', '')),
|
||||
'distanceRemaining': dist_remaining,
|
||||
'timeRemaining': time_remaining,
|
||||
'timeRemainingTypical': float(safe_get(nav, 'timeRemainingTypical', 0)),
|
||||
'speedLimit': speed_limit_ms,
|
||||
'speedLimitSign': str(safe_get(nav, 'speedLimitSign', '')),
|
||||
# Pre-formatted display values
|
||||
'maneuverDistanceDisplay': format_distance(maneuver_dist),
|
||||
'distanceRemainingDisplay': format_distance(dist_remaining),
|
||||
'timeRemainingDisplay': format_time(time_remaining),
|
||||
'speedLimitDisplay': format_speed(speed_limit_ms) if speed_limit_ms > 0 else '',
|
||||
}
|
||||
|
||||
|
||||
def extract_nav_instruction_ext(sm):
|
||||
"""Extract navInstructionExt fields used by dashy (extended nav data)."""
|
||||
nav_ext = sm['navInstructionExt']
|
||||
# Extract allManeuvers list (with name field and formatted distance)
|
||||
all_maneuvers = []
|
||||
if hasattr(nav_ext, 'allManeuvers'):
|
||||
for m in nav_ext.allManeuvers:
|
||||
dist = float(safe_get(m, 'distance', 0))
|
||||
all_maneuvers.append({
|
||||
'distance': dist,
|
||||
'distanceDisplay': format_distance(dist),
|
||||
'type': str(safe_get(m, 'type', '')),
|
||||
'modifier': str(safe_get(m, 'modifier', '')),
|
||||
'name': str(safe_get(m, 'name', '')),
|
||||
})
|
||||
return {
|
||||
'turnAngle': float(safe_get(nav_ext, 'turnAngle', 0)),
|
||||
'turnCurvature': float(safe_get(nav_ext, 'turnCurvature', 0)),
|
||||
'allManeuvers': all_maneuvers,
|
||||
}
|
||||
|
||||
|
||||
def extract_nav_route(sm):
|
||||
"""Extract navRoute coordinates used by dashy."""
|
||||
route = sm['navRoute']
|
||||
coords = []
|
||||
if hasattr(route, 'coordinates'):
|
||||
for c in route.coordinates:
|
||||
coords.append([float(c.longitude), float(c.latitude)])
|
||||
return {
|
||||
'coordinates': coords,
|
||||
}
|
||||
|
||||
|
||||
def extract_model_v2(sm):
|
||||
"""Extract modelV2 fields used by dashy (downsampled)."""
|
||||
model = sm['modelV2']
|
||||
|
||||
# Position
|
||||
pos = model.position
|
||||
position = {
|
||||
'x': downsample(list(pos.x)),
|
||||
'y': downsample(list(pos.y)),
|
||||
'z': downsample(list(pos.z)),
|
||||
}
|
||||
|
||||
# Lane lines (4 lines)
|
||||
lane_lines = []
|
||||
for line in model.laneLines:
|
||||
lane_lines.append({
|
||||
'x': downsample(list(line.x)),
|
||||
'y': downsample(list(line.y)),
|
||||
'z': downsample(list(line.z)),
|
||||
})
|
||||
|
||||
# Road edges (2 edges)
|
||||
road_edges = []
|
||||
for edge in model.roadEdges:
|
||||
road_edges.append({
|
||||
'x': downsample(list(edge.x)),
|
||||
'y': downsample(list(edge.y)),
|
||||
'z': downsample(list(edge.z)),
|
||||
})
|
||||
|
||||
return {
|
||||
'position': position,
|
||||
'laneLines': lane_lines,
|
||||
'laneLineProbs': list(model.laneLineProbs) if hasattr(model, 'laneLineProbs') else [0, 0, 0, 0],
|
||||
'roadEdges': road_edges,
|
||||
'roadEdgeStds': list(model.roadEdgeStds) if hasattr(model, 'roadEdgeStds') else [1, 1],
|
||||
}
|
||||
|
||||
|
||||
def extract_live_calibration(sm):
|
||||
"""Extract liveCalibration fields used by dashy."""
|
||||
cal = sm['liveCalibration']
|
||||
return {
|
||||
'rpyCalib': list(cal.rpyCalib) if hasattr(cal, 'rpyCalib') and cal.rpyCalib else [],
|
||||
'calStatus': str(cal.calStatus) if hasattr(cal, 'calStatus') else 'uncalibrated',
|
||||
'height': list(cal.height) if hasattr(cal, 'height') else [],
|
||||
}
|
||||
|
||||
|
||||
def extract_longitudinal_plan(sm):
|
||||
"""Extract longitudinalPlan fields used by dashy."""
|
||||
lp = sm['longitudinalPlan']
|
||||
return {
|
||||
'allowThrottle': bool(safe_get(lp, 'allowThrottle', True)),
|
||||
}
|
||||
|
||||
|
||||
def extract_controls_state_ext(sm):
|
||||
"""Extract controlsStateExt fields used by dashy."""
|
||||
cse = sm['controlsStateExt']
|
||||
return {
|
||||
'alkaActive': bool(safe_get(cse, 'alkaActive', False)),
|
||||
}
|
||||
|
||||
|
||||
def extract_car_params(sm):
|
||||
"""Extract carParams fields used by dashy."""
|
||||
cp = sm['carParams']
|
||||
return {
|
||||
'openpilotLongitudinalControl': bool(safe_get(cp, 'openpilotLongitudinalControl', False)),
|
||||
}
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# TOPIC CONFIGURATION
|
||||
# =============================================================================
|
||||
# Single source of truth for all subscribed topics.
|
||||
# Comment out a line to disable that topic entirely.
|
||||
#
|
||||
# Fields:
|
||||
# extractor: function(sm) -> dict, extracts data from message
|
||||
# rate: 'fast' = every frame when updated
|
||||
# number = slow poll divider (e.g., LOOP_RATE = 1Hz)
|
||||
# 'valid' = just track valid state, no extraction
|
||||
# 'subscribe' = subscribed but extracted within other extractors
|
||||
# default: initial cache value (None if not specified)
|
||||
# =============================================================================
|
||||
TOPICS = {
|
||||
# Fast topics - extract every frame when updated
|
||||
'carState': {'extractor': extract_car_state, 'rate': 'fast'},
|
||||
'selfdriveState': {'extractor': extract_selfdrive_state, 'rate': 'fast'},
|
||||
'radarState': {'extractor': extract_radar_state, 'rate': 'fast'},
|
||||
'liveTracks': {'extractor': extract_live_tracks, 'rate': 'fast'},
|
||||
'modelV2': {'extractor': extract_model_v2, 'rate': 'fast'},
|
||||
'longitudinalPlan': {'extractor': extract_longitudinal_plan, 'rate': 'fast'},
|
||||
'liveGPS': {'extractor': extract_live_gps, 'rate': 'fast'},
|
||||
|
||||
# Slow topics - poll at fixed intervals
|
||||
'deviceState': {'extractor': extract_device_state, 'rate': LOOP_RATE // 2},
|
||||
'liveCalibration': {'extractor': extract_live_calibration, 'rate': LOOP_RATE},
|
||||
'navInstruction': {'extractor': extract_nav_instruction, 'rate': LOOP_RATE},
|
||||
'navInstructionExt': {'extractor': extract_nav_instruction_ext, 'rate': LOOP_RATE},
|
||||
'navRoute': {'extractor': extract_nav_route, 'rate': LOOP_RATE},
|
||||
'carParams': {'extractor': extract_car_params, 'rate': LOOP_RATE * 2},
|
||||
|
||||
# Valid-only topics - just track valid state
|
||||
'roadCameraState': {'rate': 'valid', 'default': False},
|
||||
|
||||
# Subscribe-only topics - subscribed but extracted within other extractors
|
||||
'controlsState': {'rate': 'subscribe'},
|
||||
|
||||
# Optional/dragonpilot-specific topics - comment out to disable
|
||||
'controlsStateExt': {'extractor': extract_controls_state_ext, 'rate': 'fast', 'default': {'alkaActive': False}},
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
cloudlog.info("dashyd: starting")
|
||||
|
||||
# Initialize metric preference
|
||||
refresh_metric_preference()
|
||||
|
||||
# Derive services list from TOPICS config
|
||||
services = list(TOPICS.keys())
|
||||
sm = messaging.SubMaster(services)
|
||||
pm = messaging.PubMaster(['dashyState'])
|
||||
rk = Ratekeeper(LOOP_RATE)
|
||||
|
||||
# Initialize cache from TOPICS defaults (exclude subscribe-only topics)
|
||||
cache = {t: cfg.get('default') for t, cfg in TOPICS.items() if cfg.get('rate') != 'subscribe'}
|
||||
cache['carParams'] = get_car_params_from_params() # special: init from Params
|
||||
|
||||
# Build topic lists from TOPICS config
|
||||
fast_topics = {t: cfg['extractor'] for t, cfg in TOPICS.items() if cfg.get('rate') == 'fast'}
|
||||
slow_topics = {t: (cfg['extractor'], cfg['rate']) for t, cfg in TOPICS.items()
|
||||
if isinstance(cfg.get('rate'), int)}
|
||||
valid_topics = [t for t, cfg in TOPICS.items() if cfg.get('rate') == 'valid']
|
||||
|
||||
cache_dirty = True
|
||||
frame_count = 0
|
||||
|
||||
while True:
|
||||
sm.update(0)
|
||||
frame_count += 1
|
||||
|
||||
# Refresh metric preference every ~2 seconds
|
||||
if frame_count % (LOOP_RATE * 2) == 0:
|
||||
refresh_metric_preference()
|
||||
cache_dirty = True # Force re-format with new units
|
||||
|
||||
# Fast topics - extract when updated
|
||||
for topic, extractor in fast_topics.items():
|
||||
if sm.updated[topic]:
|
||||
cache[topic] = extractor(sm)
|
||||
cache_dirty = True
|
||||
|
||||
# Slow topics - extract at fixed intervals (ignore sm.updated)
|
||||
for topic, (extractor, divider) in slow_topics.items():
|
||||
if frame_count % divider == 0:
|
||||
cache[topic] = extractor(sm)
|
||||
cache_dirty = True
|
||||
|
||||
# Valid-only topics - just track valid state
|
||||
for topic in valid_topics:
|
||||
if sm.updated[topic]:
|
||||
new_val = sm.valid[topic]
|
||||
if cache[topic] != new_val:
|
||||
cache[topic] = new_val
|
||||
cache_dirty = True
|
||||
|
||||
# Only serialize and publish if something changed
|
||||
if cache_dirty:
|
||||
# Only publish when critical data exists (nav data can be null)
|
||||
critical_ready = (
|
||||
cache.get('carState') is not None and
|
||||
cache.get('modelV2') is not None and
|
||||
cache.get('selfdriveState') is not None
|
||||
)
|
||||
|
||||
if critical_ready:
|
||||
state = {
|
||||
'ts': sm.logMonoTime['carState'],
|
||||
'display': {
|
||||
'isMetric': _is_metric,
|
||||
'speedUnit': get_speed_unit(),
|
||||
'distanceUnit': get_distance_unit(),
|
||||
},
|
||||
**cache, # include all cached topics
|
||||
}
|
||||
msg = messaging.new_message('dashyState')
|
||||
msg.dashyState.json = json.dumps(state).encode()
|
||||
pm.send('dashyState', msg)
|
||||
|
||||
cache_dirty = False
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,423 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
GPS Location Service - GPS + livePose fusion with Kalman filter.
|
||||
- Position: 2D Kalman filter fusing GPS with livePose velocity
|
||||
- Bearing: livePose yaw + GPS-calibrated offset (with slow drift correction)
|
||||
"""
|
||||
import numpy as np
|
||||
from cereal import messaging, custom
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.common.transformations.coordinates import LocalCoord
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
|
||||
|
||||
def wrap_angle(x):
|
||||
return np.arctan2(np.sin(x), np.cos(x))
|
||||
|
||||
|
||||
class GPSKalman:
|
||||
"""
|
||||
3D Kalman filter for GPS fusion.
|
||||
State: [north, east, yaw_offset] where yaw_offset calibrates livePose to true north.
|
||||
Adapts automatically to GPS accuracy (ublox vs qcom).
|
||||
"""
|
||||
# Process noise
|
||||
POS_NOISE = 0.5 # m²/s - position uncertainty growth
|
||||
YAW_NOISE = 0.0001 # rad²/s - yaw offset drift (~0.6°/min)
|
||||
|
||||
def __init__(self):
|
||||
self.x = np.zeros(3) # [north, east, yaw_offset]
|
||||
self.P = np.diag([100.0, 100.0, 1.0]) # uncertainty (position high, yaw moderate)
|
||||
|
||||
def get_yaw(self, pose_yaw):
|
||||
"""Get calibrated yaw from pose yaw + estimated offset."""
|
||||
return wrap_angle(pose_yaw + self.x[2])
|
||||
|
||||
def predict(self, vel_ned, dt):
|
||||
"""Predict state using velocity from livePose."""
|
||||
# Position prediction
|
||||
self.x[0] += vel_ned[0] * dt
|
||||
self.x[1] += vel_ned[1] * dt
|
||||
# yaw_offset: no change (constant), just add process noise
|
||||
|
||||
# Process noise - more yaw drift when stopped (gyro drift accumulates)
|
||||
speed = np.linalg.norm(vel_ned[:2])
|
||||
yaw_noise = self.YAW_NOISE if speed > 1.0 else self.YAW_NOISE * 10
|
||||
Q = np.diag([self.POS_NOISE * dt, self.POS_NOISE * dt, yaw_noise * dt])
|
||||
self.P += Q
|
||||
|
||||
def update_position(self, gps_ned, gps_accuracy):
|
||||
"""Update position with GPS measurement."""
|
||||
# Observation matrix: observe [north, east], not yaw_offset
|
||||
H = np.array([[1, 0, 0],
|
||||
[0, 1, 0]])
|
||||
|
||||
# Measurement noise from GPS accuracy
|
||||
R = np.eye(2) * (gps_accuracy ** 2)
|
||||
|
||||
# Innovation
|
||||
z = gps_ned[:2]
|
||||
y = z - H @ self.x
|
||||
|
||||
# Kalman gain
|
||||
S = H @ self.P @ H.T + R
|
||||
det = S[0, 0] * S[1, 1] - S[0, 1] * S[1, 0]
|
||||
if abs(det) < 1e-10:
|
||||
return
|
||||
S_inv = np.array([[S[1, 1], -S[0, 1]],
|
||||
[-S[1, 0], S[0, 0]]]) / det
|
||||
K = self.P @ H.T @ S_inv
|
||||
|
||||
# Update
|
||||
self.x += K @ y
|
||||
self.P = (np.eye(3) - K @ H) @ self.P
|
||||
self._ensure_positive_definite()
|
||||
|
||||
def update_yaw(self, gps_bearing, pose_yaw, bearing_std):
|
||||
"""Update yaw_offset with GPS bearing measurement."""
|
||||
# Observation: yaw_offset = gps_bearing - pose_yaw
|
||||
# H = [0, 0, 1] - we observe yaw_offset directly
|
||||
H = np.array([[0, 0, 1]])
|
||||
|
||||
# Measurement noise from GPS bearing uncertainty
|
||||
R = np.array([[bearing_std ** 2]])
|
||||
|
||||
# Expected yaw_offset from GPS
|
||||
observed_offset = wrap_angle(gps_bearing - pose_yaw)
|
||||
|
||||
# Innovation (handle angle wrapping)
|
||||
predicted_offset = self.x[2]
|
||||
y = np.array([wrap_angle(observed_offset - predicted_offset)])
|
||||
|
||||
# Kalman gain
|
||||
S = H @ self.P @ H.T + R
|
||||
K = self.P @ H.T / S[0, 0]
|
||||
|
||||
# Update
|
||||
self.x += (K @ y).flatten()
|
||||
self.x[2] = wrap_angle(self.x[2]) # keep yaw_offset wrapped
|
||||
self.P = (np.eye(3) - K @ H) @ self.P
|
||||
self._ensure_positive_definite()
|
||||
|
||||
def _ensure_positive_definite(self):
|
||||
"""Ensure covariance stays positive definite. Reinit if corrupted."""
|
||||
self.P = (self.P + self.P.T) / 2
|
||||
if np.any(np.diag(self.P) < 0):
|
||||
cloudlog.warning("gpsd: negative covariance detected, reinitializing filter")
|
||||
self.P = np.diag([100.0, 100.0, 1.0])
|
||||
return
|
||||
min_var = np.array([0.01, 0.01, 0.0001]) # minimum variances
|
||||
for i in range(3):
|
||||
self.P[i, i] = max(self.P[i, i], min_var[i])
|
||||
|
||||
def reset(self, pos, yaw_offset=None):
|
||||
"""Reset to known position, optionally with yaw offset."""
|
||||
self.x[0] = pos[0]
|
||||
self.x[1] = pos[1]
|
||||
if yaw_offset is not None:
|
||||
self.x[2] = yaw_offset
|
||||
self.P = np.diag([1.0, 1.0, 0.1]) # low uncertainty
|
||||
else:
|
||||
self.P = np.diag([1.0, 1.0, self.P[2, 2]]) # keep yaw uncertainty
|
||||
|
||||
@property
|
||||
def pos(self):
|
||||
"""Position [north, east]."""
|
||||
return self.x[:2]
|
||||
|
||||
@property
|
||||
def yaw_offset(self):
|
||||
"""Yaw offset estimate."""
|
||||
return self.x[2]
|
||||
|
||||
@property
|
||||
def pos_uncertainty(self):
|
||||
"""Position uncertainty (meters)."""
|
||||
return np.sqrt(max(0.0, (self.P[0, 0] + self.P[1, 1]) / 2))
|
||||
|
||||
@property
|
||||
def yaw_uncertainty(self):
|
||||
"""Yaw offset uncertainty (radians)."""
|
||||
return np.sqrt(max(0.0, self.P[2, 2]))
|
||||
|
||||
|
||||
class LiveGPS:
|
||||
"""
|
||||
GPS + livePose fusion with 3D Kalman filter.
|
||||
- Position: Kalman filter fusing GPS with livePose velocity
|
||||
- Bearing: Kalman-estimated yaw_offset + livePose yaw
|
||||
"""
|
||||
GPS_MIN_SPEED = 5.0 # m/s (18 km/h) - need speed for reliable GPS bearing
|
||||
GPS_MAX_ACCURACY = 50.0 # m - reject very bad GPS
|
||||
BEARING_STD_BASE = 0.1 # rad (~6°) - base GPS bearing uncertainty
|
||||
BEARING_STD_PER_ACC = 0.02 # rad per meter of GPS accuracy
|
||||
|
||||
def __init__(self):
|
||||
# pose inputs
|
||||
self.orientation_ned = np.zeros(3)
|
||||
self.vel_device = np.zeros(3)
|
||||
|
||||
# gps inputs
|
||||
self.gps = None
|
||||
self.last_gps_t = 0.0
|
||||
self.unix_timestamp_millis = 0
|
||||
|
||||
# Kalman filter: [north, east, yaw_offset]
|
||||
self.origin = None # LocalCoord of first GPS fix
|
||||
self.kf = GPSKalman() # 3D Kalman filter
|
||||
self.altitude = 0.0 # altitude tracked separately (1D)
|
||||
self.last_gps_update_t = 0.0 # track when we last updated Kalman with GPS
|
||||
|
||||
# timing
|
||||
self.last_t = None
|
||||
self.last_pose_yaw = None # for yaw rate calculation
|
||||
self.live_pose_ok = False # for monitoring
|
||||
|
||||
# -----------------------------
|
||||
# inputs
|
||||
# -----------------------------
|
||||
|
||||
def handle_pose(self, pose):
|
||||
if pose.orientationNED.valid:
|
||||
self.orientation_ned[:] = [
|
||||
pose.orientationNED.x,
|
||||
pose.orientationNED.y,
|
||||
pose.orientationNED.z,
|
||||
]
|
||||
if pose.velocityDevice.valid:
|
||||
self.vel_device[:] = [
|
||||
pose.velocityDevice.x,
|
||||
pose.velocityDevice.y,
|
||||
pose.velocityDevice.z,
|
||||
]
|
||||
# For monitoring
|
||||
self.live_pose_ok = pose.orientationNED.valid and pose.velocityDevice.valid
|
||||
|
||||
def handle_gps(self, t, gps):
|
||||
if gps.horizontalAccuracy > 0 and gps.horizontalAccuracy > self.GPS_MAX_ACCURACY:
|
||||
return
|
||||
if abs(gps.latitude) < 0.1 or abs(gps.longitude) < 0.1:
|
||||
return
|
||||
|
||||
self.gps = gps
|
||||
self.last_gps_t = t
|
||||
self.unix_timestamp_millis = gps.unixTimestampMillis
|
||||
|
||||
# -----------------------------
|
||||
# core update
|
||||
# -----------------------------
|
||||
|
||||
def update(self, t):
|
||||
dt = (t - self.last_t) if self.last_t else 0.05
|
||||
self.last_t = t
|
||||
|
||||
if self.gps is None:
|
||||
return
|
||||
|
||||
# initialize origin on first GPS
|
||||
if self.origin is None:
|
||||
self.origin = LocalCoord.from_geodetic([self.gps.latitude, self.gps.longitude, self.gps.altitude])
|
||||
self.kf.reset(np.zeros(2))
|
||||
self.altitude = self.gps.altitude
|
||||
cloudlog.info(f"gpsd: origin set at {self.gps.latitude:.6f}, {self.gps.longitude:.6f}")
|
||||
return
|
||||
|
||||
# get current yaw from Kalman (pose_yaw + estimated yaw_offset)
|
||||
pose_yaw = self.orientation_ned[2]
|
||||
yaw = self.kf.get_yaw(pose_yaw)
|
||||
|
||||
# transform velocity from device frame to NED
|
||||
cos_yaw, sin_yaw = np.cos(yaw), np.sin(yaw)
|
||||
vel_ned = np.array([
|
||||
self.vel_device[0] * cos_yaw - self.vel_device[1] * sin_yaw,
|
||||
self.vel_device[0] * sin_yaw + self.vel_device[1] * cos_yaw,
|
||||
-self.vel_device[2]
|
||||
])
|
||||
|
||||
# Kalman predict: propagate position using livePose velocity
|
||||
# Skip prediction when stationary (GPS wanders, IMU drifts)
|
||||
# Threshold 0.1 m/s - above noise (~0.04) but catches actual stops
|
||||
speed = np.linalg.norm(self.vel_device[:2])
|
||||
is_moving = speed > 0.1
|
||||
if is_moving:
|
||||
self.kf.predict(vel_ned, dt)
|
||||
|
||||
# Kalman update: only on NEW GPS data (not stale)
|
||||
# Skip position update when stopped - prevents GPS wander from moving position
|
||||
new_gps = self.last_gps_t > self.last_gps_update_t
|
||||
if new_gps and is_moving:
|
||||
self.last_gps_update_t = self.last_gps_t
|
||||
gps_ned = self.origin.geodetic2ned([self.gps.latitude, self.gps.longitude, self.gps.altitude])
|
||||
gps_accuracy = self.gps.horizontalAccuracy if self.gps.horizontalAccuracy > 0 else 15.0
|
||||
|
||||
# Check for hard reset (large error after tunnel/GPS loss)
|
||||
error = np.linalg.norm(gps_ned[:2] - self.kf.pos)
|
||||
if error > 100.0 and gps_accuracy < 20.0:
|
||||
# GPS is good but we're far off - full reset (new origin, fresh Kalman)
|
||||
yaw_err_deg = np.degrees(self.kf.yaw_uncertainty)
|
||||
cloudlog.warning(f"gpsd: hard reset, error={error:.1f}m, yaw_unc={yaw_err_deg:.1f}°, gps_acc={gps_accuracy:.1f}m, livePoseOk={self.live_pose_ok}")
|
||||
# Reset origin to current GPS - starts fresh
|
||||
self.origin = LocalCoord.from_geodetic([self.gps.latitude, self.gps.longitude, self.gps.altitude])
|
||||
self.kf = GPSKalman() # fresh Kalman filter
|
||||
self.kf.reset(np.zeros(2)) # position (0,0) at new origin
|
||||
self.altitude = self.gps.altitude
|
||||
return # skip normal update this frame
|
||||
else:
|
||||
# Position update - adapts to GPS accuracy:
|
||||
# - ublox (2-5m): high gain, trusts GPS
|
||||
# - qcom (10-30m): low gain, trusts IMU more
|
||||
self.kf.update_position(gps_ned, gps_accuracy)
|
||||
|
||||
# simple altitude tracking (no Kalman needed)
|
||||
self.altitude = 0.9 * self.altitude + 0.1 * self.gps.altitude
|
||||
|
||||
# Yaw update (need speed for reliable GPS bearing)
|
||||
if self.gps.speed > self.GPS_MIN_SPEED:
|
||||
# compute yaw rate to check if driving straight
|
||||
yaw_rate = 0.0
|
||||
if self.last_pose_yaw is not None and dt > 0:
|
||||
yaw_rate = abs(wrap_angle(pose_yaw - self.last_pose_yaw)) / dt
|
||||
|
||||
# GPS bearing is unreliable during turns - increase uncertainty
|
||||
gps_bearing = np.radians(self.gps.bearingDeg)
|
||||
bearing_std = self.BEARING_STD_BASE + self.BEARING_STD_PER_ACC * gps_accuracy
|
||||
if yaw_rate > 0.1: # turning - GPS bearing lags
|
||||
bearing_std *= 3.0
|
||||
|
||||
self.kf.update_yaw(gps_bearing, pose_yaw, bearing_std)
|
||||
|
||||
self.last_pose_yaw = pose_yaw
|
||||
|
||||
# -----------------------------
|
||||
# output
|
||||
# -----------------------------
|
||||
|
||||
def get_msg(self, log_mono_time):
|
||||
msg = messaging.new_message("liveGPS", valid=True)
|
||||
msg.logMonoTime = log_mono_time
|
||||
out = msg.liveGPS
|
||||
|
||||
t = log_mono_time * 1e-9
|
||||
gps_fresh = self.gps is not None and (t - self.last_gps_t) < 5.0
|
||||
pos_initialized = self.origin is not None
|
||||
# yaw is calibrated when uncertainty < 0.5 rad (~30°)
|
||||
yaw_calibrated = self.kf.yaw_uncertainty < 0.5
|
||||
|
||||
if pos_initialized:
|
||||
# position from Kalman filter (NED -> geodetic)
|
||||
pos_ned = np.array([self.kf.pos[0], self.kf.pos[1], self.altitude])
|
||||
geodetic = self.origin.ned2geodetic(pos_ned)
|
||||
out.latitude = float(geodetic[0])
|
||||
out.longitude = float(geodetic[1])
|
||||
out.altitude = float(geodetic[2])
|
||||
out.speed = float(np.linalg.norm(self.vel_device[:2]))
|
||||
|
||||
# horizontalAccuracy from Kalman uncertainty
|
||||
out.horizontalAccuracy = float(self.kf.pos_uncertainty)
|
||||
out.verticalAccuracy = float(self.gps.verticalAccuracy) if hasattr(self.gps, 'verticalAccuracy') and self.gps.verticalAccuracy > 0 else 15.0
|
||||
|
||||
# bearing from Kalman (pose_yaw + estimated yaw_offset)
|
||||
has_livePose = np.any(self.orientation_ned != 0)
|
||||
if yaw_calibrated and has_livePose and gps_fresh:
|
||||
yaw = self.kf.get_yaw(self.orientation_ned[2])
|
||||
out.bearingDeg = float(np.degrees(yaw) % 360)
|
||||
out.status = custom.LiveGPS.Status.valid
|
||||
else:
|
||||
# fallback to raw GPS bearing
|
||||
out.bearingDeg = float(self.gps.bearingDeg)
|
||||
out.status = custom.LiveGPS.Status.uncalibrated
|
||||
elif self.gps is not None:
|
||||
# have GPS but not initialized yet - pass through raw
|
||||
out.latitude = float(self.gps.latitude)
|
||||
out.longitude = float(self.gps.longitude)
|
||||
out.altitude = float(self.gps.altitude)
|
||||
out.speed = float(self.gps.speed)
|
||||
out.bearingDeg = float(self.gps.bearingDeg)
|
||||
out.horizontalAccuracy = float(self.gps.horizontalAccuracy) if self.gps.horizontalAccuracy > 0 else 20.0
|
||||
out.status = custom.LiveGPS.Status.uncalibrated
|
||||
else:
|
||||
out.status = custom.LiveGPS.Status.uninitialized
|
||||
|
||||
# gpsOK = position is usable (bearing calibration tracked separately via status)
|
||||
out.gpsOK = gps_fresh and pos_initialized
|
||||
out.unixTimestampMillis = self.unix_timestamp_millis
|
||||
out.lastGpsTimestamp = int(self.last_gps_t * 1e9)
|
||||
|
||||
# livePose health - for monitoring
|
||||
out.livePoseOk = self.live_pose_ok
|
||||
|
||||
return msg
|
||||
|
||||
def main():
|
||||
import os
|
||||
params = Params()
|
||||
|
||||
# EXT=1 forces gpsLocationExternal (ublox), EXT=0 forces gpsLocation (qcom)
|
||||
ext_override = os.environ.get("EXT")
|
||||
if ext_override == "1":
|
||||
gps_service = "gpsLocationExternal"
|
||||
cloudlog.info("gpsd: EXT=1, using gpsLocationExternal (ublox)")
|
||||
elif ext_override == "0":
|
||||
gps_service = "gpsLocation"
|
||||
cloudlog.info("gpsd: EXT=0, using gpsLocation (qcom)")
|
||||
else:
|
||||
gps_service = get_gps_location_service(params)
|
||||
|
||||
pm = messaging.PubMaster(["liveGPS"])
|
||||
sm = messaging.SubMaster([gps_service, "livePose"], ignore_alive=[gps_service])
|
||||
|
||||
gps = LiveGPS()
|
||||
rk = Ratekeeper(20)
|
||||
|
||||
while True:
|
||||
try:
|
||||
sm.update(0)
|
||||
|
||||
if sm.logMonoTime["livePose"] > 0:
|
||||
t = sm.logMonoTime["livePose"] * 1e-9
|
||||
log_mono_time = sm.logMonoTime["livePose"]
|
||||
else:
|
||||
log_mono_time = int(rk.frame * 1e9 / 20)
|
||||
t = log_mono_time * 1e-9
|
||||
|
||||
if sm.updated[gps_service]:
|
||||
gps.handle_gps(t, sm[gps_service])
|
||||
|
||||
if sm.updated["livePose"] and sm.valid["livePose"]:
|
||||
gps.handle_pose(sm["livePose"])
|
||||
|
||||
gps.update(t)
|
||||
pm.send("liveGPS", gps.get_msg(log_mono_time))
|
||||
|
||||
except Exception:
|
||||
cloudlog.exception("gpsd: error in main loop")
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,30 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
dragonpilot Map-Aware Assist (MAA)
|
||||
|
||||
Navigation-assisted driving module that provides:
|
||||
- Route tracking and turn-by-turn navigation
|
||||
- Turn speed limits based on road curvature
|
||||
- Blinker-based turn desire for lane changes
|
||||
- Physics-based acceleration limiting
|
||||
"""
|
||||
@@ -1,657 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Navigation helpers for dragonpilot MAA.
|
||||
|
||||
Coordinate math, route parsing, and curvature computation utilities.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from typing import Any, cast
|
||||
|
||||
from opendbc.car.common.conversions import Conversions
|
||||
from openpilot.common.params import Params
|
||||
|
||||
DIRECTIONS = ('left', 'right', 'straight')
|
||||
MODIFIABLE_DIRECTIONS = ('left', 'right')
|
||||
EARTH_MEAN_RADIUS = 6371007.2
|
||||
|
||||
# Speed unit conversions to m/s
|
||||
SPEED_CONVERSIONS = {
|
||||
'km/h': Conversions.KPH_TO_MS,
|
||||
'mph': Conversions.MPH_TO_MS,
|
||||
}
|
||||
|
||||
|
||||
class Coordinate:
|
||||
def __init__(self, latitude: float, longitude: float) -> None:
|
||||
self.latitude = latitude
|
||||
self.longitude = longitude
|
||||
self.annotations: dict[str, float] = {}
|
||||
|
||||
@classmethod
|
||||
def from_mapbox_tuple(cls, t: tuple[float, float]) -> Coordinate:
|
||||
return cls(t[1], t[0])
|
||||
|
||||
@classmethod
|
||||
def from_osrm_tuple(cls, t: list[float]) -> Coordinate:
|
||||
"""OSRM uses [lon, lat] order."""
|
||||
return cls(t[1], t[0])
|
||||
|
||||
def as_dict(self) -> dict[str, float]:
|
||||
return {'latitude': self.latitude, 'longitude': self.longitude}
|
||||
|
||||
def __str__(self) -> str:
|
||||
return f'Coordinate({self.latitude}, {self.longitude})'
|
||||
|
||||
def __repr__(self) -> str:
|
||||
return self.__str__()
|
||||
|
||||
def __eq__(self, other) -> bool:
|
||||
if not isinstance(other, Coordinate):
|
||||
return False
|
||||
return self.latitude == other.latitude and self.longitude == other.longitude
|
||||
|
||||
def __hash__(self) -> int:
|
||||
return hash((self.latitude, self.longitude))
|
||||
|
||||
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, c: float) -> Coordinate:
|
||||
return Coordinate(self.latitude * c, self.longitude * c)
|
||||
|
||||
def dot(self, other: Coordinate) -> float:
|
||||
return self.latitude * other.latitude + self.longitude * other.longitude
|
||||
|
||||
def distance_to(self, other: Coordinate) -> float:
|
||||
"""Haversine distance in meters."""
|
||||
dlat = math.radians(other.latitude - self.latitude)
|
||||
dlon = math.radians(other.longitude - self.longitude)
|
||||
|
||||
haversine_dlat = math.sin(dlat / 2.0)
|
||||
haversine_dlat *= haversine_dlat
|
||||
haversine_dlon = math.sin(dlon / 2.0)
|
||||
haversine_dlon *= haversine_dlon
|
||||
|
||||
y = haversine_dlat \
|
||||
+ math.cos(math.radians(self.latitude)) \
|
||||
* math.cos(math.radians(other.latitude)) \
|
||||
* haversine_dlon
|
||||
x = 2 * math.asin(math.sqrt(y))
|
||||
return x * EARTH_MEAN_RADIUS
|
||||
|
||||
def bearing_to(self, other: Coordinate) -> float:
|
||||
"""Bearing to other coordinate in degrees (0-360)."""
|
||||
lat1, lat2 = math.radians(self.latitude), math.radians(other.latitude)
|
||||
dlon = math.radians(other.longitude - self.longitude)
|
||||
x = math.sin(dlon) * math.cos(lat2)
|
||||
y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
|
||||
bearing = math.degrees(math.atan2(x, y))
|
||||
return (bearing + 360) % 360
|
||||
|
||||
|
||||
def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate) -> float:
|
||||
"""Minimum distance from point p to line segment ab."""
|
||||
if a.distance_to(b) < 0.01:
|
||||
return a.distance_to(p)
|
||||
|
||||
ap = p - a
|
||||
ab = b - a
|
||||
t = max(0.0, min(1.0, ap.dot(ab) / ab.dot(ab)))
|
||||
projection = a + ab * t
|
||||
return projection.distance_to(p)
|
||||
|
||||
|
||||
def project_onto_segment(a: Coordinate, b: Coordinate, p: Coordinate) -> tuple[Coordinate, float, float]:
|
||||
"""Project point p onto line segment ab (snap to road).
|
||||
|
||||
Returns:
|
||||
(projected_point, t, distance) where:
|
||||
- projected_point: the closest point on segment ab to p
|
||||
- t: parameter 0-1 indicating position along segment (0=a, 1=b)
|
||||
- distance: distance from p to the projected point
|
||||
"""
|
||||
seg_len = a.distance_to(b)
|
||||
if seg_len < 0.01:
|
||||
return a, 0.0, a.distance_to(p)
|
||||
|
||||
ap = p - a
|
||||
ab = b - a
|
||||
t = max(0.0, min(1.0, ap.dot(ab) / ab.dot(ab)))
|
||||
projection = a + ab * t
|
||||
return projection, t, projection.distance_to(p)
|
||||
|
||||
|
||||
def distance_along_geometry(geometry: list[Coordinate], pos: Coordinate) -> float:
|
||||
"""Calculate distance traveled along geometry from start to closest point to pos."""
|
||||
if len(geometry) <= 2:
|
||||
return geometry[0].distance_to(pos)
|
||||
|
||||
total_distance = 0.0
|
||||
total_distance_closest = 0.0
|
||||
closest_distance = 1e9
|
||||
|
||||
for i in range(len(geometry) - 1):
|
||||
d = minimum_distance(geometry[i], geometry[i + 1], pos)
|
||||
|
||||
if d < closest_distance:
|
||||
closest_distance = d
|
||||
total_distance_closest = total_distance + geometry[i].distance_to(pos)
|
||||
|
||||
total_distance += geometry[i].distance_to(geometry[i + 1])
|
||||
|
||||
return total_distance_closest
|
||||
|
||||
|
||||
def normalize_angle(angle: float) -> float:
|
||||
"""Normalize angle to -180 to 180 degrees."""
|
||||
while angle > 180:
|
||||
angle -= 360
|
||||
while angle < -180:
|
||||
angle += 360
|
||||
return angle
|
||||
|
||||
|
||||
def calculate_turn_angle(
|
||||
current_geometry: list[Coordinate],
|
||||
next_geometry: list[Coordinate],
|
||||
samples: int = 3
|
||||
) -> float:
|
||||
"""
|
||||
Calculate turn angle between two road segments.
|
||||
|
||||
Uses the bearing of the end of current geometry vs the bearing
|
||||
of the start of next geometry to determine turn angle.
|
||||
|
||||
Args:
|
||||
current_geometry: Coordinates of current road segment (before turn)
|
||||
next_geometry: Coordinates of next road segment (after turn)
|
||||
samples: Number of points to use for bearing calculation (for stability)
|
||||
|
||||
Returns:
|
||||
Turn angle in degrees. Positive = left turn, negative = right turn.
|
||||
Example: 90 = 90° left turn, -90 = 90° right turn
|
||||
"""
|
||||
if len(current_geometry) < 2 or len(next_geometry) < 2:
|
||||
return 0.0
|
||||
|
||||
# Get bearing of current road (last segment)
|
||||
# Use last few points for stability
|
||||
start_idx = max(0, len(current_geometry) - samples)
|
||||
current_bearing = current_geometry[start_idx].bearing_to(current_geometry[-1])
|
||||
|
||||
# Get bearing of next road (first segment)
|
||||
# Use first few points for stability
|
||||
end_idx = min(samples, len(next_geometry) - 1)
|
||||
next_bearing = next_geometry[0].bearing_to(next_geometry[end_idx])
|
||||
|
||||
# Calculate turn angle (positive = left, negative = right)
|
||||
# This matches openpilot convention where left is positive curvature
|
||||
angle = normalize_angle(current_bearing - next_bearing)
|
||||
|
||||
return angle
|
||||
|
||||
|
||||
def coordinate_from_param(key: str, params: Params = None) -> Coordinate | None:
|
||||
"""Read coordinate from params.
|
||||
|
||||
Handles both JSON type params (returns dict) and STRING type params (returns string).
|
||||
"""
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
try:
|
||||
value = params.get(key)
|
||||
if value is None:
|
||||
return None
|
||||
|
||||
# JSON type params return dict directly, STRING type needs json.loads()
|
||||
if isinstance(value, str):
|
||||
pos = json.loads(value)
|
||||
else:
|
||||
pos = value
|
||||
|
||||
if 'latitude' not in pos or 'longitude' not in pos:
|
||||
return None
|
||||
|
||||
return Coordinate(pos['latitude'], pos['longitude'])
|
||||
except (json.JSONDecodeError, KeyError, TypeError):
|
||||
return None
|
||||
|
||||
|
||||
def find_closest_point_on_route(pos: Coordinate, route_coords: list[Coordinate]) -> tuple[int, float]:
|
||||
"""Find closest point index and distance on route."""
|
||||
if not route_coords:
|
||||
return 0, float('inf')
|
||||
|
||||
min_dist = float('inf')
|
||||
closest_idx = 0
|
||||
|
||||
for i in range(len(route_coords) - 1):
|
||||
# Check distance to segment, not just point
|
||||
d = minimum_distance(route_coords[i], route_coords[i + 1], pos)
|
||||
if d < min_dist:
|
||||
min_dist = d
|
||||
closest_idx = i
|
||||
|
||||
return closest_idx, min_dist
|
||||
|
||||
|
||||
def calculate_remaining_distance(route_coords: list[Coordinate], from_idx: int) -> float:
|
||||
"""Calculate remaining distance along route from index."""
|
||||
if from_idx >= len(route_coords) - 1:
|
||||
return 0.0
|
||||
|
||||
total = 0.0
|
||||
for i in range(from_idx, len(route_coords) - 1):
|
||||
total += route_coords[i].distance_to(route_coords[i + 1])
|
||||
return total
|
||||
|
||||
|
||||
# --- Instruction Parsing ---
|
||||
|
||||
def string_to_direction(direction: str) -> str:
|
||||
"""Convert direction string to standard format."""
|
||||
for d in DIRECTIONS:
|
||||
if d in direction:
|
||||
if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
|
||||
return 'slight' + d.capitalize()
|
||||
return d
|
||||
return 'none'
|
||||
|
||||
|
||||
def maxspeed_to_ms(maxspeed: dict[str, str | float]) -> float:
|
||||
"""Convert speed limit dict to m/s."""
|
||||
unit = cast(str, maxspeed['unit'])
|
||||
speed = cast(float, maxspeed['speed'])
|
||||
return SPEED_CONVERSIONS.get(unit, 1.0) * speed
|
||||
|
||||
|
||||
def field_valid(dat: dict, field: str) -> bool:
|
||||
"""Check if field exists and is not None."""
|
||||
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:
|
||||
"""Parse Mapbox/OSRM banner instructions."""
|
||||
if not banners or not len(banners):
|
||||
return None
|
||||
|
||||
instruction = {}
|
||||
|
||||
# A segment can contain multiple banners, find one that we need to show now
|
||||
current_banner = banners[0]
|
||||
for banner in banners:
|
||||
if distance_to_maneuver < banner.get('distanceAlongGeometry', 0):
|
||||
current_banner = banner
|
||||
|
||||
# Only show banner when close enough to maneuver
|
||||
instruction['showFull'] = distance_to_maneuver < current_banner.get('distanceAlongGeometry', 0)
|
||||
|
||||
# Primary
|
||||
p = current_banner.get('primary', {})
|
||||
if field_valid(p, 'text'):
|
||||
instruction['maneuverPrimaryText'] = p['text']
|
||||
if field_valid(p, 'type'):
|
||||
instruction['maneuverType'] = p['type']
|
||||
if field_valid(p, 'modifier'):
|
||||
instruction['maneuverModifier'] = p['modifier']
|
||||
|
||||
# Secondary
|
||||
if field_valid(current_banner, 'secondary'):
|
||||
instruction['maneuverSecondaryText'] = current_banner['secondary']['text']
|
||||
|
||||
# Lane lines
|
||||
if field_valid(current_banner, 'sub'):
|
||||
lanes = []
|
||||
for component in current_banner['sub'].get('components', []):
|
||||
if component.get('type') != 'lane':
|
||||
continue
|
||||
|
||||
lane = {
|
||||
'active': component.get('active', False),
|
||||
'directions': [string_to_direction(d) for d in component.get('directions', [])],
|
||||
}
|
||||
|
||||
if field_valid(component, 'active_direction'):
|
||||
lane['activeDirection'] = string_to_direction(component['active_direction'])
|
||||
|
||||
lanes.append(lane)
|
||||
instruction['lanes'] = lanes
|
||||
|
||||
return instruction
|
||||
|
||||
|
||||
def parse_osrm_step(step: dict) -> dict[str, Any]:
|
||||
"""Parse OSRM route step into instruction format."""
|
||||
maneuver = step.get('maneuver', {})
|
||||
instruction = {
|
||||
'distance': step.get('distance', 0),
|
||||
'duration': step.get('duration', 0),
|
||||
'name': step.get('name', ''),
|
||||
'maneuverType': maneuver.get('type', ''),
|
||||
'maneuverModifier': maneuver.get('modifier', ''),
|
||||
'location': maneuver.get('location', []), # [lon, lat]
|
||||
}
|
||||
return instruction
|
||||
|
||||
|
||||
def classify_maneuver(maneuver_type: str, maneuver_modifier: str) -> str:
|
||||
"""
|
||||
Classify OSRM maneuver as 'turn' or 'laneChange'.
|
||||
|
||||
Highway exits/forks use laneChange desires (smoother).
|
||||
Intersection turns use turn desires (sharper).
|
||||
|
||||
OSRM maneuver types:
|
||||
- turn: regular intersection turn
|
||||
- fork: highway split/junction
|
||||
- off ramp: highway exit
|
||||
- on ramp: highway entrance
|
||||
- merge: merging lanes
|
||||
- roundabout turn: roundabout exit
|
||||
- exit roundabout: leaving roundabout
|
||||
- continue: straight (no maneuver)
|
||||
- depart/arrive: start/end
|
||||
|
||||
Returns:
|
||||
'turn' for intersection turns
|
||||
'laneChange' for highway exits/forks
|
||||
'none' for straight/no maneuver
|
||||
"""
|
||||
maneuver_type = maneuver_type.lower()
|
||||
maneuver_modifier = maneuver_modifier.lower()
|
||||
|
||||
# Highway exits and forks -> lane change desire
|
||||
LANE_CHANGE_TYPES = {
|
||||
'fork', # highway fork/split
|
||||
'off ramp', # highway exit
|
||||
'on ramp', # highway entrance
|
||||
'merge', # merging
|
||||
'exit rotary', # leaving rotary
|
||||
'exit roundabout',# leaving roundabout
|
||||
}
|
||||
|
||||
# Intersection turns -> turn desire
|
||||
TURN_TYPES = {
|
||||
'turn', # regular turn
|
||||
'end of road', # forced turn at end of road
|
||||
'rotary', # entering rotary
|
||||
'roundabout', # entering roundabout
|
||||
'roundabout turn',# turn within roundabout
|
||||
}
|
||||
|
||||
# No maneuver
|
||||
NO_MANEUVER_TYPES = {
|
||||
'continue',
|
||||
'depart',
|
||||
'arrive',
|
||||
'new name',
|
||||
'notification',
|
||||
}
|
||||
|
||||
if maneuver_type in LANE_CHANGE_TYPES:
|
||||
return 'laneChange'
|
||||
|
||||
if maneuver_type in TURN_TYPES:
|
||||
# For turns, check modifier - slight turns at highway speeds might be lane changes
|
||||
if 'slight' in maneuver_modifier:
|
||||
# Slight turns could be either - default to turn but could be lane change
|
||||
# CarrotPilot uses additional context like road speed limit
|
||||
return 'turn'
|
||||
return 'turn'
|
||||
|
||||
if maneuver_type in NO_MANEUVER_TYPES:
|
||||
return 'none'
|
||||
|
||||
# Unknown type - default to turn
|
||||
return 'turn'
|
||||
|
||||
|
||||
def get_turn_direction(maneuver_modifier: str) -> str:
|
||||
"""
|
||||
Get turn direction from OSRM maneuver modifier.
|
||||
|
||||
Returns:
|
||||
'left', 'right', or 'none'
|
||||
"""
|
||||
modifier = maneuver_modifier.lower()
|
||||
if 'left' in modifier:
|
||||
return 'left'
|
||||
if 'right' in modifier:
|
||||
return 'right'
|
||||
return 'none'
|
||||
|
||||
|
||||
# --- Curvature Computation ---
|
||||
|
||||
def compute_path_curvature(pos: Coordinate, bearing: float, route_coords: list[Coordinate],
|
||||
closest_idx: int, v_ego: float, lookahead_time: float = 2.5) -> float:
|
||||
"""
|
||||
Compute desired curvature from route geometry using pure pursuit.
|
||||
|
||||
Args:
|
||||
pos: Current position
|
||||
bearing: Current heading in degrees
|
||||
route_coords: List of route coordinates
|
||||
closest_idx: Index of closest point on route
|
||||
v_ego: Current vehicle speed m/s
|
||||
lookahead_time: How far ahead to look in seconds
|
||||
|
||||
Returns:
|
||||
Desired curvature in 1/m (positive = left turn, negative = right turn)
|
||||
"""
|
||||
if not route_coords or closest_idx >= len(route_coords) - 1:
|
||||
return 0.0
|
||||
|
||||
# Calculate lookahead distance (min 30m, based on speed)
|
||||
lookahead_dist = max(v_ego * lookahead_time, 30.0)
|
||||
|
||||
# Find lookahead point along route
|
||||
dist_traveled = 0.0
|
||||
lookahead_idx = closest_idx
|
||||
|
||||
for i in range(closest_idx, len(route_coords) - 1):
|
||||
segment_dist = route_coords[i].distance_to(route_coords[i + 1])
|
||||
if dist_traveled + segment_dist >= lookahead_dist:
|
||||
# Interpolate within segment
|
||||
remaining = lookahead_dist - dist_traveled
|
||||
ratio = remaining / segment_dist if segment_dist > 0 else 0
|
||||
lookahead_idx = i
|
||||
# Could interpolate here, but using next point is simpler
|
||||
if ratio > 0.5:
|
||||
lookahead_idx = i + 1
|
||||
break
|
||||
dist_traveled += segment_dist
|
||||
lookahead_idx = i + 1
|
||||
|
||||
if lookahead_idx >= len(route_coords):
|
||||
lookahead_idx = len(route_coords) - 1
|
||||
|
||||
lookahead_point = route_coords[lookahead_idx]
|
||||
|
||||
# Calculate desired heading to lookahead point
|
||||
desired_bearing = pos.bearing_to(lookahead_point)
|
||||
|
||||
# Calculate heading error (normalized to -180 to 180)
|
||||
heading_error = desired_bearing - bearing
|
||||
if heading_error > 180:
|
||||
heading_error -= 360
|
||||
elif heading_error < -180:
|
||||
heading_error += 360
|
||||
|
||||
# Convert heading error to yaw (radians)
|
||||
yaw_error = math.radians(heading_error)
|
||||
|
||||
# Distance to lookahead point
|
||||
dist_to_lookahead = pos.distance_to(lookahead_point)
|
||||
if dist_to_lookahead < 1.0:
|
||||
return 0.0
|
||||
|
||||
# Pure pursuit curvature: 2 * sin(yaw_error) / lookahead_distance
|
||||
# Note: Negative because heading_error > 0 means target is to the RIGHT (clockwise),
|
||||
# and right turn should produce negative curvature
|
||||
curvature = -2.0 * math.sin(yaw_error) / dist_to_lookahead
|
||||
|
||||
# Clamp to reasonable values (max ~7m radius turn)
|
||||
MAX_CURVATURE = 0.15
|
||||
return max(-MAX_CURVATURE, min(MAX_CURVATURE, curvature))
|
||||
|
||||
|
||||
def smooth_curvature(new_curv: float, prev_curv: float, alpha: float = 0.3) -> float:
|
||||
"""Exponential smoothing for curvature."""
|
||||
return alpha * new_curv + (1 - alpha) * prev_curv
|
||||
|
||||
|
||||
def curvature_to_radius(curvature: float) -> float:
|
||||
"""Convert curvature to turn radius in meters."""
|
||||
if abs(curvature) < 0.001:
|
||||
return float('inf')
|
||||
return 1.0 / abs(curvature)
|
||||
|
||||
|
||||
def compute_turn_angle_at_index(route_coords: list[Coordinate], turn_idx: int,
|
||||
sample_dist: float = 20.0) -> float:
|
||||
"""
|
||||
Compute turn angle at a specific point on the route.
|
||||
Uses points sample_dist meters before and after for stability.
|
||||
Returns angle in degrees (positive = left, negative = right).
|
||||
"""
|
||||
if turn_idx < 1 or turn_idx >= len(route_coords) - 1:
|
||||
return 0.0
|
||||
|
||||
# Find points ~sample_dist before and after the turn for stable bearing
|
||||
before_idx = turn_idx
|
||||
after_idx = turn_idx
|
||||
|
||||
# Walk backwards to find before point
|
||||
dist = 0.0
|
||||
for i in range(turn_idx, 0, -1):
|
||||
dist += route_coords[i].distance_to(route_coords[i - 1])
|
||||
if dist >= sample_dist:
|
||||
before_idx = i - 1
|
||||
break
|
||||
else:
|
||||
before_idx = 0
|
||||
|
||||
# Walk forwards to find after point
|
||||
dist = 0.0
|
||||
for i in range(turn_idx, len(route_coords) - 1):
|
||||
dist += route_coords[i].distance_to(route_coords[i + 1])
|
||||
if dist >= sample_dist:
|
||||
after_idx = i + 1
|
||||
break
|
||||
else:
|
||||
after_idx = len(route_coords) - 1
|
||||
|
||||
if before_idx == turn_idx or after_idx == turn_idx:
|
||||
return 0.0
|
||||
|
||||
# Calculate bearings using the sampled points
|
||||
p1 = route_coords[before_idx]
|
||||
p2 = route_coords[turn_idx]
|
||||
p3 = route_coords[after_idx]
|
||||
|
||||
bearing1 = p1.bearing_to(p2)
|
||||
bearing2 = p2.bearing_to(p3)
|
||||
|
||||
# Angle difference (positive = left, negative = right)
|
||||
angle = bearing1 - bearing2
|
||||
|
||||
# Normalize to -180 to 180
|
||||
while angle > 180:
|
||||
angle -= 360
|
||||
while angle < -180:
|
||||
angle += 360
|
||||
|
||||
return angle
|
||||
|
||||
|
||||
def compute_turn_curvature_at_index(route_coords: list[Coordinate], turn_idx: int,
|
||||
sample_dist: float = 15.0) -> float:
|
||||
"""
|
||||
Compute curvature at turn point using three-point circle fitting.
|
||||
Returns curvature in 1/m (positive = left, negative = right).
|
||||
"""
|
||||
if turn_idx < 1 or turn_idx >= len(route_coords) - 1:
|
||||
return 0.0
|
||||
|
||||
# Find points sample_dist before and after
|
||||
before_idx = turn_idx
|
||||
after_idx = turn_idx
|
||||
|
||||
dist = 0.0
|
||||
for i in range(turn_idx, 0, -1):
|
||||
dist += route_coords[i].distance_to(route_coords[i - 1])
|
||||
if dist >= sample_dist:
|
||||
before_idx = i - 1
|
||||
break
|
||||
|
||||
dist = 0.0
|
||||
for i in range(turn_idx, len(route_coords) - 1):
|
||||
dist += route_coords[i].distance_to(route_coords[i + 1])
|
||||
if dist >= sample_dist:
|
||||
after_idx = i + 1
|
||||
break
|
||||
|
||||
if before_idx == turn_idx or after_idx == turn_idx:
|
||||
return 0.0
|
||||
|
||||
# Three points for circle fitting
|
||||
p1 = route_coords[before_idx]
|
||||
p2 = route_coords[turn_idx]
|
||||
p3 = route_coords[after_idx]
|
||||
|
||||
# Convert to local meters (approximate)
|
||||
lat_center = p2.latitude
|
||||
lon_scale = math.cos(math.radians(lat_center))
|
||||
m_per_deg = 111319.5 # meters per degree latitude
|
||||
|
||||
x1 = (p1.longitude - p2.longitude) * lon_scale * m_per_deg
|
||||
y1 = (p1.latitude - p2.latitude) * m_per_deg
|
||||
x2 = 0.0
|
||||
y2 = 0.0
|
||||
x3 = (p3.longitude - p2.longitude) * lon_scale * m_per_deg
|
||||
y3 = (p3.latitude - p2.latitude) * m_per_deg
|
||||
|
||||
# Calculate curvature using cross product / (product of distances)
|
||||
d12 = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
|
||||
d13 = math.sqrt((x3 - x1)**2 + (y3 - y1)**2)
|
||||
d23 = math.sqrt((x3 - x2)**2 + (y3 - y2)**2)
|
||||
|
||||
if d12 < 0.1 or d13 < 0.1 or d23 < 0.1:
|
||||
return 0.0
|
||||
|
||||
# Cross product (p2-p1) x (p3-p1) - z component only
|
||||
cross = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1)
|
||||
|
||||
curvature = 2.0 * cross / (d12 * d13 * d23)
|
||||
|
||||
# Clamp to reasonable values
|
||||
MAX_CURVATURE = 0.2 # ~5m radius
|
||||
return max(-MAX_CURVATURE, min(MAX_CURVATURE, curvature))
|
||||
@@ -1,47 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
dragonpilot Map-Aware Assist Library
|
||||
|
||||
Core modules:
|
||||
- maa_desire: Simple turn desire logic (blinker confirmation, lane change blocking, RHD/LHD)
|
||||
- model_helper: Helper class for modeld integration
|
||||
- longitudinal_helper: Planner integration for speed/accel limiting
|
||||
"""
|
||||
|
||||
from dragonpilot.dashy.maa.lib.maa_desire import (
|
||||
should_block_lane_change,
|
||||
get_turn_desire,
|
||||
is_crossing_turn,
|
||||
get_turn_trigger_distance,
|
||||
)
|
||||
from dragonpilot.dashy.maa.lib.model_helper import ModelHelper
|
||||
from dragonpilot.dashy.maa.lib.longitudinal_helper import LongitudinalHelper
|
||||
|
||||
__all__ = [
|
||||
'should_block_lane_change',
|
||||
'get_turn_desire',
|
||||
'is_crossing_turn',
|
||||
'get_turn_trigger_distance',
|
||||
'ModelHelper',
|
||||
'LongitudinalHelper',
|
||||
]
|
||||
@@ -1,238 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Acceleration Limiter
|
||||
|
||||
Physics-based acceleration limiting for turns.
|
||||
|
||||
Algorithm adapted from CarrotPilot (https://github.com/ajouatom/openpilot)
|
||||
Credit: carrotpilot team for the physics-based lateral acceleration approach.
|
||||
|
||||
The key insight is that total acceleration is limited by tire grip:
|
||||
a_total² = a_x² + a_y² ≤ a_max²
|
||||
|
||||
Where:
|
||||
a_x = longitudinal acceleration (throttle/brake)
|
||||
a_y = lateral acceleration (from turning) = v² × curvature
|
||||
|
||||
This means during turns, we must reduce longitudinal acceleration
|
||||
to stay within the grip circle.
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Tuple, Optional
|
||||
|
||||
|
||||
@dataclass
|
||||
class AccelLimits:
|
||||
"""Acceleration limits."""
|
||||
min_accel: float # m/s² (negative = braking)
|
||||
max_accel: float # m/s² (positive = acceleration)
|
||||
|
||||
|
||||
@dataclass
|
||||
class AccelLimiterState:
|
||||
"""Current state of acceleration limiter."""
|
||||
v_ego: float = 0.0
|
||||
curvature: float = 0.0
|
||||
lateral_accel: float = 0.0
|
||||
available_long_accel: float = 0.0
|
||||
is_limiting: bool = False
|
||||
|
||||
|
||||
class AccelLimiter:
|
||||
"""
|
||||
Physics-based acceleration limiter for turns.
|
||||
|
||||
Uses the friction circle concept: total acceleration magnitude
|
||||
is limited by tire grip. During turns, lateral acceleration
|
||||
consumes part of this budget, leaving less for longitudinal
|
||||
acceleration.
|
||||
|
||||
Adapted from CarrotPilot's limit_accel_in_turns function.
|
||||
"""
|
||||
|
||||
# Default lateral acceleration limit (m/s²)
|
||||
# Comfortable limit is ~2-3 m/s², sporty is ~4 m/s²
|
||||
DEFAULT_LAT_ACCEL_MAX = 2.5
|
||||
|
||||
# Lookup table for max total acceleration vs speed
|
||||
# Higher speeds = lower max lateral accel for comfort
|
||||
A_TOTAL_MAX_BP = [0., 10., 20., 30., 40.] # m/s
|
||||
A_TOTAL_MAX_V = [3.0, 2.8, 2.5, 2.2, 2.0] # m/s²
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
lat_accel_max: float = DEFAULT_LAT_ACCEL_MAX,
|
||||
comfort_mode: bool = True
|
||||
):
|
||||
"""
|
||||
Initialize acceleration limiter.
|
||||
|
||||
Args:
|
||||
lat_accel_max: Maximum allowed lateral acceleration (m/s²)
|
||||
comfort_mode: If True, use speed-dependent limits
|
||||
"""
|
||||
self.lat_accel_max = lat_accel_max
|
||||
self.comfort_mode = comfort_mode
|
||||
self.state = AccelLimiterState()
|
||||
|
||||
def get_max_total_accel(self, v_ego: float) -> float:
|
||||
"""
|
||||
Get maximum total acceleration for current speed.
|
||||
|
||||
In comfort mode, reduces limit at higher speeds.
|
||||
"""
|
||||
if not self.comfort_mode:
|
||||
return self.lat_accel_max
|
||||
|
||||
# Interpolate from lookup table
|
||||
import numpy as np
|
||||
return np.interp(v_ego, self.A_TOTAL_MAX_BP, self.A_TOTAL_MAX_V)
|
||||
|
||||
def compute_lateral_accel(self, v_ego: float, curvature: float) -> float:
|
||||
"""
|
||||
Compute lateral acceleration from speed and curvature.
|
||||
|
||||
a_y = v² × κ
|
||||
|
||||
Args:
|
||||
v_ego: Vehicle speed in m/s
|
||||
curvature: Road curvature in 1/m
|
||||
|
||||
Returns:
|
||||
Lateral acceleration in m/s²
|
||||
"""
|
||||
return v_ego * v_ego * abs(curvature)
|
||||
|
||||
def compute_available_long_accel(
|
||||
self,
|
||||
v_ego: float,
|
||||
curvature: float,
|
||||
a_max: Optional[float] = None
|
||||
) -> float:
|
||||
"""
|
||||
Compute available longitudinal acceleration given current turn.
|
||||
|
||||
Uses friction circle: a_x² + a_y² ≤ a_max²
|
||||
Solving for a_x: a_x = sqrt(a_max² - a_y²)
|
||||
|
||||
Adapted from CarrotPilot's limit_accel_in_turns.
|
||||
|
||||
Args:
|
||||
v_ego: Vehicle speed in m/s
|
||||
curvature: Road curvature in 1/m
|
||||
a_max: Override for max total acceleration
|
||||
|
||||
Returns:
|
||||
Maximum available longitudinal acceleration in m/s²
|
||||
"""
|
||||
if a_max is None:
|
||||
a_max = self.get_max_total_accel(v_ego)
|
||||
|
||||
# Compute lateral acceleration
|
||||
a_y = self.compute_lateral_accel(v_ego, curvature)
|
||||
a_y_abs = abs(a_y)
|
||||
|
||||
# Update state
|
||||
self.state.v_ego = v_ego
|
||||
self.state.curvature = curvature
|
||||
self.state.lateral_accel = a_y
|
||||
|
||||
# Check if lateral accel exceeds limit
|
||||
if a_y_abs >= a_max:
|
||||
# Already at or over limit - no longitudinal accel available
|
||||
self.state.available_long_accel = 0.0
|
||||
self.state.is_limiting = True
|
||||
return 0.0
|
||||
|
||||
# Compute remaining budget for longitudinal acceleration
|
||||
a_x_available = math.sqrt(a_max * a_max - a_y_abs * a_y_abs)
|
||||
|
||||
self.state.available_long_accel = a_x_available
|
||||
self.state.is_limiting = a_x_available < a_max * 0.9 # Limiting if < 90% available
|
||||
|
||||
return a_x_available
|
||||
|
||||
def limit_accel(
|
||||
self,
|
||||
v_ego: float,
|
||||
curvature: float,
|
||||
accel_limits: AccelLimits,
|
||||
a_max: Optional[float] = None
|
||||
) -> AccelLimits:
|
||||
"""
|
||||
Apply turn-based acceleration limiting.
|
||||
|
||||
Args:
|
||||
v_ego: Vehicle speed in m/s
|
||||
curvature: Road curvature in 1/m
|
||||
accel_limits: Current acceleration limits
|
||||
a_max: Override for max total acceleration
|
||||
|
||||
Returns:
|
||||
New AccelLimits with turn limiting applied
|
||||
"""
|
||||
a_x_available = self.compute_available_long_accel(v_ego, curvature, a_max)
|
||||
|
||||
# Clamp max acceleration to available budget
|
||||
new_max = min(accel_limits.max_accel, a_x_available)
|
||||
|
||||
# Don't limit braking as much - we may need to slow down
|
||||
# But still apply some limit for comfort
|
||||
new_min = max(accel_limits.min_accel, -a_x_available * 1.5)
|
||||
|
||||
return AccelLimits(
|
||||
min_accel=new_min,
|
||||
max_accel=new_max
|
||||
)
|
||||
|
||||
def limit_accel_tuple(
|
||||
self,
|
||||
v_ego: float,
|
||||
curvature: float,
|
||||
accel_limits: Tuple[float, float],
|
||||
a_max: Optional[float] = None
|
||||
) -> Tuple[float, float]:
|
||||
"""
|
||||
Apply turn-based acceleration limiting (tuple interface).
|
||||
|
||||
For compatibility with existing planner code.
|
||||
|
||||
Args:
|
||||
v_ego: Vehicle speed in m/s
|
||||
curvature: Road curvature in 1/m
|
||||
accel_limits: (min_accel, max_accel) tuple
|
||||
a_max: Override for max total acceleration
|
||||
|
||||
Returns:
|
||||
(min_accel, max_accel) tuple with turn limiting applied
|
||||
"""
|
||||
limits = self.limit_accel(
|
||||
v_ego,
|
||||
curvature,
|
||||
AccelLimits(accel_limits[0], accel_limits[1]),
|
||||
a_max
|
||||
)
|
||||
return (limits.min_accel, limits.max_accel)
|
||||
@@ -1,360 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Map-Aware Assist Longitudinal Helper
|
||||
|
||||
Provides navigation-based speed and acceleration limiting for the longitudinal planner.
|
||||
This module keeps all nav-related planner logic isolated from the core planner code.
|
||||
|
||||
Features:
|
||||
- Turn speed limiting based on maaControl cereal message
|
||||
- Physics-based acceleration limiting using friction circle
|
||||
- Smooth slowdown/resume transitions
|
||||
- Driver acknowledgment (blinker) required to activate speed limiting
|
||||
|
||||
Adapted from CarrotPilot (https://github.com/ajouatom/openpilot)
|
||||
Credit: carrotpilot team for curvature-based speed and physics-based accel limiting.
|
||||
|
||||
Usage in longitudinal_planner.py:
|
||||
from dragonpilot.dashy.maa.lib.longitudinal_helper import LongitudinalHelper
|
||||
|
||||
# Add 'maaControl' to SubMaster
|
||||
# In update:
|
||||
maa = sm['maaControl']
|
||||
if sm.valid['maaControl'] and maa.turnValid and maa.speedLimitActive:
|
||||
# Only apply speed limit when driver acknowledged (blinker on)
|
||||
turn_speed = maa.turnSpeedLimit
|
||||
turn_distance = maa.turnDistance
|
||||
else:
|
||||
turn_speed, turn_distance = None, None
|
||||
v_cruise = self.maa_helper.apply_nav_speed_limit(v_cruise, turn_speed, turn_distance)
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional, List
|
||||
|
||||
# Import nav modules
|
||||
try:
|
||||
from dragonpilot.dashy.maa.lib.accel_limiter import AccelLimiter
|
||||
ACCEL_LIMITER_AVAILABLE = True
|
||||
except ImportError:
|
||||
ACCEL_LIMITER_AVAILABLE = False
|
||||
|
||||
|
||||
@dataclass
|
||||
class NavPlannerState:
|
||||
"""Current state of nav planner integration."""
|
||||
nav_limited_speed: Optional[float] = None
|
||||
is_limiting_speed: bool = False
|
||||
is_limiting_accel: bool = False
|
||||
|
||||
|
||||
@dataclass
|
||||
class VirtualLead:
|
||||
"""Virtual lead car for turn deceleration."""
|
||||
status: bool = False
|
||||
dRel: float = 100.0 # distance to virtual lead
|
||||
vLead: float = 0.0 # speed of virtual lead
|
||||
aLeadK: float = 0.0 # acceleration
|
||||
aLeadTau: float = 0.3
|
||||
|
||||
|
||||
class RadarStateWrapper:
|
||||
"""
|
||||
Wrapper to inject virtual lead into radarState for MPC.
|
||||
|
||||
This allows the MPC to "see" a fake slow car at the turn point,
|
||||
triggering natural deceleration using the well-tuned lead following logic.
|
||||
"""
|
||||
def __init__(self, radar_state, virtual_lead=None):
|
||||
self._radar_state = radar_state
|
||||
self._virtual_lead = virtual_lead
|
||||
|
||||
@property
|
||||
def leadOne(self):
|
||||
real_lead = self._radar_state.leadOne
|
||||
# If no virtual lead, use real lead
|
||||
if self._virtual_lead is None or not self._virtual_lead.status:
|
||||
return real_lead
|
||||
# If real lead exists and is closer, use real lead
|
||||
if real_lead.status and real_lead.dRel < self._virtual_lead.dRel:
|
||||
return real_lead
|
||||
# Use virtual lead (turn point)
|
||||
return self._virtual_lead
|
||||
|
||||
@property
|
||||
def leadTwo(self):
|
||||
return self._radar_state.leadTwo
|
||||
|
||||
def __getattr__(self, name):
|
||||
return getattr(self._radar_state, name)
|
||||
|
||||
|
||||
class LongitudinalHelper:
|
||||
"""
|
||||
Navigation helper for longitudinal planner.
|
||||
|
||||
Provides turn speed limiting and physics-based acceleration limiting
|
||||
based on navigation data. Keeps all nav logic isolated from core planner.
|
||||
|
||||
Design rationale (based on automotive research):
|
||||
- Stopping distance at 50 km/h is ~48m (25m braking + 15m reaction)
|
||||
- Intersection approach speeds typically 30-50 km/h
|
||||
- Comfortable deceleration: 1.5-2.0 m/s² for normal driving
|
||||
"""
|
||||
|
||||
# Configuration - Speed limiting
|
||||
# Physics-based: calculate braking distance from speed difference
|
||||
# More natural driving: maintain speed, brake closer to turn
|
||||
SLOWDOWN_END_DIST = 10.0 # meters - be at turn speed by this distance (buffer before turn)
|
||||
SLOWDOWN_BUFFER = 15.0 # meters - extra buffer added to braking distance
|
||||
|
||||
# Physics-based accel limiting
|
||||
ACCEL_LIMIT_ENABLED = True
|
||||
LAT_ACCEL_MAX = 2.5 # m/s² - max comfortable lateral acceleration
|
||||
|
||||
# Comfortable deceleration target (m/s²)
|
||||
# Research shows 1.5-2.0 m/s² is comfortable for passengers
|
||||
# Using 2.0 for more natural, later braking
|
||||
COMFORT_DECEL = 2.0
|
||||
|
||||
def __init__(self):
|
||||
"""Initialize nav planner helper."""
|
||||
self.state = NavPlannerState()
|
||||
|
||||
# Physics-based acceleration limiter
|
||||
if ACCEL_LIMITER_AVAILABLE and self.ACCEL_LIMIT_ENABLED:
|
||||
self.accel_limiter = AccelLimiter(
|
||||
lat_accel_max=self.LAT_ACCEL_MAX,
|
||||
comfort_mode=True
|
||||
)
|
||||
else:
|
||||
self.accel_limiter = None
|
||||
|
||||
def apply_nav_speed_limit(
|
||||
self,
|
||||
v_cruise: float,
|
||||
turn_speed: Optional[float] = None,
|
||||
distance: Optional[float] = None
|
||||
) -> float:
|
||||
"""
|
||||
Apply navigation-based speed limiting with physics-based braking.
|
||||
|
||||
Uses kinematic equation: d = (v² - v_target²) / (2 * decel)
|
||||
Starts braking at calculated distance + buffer for natural driving feel.
|
||||
|
||||
Args:
|
||||
v_cruise: Current cruise speed in m/s
|
||||
turn_speed: Turn speed limit from maaControl (None if invalid)
|
||||
distance: Distance to turn from maaControl (None if invalid)
|
||||
|
||||
Returns:
|
||||
Limited cruise speed
|
||||
"""
|
||||
if turn_speed is None or distance is None:
|
||||
self.state.nav_limited_speed = None
|
||||
self.state.is_limiting_speed = False
|
||||
return v_cruise
|
||||
|
||||
# No need to slow if already at or below turn speed
|
||||
if v_cruise <= turn_speed:
|
||||
self.state.nav_limited_speed = None
|
||||
self.state.is_limiting_speed = False
|
||||
return v_cruise
|
||||
|
||||
# Calculate required braking distance using kinematics
|
||||
# d = (v² - v_target²) / (2 * decel)
|
||||
speed_diff_sq = v_cruise ** 2 - turn_speed ** 2
|
||||
braking_distance = speed_diff_sq / (2 * self.COMFORT_DECEL)
|
||||
|
||||
# Start braking at: braking_distance + buffer + end_distance
|
||||
slowdown_start = braking_distance + self.SLOWDOWN_BUFFER + self.SLOWDOWN_END_DIST
|
||||
|
||||
# Outside slowdown zone - no limit
|
||||
if distance > slowdown_start:
|
||||
self.state.nav_limited_speed = None
|
||||
self.state.is_limiting_speed = False
|
||||
return v_cruise
|
||||
|
||||
# In slowdown zone - calculate target speed at this distance
|
||||
# v² = v_target² + 2 * decel * (distance - end_dist)
|
||||
if distance > self.SLOWDOWN_END_DIST:
|
||||
remaining = distance - self.SLOWDOWN_END_DIST
|
||||
# Target speed that allows comfortable braking to turn_speed
|
||||
target_speed_sq = turn_speed ** 2 + 2 * self.COMFORT_DECEL * remaining
|
||||
limited_speed = min(v_cruise, target_speed_sq ** 0.5)
|
||||
else:
|
||||
# Very close to turn - calculate achievable speed with comfort decel
|
||||
# This handles late blinker: never brake harder than COMFORT_DECEL
|
||||
# If we can't reach turn_speed in time, accept entering turn faster
|
||||
achievable_speed_sq = turn_speed ** 2 + 2 * self.COMFORT_DECEL * max(0, distance)
|
||||
achievable_speed = achievable_speed_sq ** 0.5
|
||||
# Never target lower than achievable (prevents harsh braking)
|
||||
limited_speed = min(v_cruise, max(turn_speed, achievable_speed))
|
||||
|
||||
self.state.nav_limited_speed = limited_speed
|
||||
self.state.is_limiting_speed = True
|
||||
return limited_speed
|
||||
|
||||
# Minimum distance for virtual lead - below this, don't use virtual lead
|
||||
# to avoid MPC braking to stop
|
||||
VIRTUAL_LEAD_MIN_DIST = 15.0 # meters
|
||||
|
||||
def get_virtual_lead(
|
||||
self,
|
||||
v_ego: float,
|
||||
turn_speed: Optional[float] = None,
|
||||
distance: Optional[float] = None
|
||||
) -> Optional[VirtualLead]:
|
||||
"""
|
||||
Create a virtual lead car at the turn point for natural deceleration.
|
||||
|
||||
The virtual lead "drives" at turn_speed, positioned at the turn point.
|
||||
This makes the MPC decelerate naturally as if following a slow car.
|
||||
|
||||
Args:
|
||||
v_ego: Current vehicle speed in m/s
|
||||
turn_speed: Turn speed limit from maaControl (None if invalid)
|
||||
distance: Distance to turn from maaControl (None if invalid)
|
||||
|
||||
Returns:
|
||||
VirtualLead object if turn is approaching, None otherwise
|
||||
"""
|
||||
if turn_speed is None or distance is None:
|
||||
return None
|
||||
|
||||
# Only create virtual lead when we need to slow down
|
||||
if v_ego <= turn_speed:
|
||||
return None
|
||||
|
||||
# Don't use virtual lead when very close to turn - avoids brake-to-stop
|
||||
# At this point, we should already be at turn speed from earlier braking
|
||||
if distance < self.VIRTUAL_LEAD_MIN_DIST:
|
||||
return None
|
||||
|
||||
# Calculate when to start showing virtual lead
|
||||
# Use same physics: braking distance + buffer
|
||||
speed_diff_sq = v_ego ** 2 - turn_speed ** 2
|
||||
braking_distance = speed_diff_sq / (2 * self.COMFORT_DECEL)
|
||||
activation_distance = braking_distance + self.SLOWDOWN_BUFFER + self.SLOWDOWN_END_DIST
|
||||
|
||||
if distance > activation_distance:
|
||||
return None
|
||||
|
||||
# Create virtual lead at turn point, moving at turn speed
|
||||
# The MPC will naturally decelerate to follow it
|
||||
return VirtualLead(
|
||||
status=True,
|
||||
dRel=distance, # distance to virtual lead
|
||||
vLead=turn_speed, # virtual lead moves at turn speed
|
||||
aLeadK=0.0, # no acceleration (constant speed)
|
||||
aLeadTau=0.3 # response time constant
|
||||
)
|
||||
|
||||
def apply_nav_accel_limit(
|
||||
self,
|
||||
v_ego: float,
|
||||
curvature: float,
|
||||
accel_clip: List[float]
|
||||
) -> List[float]:
|
||||
"""
|
||||
Apply physics-based acceleration limiting for turns.
|
||||
|
||||
Uses friction circle: a_x² + a_y² ≤ a_max²
|
||||
|
||||
Args:
|
||||
v_ego: Current vehicle speed in m/s
|
||||
curvature: Current road curvature (from nav or model)
|
||||
accel_clip: Current [min_accel, max_accel] limits
|
||||
|
||||
Returns:
|
||||
Updated [min_accel, max_accel] with turn limiting applied
|
||||
"""
|
||||
if self.accel_limiter is None:
|
||||
return accel_clip
|
||||
|
||||
if abs(curvature) < 0.001: # Essentially straight
|
||||
self.state.is_limiting_accel = False
|
||||
return accel_clip
|
||||
|
||||
limited = list(self.accel_limiter.limit_accel_tuple(
|
||||
v_ego, curvature, tuple(accel_clip)
|
||||
))
|
||||
|
||||
self.state.is_limiting_accel = self.accel_limiter.state.is_limiting
|
||||
return limited
|
||||
|
||||
# Staleness threshold for maaControl message (nanoseconds)
|
||||
STALE_THRESHOLD_NS = 5e8 # 0.5 seconds
|
||||
|
||||
def process(
|
||||
self,
|
||||
sm,
|
||||
v_ego: float,
|
||||
v_cruise: float,
|
||||
accel_clip: List[float]
|
||||
) -> tuple:
|
||||
"""
|
||||
Process maaControl and return updated planner values.
|
||||
|
||||
Encapsulates all MAA logic:
|
||||
- Validity and staleness checking
|
||||
- Speed limiting (when driver acknowledged via blinker)
|
||||
- Virtual lead creation for natural deceleration
|
||||
- Curvature-based acceleration limiting
|
||||
|
||||
Args:
|
||||
sm: SubMaster with 'maaControl' and 'carState'
|
||||
v_ego: Current vehicle speed in m/s
|
||||
v_cruise: Current cruise speed in m/s
|
||||
accel_clip: Current [min_accel, max_accel] limits
|
||||
|
||||
Returns:
|
||||
tuple: (v_cruise, accel_clip, virtual_lead)
|
||||
"""
|
||||
virtual_lead = None
|
||||
|
||||
maa = sm['maaControl']
|
||||
|
||||
# Check valid and not stale
|
||||
maa_valid = sm.valid['maaControl'] and maa.turnValid
|
||||
if maa_valid and (sm.logMonoTime['carState'] - sm.logMonoTime['maaControl']) > self.STALE_THRESHOLD_NS:
|
||||
maa_valid = False
|
||||
|
||||
if not maa_valid:
|
||||
self.state.nav_limited_speed = None
|
||||
self.state.is_limiting_speed = False
|
||||
self.state.is_limiting_accel = False
|
||||
return v_cruise, accel_clip, virtual_lead
|
||||
|
||||
# Speed limiting only when driver acknowledged (blinker on)
|
||||
# Without blinker: informational only, no speed reduction
|
||||
if maa.speedLimitActive:
|
||||
virtual_lead = self.get_virtual_lead(v_ego, maa.turnSpeedLimit, maa.turnDistance)
|
||||
v_cruise = self.apply_nav_speed_limit(v_cruise, maa.turnSpeedLimit, maa.turnDistance)
|
||||
|
||||
# Curvature-based acceleration limiting (always active when valid)
|
||||
if maa.curvatureValid and abs(maa.curvature) > 0.001:
|
||||
accel_clip = self.apply_nav_accel_limit(v_ego, maa.curvature, accel_clip)
|
||||
|
||||
return v_cruise, accel_clip, virtual_lead
|
||||
@@ -1,119 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
MAA Desire Helper
|
||||
|
||||
Turn execution flow:
|
||||
1. APPROACHING (200m): Start dead reckoning, show turn suggestion
|
||||
2. Driver turns on blinker: acknowledged
|
||||
3. At 100m with blinker: COMMIT - block lane change, slow down, send desire
|
||||
4. EXECUTING (30m): Track heading change
|
||||
5. COMPLETE: When heading change ≈ expected turn angle
|
||||
|
||||
Turn desire is sent when:
|
||||
- desireActive is true (from maa_controld)
|
||||
- This requires: driver acknowledged (blinker) + committed (<100m) + EXECUTING state
|
||||
|
||||
Without blinker:
|
||||
- System shows turn info but doesn't intervene
|
||||
- No speed reduction, no desire sent
|
||||
- Driver maintains full control
|
||||
"""
|
||||
|
||||
from cereal import log, custom
|
||||
|
||||
TurnDirection = custom.MaaControl.TurnDirection
|
||||
ManeuverType = custom.MaaControl.ManeuverType
|
||||
TurnState = {
|
||||
'NONE': 0,
|
||||
'APPROACHING': 1,
|
||||
'EXECUTING': 2,
|
||||
'COMPLETE': 3,
|
||||
'MISSED': 4,
|
||||
}
|
||||
|
||||
# Configuration
|
||||
MAX_TURN_SPEED = 50.0 / 3.6 # m/s (50 km/h) - no turn assist above this
|
||||
SHARP_TURN_ANGLE = 45.0 # degrees - above: turnLeft/Right, below: laneChangeLeft/Right
|
||||
|
||||
|
||||
def should_block_lane_change(maa_control, v_ego: float) -> bool:
|
||||
"""
|
||||
Check if lane change should be blocked due to approaching turn.
|
||||
|
||||
Uses blockLaneChange from maa_controld which is set when:
|
||||
- Driver acknowledged (blinker on matching direction)
|
||||
- Within commit distance (100m)
|
||||
"""
|
||||
if maa_control is None:
|
||||
return False
|
||||
if v_ego > MAX_TURN_SPEED:
|
||||
return False
|
||||
# Use the pre-computed blockLaneChange from maa_controld
|
||||
return maa_control.blockLaneChange
|
||||
|
||||
|
||||
def get_turn_desire(maa_control, carstate, is_rhd: bool = False) -> log.Desire:
|
||||
"""
|
||||
Get turn desire based on maaControl.
|
||||
|
||||
desireActive from maa_controld is true when:
|
||||
- Driver acknowledged (blinker matches turn direction)
|
||||
- Committed (<100m) OR in EXECUTING state
|
||||
|
||||
This function adds additional checks:
|
||||
- Speed < 50 km/h
|
||||
- maneuverType == turn
|
||||
|
||||
Args:
|
||||
maa_control: MaaControl message
|
||||
carstate: CarState message
|
||||
is_rhd: True if right-hand drive (UK/Japan), False for left-hand drive (US/Taiwan)
|
||||
|
||||
Returns:
|
||||
- turnLeft/Right if angle >= 45°
|
||||
- laneChangeLeft/Right if angle < 45°
|
||||
- none if conditions not met
|
||||
"""
|
||||
if maa_control is None or not maa_control.turnValid:
|
||||
return log.Desire.none
|
||||
|
||||
if maa_control.maneuverType != ManeuverType.turn:
|
||||
return log.Desire.none
|
||||
|
||||
if carstate.vEgo > MAX_TURN_SPEED:
|
||||
return log.Desire.none
|
||||
|
||||
# desireActive encapsulates: acknowledged + (committed OR executing)
|
||||
if not maa_control.desireActive:
|
||||
return log.Desire.none
|
||||
|
||||
# Sharp turn (>= 45°) uses turnLeft/Right, gentle uses laneChange
|
||||
is_sharp = abs(maa_control.turnAngle) >= SHARP_TURN_ANGLE
|
||||
|
||||
if maa_control.turnDirection == TurnDirection.left:
|
||||
return log.Desire.turnLeft if is_sharp else log.Desire.laneChangeLeft
|
||||
elif maa_control.turnDirection == TurnDirection.right:
|
||||
return log.Desire.turnRight if is_sharp else log.Desire.laneChangeRight
|
||||
|
||||
return log.Desire.none
|
||||
@@ -1,118 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Model Helper for modeld integration.
|
||||
|
||||
Provides turn desire logic for modeld.py.
|
||||
"""
|
||||
|
||||
from cereal import log
|
||||
from dragonpilot.dashy.maa.lib.maa_desire import (
|
||||
should_block_lane_change,
|
||||
get_turn_desire,
|
||||
)
|
||||
|
||||
|
||||
class ModelHelper:
|
||||
"""
|
||||
Helper class for MAA integration with modeld.
|
||||
|
||||
Usage in modeld.py:
|
||||
from dragonpilot.dashy.maa.lib import ModelHelper
|
||||
model_helper = ModelHelper()
|
||||
|
||||
# In the loop:
|
||||
is_rhd = sm["driverMonitoringState"].isRHD
|
||||
desire = model_helper.get_desire(sm, lane_change_desire, is_rhd)
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
self.active = False
|
||||
self.last_desire = log.Desire.none
|
||||
|
||||
def get_desire(self, sm, lane_change_desire: int, is_rhd: bool = False) -> int:
|
||||
"""
|
||||
Get combined desire from MAA turn logic and lane change.
|
||||
|
||||
Priority:
|
||||
1. Active lane change (driver initiated) - don't interrupt
|
||||
2. MAA turn desire (navigation turn)
|
||||
3. Lane change desire (pre-lane-change, keep, etc.)
|
||||
|
||||
Args:
|
||||
sm: SubMaster with maaControl and carState
|
||||
lane_change_desire: Desire from DesireHelper (lane change logic)
|
||||
is_rhd: True if right-hand drive
|
||||
|
||||
Returns:
|
||||
Final desire value
|
||||
"""
|
||||
# Check if MAA data is available
|
||||
if not sm.valid.get('maaControl', False) or not sm.valid.get('carState', False):
|
||||
return lane_change_desire
|
||||
|
||||
# Check for stale maaControl (wait for 0.5s)
|
||||
if (sm.logMonoTime['carState'] - sm.logMonoTime['maaControl']) > 5e8:
|
||||
return lane_change_desire
|
||||
|
||||
maa_control = sm['maaControl']
|
||||
carstate = sm['carState']
|
||||
|
||||
# Don't interrupt active lane change
|
||||
if lane_change_desire in (log.Desire.laneChangeLeft, log.Desire.laneChangeRight):
|
||||
self.active = False
|
||||
return lane_change_desire
|
||||
|
||||
# Check if lane change should be blocked due to approaching turn
|
||||
if should_block_lane_change(maa_control, carstate.vEgo):
|
||||
# Block lane change desires, but allow none/keep
|
||||
if lane_change_desire in (log.Desire.laneChangeLeft, log.Desire.laneChangeRight):
|
||||
lane_change_desire = log.Desire.none
|
||||
|
||||
# Get MAA turn desire
|
||||
maa_desire = get_turn_desire(maa_control, carstate, is_rhd)
|
||||
|
||||
# MAA turn desire takes priority over none/keep
|
||||
if maa_desire != log.Desire.none:
|
||||
self.active = True
|
||||
self.last_desire = maa_desire
|
||||
return maa_desire
|
||||
|
||||
# If MAA was active but now returns none, we completed the turn
|
||||
if self.active and maa_desire == log.Desire.none:
|
||||
self.active = False
|
||||
|
||||
return lane_change_desire
|
||||
|
||||
def update(self, modelv2, desire_state):
|
||||
"""
|
||||
Update helper with model output (for future use).
|
||||
|
||||
Can be used to detect turn completion via desireState probabilities.
|
||||
|
||||
Args:
|
||||
modelv2: ModelV2 message
|
||||
desire_state: Model's desire state output
|
||||
"""
|
||||
# Reserved for future use - detecting turn completion via model output
|
||||
pass
|
||||
@@ -1,919 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
MAA Control Daemon - Turn Assist with Dead Reckoning
|
||||
|
||||
================================================================================
|
||||
OVERVIEW
|
||||
================================================================================
|
||||
|
||||
This daemon provides high-frequency (20Hz) turn assist signals for navigation.
|
||||
The key insight is that navInstruction updates at only 1Hz (too slow for smooth
|
||||
turn tracking), so we use "snapshot and coast" - capture turn info once, then
|
||||
dead reckon using the car's own sensors.
|
||||
|
||||
Subscribes to:
|
||||
- navInstruction: turn info from maad (1Hz) - used as TRIGGER only
|
||||
- liveGPS: position, bearing (1Hz)
|
||||
- navRoute: route geometry - reset on route change
|
||||
- carState: vEgo, blinker, yawRate (100Hz) - for dead reckoning
|
||||
|
||||
Publishes:
|
||||
- maaControl: turnDistance, turnDirection, desireActive, turnState, etc.
|
||||
|
||||
================================================================================
|
||||
DEAD RECKONING APPROACH
|
||||
================================================================================
|
||||
|
||||
Problem:
|
||||
Navigation updates come in slow (1Hz). At 60 km/h, that's 17m between updates.
|
||||
This causes jerky distance countdown and imprecise turn detection.
|
||||
|
||||
Solution: "Snapshot and Coast"
|
||||
1. TRIGGER at ~200m: capture turn params (angle, distance, direction)
|
||||
2. IGNORE subsequent navInstruction updates for this turn
|
||||
3. DEAD RECKON distance: integrate vEgo from carState (100Hz)
|
||||
4. TRACK HEADING: integrate yawRate from carState during turn execution
|
||||
|
||||
Distance remaining = initial_distance - ∫(vEgo × dt)
|
||||
Heading change = ∫(yawRate × dt)
|
||||
|
||||
================================================================================
|
||||
STATE MACHINE
|
||||
================================================================================
|
||||
|
||||
NONE ──────────────────────────────────────────────────────────────┐
|
||||
│ │
|
||||
│ navInstruction shows turn within 200m │
|
||||
│ (significant angle ≥20°, has direction) │
|
||||
▼ │
|
||||
APPROACHING ───────────────────────────────────────────────────────┤
|
||||
│ • Show turn suggestion at <150m │
|
||||
│ • Wait for driver blinker (acknowledgment) │
|
||||
│ • Dead reckon distance using vEgo │
|
||||
│ • NOT tracking yaw yet (allows overtaking) │
|
||||
│ │
|
||||
│ At <100m WITH blinker: COMMIT │
|
||||
│ • blockLaneChange = true │
|
||||
│ • speedLimitActive = true │
|
||||
│ • desireActive = true │
|
||||
│ │
|
||||
│ Dead reckoned distance < 30m │
|
||||
▼ │
|
||||
EXECUTING ─────────────────────────────────────────────────────────┤
|
||||
│ • Now tracking yaw (accumulated heading change) │
|
||||
│ • Compare accumulated vs expected turn angle │
|
||||
│ • desireActive = true (only if acknowledged) │
|
||||
│ │
|
||||
│ |accumulated_yaw| >= |expected_angle| - tolerance │
|
||||
▼ │
|
||||
COMPLETE ──────► (2s cooldown) ──────► NONE │
|
||||
│
|
||||
MISSED ◄─── (any abort condition) ◄────────────────────────────────┘
|
||||
│
|
||||
│ Wait for route change (navRoute coords change)
|
||||
▼
|
||||
NONE
|
||||
|
||||
================================================================================
|
||||
DRIVER ACKNOWLEDGMENT (Two-Step, Like Lane Change)
|
||||
================================================================================
|
||||
|
||||
Two confirmation steps:
|
||||
1. Blinker matching turn direction = approach confirmed (speed limit, block lane change)
|
||||
2. Steering torque in turn direction = turn execution confirmed (desire sent)
|
||||
|
||||
Blinker is the main confirmation for slowing down. Steering is required to
|
||||
actually send the turn desire to the model (like lane change).
|
||||
|
||||
Distance | Without Blinker | With Blinker | + Steering
|
||||
-----------|--------------------------|----------------------------|------------------
|
||||
200m-150m | Informational only | Informational only | (same)
|
||||
150m-100m | Show suggestion | speedLimitActive = true | (same)
|
||||
<100m | Show suggestion | slow down, block LC | + desireActive
|
||||
<30m | Enter EXECUTING | slow down, block LC | + desireActive
|
||||
|
||||
Key behaviors:
|
||||
- speedLimitActive: When blinker on, enforce turn speed limit
|
||||
- blockLaneChange: At <100m with blinker, block lane change desire
|
||||
- desireActive: Only sent when blinker AND steering confirmed (at turn)
|
||||
|
||||
If driver has blinker but doesn't steer:
|
||||
- System slows down for the turn
|
||||
- But doesn't send turn desire (driver steers manually)
|
||||
- Once driver steers in turn direction, desire activates
|
||||
|
||||
================================================================================
|
||||
ABORT/MISS DETECTION
|
||||
================================================================================
|
||||
|
||||
Driver can override at any time. We detect "missed turn" via:
|
||||
|
||||
1. DROVE TOO FAR (no turn)
|
||||
- Condition: distance_traveled > 2× initial_distance
|
||||
- Meaning: drove way past expected turn point without turning
|
||||
- Example: triggered at 200m, drove 400m, barely any yaw change
|
||||
|
||||
2. WRONG DIRECTION
|
||||
- Condition: accumulated_yaw > 20° in opposite direction
|
||||
- Meaning: driver turned the wrong way
|
||||
- Example: expected right turn, driver turned 25° left
|
||||
|
||||
3. INSUFFICIENT TURN
|
||||
- Condition: drove 2× total distance but <30% of expected yaw
|
||||
- Meaning: drove through intersection without completing turn
|
||||
- Example: expected 90° turn, only turned 20° after driving 500m
|
||||
|
||||
4. TIMEOUT
|
||||
- Condition: 30 seconds of MOVING time (v_ego > 1 m/s)
|
||||
- Meaning: something went wrong, taking too long
|
||||
- Note: stopped time (traffic light) doesn't count
|
||||
|
||||
5. PASSED TURN (nav jumped to next)
|
||||
- Condition: dead_reckon < 50m but nav says > 150m
|
||||
- Meaning: nav is now showing NEXT turn, we passed this one
|
||||
- Example: we think 30m to turn, nav says 400m (next turn)
|
||||
|
||||
6. DIRECTION CHANGED
|
||||
- Condition: turnAngle sign flipped (left ↔ right)
|
||||
- Meaning: route recalculated or nav corrected itself
|
||||
- Example: was +90° (left), now -45° (right)
|
||||
|
||||
7. TURN DISAPPEARED
|
||||
- Condition: turnAngle dropped below 20° threshold
|
||||
- Meaning: no longer a significant turn (route changed or passed)
|
||||
|
||||
================================================================================
|
||||
DRIVER OVERRIDE SCENARIOS
|
||||
================================================================================
|
||||
|
||||
Overtaking another car:
|
||||
- During APPROACHING: No problem! We only track distance, not yaw.
|
||||
Driver can swerve left/right to overtake, doesn't affect tracking.
|
||||
- During EXECUTING: Brief swerves (<20°) won't trigger wrong direction.
|
||||
Only sustained opposite turn triggers abort.
|
||||
|
||||
Stopping at traffic light:
|
||||
- moving_time only increments when v_ego > 1 m/s
|
||||
- Can wait indefinitely at red light without timeout
|
||||
- Distance tracking pauses when stopped (vEgo ≈ 0)
|
||||
|
||||
Taking a different route intentionally:
|
||||
- System correctly detects as MISSED
|
||||
- Waits for navRoute to change (reroute)
|
||||
- Then ready to track new turn
|
||||
|
||||
================================================================================
|
||||
DATA FLOW
|
||||
================================================================================
|
||||
|
||||
navInstruction (1Hz) carState (100Hz)
|
||||
│ │
|
||||
│ turnAngle, maneuverDistance │ vEgo, yawRate, blinker
|
||||
│ maneuverType, modifier │
|
||||
▼ ▼
|
||||
┌─────────────────────────────────────────────────────────┐
|
||||
│ TurnTracker │
|
||||
│ │
|
||||
│ trigger() ◄── at 200m, capture params │
|
||||
│ │ │
|
||||
│ ▼ │
|
||||
│ update(vEgo, yawRate) ◄── every 50ms (20Hz) │
|
||||
│ │ │
|
||||
│ ├── distance_traveled += vEgo × dt │
|
||||
│ ├── accumulated_yaw += yawRate × dt (if EXECUTING)│
|
||||
│ └── check abort conditions │
|
||||
│ │
|
||||
└─────────────────────────────────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
maaControl (20Hz)
|
||||
• turnDistance (dead reckoned)
|
||||
• turnState (NONE/APPROACHING/EXECUTING/COMPLETE/MISSED)
|
||||
• desireActive (for blinker/lane change desire)
|
||||
• turnProgress (accumulated yaw in degrees)
|
||||
|
||||
================================================================================
|
||||
CONFIGURATION
|
||||
================================================================================
|
||||
|
||||
TURN_TRIGGER_DISTANCE = 200m # Start dead reckoning
|
||||
TURN_DESIRE_DISTANCE = 150m # Show turn suggestion, wait for blinker
|
||||
TURN_COMMIT_DISTANCE = 100m # With blinker: commit (block lane change, slow)
|
||||
TURN_EXECUTE_DISTANCE = 30m # Start tracking yaw
|
||||
TURN_ANGLE_TOLERANCE = 15° # Turn complete within this
|
||||
TURN_MIN_ANGLE = 20° # Minimum to be "significant"
|
||||
TURN_TIMEOUT = 30s # Max moving time
|
||||
|
||||
================================================================================
|
||||
OUTPUT FIELDS (maaControl)
|
||||
================================================================================
|
||||
|
||||
turnDistance - Dead reckoned distance to turn (m)
|
||||
turnDirection - left/right/none
|
||||
turnAngle - Expected turn angle (deg, + = left)
|
||||
turnState - 0=none, 1=approaching, 2=executing, 3=complete, 4=missed
|
||||
turnProgress - Accumulated yaw during turn (deg)
|
||||
desireActive - Send turn desire to model (blinker + steering confirmed)
|
||||
driverAcknowledged - Driver turned on matching blinker
|
||||
speedLimitActive - Enforce turn speed limit (blinker on)
|
||||
blockLaneChange - Block lane change desire (blinker + committed)
|
||||
turnSpeedLimit - Target speed for turn (m/s)
|
||||
|
||||
================================================================================
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from enum import IntEnum
|
||||
|
||||
import numpy as np
|
||||
from cereal import messaging, log, custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from dragonpilot.dashy.maa.helpers import (
|
||||
Coordinate,
|
||||
compute_path_curvature,
|
||||
find_closest_point_on_route,
|
||||
)
|
||||
|
||||
|
||||
# CarrotPilot curvature-to-speed lookup table
|
||||
# Adapted from https://github.com/ajouatom/openpilot
|
||||
# Maps curvature (1/m) to recommended speed (km/h)
|
||||
# Based on physics: v = sqrt(a_lat / κ) where a_lat ≈ 2.5 m/s² for comfort
|
||||
V_CURVE_LOOKUP_BP = [0., 1./800., 1./670., 1./560., 1./440., 1./360., 1./265., 1./190., 1./135., 1./85., 1./55., 1./30., 1./25.]
|
||||
V_CURVE_LOOKUP_VALS = [300., 150., 120., 110., 100., 90., 80., 70., 60., 50., 40., 15., 5.] # km/h
|
||||
|
||||
|
||||
# Configuration
|
||||
CURVATURE_ASSIST_ENABLED = False # Disable continuous curvature steering assist
|
||||
CURVATURE_LOOKAHEAD = 2.5 # seconds ahead for curvature calculation
|
||||
TURN_VALID_DISTANCE = 500.0 # meters - turn is valid if within this distance
|
||||
MIN_SPEED_FOR_CURVATURE = 1.0 # m/s - minimum speed to use curvature
|
||||
|
||||
# Turn execution - dead reckoning based
|
||||
TURN_TRIGGER_DISTANCE = 200.0 # meters - capture turn info and start dead reckoning
|
||||
TURN_DESIRE_DISTANCE = 150.0 # meters - show turn suggestion, wait for blinker
|
||||
TURN_COMMIT_DISTANCE = 100.0 # meters - if blinker on, commit (block lane change, slow down)
|
||||
TURN_EXECUTE_DISTANCE = 30.0 # meters - start tracking heading (entering intersection)
|
||||
TURN_ANGLE_TOLERANCE = 15.0 # degrees - turn complete when within this of target
|
||||
TURN_MIN_ANGLE = 20.0 # degrees - minimum angle to consider a "turn"
|
||||
|
||||
# Abort detection thresholds
|
||||
TURN_MISS_DISTANCE_FACTOR = 2.0 # drove 2x expected distance without turning = missed
|
||||
TURN_MISS_YAW_THRESHOLD = 0.3 # must achieve at least 30% of turn angle
|
||||
TURN_WRONG_DIRECTION_ANGLE = 20.0 # degrees in wrong direction = missed
|
||||
TURN_TIMEOUT = 30.0 # seconds - max time in APPROACHING/EXECUTING
|
||||
|
||||
|
||||
class TurnState(IntEnum):
|
||||
NONE = 0 # no turn pending
|
||||
APPROACHING = 1 # turn ahead, dead reckoning distance
|
||||
EXECUTING = 2 # in intersection, tracking heading change
|
||||
COMPLETE = 3 # turn done, cooldown before next
|
||||
MISSED = 4 # turn missed/aborted, wait for reroute
|
||||
|
||||
|
||||
class TurnTracker:
|
||||
"""
|
||||
Tracks turn execution using dead reckoning.
|
||||
|
||||
Once triggered at ~200m, ignores navInstruction updates and purely
|
||||
uses vEgo (distance) and yawRate (heading) from carState.
|
||||
|
||||
Driver acknowledgment flow (two-step, like lane change):
|
||||
- System shows turn suggestion at 150m
|
||||
- Driver turns on blinker = acknowledged (speed limit, block lane change)
|
||||
- Driver steers in turn direction = steering confirmed (desire sent)
|
||||
- Blinker gates the approach, steering gates the turn execution
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
self.state = TurnState.NONE
|
||||
# Captured at trigger
|
||||
self.expected_angle = 0.0 # degrees (positive=left, negative=right)
|
||||
self.initial_distance = 0.0 # distance to turn when triggered
|
||||
self.direction = 'none' # 'left' or 'right'
|
||||
self.maneuver_type = 0 # MaaControl.ManeuverType
|
||||
self.turn_speed_limit = 0.0 # m/s
|
||||
# Dead reckoning state
|
||||
self.distance_traveled = 0.0 # meters since trigger
|
||||
self.accumulated_yaw = 0.0 # degrees turned since execute start
|
||||
self.moving_time = 0.0 # accumulated time while moving (for timeout)
|
||||
self.execute_start_time = 0.0 # when EXECUTING started
|
||||
self.complete_time = 0.0 # when turn completed
|
||||
self.last_update_time = 0.0 # for dt calculation
|
||||
# Driver acknowledgment (two-step like lane change)
|
||||
self.driver_acknowledged = False # blinker matches turn direction
|
||||
self.steering_confirmed = False # steering torque matches turn direction
|
||||
|
||||
def reset(self):
|
||||
self.state = TurnState.NONE
|
||||
self.expected_angle = 0.0
|
||||
self.initial_distance = 0.0
|
||||
self.direction = 'none'
|
||||
self.maneuver_type = 0
|
||||
self.turn_speed_limit = 0.0
|
||||
self.distance_traveled = 0.0
|
||||
self.accumulated_yaw = 0.0
|
||||
self.moving_time = 0.0
|
||||
self.execute_start_time = 0.0
|
||||
self.complete_time = 0.0
|
||||
self.last_update_time = 0.0
|
||||
self.driver_acknowledged = False
|
||||
self.steering_confirmed = False
|
||||
|
||||
def get_estimated_distance(self) -> float:
|
||||
"""Get estimated distance to turn based on dead reckoning."""
|
||||
if self.state in (TurnState.APPROACHING, TurnState.EXECUTING):
|
||||
return max(0.0, self.initial_distance - self.distance_traveled)
|
||||
return 9999.0
|
||||
|
||||
def trigger(self, t: float, turn_distance: float, turn_angle: float,
|
||||
maneuver_type: int, turn_direction: int, turn_speed_limit: float):
|
||||
"""Capture turn parameters and start dead reckoning."""
|
||||
self.state = TurnState.APPROACHING
|
||||
self.expected_angle = turn_angle
|
||||
self.initial_distance = turn_distance
|
||||
self.maneuver_type = maneuver_type
|
||||
self.turn_speed_limit = turn_speed_limit
|
||||
self.direction = 'left' if turn_direction == custom.MaaControl.TurnDirection.left else 'right'
|
||||
self.distance_traveled = 0.0
|
||||
self.accumulated_yaw = 0.0
|
||||
self.moving_time = 0.0
|
||||
self.last_update_time = t
|
||||
cloudlog.info(f"maa: turn triggered, angle={turn_angle:.1f}°, dist={turn_distance:.0f}m, dir={self.direction}")
|
||||
|
||||
def check_blinker(self, left_blinker: bool, right_blinker: bool):
|
||||
"""Check if driver turned on blinker matching turn direction."""
|
||||
if self.direction == 'left' and left_blinker:
|
||||
self.driver_acknowledged = True
|
||||
elif self.direction == 'right' and right_blinker:
|
||||
self.driver_acknowledged = True
|
||||
# Note: we don't reset acknowledged if blinker turns off
|
||||
# (driver may have tapped blinker briefly to acknowledge)
|
||||
|
||||
def check_steering(self, steering_pressed: bool, steering_torque: float):
|
||||
"""Check if driver is steering in the turn direction (like lane change confirmation)."""
|
||||
if not steering_pressed:
|
||||
return
|
||||
# Same logic as lane change: positive torque = left, negative = right
|
||||
if self.direction == 'left' and steering_torque > 0:
|
||||
self.steering_confirmed = True
|
||||
elif self.direction == 'right' and steering_torque < 0:
|
||||
self.steering_confirmed = True
|
||||
# Once confirmed, stays confirmed (one-time check)
|
||||
|
||||
def update(self, t: float, v_ego: float, yaw_rate: float) -> bool:
|
||||
"""
|
||||
Update turn state with dead reckoning.
|
||||
|
||||
Args:
|
||||
t: current time (monotonic)
|
||||
v_ego: vehicle speed (m/s) from carState
|
||||
yaw_rate: yaw rate (rad/s) from carState
|
||||
|
||||
Returns True if desire should be sent (acknowledged + within commit distance).
|
||||
"""
|
||||
if self.state == TurnState.NONE:
|
||||
return False
|
||||
|
||||
# Calculate dt
|
||||
dt = t - self.last_update_time if self.last_update_time > 0 else 0.05
|
||||
dt = min(dt, 0.2) # Cap dt to handle timing glitches
|
||||
self.last_update_time = t
|
||||
|
||||
# Integrate distance traveled (and moving time for timeout)
|
||||
self.distance_traveled += v_ego * dt
|
||||
if v_ego > 1.0: # Only count time when actually moving
|
||||
self.moving_time += dt
|
||||
estimated_dist = self.get_estimated_distance()
|
||||
|
||||
# Timeout check - based on moving time (allows waiting at traffic lights)
|
||||
if self.moving_time > TURN_TIMEOUT:
|
||||
cloudlog.warning(f"maa: turn timeout after {self.moving_time:.0f}s moving")
|
||||
self.state = TurnState.MISSED
|
||||
return False
|
||||
|
||||
if self.state == TurnState.APPROACHING:
|
||||
# Check if we've reached the intersection
|
||||
if estimated_dist <= TURN_EXECUTE_DISTANCE:
|
||||
self.state = TurnState.EXECUTING
|
||||
self.execute_start_time = t
|
||||
self.accumulated_yaw = 0.0
|
||||
cloudlog.info(f"maa: turn executing, traveled={self.distance_traveled:.0f}m")
|
||||
|
||||
# Miss detection: drove way past without entering execute
|
||||
if self.distance_traveled > self.initial_distance * TURN_MISS_DISTANCE_FACTOR:
|
||||
cloudlog.warning(f"maa: turn missed (drove past), traveled={self.distance_traveled:.0f}m")
|
||||
self.state = TurnState.MISSED
|
||||
return False
|
||||
|
||||
elif self.state == TurnState.EXECUTING:
|
||||
# Integrate yaw rate (convert rad/s to deg)
|
||||
yaw_change_deg = math.degrees(yaw_rate) * dt
|
||||
self.accumulated_yaw += yaw_change_deg
|
||||
|
||||
# Fix 3: Early abort if going straight through the "turn" (map error)
|
||||
# If we've traveled 20m past the turn point with no significant yaw change,
|
||||
# the turn doesn't exist - abort quickly instead of extended false braking
|
||||
distance_past_turn = self.distance_traveled - self.initial_distance
|
||||
if distance_past_turn > 20.0 and abs(self.accumulated_yaw) < 5.0:
|
||||
cloudlog.warning(f"maa: no turn detected (past {distance_past_turn:.0f}m, yaw={self.accumulated_yaw:.1f}°), aborting")
|
||||
self.state = TurnState.MISSED
|
||||
return False
|
||||
|
||||
# Check completion: achieved target heading
|
||||
# For left turn: expected_angle > 0, accumulated should go positive
|
||||
# For right turn: expected_angle < 0, accumulated should go negative
|
||||
progress = abs(self.accumulated_yaw) / abs(self.expected_angle) if self.expected_angle != 0 else 0
|
||||
angle_remaining = abs(self.expected_angle) - abs(self.accumulated_yaw)
|
||||
|
||||
if angle_remaining <= TURN_ANGLE_TOLERANCE:
|
||||
self.state = TurnState.COMPLETE
|
||||
self.complete_time = t
|
||||
cloudlog.info(f"maa: turn complete, yaw={self.accumulated_yaw:.1f}°, expected={self.expected_angle:.1f}°")
|
||||
return False
|
||||
|
||||
# Wrong direction detection: significant yaw in opposite direction
|
||||
if self.expected_angle > 0 and self.accumulated_yaw < -TURN_WRONG_DIRECTION_ANGLE:
|
||||
cloudlog.warning(f"maa: turn wrong direction (expected left, went right)")
|
||||
self.state = TurnState.MISSED
|
||||
return False
|
||||
if self.expected_angle < 0 and self.accumulated_yaw > TURN_WRONG_DIRECTION_ANGLE:
|
||||
cloudlog.warning(f"maa: turn wrong direction (expected right, went left)")
|
||||
self.state = TurnState.MISSED
|
||||
return False
|
||||
|
||||
# Miss detection: drove 2x expected total distance without sufficient turn
|
||||
total_expected = self.initial_distance + 50.0 # Add some buffer for turn itself
|
||||
if self.distance_traveled > total_expected * TURN_MISS_DISTANCE_FACTOR:
|
||||
if progress < TURN_MISS_YAW_THRESHOLD:
|
||||
cloudlog.warning(f"maa: turn missed (insufficient yaw), progress={progress:.1%}")
|
||||
self.state = TurnState.MISSED
|
||||
return False
|
||||
|
||||
elif self.state == TurnState.COMPLETE:
|
||||
# Short cooldown then reset
|
||||
if t - self.complete_time > 2.0:
|
||||
self.reset()
|
||||
return False
|
||||
|
||||
elif self.state == TurnState.MISSED:
|
||||
# Stay in missed state until route changes (reset called externally)
|
||||
return False
|
||||
|
||||
# Desire active when:
|
||||
# Driver acknowledged (blinker on) AND steering confirmed AND either:
|
||||
# 1. APPROACHING and within commit distance (<100m), OR
|
||||
# 2. EXECUTING (in the turn)
|
||||
# Without both confirmations, NO desire is sent (driver maintains control)
|
||||
if not self.driver_acknowledged or not self.steering_confirmed:
|
||||
return False
|
||||
if self.state == TurnState.APPROACHING:
|
||||
return estimated_dist <= TURN_COMMIT_DISTANCE
|
||||
if self.state == TurnState.EXECUTING:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def get_turn_speed_from_curvature(curvature: float) -> float:
|
||||
"""
|
||||
Compute recommended speed for turn using CarrotPilot's curvature lookup table.
|
||||
This is physics-based: v = sqrt(a_lat / κ) where a_lat ≈ 2.5 m/s² for comfort.
|
||||
|
||||
Args:
|
||||
curvature: Road curvature in 1/m (positive or negative)
|
||||
|
||||
Returns:
|
||||
Recommended speed in m/s
|
||||
"""
|
||||
abs_curv = abs(curvature)
|
||||
speed_kph = np.interp(abs_curv, V_CURVE_LOOKUP_BP, V_CURVE_LOOKUP_VALS)
|
||||
return speed_kph / 3.6 # Convert to m/s
|
||||
|
||||
|
||||
def get_turn_speed_limit(modifier: str, turn_angle: float, nav_type: str = '', curvature: float = 0.0) -> float:
|
||||
"""
|
||||
Compute recommended speed for turn based on curvature (preferred) or heuristics.
|
||||
|
||||
Uses CarrotPilot's curvature lookup table when curvature is available.
|
||||
Falls back to heuristic-based calculation when curvature is not available.
|
||||
|
||||
Args:
|
||||
modifier: OSRM maneuver modifier (e.g., 'left', 'right', 'sharp left')
|
||||
turn_angle: Turn angle in degrees (positive=left, negative=right)
|
||||
nav_type: OSRM maneuver type (e.g., 'turn', 'off ramp')
|
||||
curvature: Road curvature in 1/m (from geometry calculation)
|
||||
|
||||
Returns:
|
||||
Recommended speed in m/s
|
||||
"""
|
||||
# Use curvature-based calculation when curvature is available
|
||||
# This is more accurate than heuristics as it's physics-based
|
||||
if abs(curvature) > 0.001: # Meaningful curvature
|
||||
speed_from_curv = get_turn_speed_from_curvature(curvature)
|
||||
# Clamp to reasonable range: 5 km/h to 100 km/h for turns
|
||||
return max(5.0 / 3.6, min(100.0 / 3.6, speed_from_curv))
|
||||
|
||||
# Fallback: Heuristic-based calculation when curvature not available
|
||||
|
||||
# Highway maneuvers (exits, ramps, merges) - much higher speeds
|
||||
if nav_type in ('depart', 'off ramp', 'on ramp', 'merge', 'fork'):
|
||||
if 'sharp' in modifier:
|
||||
base_speed = 50.0 # km/h - sharp highway exit
|
||||
elif 'slight' in modifier:
|
||||
base_speed = 80.0 # km/h - gentle curve
|
||||
else:
|
||||
base_speed = 60.0 # km/h - normal exit ramp
|
||||
return base_speed / 3.6 # Don't apply angle adjustments to highway maneuvers
|
||||
|
||||
# Intersection turns - lower speeds
|
||||
if 'sharp' in modifier:
|
||||
base_speed = 15.0 # km/h
|
||||
elif 'slight' in modifier:
|
||||
base_speed = 45.0 # km/h
|
||||
elif modifier in ('left', 'right'):
|
||||
base_speed = 25.0 # km/h
|
||||
elif 'uturn' in modifier:
|
||||
base_speed = 10.0 # km/h
|
||||
else:
|
||||
base_speed = 50.0 # km/h default
|
||||
|
||||
# Adjust based on actual angle if available (only for intersection turns)
|
||||
abs_angle = abs(turn_angle)
|
||||
if abs_angle > 90:
|
||||
base_speed = min(base_speed, 15.0)
|
||||
elif abs_angle > 60:
|
||||
base_speed = min(base_speed, 25.0)
|
||||
elif abs_angle > 30:
|
||||
base_speed = min(base_speed, 35.0)
|
||||
|
||||
return base_speed / 3.6 # Convert to m/s
|
||||
|
||||
|
||||
def get_maneuver_type(nav_type: str, turn_angle: float = 0.0) -> int:
|
||||
"""Determine maneuver type primarily from geometry, with OSRM hints.
|
||||
|
||||
Geometry is the source of truth - OSRM labels can be wrong.
|
||||
"""
|
||||
# Geometry-first: significant turn angle = it's a turn
|
||||
if abs(turn_angle) >= TURN_MIN_ANGLE:
|
||||
return custom.MaaControl.ManeuverType.turn
|
||||
|
||||
# Highway maneuvers (even with small angle) - use laneChange desire
|
||||
if nav_type in ('off ramp', 'on ramp', 'merge', 'fork'):
|
||||
return custom.MaaControl.ManeuverType.laneChange
|
||||
|
||||
# OSRM says turn but geometry is minor - still treat as turn
|
||||
if nav_type in ('turn', 'end of road'):
|
||||
return custom.MaaControl.ManeuverType.turn
|
||||
|
||||
# Everything else: no action needed
|
||||
return custom.MaaControl.ManeuverType.none
|
||||
|
||||
|
||||
def get_last_gps_position(params: Params) -> tuple:
|
||||
"""Get last known GPS position from params. Returns (lat, lon, bearing) or None."""
|
||||
try:
|
||||
data = params.get("LastGPSPosition")
|
||||
if data:
|
||||
pos = json.loads(data)
|
||||
return pos.get('latitude'), pos.get('longitude'), pos.get('bearing', 0.0)
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
|
||||
def main():
|
||||
cloudlog.info("maa_controld: starting")
|
||||
|
||||
params = Params()
|
||||
sm = messaging.SubMaster(
|
||||
['navRoute', 'navInstruction', 'liveGPS', 'carState'],
|
||||
ignore_alive=['navRoute', 'navInstruction', 'liveGPS', 'carState']
|
||||
)
|
||||
pm = messaging.PubMaster(['maaControl'])
|
||||
|
||||
rk = Ratekeeper(20)
|
||||
|
||||
# State
|
||||
route_coords: list[Coordinate] = []
|
||||
last_route_len = 0
|
||||
last_gps_time = 0.0
|
||||
closest_idx = 0
|
||||
|
||||
# Turn tracker (dead reckoning based)
|
||||
turn_tracker = TurnTracker()
|
||||
|
||||
# Fallback position from params (used before liveGPS is ready)
|
||||
fallback_pos = get_last_gps_position(params)
|
||||
if fallback_pos:
|
||||
cloudlog.info(f"maa_controld: fallback position {fallback_pos[0]:.6f}, {fallback_pos[1]:.6f}")
|
||||
|
||||
while True:
|
||||
sm.update(0)
|
||||
t = time.monotonic()
|
||||
|
||||
# Get current position - prefer liveGPS, fall back to LastGPSPosition
|
||||
if sm.updated['liveGPS']:
|
||||
last_gps_time = time.monotonic()
|
||||
|
||||
gps = sm['liveGPS']
|
||||
gps_stale = (time.monotonic() - last_gps_time) > 2.0
|
||||
gps_valid = gps.status == custom.LiveGPS.Status.valid and not gps_stale
|
||||
|
||||
# Use fallback if liveGPS not ready
|
||||
use_fallback = not gps_valid and fallback_pos is not None
|
||||
if gps_valid:
|
||||
current_lat = gps.latitude
|
||||
current_lon = gps.longitude
|
||||
current_bearing = gps.bearingDeg
|
||||
elif use_fallback:
|
||||
current_lat, current_lon, current_bearing = fallback_pos
|
||||
else:
|
||||
current_lat = current_lon = current_bearing = None
|
||||
|
||||
# Get turn info from navInstruction
|
||||
nav = sm['navInstruction']
|
||||
nav_valid = sm.valid['navInstruction']
|
||||
|
||||
# Always update route coordinates (needed for turn angle calculation)
|
||||
nav_route = sm['navRoute']
|
||||
if sm.valid['navRoute'] and nav_route.coordinates:
|
||||
new_len = len(nav_route.coordinates)
|
||||
if new_len != last_route_len:
|
||||
route_coords = [
|
||||
Coordinate(c.latitude, c.longitude)
|
||||
for c in nav_route.coordinates
|
||||
]
|
||||
last_route_len = new_len
|
||||
closest_idx = 0
|
||||
turn_tracker.reset() # Reset turn state when route changes
|
||||
cloudlog.debug(f"maa_controld: route updated, {new_len} points")
|
||||
|
||||
# Find current position on route
|
||||
has_position = current_lat is not None
|
||||
if route_coords and has_position:
|
||||
current_pos = Coordinate(current_lat, current_lon)
|
||||
try:
|
||||
# Optimization: Search locally around last known position
|
||||
search_start = max(0, closest_idx - 10)
|
||||
search_end = min(len(route_coords), closest_idx + 50)
|
||||
|
||||
if closest_idx == 0:
|
||||
search_start = 0
|
||||
search_end = len(route_coords)
|
||||
|
||||
subset = route_coords[search_start:search_end]
|
||||
local_idx, _ = find_closest_point_on_route(current_pos, subset)
|
||||
closest_idx = search_start + local_idx
|
||||
except Exception as e:
|
||||
cloudlog.warning(f"maa_controld: position error: {e}")
|
||||
|
||||
# Get speed, blinker, and yawRate from carState
|
||||
cs = sm['carState']
|
||||
cs_valid = sm.valid['carState']
|
||||
v_ego = cs.vEgo if cs_valid else 0.0
|
||||
left_blinker = cs.leftBlinker if cs_valid else False
|
||||
right_blinker = cs.rightBlinker if cs_valid else False
|
||||
yaw_rate = cs.yawRate if cs_valid else 0.0
|
||||
|
||||
# Continuous curvature assist (optional - for steering)
|
||||
curvature = 0.0
|
||||
curvature_valid = False
|
||||
if CURVATURE_ASSIST_ENABLED and route_coords and has_position and v_ego > MIN_SPEED_FOR_CURVATURE:
|
||||
try:
|
||||
curvature = compute_path_curvature(
|
||||
current_pos, current_bearing, route_coords, closest_idx, v_ego, CURVATURE_LOOKAHEAD
|
||||
)
|
||||
curvature_valid = True
|
||||
except Exception as e:
|
||||
cloudlog.warning(f"maa_controld: curvature error: {e}")
|
||||
|
||||
# Build maaControl message
|
||||
msg = messaging.new_message('maaControl', valid=True)
|
||||
maa = msg.maaControl
|
||||
|
||||
maa.curvature = float(curvature)
|
||||
maa.curvatureValid = curvature_valid
|
||||
|
||||
# Get maneuver info from navInstruction (1Hz, used only for trigger)
|
||||
maneuver_dist = getattr(nav, 'maneuverDistance', None) if nav_valid else None
|
||||
|
||||
# If turn tracker is active (including MISSED/COMPLETE), handle accordingly
|
||||
if turn_tracker.state in (TurnState.APPROACHING, TurnState.EXECUTING):
|
||||
# Safety checks: detect if turn info changed significantly
|
||||
if nav_valid and maneuver_dist is not None:
|
||||
# Use turnAngle from navInstructionExt - this is geometry-based (reliable)
|
||||
turn_angle = getattr(nav, 'turnAngle', 0.0) or 0.0
|
||||
estimated_dist = turn_tracker.get_estimated_distance()
|
||||
|
||||
# Check 1: Did we pass the turn? (nav distance jumped up = now showing NEXT turn)
|
||||
# If we think we're close (<50m) but nav says far (>150m), we probably passed it
|
||||
if estimated_dist < 50.0 and maneuver_dist > 150.0:
|
||||
cloudlog.warning(f"maa: likely passed turn (est={estimated_dist:.0f}m, nav={maneuver_dist:.0f}m), resetting")
|
||||
turn_tracker.reset()
|
||||
|
||||
# Check 2: Did direction flip? (route recalculated or now showing different turn)
|
||||
else:
|
||||
current_nav_dir = None
|
||||
if turn_angle > TURN_MIN_ANGLE:
|
||||
current_nav_dir = 'left'
|
||||
elif turn_angle < -TURN_MIN_ANGLE:
|
||||
current_nav_dir = 'right'
|
||||
|
||||
if current_nav_dir and current_nav_dir != turn_tracker.direction:
|
||||
cloudlog.warning(f"maa: turn direction changed ({turn_tracker.direction} → {current_nav_dir}), angle={turn_angle:.1f}°, resetting")
|
||||
turn_tracker.reset()
|
||||
|
||||
# Check 3: Turn disappeared (angle now below threshold)
|
||||
elif abs(turn_angle) < TURN_MIN_ANGLE:
|
||||
nav_type = getattr(nav, 'maneuverType', '') or ''
|
||||
if get_maneuver_type(nav_type, turn_angle) == custom.MaaControl.ManeuverType.none:
|
||||
cloudlog.warning(f"maa: turn no longer valid (angle={turn_angle:.1f}°), resetting")
|
||||
turn_tracker.reset()
|
||||
|
||||
if turn_tracker.state in (TurnState.APPROACHING, TurnState.EXECUTING):
|
||||
# Check blinker and steering for driver acknowledgment (like lane change)
|
||||
turn_tracker.check_blinker(left_blinker, right_blinker)
|
||||
turn_tracker.check_steering(cs.steeringPressed, cs.steeringTorque)
|
||||
|
||||
# Dead reckon distance - ignore navInstruction updates
|
||||
estimated_dist = turn_tracker.get_estimated_distance()
|
||||
|
||||
# Fix 2: Allow abort if blinker turns off before commitment (>100m)
|
||||
# User can change their mind if not yet committed to the turn
|
||||
if turn_tracker.state == TurnState.APPROACHING and turn_tracker.driver_acknowledged:
|
||||
blinker_matches = (turn_tracker.direction == 'left' and left_blinker) or \
|
||||
(turn_tracker.direction == 'right' and right_blinker)
|
||||
if not blinker_matches and estimated_dist > TURN_COMMIT_DISTANCE:
|
||||
turn_tracker.driver_acknowledged = False
|
||||
turn_tracker.steering_confirmed = False
|
||||
cloudlog.info("maa: blinker canceled before commit, aborting turn assist")
|
||||
maa.turnDistance = float(estimated_dist)
|
||||
maa.turnValid = True
|
||||
maa.turnAngle = float(turn_tracker.expected_angle)
|
||||
maa.maneuverType = turn_tracker.maneuver_type
|
||||
maa.turnSpeedLimit = float(turn_tracker.turn_speed_limit)
|
||||
|
||||
# Direction from captured state
|
||||
if turn_tracker.direction == 'left':
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.left
|
||||
elif turn_tracker.direction == 'right':
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.right
|
||||
else:
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.none
|
||||
|
||||
# Update tracker with dead reckoning
|
||||
desire_active = turn_tracker.update(t, v_ego, yaw_rate)
|
||||
maa.turnState = int(turn_tracker.state)
|
||||
maa.turnProgress = float(turn_tracker.accumulated_yaw)
|
||||
|
||||
# Driver acknowledgment status
|
||||
maa.driverAcknowledged = turn_tracker.driver_acknowledged
|
||||
|
||||
# Speed limit active when blinker on (driver acknowledged)
|
||||
maa.speedLimitActive = turn_tracker.driver_acknowledged
|
||||
|
||||
# Block lane change when committed (blinker + within commit distance)
|
||||
maa.blockLaneChange = turn_tracker.driver_acknowledged and estimated_dist <= TURN_COMMIT_DISTANCE
|
||||
|
||||
# desireActive: send turn desire to model (requires blinker)
|
||||
maa.desireActive = desire_active
|
||||
|
||||
# Curvature from captured state (not recalculated)
|
||||
maa.curvature = 0.0
|
||||
maa.curvatureValid = False
|
||||
maa.turnCurvature = 0.0
|
||||
|
||||
elif nav_valid and maneuver_dist is not None:
|
||||
# No active turn tracking - check if we should trigger
|
||||
maa.turnDistance = float(maneuver_dist)
|
||||
maa.turnValid = maneuver_dist < TURN_VALID_DISTANCE
|
||||
|
||||
nav_type = getattr(nav, 'maneuverType', '') or ''
|
||||
modifier = getattr(nav, 'maneuverModifier', '') or ''
|
||||
|
||||
# Get pre-computed turn geometry from navInstruction
|
||||
turn_angle = getattr(nav, 'turnAngle', 0.0) or 0.0
|
||||
turn_curvature = getattr(nav, 'turnCurvature', 0.0) or 0.0
|
||||
|
||||
maa.turnAngle = float(turn_angle)
|
||||
maa.turnCurvature = float(turn_curvature)
|
||||
|
||||
# Compute maneuver type
|
||||
maa.maneuverType = get_maneuver_type(nav_type, turn_angle)
|
||||
|
||||
# Set turn direction
|
||||
if maa.maneuverType != custom.MaaControl.ManeuverType.none:
|
||||
if 'left' in modifier.lower():
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.left
|
||||
elif 'right' in modifier.lower():
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.right
|
||||
elif turn_angle > TURN_MIN_ANGLE:
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.left
|
||||
elif turn_angle < -TURN_MIN_ANGLE:
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.right
|
||||
else:
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.none
|
||||
else:
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.none
|
||||
|
||||
# Compute turn speed limit using CarrotPilot curvature lookup table
|
||||
# Curvature-based is more accurate than heuristics when available
|
||||
turn_speed_limit = get_turn_speed_limit(modifier, turn_angle, nav_type, turn_curvature)
|
||||
maa.turnSpeedLimit = float(turn_speed_limit)
|
||||
|
||||
# Check if we should trigger dead reckoning
|
||||
# Don't trigger if already in MISSED/COMPLETE state (wait for route change)
|
||||
can_trigger = turn_tracker.state == TurnState.NONE
|
||||
is_actionable = maa.maneuverType != custom.MaaControl.ManeuverType.none
|
||||
has_direction = maa.turnDirection != custom.MaaControl.TurnDirection.none
|
||||
is_significant = abs(turn_angle) >= TURN_MIN_ANGLE
|
||||
in_trigger_range = maneuver_dist <= TURN_TRIGGER_DISTANCE
|
||||
|
||||
if can_trigger and is_actionable and has_direction and is_significant and in_trigger_range:
|
||||
# Trigger! Capture turn params and start dead reckoning
|
||||
turn_tracker.trigger(
|
||||
t, maneuver_dist, turn_angle,
|
||||
maa.maneuverType, maa.turnDirection, turn_speed_limit
|
||||
)
|
||||
# Check blinker and steering immediately after trigger
|
||||
turn_tracker.check_blinker(left_blinker, right_blinker)
|
||||
turn_tracker.check_steering(cs.steeringPressed, cs.steeringTorque)
|
||||
# Blinker = approach confirmation, steering = turn execution confirmation
|
||||
maa.desireActive = turn_tracker.driver_acknowledged and turn_tracker.steering_confirmed and maneuver_dist <= TURN_COMMIT_DISTANCE
|
||||
maa.turnState = int(turn_tracker.state)
|
||||
maa.turnProgress = 0.0
|
||||
maa.driverAcknowledged = turn_tracker.driver_acknowledged
|
||||
maa.speedLimitActive = turn_tracker.driver_acknowledged
|
||||
maa.blockLaneChange = turn_tracker.driver_acknowledged and maneuver_dist <= TURN_COMMIT_DISTANCE
|
||||
elif turn_tracker.state in (TurnState.COMPLETE, TurnState.MISSED):
|
||||
# In cooldown - call update to handle state transitions
|
||||
turn_tracker.update(t, v_ego, yaw_rate)
|
||||
maa.desireActive = False
|
||||
maa.turnState = int(turn_tracker.state)
|
||||
maa.turnProgress = float(turn_tracker.accumulated_yaw)
|
||||
maa.driverAcknowledged = False
|
||||
maa.speedLimitActive = False
|
||||
maa.blockLaneChange = False
|
||||
else:
|
||||
# Not triggered yet (turn too far away)
|
||||
maa.desireActive = False
|
||||
maa.turnState = int(TurnState.NONE)
|
||||
maa.turnProgress = 0.0
|
||||
maa.driverAcknowledged = False
|
||||
maa.speedLimitActive = False
|
||||
maa.blockLaneChange = False
|
||||
|
||||
else:
|
||||
# No valid nav instruction
|
||||
maa.turnValid = False
|
||||
maa.turnDirection = custom.MaaControl.TurnDirection.none
|
||||
maa.turnSpeedLimit = 50.0 / 3.6
|
||||
maa.maneuverType = custom.MaaControl.ManeuverType.none
|
||||
maa.turnAngle = 0.0
|
||||
maa.turnCurvature = 0.0
|
||||
maa.turnDistance = 9999.0
|
||||
maa.desireActive = False
|
||||
maa.turnState = int(TurnState.NONE)
|
||||
maa.turnProgress = 0.0
|
||||
maa.driverAcknowledged = False
|
||||
maa.speedLimitActive = False
|
||||
maa.blockLaneChange = False
|
||||
|
||||
# Only reset if not in MISSED state (wait for route change)
|
||||
if turn_tracker.state != TurnState.MISSED:
|
||||
turn_tracker.reset()
|
||||
|
||||
pm.send('maaControl', msg)
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,566 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
dragonpilot Map-Aware Assist Daemon (maad)
|
||||
|
||||
Handles route calculation and navigation instructions using OSRM routing.
|
||||
Similar to openpilot's navd but uses free OSRM API instead of Mapbox.
|
||||
|
||||
Control signals (maaControl) are published by maa_controld.py separately
|
||||
for low-latency, deterministic control.
|
||||
|
||||
Flow:
|
||||
1. Reads dp_maa_destination from params (set by dashy)
|
||||
2. Fetches route from OSRM (free routing API) - async to avoid blocking
|
||||
3. Subscribes to liveGPS for position updates
|
||||
4. Publishes navInstruction (for UI) and navRoute (for map display)
|
||||
"""
|
||||
import json
|
||||
import time
|
||||
import threading
|
||||
import queue
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import custom, log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from dragonpilot.dashy.maa.helpers import (
|
||||
Coordinate,
|
||||
coordinate_from_param,
|
||||
find_closest_point_on_route,
|
||||
distance_along_geometry,
|
||||
compute_turn_angle_at_index,
|
||||
compute_turn_curvature_at_index,
|
||||
)
|
||||
from dragonpilot.dashy.maa.providers import MapService
|
||||
from dragonpilot.dashy.maa.providers.models import Coordinate as ProviderCoordinate
|
||||
from dragonpilot.dashy.maa.route_tracker import RouteTracker, REROUTE_DISTANCE_BASE, REROUTE_DEBOUNCE_TIME
|
||||
|
||||
MANEUVER_TRANSITION_THRESHOLD = 10 # meters past maneuver to transition
|
||||
NAV_RATE = 1.0 # Hz - navigation update rate
|
||||
|
||||
|
||||
@dataclass
|
||||
class Step:
|
||||
"""Represents a navigation step/segment."""
|
||||
distance: float # total distance of step in meters
|
||||
duration: float # duration in seconds
|
||||
duration_typical: Optional[float]
|
||||
name: str
|
||||
maneuver_type: str
|
||||
maneuver_modifier: str
|
||||
geometry: list[Coordinate] # coordinates for this step
|
||||
speed_limit: Optional[float] # m/s
|
||||
speed_limit_sign: str # 'mutcd' or 'vienna'
|
||||
# Pre-computed turn geometry (computed once when route is fetched)
|
||||
turn_angle: float = 0.0 # degrees, positive=left, negative=right
|
||||
turn_curvature: float = 0.0 # 1/m, positive=left, negative=right
|
||||
# Explicit maneuver point from OSRM (for fork/turn detection)
|
||||
maneuver_point: Optional[Coordinate] = None
|
||||
|
||||
|
||||
class RouteEngine:
|
||||
def __init__(self, sm: messaging.SubMaster, pm: messaging.PubMaster):
|
||||
self.sm = sm
|
||||
self.pm = pm
|
||||
self.params = Params()
|
||||
self.map_service = MapService(self.params)
|
||||
|
||||
# Get last GPS position from params
|
||||
self.last_position = coordinate_from_param("LastGPSPosition", self.params)
|
||||
if self.last_position is None:
|
||||
# Default to Taipei 101 for bench testing
|
||||
self.last_position = Coordinate(25.033976, 121.564472)
|
||||
self.last_bearing: Optional[float] = None
|
||||
|
||||
self.gps_ok = False
|
||||
self.gps_speed = 0.0
|
||||
self.gps_accuracy = 0.0 # horizontal accuracy in meters
|
||||
self.last_gps_time = 0.0
|
||||
|
||||
# Route state
|
||||
self.nav_destination: Optional[Coordinate] = None
|
||||
self.route: Optional[list[Step]] = None
|
||||
self.tracker = RouteTracker() # OsmAnd-style route tracking
|
||||
|
||||
# Recompute state
|
||||
self.recompute_backoff = 0
|
||||
self.recompute_countdown = 0
|
||||
|
||||
# Async route calculation
|
||||
self._route_queue: queue.Queue = queue.Queue()
|
||||
self._route_thread: Optional[threading.Thread] = None
|
||||
self._route_calculating = False
|
||||
|
||||
# Timing diagnostics
|
||||
self._frame_count = 0
|
||||
self._last_timing_log = time.monotonic()
|
||||
self._gps_save_counter = 0
|
||||
self._route_send_counter = 0
|
||||
|
||||
# Params cache - avoid reading params every frame
|
||||
self._cached_destination: Optional[Coordinate] = None
|
||||
self._last_destination_check = 0.0
|
||||
self._destination_check_interval = 3.0 # seconds
|
||||
|
||||
# First valid GPS flag - triggers immediate route calculation
|
||||
self._had_first_valid_gps = False
|
||||
|
||||
def update(self):
|
||||
t0 = time.monotonic()
|
||||
|
||||
self.sm.update(0)
|
||||
t1 = time.monotonic()
|
||||
|
||||
self._update_location()
|
||||
self._check_route_result() # Non-blocking check for async route result
|
||||
t2 = time.monotonic()
|
||||
|
||||
try:
|
||||
self._recompute_route()
|
||||
t3 = time.monotonic()
|
||||
self._send_instruction()
|
||||
# Resend route periodically (every 1s) so new UI clients can see it
|
||||
if self.route is not None:
|
||||
self.send_route()
|
||||
t4 = time.monotonic()
|
||||
except Exception:
|
||||
cloudlog.exception("maad.failed_to_compute")
|
||||
t3 = t4 = time.monotonic()
|
||||
|
||||
# Log timing every 10 seconds
|
||||
self._frame_count += 1
|
||||
total_time = t4 - t0
|
||||
if total_time > 0.05: # Log if frame took > 50ms
|
||||
cloudlog.warning(f"maad slow frame: total={total_time*1000:.0f}ms "
|
||||
f"(sm={1000*(t1-t0):.0f}, loc={1000*(t2-t1):.0f}, "
|
||||
f"route={1000*(t3-t2):.0f}, instr={1000*(t4-t3):.0f})")
|
||||
|
||||
if t4 - self._last_timing_log > 10.0:
|
||||
actual_rate = self._frame_count / (t4 - self._last_timing_log)
|
||||
cloudlog.info(f"maad rate: {actual_rate:.1f} Hz (target {NAV_RATE} Hz), frames={self._frame_count}")
|
||||
self._frame_count = 0
|
||||
self._last_timing_log = t4
|
||||
|
||||
def _check_route_result(self):
|
||||
"""Check for async route calculation result (non-blocking)."""
|
||||
try:
|
||||
result = self._route_queue.get_nowait()
|
||||
self._route_calculating = False
|
||||
|
||||
if result is None:
|
||||
# Route calculation failed
|
||||
self._clear_route(clear_destination=False)
|
||||
return
|
||||
|
||||
# Apply the calculated route
|
||||
self.route, route_data = result
|
||||
self.nav_curvature_valid = True
|
||||
|
||||
# Start at first step (simple, like navd.py)
|
||||
self.tracker.set_step(0)
|
||||
self._reset_recompute_limits() # Reset backoff on successful route
|
||||
cloudlog.warning(f"maad: route calculated - {route_data['distance']/1000:.1f}km, "
|
||||
f"{route_data['duration']/60:.0f}min, {len(self.route)} steps")
|
||||
|
||||
# Debug: log each step's maneuver info
|
||||
for i, s in enumerate(self.route):
|
||||
mp_str = f"({s.maneuver_point.latitude:.6f},{s.maneuver_point.longitude:.6f})" if s.maneuver_point else "None"
|
||||
cloudlog.info(f"maad step {i}: {s.maneuver_type} {s.maneuver_modifier} -> '{s.name}' ({s.distance:.0f}m) mp={mp_str}")
|
||||
|
||||
self.send_route()
|
||||
|
||||
except queue.Empty:
|
||||
pass # No result yet
|
||||
|
||||
def _update_location(self):
|
||||
"""Update position from GPS."""
|
||||
# Debug: log GPS reception status every 50 frames
|
||||
self._gps_debug_counter = getattr(self, '_gps_debug_counter', 0) + 1
|
||||
if self._gps_debug_counter >= 50:
|
||||
self._gps_debug_counter = 0
|
||||
gps = self.sm['liveGPS']
|
||||
cloudlog.info(f"maad GPS: updated={self.sm.updated['liveGPS']} valid={self.sm.valid['liveGPS']} "
|
||||
f"gpsOK={gps.gpsOK} pos=({gps.latitude:.6f},{gps.longitude:.6f})")
|
||||
|
||||
if self.sm.updated['liveGPS']:
|
||||
gps = self.sm['liveGPS']
|
||||
|
||||
# Always update position and speed from GPS (needed for route calculation)
|
||||
if gps.gpsOK:
|
||||
self.last_position = Coordinate(gps.latitude, gps.longitude)
|
||||
self.gps_speed = gps.speed
|
||||
self.gps_accuracy = gps.horizontalAccuracy # Track for OsmAnd-style tolerance
|
||||
self.last_gps_time = time.monotonic()
|
||||
|
||||
# Save last position every ~60 frames (12 seconds at 5Hz) to reduce I/O
|
||||
self._gps_save_counter += 1
|
||||
if self._gps_save_counter >= 60:
|
||||
self._gps_save_counter = 0
|
||||
self.params.put("LastGPSPosition", json.dumps({
|
||||
'latitude': gps.latitude,
|
||||
'longitude': gps.longitude
|
||||
}))
|
||||
|
||||
# GPS is valid when OK, good accuracy, AND fully calibrated
|
||||
was_gps_ok = self.gps_ok
|
||||
is_calibrated = gps.status == custom.LiveGPS.Status.valid
|
||||
self.gps_ok = gps.gpsOK and gps.horizontalAccuracy <= 20.0 and is_calibrated
|
||||
|
||||
# Only use bearing when fusion is fully calibrated
|
||||
if is_calibrated:
|
||||
self.last_bearing = gps.bearingDeg
|
||||
else:
|
||||
self.last_bearing = None # clear stale bearing
|
||||
|
||||
# Detect first valid GPS - triggers immediate route check
|
||||
if self.gps_ok and not was_gps_ok and not self._had_first_valid_gps:
|
||||
self._had_first_valid_gps = True
|
||||
cloudlog.info("maad: first valid GPS fix (calibrated), checking for destination")
|
||||
|
||||
# Staleness check
|
||||
if time.monotonic() - self.last_gps_time > 2.0:
|
||||
self.gps_ok = False
|
||||
|
||||
def _recompute_route(self):
|
||||
"""Check if we need to recompute route."""
|
||||
# Don't start route until GPS is valid (OK + calibrated)
|
||||
if not self.gps_ok:
|
||||
return
|
||||
|
||||
# Skip if route calculation is already in progress
|
||||
if self._route_calculating:
|
||||
return
|
||||
|
||||
# Check params - immediately on first valid GPS, otherwise every 3 seconds
|
||||
now = time.monotonic()
|
||||
first_gps_check = self._had_first_valid_gps and self.route is None and self._cached_destination is None
|
||||
if first_gps_check or now - self._last_destination_check >= self._destination_check_interval:
|
||||
self._last_destination_check = now
|
||||
self._cached_destination = coordinate_from_param("dp_maa_destination", self.params)
|
||||
|
||||
new_destination = self._cached_destination
|
||||
if new_destination is None:
|
||||
if self.nav_destination is not None or self.route is not None:
|
||||
self._clear_route()
|
||||
self._reset_recompute_limits()
|
||||
return
|
||||
|
||||
should_recompute = self._should_recompute()
|
||||
if should_recompute and self.route is not None:
|
||||
cloudlog.warning(f"maad: reroute triggered, countdown={self.recompute_countdown}, backoff={self.recompute_backoff}")
|
||||
|
||||
# New destination
|
||||
if new_destination != self.nav_destination:
|
||||
cloudlog.warning(f"Got new destination from dp_maa_destination param {new_destination}")
|
||||
self.nav_destination = new_destination
|
||||
should_recompute = True
|
||||
|
||||
# Don't recompute when GPS drifts in tunnels
|
||||
if not self.gps_ok and self.tracker.step_idx is not None:
|
||||
return
|
||||
|
||||
# First route calculation (no existing route) - start immediately without backoff
|
||||
is_first_route = self.route is None and should_recompute
|
||||
if is_first_route or (self.recompute_countdown == 0 and should_recompute):
|
||||
if not is_first_route:
|
||||
self.recompute_countdown = 2 ** self.recompute_backoff
|
||||
self.recompute_backoff = min(3, self.recompute_backoff + 1) # Max 8 second backoff
|
||||
self._start_route_calculation(new_destination)
|
||||
else:
|
||||
self.recompute_countdown = max(0, self.recompute_countdown - 1)
|
||||
|
||||
def _start_route_calculation(self, destination: Coordinate):
|
||||
"""Start async route calculation in a separate thread."""
|
||||
start_pos = self.last_position
|
||||
bearing = self.last_bearing
|
||||
|
||||
self._route_calculating = True
|
||||
self.nav_destination = destination
|
||||
|
||||
cloudlog.info(f"maad: starting async route calculation {start_pos} -> {destination}")
|
||||
|
||||
def calculate():
|
||||
try:
|
||||
result = self._fetch_route(start_pos, destination, bearing)
|
||||
self._route_queue.put(result)
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"maad: route calculation failed: {e}")
|
||||
self._route_queue.put(None)
|
||||
|
||||
self._route_thread = threading.Thread(target=calculate, daemon=True)
|
||||
self._route_thread.start()
|
||||
|
||||
def _fetch_route(self, start: Coordinate, destination: Coordinate,
|
||||
bearing: Optional[float]) -> Optional[tuple]:
|
||||
"""Fetch route using MapService (runs in thread). Returns (route, route_data) or None."""
|
||||
origin = ProviderCoordinate(start.latitude, start.longitude)
|
||||
dest = ProviderCoordinate(destination.latitude, destination.longitude)
|
||||
|
||||
provider_route = self.map_service.route_provider.get_route_sync(
|
||||
origin=origin,
|
||||
destination=dest,
|
||||
bearing=bearing
|
||||
)
|
||||
|
||||
if provider_route is None:
|
||||
cloudlog.warning("maad: route provider returned None")
|
||||
return None
|
||||
|
||||
# Convert provider Route to local Step format
|
||||
# Filter out depart/arrive - merge their geometry with adjacent steps
|
||||
route = []
|
||||
all_coords = [] # Full route geometry for turn angle computation
|
||||
pending_geometry = [] # Geometry from depart to merge with first real step
|
||||
|
||||
for provider_step in provider_route.steps:
|
||||
# Convert provider Coordinates to helper Coordinates
|
||||
geometry = [
|
||||
Coordinate(c.latitude, c.longitude)
|
||||
for c in provider_step.geometry
|
||||
]
|
||||
all_coords.extend(geometry)
|
||||
|
||||
# Skip depart/arrive steps but keep their geometry
|
||||
if provider_step.maneuver_type in ('depart', 'arrive'):
|
||||
if provider_step.maneuver_type == 'depart':
|
||||
pending_geometry = geometry # Save for merging with first real step
|
||||
elif provider_step.maneuver_type == 'arrive' and route:
|
||||
# Merge arrive geometry with last step
|
||||
route[-1].geometry.extend(geometry)
|
||||
route[-1].distance += provider_step.distance
|
||||
route[-1].duration += provider_step.duration
|
||||
continue
|
||||
|
||||
# Merge pending depart geometry with this step
|
||||
if pending_geometry:
|
||||
geometry = pending_geometry + geometry
|
||||
pending_geometry = []
|
||||
|
||||
# Convert provider maneuver_point to helpers Coordinate
|
||||
maneuver_pt = None
|
||||
if provider_step.maneuver_point:
|
||||
maneuver_pt = Coordinate(
|
||||
provider_step.maneuver_point.latitude,
|
||||
provider_step.maneuver_point.longitude
|
||||
)
|
||||
|
||||
route_step = Step(
|
||||
distance=provider_step.distance,
|
||||
duration=provider_step.duration,
|
||||
duration_typical=provider_step.duration_typical or provider_step.duration,
|
||||
name=provider_step.name,
|
||||
maneuver_type=provider_step.maneuver_type,
|
||||
maneuver_modifier=provider_step.maneuver_modifier,
|
||||
geometry=geometry,
|
||||
speed_limit=provider_step.speed_limit,
|
||||
speed_limit_sign=provider_step.speed_limit_sign,
|
||||
maneuver_point=maneuver_pt,
|
||||
)
|
||||
route.append(route_step)
|
||||
|
||||
# Pre-compute turn geometry at each step's maneuver point (end of step)
|
||||
coord_idx = 0
|
||||
for step in route:
|
||||
coord_idx += len(step.geometry)
|
||||
# Turn point is at the end of this step (start of next)
|
||||
turn_idx = min(coord_idx - 1, len(all_coords) - 2)
|
||||
if turn_idx > 0:
|
||||
step.turn_angle = compute_turn_angle_at_index(all_coords, turn_idx)
|
||||
step.turn_curvature = compute_turn_curvature_at_index(all_coords, turn_idx)
|
||||
|
||||
# Build route_data dict for compatibility
|
||||
route_data = {
|
||||
'distance': provider_route.distance,
|
||||
'duration': provider_route.duration,
|
||||
}
|
||||
|
||||
cloudlog.info(f"maad: route from {provider_route.provider} - "
|
||||
f"{provider_route.distance/1000:.1f}km, {len(route)} steps")
|
||||
|
||||
return (route, route_data)
|
||||
|
||||
def _send_instruction(self):
|
||||
"""Send navInstruction message ."""
|
||||
msg = messaging.new_message('navInstruction', valid=True)
|
||||
|
||||
if self.tracker.step_idx is None or self.route is None or self.last_position is None or not self.gps_ok:
|
||||
# Debug: log why we're sending invalid
|
||||
reasons = []
|
||||
if self.tracker.step_idx is None:
|
||||
reasons.append("step_idx=None")
|
||||
if self.route is None:
|
||||
reasons.append("route=None")
|
||||
if self.last_position is None:
|
||||
reasons.append("position=None")
|
||||
if not self.gps_ok:
|
||||
reasons.append(f"gps_ok=False")
|
||||
cloudlog.info(f"maad: sending invalid navInstruction: {', '.join(reasons)}")
|
||||
msg.valid = False
|
||||
self.pm.send('navInstruction', msg)
|
||||
return
|
||||
|
||||
# Sanity check: ensure step_idx is valid
|
||||
if self.tracker.step_idx >= len(self.route):
|
||||
cloudlog.error(f"maad: step_idx {self.tracker.step_idx} >= route length {len(self.route)}, resetting to 0")
|
||||
self.tracker.set_step(0)
|
||||
|
||||
step = self.route[self.tracker.step_idx]
|
||||
geometry = step.geometry
|
||||
|
||||
# Calculate distance along current step geometry
|
||||
along_geometry = distance_along_geometry(geometry, self.last_position)
|
||||
distance_to_maneuver = step.distance - along_geometry
|
||||
|
||||
# Current instruction (depart/arrive already filtered out during route build)
|
||||
msg.navInstruction.maneuverDistance = distance_to_maneuver
|
||||
msg.navInstruction.maneuverPrimaryText = step.name or step.maneuver_type
|
||||
|
||||
# Override maneuver type/modifier based on geometry
|
||||
# Geometry-first: always use turn_angle for direction when significant
|
||||
TURN_MIN_ANGLE = 20.0
|
||||
if abs(step.turn_angle) >= TURN_MIN_ANGLE:
|
||||
# Significant turn - use geometry for both type and direction
|
||||
if step.maneuver_type in ('continue', 'new name'):
|
||||
msg.navInstruction.maneuverType = 'turn'
|
||||
else:
|
||||
msg.navInstruction.maneuverType = step.maneuver_type
|
||||
# Always use geometry-based direction for significant turns
|
||||
msg.navInstruction.maneuverModifier = 'left' if step.turn_angle > 0 else 'right'
|
||||
else:
|
||||
msg.navInstruction.maneuverType = step.maneuver_type
|
||||
msg.navInstruction.maneuverModifier = step.maneuver_modifier
|
||||
|
||||
# Next step's road name (the road to turn onto)
|
||||
if self.tracker.step_idx + 1 < len(self.route):
|
||||
next_step = self.route[self.tracker.step_idx + 1]
|
||||
msg.navInstruction.maneuverSecondaryText = next_step.name or ""
|
||||
|
||||
# Compute total remaining time and distance
|
||||
remaining_ratio = 1.0 - along_geometry / max(step.distance, 1)
|
||||
total_distance = step.distance * remaining_ratio
|
||||
total_time = step.duration * remaining_ratio
|
||||
total_time_typical = (step.duration_typical or step.duration) * remaining_ratio
|
||||
|
||||
for i in range(self.tracker.step_idx + 1, len(self.route)):
|
||||
total_distance += self.route[i].distance
|
||||
total_time += self.route[i].duration
|
||||
total_time_typical += self.route[i].duration_typical or self.route[i].duration
|
||||
|
||||
msg.navInstruction.distanceRemaining = total_distance
|
||||
msg.navInstruction.timeRemaining = total_time
|
||||
msg.navInstruction.timeRemainingTypical = total_time_typical
|
||||
|
||||
# Speed limit from closest coordinate
|
||||
if geometry:
|
||||
closest_idx, _ = find_closest_point_on_route(self.last_position, geometry)
|
||||
if closest_idx < len(geometry):
|
||||
closest = geometry[closest_idx]
|
||||
if 'maxspeed' in closest.annotations and self.gps_ok:
|
||||
msg.navInstruction.speedLimit = closest.annotations['maxspeed']
|
||||
|
||||
if step.speed_limit_sign == 'mutcd':
|
||||
msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd
|
||||
else:
|
||||
msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna
|
||||
|
||||
self.pm.send('navInstruction', msg)
|
||||
|
||||
# Send extended nav instruction (turn geometry)
|
||||
msg_ext = messaging.new_message('navInstructionExt')
|
||||
msg_ext.navInstructionExt.turnAngle = step.turn_angle
|
||||
msg_ext.navInstructionExt.turnCurvature = step.turn_curvature
|
||||
self.pm.send('navInstructionExt', msg_ext)
|
||||
|
||||
# Transition to next step
|
||||
if self.tracker.update_step(self.route, self.last_position, self.last_bearing, self.gps_speed):
|
||||
self._reset_recompute_limits()
|
||||
|
||||
# Check if arrived at destination
|
||||
if self.nav_destination:
|
||||
dist = self.nav_destination.distance_to(self.last_position)
|
||||
if dist < 30: # Within 30m of destination
|
||||
cloudlog.warning("maad: destination reached")
|
||||
self.params.remove("dp_maa_destination")
|
||||
self._clear_route()
|
||||
|
||||
def send_route(self):
|
||||
"""Send navRoute message for dashy to display route on map."""
|
||||
coords = []
|
||||
|
||||
if self.route is not None:
|
||||
for step in self.route:
|
||||
coords.extend([[c.longitude, c.latitude] for c in step.geometry])
|
||||
|
||||
msg = messaging.new_message('navRoute', valid=True)
|
||||
msg.navRoute.coordinates = [{"longitude": c[0], "latitude": c[1]} for c in coords]
|
||||
self.pm.send('navRoute', msg)
|
||||
|
||||
def _clear_route(self, clear_destination=True):
|
||||
"""Clear navigation state."""
|
||||
self.route = None
|
||||
self.tracker.reset()
|
||||
if clear_destination:
|
||||
self.nav_destination = None
|
||||
|
||||
# Send empty navRoute to clear map display
|
||||
msg = messaging.new_message('navRoute', valid=False)
|
||||
msg.navRoute.coordinates = []
|
||||
self.pm.send('navRoute', msg)
|
||||
|
||||
def _reset_recompute_limits(self):
|
||||
"""Reset recompute backoff and deviation timer."""
|
||||
self.recompute_backoff = 0
|
||||
self.recompute_countdown = 0
|
||||
self.tracker.deviation_start_time = None # Reset OsmAnd-style deviation timer
|
||||
|
||||
def _should_recompute(self) -> bool:
|
||||
"""Check if route should be recomputed (delegates to RouteTracker)."""
|
||||
if self.route is None:
|
||||
return True
|
||||
return self.tracker.should_reroute(self.route, self.last_position, self.gps_accuracy)
|
||||
|
||||
|
||||
def main():
|
||||
cloudlog.info("maad starting")
|
||||
|
||||
pm = messaging.PubMaster(['navInstruction', 'navInstructionExt', 'navRoute'])
|
||||
sm = messaging.SubMaster(['liveGPS'], ignore_alive=['liveGPS'])
|
||||
|
||||
rk = Ratekeeper(NAV_RATE)
|
||||
route_engine = RouteEngine(sm, pm)
|
||||
|
||||
while True:
|
||||
try:
|
||||
route_engine.update()
|
||||
except Exception:
|
||||
cloudlog.exception("maad: error in main loop")
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,54 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Map Provider Abstraction Layer
|
||||
|
||||
Uses api.dragonpilot.org for search and routing with device JWT authentication.
|
||||
Falls back to free providers (Photon/OSRM) when not authenticated.
|
||||
|
||||
Usage:
|
||||
from dragonpilot.dashy.maa.providers import MapService
|
||||
|
||||
map_service = MapService()
|
||||
route = map_service.route_provider.get_route_sync(origin, dest)
|
||||
results = await map_service.search_provider.search("Taipei 101")
|
||||
"""
|
||||
|
||||
from .map_service import MapService
|
||||
from .models import Coordinate, SearchResult, Route, Step, TileConfig
|
||||
from .base import SearchProvider, RouteProvider, TileProvider
|
||||
from .dragonpilot import DragonpilotSearchProvider, DragonpilotRouteProvider, DragonpilotApiClient
|
||||
|
||||
__all__ = [
|
||||
'MapService',
|
||||
'Coordinate',
|
||||
'SearchResult',
|
||||
'Route',
|
||||
'Step',
|
||||
'TileConfig',
|
||||
'SearchProvider',
|
||||
'RouteProvider',
|
||||
'TileProvider',
|
||||
'DragonpilotSearchProvider',
|
||||
'DragonpilotRouteProvider',
|
||||
'DragonpilotApiClient',
|
||||
]
|
||||
@@ -1,177 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Abstract base classes for map providers.
|
||||
|
||||
These interfaces define the contract that all provider implementations must follow.
|
||||
"""
|
||||
|
||||
from abc import ABC, abstractmethod
|
||||
from typing import Optional
|
||||
import asyncio
|
||||
|
||||
from .models import Coordinate, SearchResult, Route, TileConfig
|
||||
|
||||
|
||||
class SearchProvider(ABC):
|
||||
"""
|
||||
Abstract search/geocoding provider.
|
||||
|
||||
Implementations: PhotonSearchProvider, MapboxSearchProvider, etc.
|
||||
"""
|
||||
|
||||
name: str = "base"
|
||||
requires_api_key: bool = False
|
||||
|
||||
@abstractmethod
|
||||
async def search(
|
||||
self,
|
||||
query: str,
|
||||
proximity: Optional[Coordinate] = None,
|
||||
limit: int = 10
|
||||
) -> list[SearchResult]:
|
||||
"""
|
||||
Search for places by query string.
|
||||
|
||||
Args:
|
||||
query: Search query (address, place name, etc.)
|
||||
proximity: Optional coordinate to bias results toward
|
||||
limit: Maximum number of results
|
||||
|
||||
Returns:
|
||||
List of SearchResult objects sorted by relevance/distance
|
||||
"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
async def reverse_geocode(
|
||||
self,
|
||||
coord: Coordinate
|
||||
) -> Optional[SearchResult]:
|
||||
"""
|
||||
Get address/place information from coordinates.
|
||||
|
||||
Args:
|
||||
coord: Coordinate to reverse geocode
|
||||
|
||||
Returns:
|
||||
SearchResult with address info, or None if not found
|
||||
"""
|
||||
pass
|
||||
|
||||
def search_sync(
|
||||
self,
|
||||
query: str,
|
||||
proximity: Optional[Coordinate] = None,
|
||||
limit: int = 10
|
||||
) -> list[SearchResult]:
|
||||
"""Synchronous wrapper for search()."""
|
||||
loop = asyncio.new_event_loop()
|
||||
try:
|
||||
return loop.run_until_complete(self.search(query, proximity, limit))
|
||||
finally:
|
||||
loop.close()
|
||||
|
||||
|
||||
class RouteProvider(ABC):
|
||||
"""
|
||||
Abstract routing provider.
|
||||
|
||||
Implementations: OSRMRouteProvider, MapboxRouteProvider, etc.
|
||||
"""
|
||||
|
||||
name: str = "base"
|
||||
requires_api_key: bool = False
|
||||
supports_traffic: bool = False
|
||||
|
||||
@abstractmethod
|
||||
async def get_route(
|
||||
self,
|
||||
origin: Coordinate,
|
||||
destination: Coordinate,
|
||||
waypoints: Optional[list[Coordinate]] = None,
|
||||
bearing: Optional[float] = None
|
||||
) -> Optional[Route]:
|
||||
"""
|
||||
Calculate route between points.
|
||||
|
||||
Args:
|
||||
origin: Starting coordinate
|
||||
destination: Ending coordinate
|
||||
waypoints: Optional intermediate waypoints
|
||||
bearing: Optional current heading in degrees (for better route start)
|
||||
|
||||
Returns:
|
||||
Route object with steps and geometry, or None if routing fails
|
||||
"""
|
||||
pass
|
||||
|
||||
def get_route_sync(
|
||||
self,
|
||||
origin: Coordinate,
|
||||
destination: Coordinate,
|
||||
waypoints: Optional[list[Coordinate]] = None,
|
||||
bearing: Optional[float] = None
|
||||
) -> Optional[Route]:
|
||||
"""
|
||||
Synchronous wrapper for get_route().
|
||||
|
||||
Use this in maad.py where async is not available.
|
||||
"""
|
||||
loop = asyncio.new_event_loop()
|
||||
try:
|
||||
return loop.run_until_complete(
|
||||
self.get_route(origin, destination, waypoints, bearing)
|
||||
)
|
||||
finally:
|
||||
loop.close()
|
||||
|
||||
|
||||
class TileProvider(ABC):
|
||||
"""
|
||||
Abstract map tile provider.
|
||||
|
||||
Implementations: OpenFreeMapTileProvider, MapboxTileProvider, etc.
|
||||
"""
|
||||
|
||||
name: str = "base"
|
||||
requires_api_key: bool = False
|
||||
|
||||
@abstractmethod
|
||||
def get_tile_config(self) -> TileConfig:
|
||||
"""
|
||||
Get tile URL template and configuration.
|
||||
|
||||
Returns:
|
||||
TileConfig with URL template and attribution
|
||||
"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_style_json(self) -> dict:
|
||||
"""
|
||||
Get MapLibre GL style JSON for this provider.
|
||||
|
||||
Returns:
|
||||
Style JSON dict for MapLibre GL JS
|
||||
"""
|
||||
pass
|
||||
@@ -1,45 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Configuration for map providers.
|
||||
|
||||
Uses api.dragonpilot.org for search and routing with device serial authentication.
|
||||
Falls back to free providers (Photon/OSRM) when not authenticated.
|
||||
All providers use WGS-84 coordinates (standard GPS).
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass
|
||||
class ProviderConfig:
|
||||
"""
|
||||
Provider configuration.
|
||||
|
||||
Uses Dragonpilot API with automatic fallback to free providers.
|
||||
All providers use WGS-84 coordinates (standard GPS).
|
||||
"""
|
||||
|
||||
@classmethod
|
||||
def from_params(cls, params) -> 'ProviderConfig':
|
||||
"""Load configuration from openpilot Params."""
|
||||
return cls()
|
||||
@@ -1,32 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Dragonpilot API Provider
|
||||
|
||||
Uses api.dragonpilot.org for search and routing with device JWT authentication.
|
||||
"""
|
||||
|
||||
from .client import DragonpilotApiClient
|
||||
from .search import DragonpilotSearchProvider
|
||||
from .routing import DragonpilotRouteProvider
|
||||
|
||||
__all__ = ['DragonpilotApiClient', 'DragonpilotSearchProvider', 'DragonpilotRouteProvider']
|
||||
@@ -1,123 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Dragonpilot API Client
|
||||
|
||||
Simple HTTP client using device serial for authentication.
|
||||
"""
|
||||
|
||||
import os
|
||||
from typing import Optional
|
||||
import aiohttp
|
||||
import requests
|
||||
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
|
||||
API_HOST = os.getenv('DRAGONPILOT_API_HOST', 'https://api.dragonpilot.org')
|
||||
|
||||
# Module-level serial cache - queried once from HARDWARE
|
||||
_serial: Optional[str] = None
|
||||
|
||||
|
||||
def _get_serial() -> Optional[str]:
|
||||
"""Get device serial (cached)."""
|
||||
global _serial
|
||||
if _serial is None:
|
||||
try:
|
||||
_serial = HARDWARE.get_serial()
|
||||
except Exception:
|
||||
pass
|
||||
return _serial
|
||||
|
||||
|
||||
class DragonpilotApiClient:
|
||||
"""
|
||||
API client for api.dragonpilot.org.
|
||||
|
||||
Uses device serial from HARDWARE for authentication.
|
||||
"""
|
||||
|
||||
def __init__(self, serial: str = None):
|
||||
self.api_host = API_HOST
|
||||
self.serial = serial if serial is not None else _get_serial()
|
||||
|
||||
@property
|
||||
def is_authenticated(self) -> bool:
|
||||
return self.serial is not None
|
||||
|
||||
def _headers(self) -> dict:
|
||||
headers = {'Content-Type': 'application/json'}
|
||||
if self.serial:
|
||||
headers['X-Device-Serial'] = self.serial
|
||||
return headers
|
||||
|
||||
def get_sync(self, endpoint: str, params: dict = None, timeout: int = 10) -> Optional[dict]:
|
||||
try:
|
||||
resp = requests.get(f"{self.api_host}{endpoint}", params=params, headers=self._headers(), timeout=timeout)
|
||||
return resp.json() if resp.status_code == 200 else None
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
def post_sync(self, endpoint: str, data: dict = None, timeout: int = 10) -> Optional[dict]:
|
||||
try:
|
||||
resp = requests.post(f"{self.api_host}{endpoint}", json=data, headers=self._headers(), timeout=timeout)
|
||||
return resp.json() if resp.status_code == 200 else None
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
async def get(self, endpoint: str, params: dict = None, timeout: int = 10) -> Optional[dict]:
|
||||
try:
|
||||
async with aiohttp.ClientSession() as session:
|
||||
async with session.get(
|
||||
f"{self.api_host}{endpoint}",
|
||||
params=params,
|
||||
headers=self._headers(),
|
||||
timeout=aiohttp.ClientTimeout(total=timeout)
|
||||
) as resp:
|
||||
return await resp.json() if resp.status == 200 else None
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
async def post(self, endpoint: str, data: dict = None, timeout: int = 10) -> Optional[dict]:
|
||||
try:
|
||||
async with aiohttp.ClientSession() as session:
|
||||
async with session.post(
|
||||
f"{self.api_host}{endpoint}",
|
||||
json=data,
|
||||
headers=self._headers(),
|
||||
timeout=aiohttp.ClientTimeout(total=timeout)
|
||||
) as resp:
|
||||
return await resp.json() if resp.status == 200 else None
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# Singleton client instance
|
||||
_client: Optional[DragonpilotApiClient] = None
|
||||
|
||||
|
||||
def get_client() -> DragonpilotApiClient:
|
||||
"""Get the shared API client instance."""
|
||||
global _client
|
||||
if _client is None:
|
||||
_client = DragonpilotApiClient()
|
||||
return _client
|
||||
@@ -1,269 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Dragonpilot Routing Provider
|
||||
|
||||
Uses api.dragonpilot.org routing endpoint with device serial authentication.
|
||||
Falls back to OSRM if not authenticated or on error.
|
||||
"""
|
||||
|
||||
from typing import Optional
|
||||
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from ..base import RouteProvider
|
||||
from ..models import Coordinate, Route, Step
|
||||
from .client import get_client
|
||||
|
||||
|
||||
def decode_polyline(polyline: str) -> list[tuple[float, float]]:
|
||||
"""
|
||||
Decode a Google/HERE Encoded Polyline into (lat, lon) tuples.
|
||||
|
||||
Algorithm: https://developers.google.com/maps/documentation/utilities/polylinealgorithm
|
||||
"""
|
||||
coordinates = []
|
||||
index = 0
|
||||
lat = 0
|
||||
lng = 0
|
||||
|
||||
while index < len(polyline):
|
||||
# Decode latitude
|
||||
result = 0
|
||||
shift = 0
|
||||
while True:
|
||||
b = ord(polyline[index]) - 63
|
||||
index += 1
|
||||
result |= (b & 0x1f) << shift
|
||||
shift += 5
|
||||
if b < 0x20:
|
||||
break
|
||||
lat += (~(result >> 1) if result & 1 else result >> 1)
|
||||
|
||||
# Decode longitude
|
||||
result = 0
|
||||
shift = 0
|
||||
while True:
|
||||
b = ord(polyline[index]) - 63
|
||||
index += 1
|
||||
result |= (b & 0x1f) << shift
|
||||
shift += 5
|
||||
if b < 0x20:
|
||||
break
|
||||
lng += (~(result >> 1) if result & 1 else result >> 1)
|
||||
|
||||
coordinates.append((lat / 1e5, lng / 1e5))
|
||||
|
||||
return coordinates
|
||||
|
||||
|
||||
# Map action to OSRM-style maneuver type/modifier
|
||||
ACTION_MAP = {
|
||||
'depart': ('depart', ''),
|
||||
'arrive': ('arrive', ''),
|
||||
'turn': ('turn', ''),
|
||||
'turn-left': ('turn', 'left'),
|
||||
'turn-right': ('turn', 'right'),
|
||||
'turn-slight-left': ('turn', 'slight left'),
|
||||
'turn-slight-right': ('turn', 'slight right'),
|
||||
'turn-sharp-left': ('turn', 'sharp left'),
|
||||
'turn-sharp-right': ('turn', 'sharp right'),
|
||||
'continue': ('continue', 'straight'),
|
||||
'keep': ('continue', ''),
|
||||
'merge': ('merge', ''),
|
||||
'roundabout': ('roundabout', ''),
|
||||
'roundaboutExit': ('roundabout', 'exit'),
|
||||
'ferry': ('ferry', ''),
|
||||
'uturn': ('turn', 'uturn'),
|
||||
}
|
||||
|
||||
|
||||
class DragonpilotRouteProvider(RouteProvider):
|
||||
"""
|
||||
Dragonpilot API routing provider.
|
||||
|
||||
Uses device serial for authentication. Falls back to OSRM on auth failure.
|
||||
|
||||
API Response format:
|
||||
{
|
||||
"route": {
|
||||
"route_id": "...",
|
||||
"distance_m": 6837,
|
||||
"duration_s": 759,
|
||||
"duration_traffic_s": 1595,
|
||||
"polyline": "...",
|
||||
"maneuvers": [{
|
||||
"instruction": "Turn left onto Main Street",
|
||||
"distance_m": 500,
|
||||
"duration_s": 60,
|
||||
"position": {"lat": 25.03, "lon": 121.56},
|
||||
"action": "turn"
|
||||
}],
|
||||
"provider": "here"
|
||||
},
|
||||
"cached": false,
|
||||
"provider": "here"
|
||||
}
|
||||
"""
|
||||
|
||||
name = "dragonpilot"
|
||||
requires_api_key = False
|
||||
supports_traffic = True
|
||||
|
||||
def __init__(self):
|
||||
self._client = get_client()
|
||||
self._fallback = None
|
||||
|
||||
def _get_fallback(self) -> RouteProvider:
|
||||
"""Get fallback OSRM provider."""
|
||||
if self._fallback is None:
|
||||
from ..routing.osrm import OSRMRouteProvider
|
||||
self._fallback = OSRMRouteProvider()
|
||||
return self._fallback
|
||||
|
||||
async def get_route(
|
||||
self,
|
||||
origin: Coordinate,
|
||||
destination: Coordinate,
|
||||
waypoints: Optional[list[Coordinate]] = None,
|
||||
bearing: Optional[float] = None
|
||||
) -> Optional[Route]:
|
||||
"""Calculate route using Dragonpilot API (async)."""
|
||||
if not self._client.is_authenticated:
|
||||
cloudlog.warning("dragonpilot routing: no serial, using osrm fallback")
|
||||
return await self._get_fallback().get_route(origin, destination, waypoints, bearing)
|
||||
|
||||
data = self._build_request(origin, destination, waypoints)
|
||||
response = await self._client.post('/v1/route', data=data, timeout=30)
|
||||
|
||||
if response is None:
|
||||
cloudlog.warning("dragonpilot routing: API error, using osrm fallback")
|
||||
return await self._get_fallback().get_route(origin, destination, waypoints, bearing)
|
||||
|
||||
return self._parse_response(response)
|
||||
|
||||
def get_route_sync(
|
||||
self,
|
||||
origin: Coordinate,
|
||||
destination: Coordinate,
|
||||
waypoints: Optional[list[Coordinate]] = None,
|
||||
bearing: Optional[float] = None
|
||||
) -> Optional[Route]:
|
||||
"""Calculate route using Dragonpilot API (synchronous)."""
|
||||
if not self._client.is_authenticated:
|
||||
cloudlog.warning("dragonpilot routing: no serial, using osrm fallback")
|
||||
return self._get_fallback().get_route_sync(origin, destination, waypoints, bearing)
|
||||
|
||||
data = self._build_request(origin, destination, waypoints)
|
||||
response = self._client.post_sync('/v1/route', data=data, timeout=30)
|
||||
|
||||
if response is None:
|
||||
cloudlog.warning("dragonpilot routing: API error, using osrm fallback")
|
||||
return self._get_fallback().get_route_sync(origin, destination, waypoints, bearing)
|
||||
|
||||
return self._parse_response(response)
|
||||
|
||||
def _build_request(
|
||||
self,
|
||||
origin: Coordinate,
|
||||
destination: Coordinate,
|
||||
waypoints: Optional[list[Coordinate]]
|
||||
) -> dict:
|
||||
"""Build API request body."""
|
||||
origin = origin.to_wgs84() if hasattr(origin, 'to_wgs84') else origin
|
||||
destination = destination.to_wgs84() if hasattr(destination, 'to_wgs84') else destination
|
||||
|
||||
data = {
|
||||
'origin': {'lat': origin.latitude, 'lon': origin.longitude},
|
||||
'destination': {'lat': destination.latitude, 'lon': destination.longitude},
|
||||
}
|
||||
|
||||
if waypoints:
|
||||
data['waypoints'] = []
|
||||
for wp in waypoints:
|
||||
wp = wp.to_wgs84() if hasattr(wp, 'to_wgs84') else wp
|
||||
data['waypoints'].append({'lat': wp.latitude, 'lon': wp.longitude})
|
||||
|
||||
return data
|
||||
|
||||
def _parse_response(self, data: dict) -> Optional[Route]:
|
||||
"""Parse API response into Route object."""
|
||||
if not data:
|
||||
return None
|
||||
|
||||
# Unwrap route object
|
||||
route_data = data.get('route', data)
|
||||
|
||||
# Decode full route geometry
|
||||
full_geometry = []
|
||||
polyline = route_data.get('polyline', '')
|
||||
if polyline:
|
||||
try:
|
||||
for lat, lon in decode_polyline(polyline):
|
||||
coord = Coordinate(lat, lon)
|
||||
full_geometry.append(coord)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Parse maneuvers into steps
|
||||
steps = []
|
||||
maneuvers = route_data.get('maneuvers', [])
|
||||
|
||||
for maneuver in maneuvers:
|
||||
pos = maneuver.get('position', {})
|
||||
action = maneuver.get('action', 'continue')
|
||||
|
||||
# Map action to maneuver type/modifier
|
||||
maneuver_type, maneuver_modifier = ACTION_MAP.get(action, ('continue', ''))
|
||||
|
||||
# Get maneuver point
|
||||
maneuver_point = None
|
||||
if pos.get('lat') is not None and pos.get('lon') is not None:
|
||||
maneuver_point = Coordinate(pos['lat'], pos['lon'])
|
||||
|
||||
# Use full instruction text as the step name
|
||||
instruction = maneuver.get('instruction', '')
|
||||
|
||||
steps.append(Step(
|
||||
distance=maneuver.get('distance_m', 0),
|
||||
duration=maneuver.get('duration_s', 0),
|
||||
name=instruction,
|
||||
maneuver_type=maneuver_type,
|
||||
maneuver_modifier=maneuver_modifier,
|
||||
geometry=[maneuver_point] if maneuver_point else [],
|
||||
speed_limit=None,
|
||||
speed_limit_sign='vienna',
|
||||
maneuver_point=maneuver_point,
|
||||
))
|
||||
|
||||
# Use traffic duration if available
|
||||
duration = route_data.get('duration_traffic_s') or route_data.get('duration_s', 0)
|
||||
|
||||
return Route(
|
||||
steps=steps,
|
||||
distance=route_data.get('distance_m', 0),
|
||||
duration=duration,
|
||||
geometry=full_geometry,
|
||||
provider=self.name,
|
||||
has_traffic=route_data.get('duration_traffic_s') is not None,
|
||||
raw=data,
|
||||
)
|
||||
@@ -1,191 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Dragonpilot Search Provider
|
||||
|
||||
Uses api.dragonpilot.org geocoding endpoint for address search.
|
||||
Falls back to Photon if not authenticated or on error.
|
||||
"""
|
||||
|
||||
from typing import Optional
|
||||
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from ..base import SearchProvider
|
||||
from ..models import Coordinate, SearchResult
|
||||
from .client import get_client
|
||||
|
||||
|
||||
class DragonpilotSearchProvider(SearchProvider):
|
||||
"""
|
||||
Dragonpilot API search provider.
|
||||
|
||||
Uses device serial for authentication. Falls back to Photon on auth failure.
|
||||
|
||||
API Response format:
|
||||
{
|
||||
"results": [{
|
||||
"id": "here:...",
|
||||
"title": "Taipei 101",
|
||||
"address": "No. 7, Section 5, Xinyi Road",
|
||||
"position": {"lat": 25.033, "lon": 121.565},
|
||||
"category": "landmark",
|
||||
"distance_m": 150
|
||||
}],
|
||||
"provider": "here"
|
||||
}
|
||||
"""
|
||||
|
||||
name = "dragonpilot"
|
||||
requires_api_key = False
|
||||
|
||||
def __init__(self):
|
||||
self._client = get_client()
|
||||
self._fallback = None
|
||||
|
||||
def _get_fallback(self) -> SearchProvider:
|
||||
"""Get fallback Photon provider."""
|
||||
if self._fallback is None:
|
||||
from ..search.photon import PhotonSearchProvider
|
||||
self._fallback = PhotonSearchProvider()
|
||||
return self._fallback
|
||||
|
||||
async def search(
|
||||
self,
|
||||
query: str,
|
||||
proximity: Optional[Coordinate] = None,
|
||||
limit: int = 10
|
||||
) -> list[SearchResult]:
|
||||
"""Search for places using Dragonpilot API (async)."""
|
||||
if not query or len(query) < 1:
|
||||
return []
|
||||
|
||||
# Fall back to Photon if not authenticated
|
||||
if not self._client.is_authenticated:
|
||||
cloudlog.debug("dragonpilot search: no serial, using photon fallback")
|
||||
return await self._get_fallback().search(query, proximity, limit)
|
||||
|
||||
params = {'q': query, 'limit': min(limit, 10)}
|
||||
|
||||
if proximity:
|
||||
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||
params['lat'] = prox.latitude
|
||||
params['lon'] = prox.longitude
|
||||
|
||||
data = await self._client.get('/v1/geocode/autocomplete', params=params)
|
||||
|
||||
if data is None:
|
||||
cloudlog.debug("dragonpilot search: API error, using photon fallback")
|
||||
return await self._get_fallback().search(query, proximity, limit)
|
||||
|
||||
return self._parse_results(data, proximity, limit)
|
||||
|
||||
def search_sync(
|
||||
self,
|
||||
query: str,
|
||||
proximity: Optional[Coordinate] = None,
|
||||
limit: int = 10
|
||||
) -> list[SearchResult]:
|
||||
"""Search for places using Dragonpilot API (synchronous)."""
|
||||
if not query or len(query) < 1:
|
||||
return []
|
||||
|
||||
# Fall back to Photon if not authenticated
|
||||
if not self._client.is_authenticated:
|
||||
cloudlog.debug("dragonpilot search: no serial, using photon fallback")
|
||||
return self._get_fallback().search_sync(query, proximity, limit)
|
||||
|
||||
params = {'q': query, 'limit': min(limit, 10)}
|
||||
|
||||
if proximity:
|
||||
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||
params['lat'] = prox.latitude
|
||||
params['lon'] = prox.longitude
|
||||
|
||||
data = self._client.get_sync('/v1/geocode/autocomplete', params=params)
|
||||
|
||||
if data is None:
|
||||
cloudlog.debug("dragonpilot search: API error, using photon fallback")
|
||||
return self._get_fallback().search_sync(query, proximity, limit)
|
||||
|
||||
return self._parse_results(data, proximity, limit)
|
||||
|
||||
async def reverse_geocode(self, coord: Coordinate) -> Optional[SearchResult]:
|
||||
"""Get address from coordinates. Falls back to Photon."""
|
||||
return await self._get_fallback().reverse_geocode(coord)
|
||||
|
||||
def _parse_results(
|
||||
self,
|
||||
data: dict,
|
||||
proximity: Optional[Coordinate],
|
||||
limit: int
|
||||
) -> list[SearchResult]:
|
||||
"""Parse API response into SearchResult list."""
|
||||
results = []
|
||||
|
||||
# Handle both GeoJSON format (features) and simple format (results)
|
||||
items = data.get('features', data.get('results', []))
|
||||
|
||||
for item in items:
|
||||
try:
|
||||
# GeoJSON format
|
||||
if 'geometry' in item:
|
||||
coords = item['geometry']['coordinates']
|
||||
lon, lat = coords[0], coords[1] # GeoJSON is [lon, lat]
|
||||
props = item.get('properties', {})
|
||||
title = props.get('title', 'Unknown')
|
||||
address = props.get('address', '')
|
||||
place_id = props.get('id')
|
||||
distance = props.get('distance_m')
|
||||
# Simple format
|
||||
else:
|
||||
pos = item.get('position', {})
|
||||
lat = pos.get('lat')
|
||||
lon = pos.get('lon')
|
||||
title = item.get('title', 'Unknown')
|
||||
address = item.get('address', '')
|
||||
place_id = item.get('id')
|
||||
distance = item.get('distance_m')
|
||||
|
||||
if lat is None or lon is None:
|
||||
continue
|
||||
|
||||
coord = Coordinate(lat, lon)
|
||||
|
||||
# Calculate distance if not provided
|
||||
if distance is None and proximity:
|
||||
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||
distance = Coordinate(lat, lon).distance_to(prox)
|
||||
|
||||
results.append(SearchResult(
|
||||
name=title,
|
||||
address=address,
|
||||
coordinate=coord,
|
||||
distance=distance,
|
||||
place_id=place_id,
|
||||
provider=self.name,
|
||||
raw=item,
|
||||
))
|
||||
except (KeyError, TypeError, IndexError):
|
||||
continue
|
||||
|
||||
return results[:limit]
|
||||
@@ -1,56 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Provider factory for creating map service instances.
|
||||
|
||||
Uses api.dragonpilot.org for search and routing.
|
||||
"""
|
||||
|
||||
from .config import ProviderConfig
|
||||
from .base import SearchProvider, RouteProvider, TileProvider
|
||||
|
||||
|
||||
class ProviderFactory:
|
||||
"""
|
||||
Factory for creating map providers.
|
||||
|
||||
Uses Dragonpilot API providers which have built-in fallback
|
||||
to free providers (Photon/OSRM) when not authenticated.
|
||||
"""
|
||||
|
||||
@classmethod
|
||||
def create_search_provider(cls, config: ProviderConfig) -> SearchProvider:
|
||||
"""Create search provider."""
|
||||
from .dragonpilot.search import DragonpilotSearchProvider
|
||||
return DragonpilotSearchProvider()
|
||||
|
||||
@classmethod
|
||||
def create_route_provider(cls, config: ProviderConfig) -> RouteProvider:
|
||||
"""Create route provider."""
|
||||
from .dragonpilot.routing import DragonpilotRouteProvider
|
||||
return DragonpilotRouteProvider()
|
||||
|
||||
@classmethod
|
||||
def create_tile_provider(cls, config: ProviderConfig) -> TileProvider:
|
||||
"""Create tile provider."""
|
||||
from .tiles.openfreemap import OpenFreeMapTileProvider
|
||||
return OpenFreeMapTileProvider()
|
||||
@@ -1,188 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Map Service - Main entry point for all map operations.
|
||||
|
||||
Usage:
|
||||
from dragonpilot.dashy.maa.providers import MapService
|
||||
|
||||
# In maad.py (sync):
|
||||
map_service = MapService()
|
||||
route = map_service.route_provider.get_route_sync(origin, dest)
|
||||
|
||||
# In server.py (async):
|
||||
results = await MapService.get_instance().search_provider.search("Taipei 101")
|
||||
|
||||
# Get tile config for frontend:
|
||||
tile_config = map_service.get_tile_config_for_js()
|
||||
"""
|
||||
|
||||
from typing import Optional
|
||||
|
||||
from .config import ProviderConfig
|
||||
from .factory import ProviderFactory
|
||||
from .base import SearchProvider, RouteProvider, TileProvider
|
||||
|
||||
|
||||
class MapService:
|
||||
"""
|
||||
Main entry point for all map operations.
|
||||
|
||||
Provides lazy-loaded access to search, routing, and tile providers.
|
||||
Configuration is read from openpilot Params.
|
||||
"""
|
||||
|
||||
_instance: Optional['MapService'] = None
|
||||
|
||||
def __init__(self, params=None):
|
||||
"""
|
||||
Initialize MapService.
|
||||
|
||||
Args:
|
||||
params: Optional openpilot Params instance.
|
||||
If None, will be created when needed.
|
||||
"""
|
||||
self._params = params
|
||||
self._config: Optional[ProviderConfig] = None
|
||||
self._search_provider: Optional[SearchProvider] = None
|
||||
self._route_provider: Optional[RouteProvider] = None
|
||||
self._tile_provider: Optional[TileProvider] = None
|
||||
|
||||
@classmethod
|
||||
def get_instance(cls, params=None) -> 'MapService':
|
||||
"""
|
||||
Get singleton instance of MapService.
|
||||
|
||||
Args:
|
||||
params: Optional Params instance for first-time initialization
|
||||
"""
|
||||
if cls._instance is None:
|
||||
cls._instance = cls(params)
|
||||
return cls._instance
|
||||
|
||||
@classmethod
|
||||
def reset_instance(cls):
|
||||
"""Reset singleton instance. Useful for testing or config reload."""
|
||||
cls._instance = None
|
||||
|
||||
def _get_params(self):
|
||||
"""Get or create Params instance."""
|
||||
if self._params is None:
|
||||
from openpilot.common.params import Params
|
||||
self._params = Params()
|
||||
return self._params
|
||||
|
||||
def reload_config(self):
|
||||
"""
|
||||
Reload configuration from params and recreate providers.
|
||||
|
||||
Call this after changing provider settings in params.
|
||||
"""
|
||||
self._config = ProviderConfig.from_params(self._get_params())
|
||||
self._search_provider = None
|
||||
self._route_provider = None
|
||||
self._tile_provider = None
|
||||
|
||||
@property
|
||||
def config(self) -> ProviderConfig:
|
||||
"""Get current configuration, loading from params if needed."""
|
||||
if self._config is None:
|
||||
self._config = ProviderConfig.from_params(self._get_params())
|
||||
return self._config
|
||||
|
||||
@property
|
||||
def search_provider(self) -> SearchProvider:
|
||||
"""Get search provider, creating if needed."""
|
||||
if self._search_provider is None:
|
||||
# Import providers to trigger registration
|
||||
self._ensure_providers_imported()
|
||||
self._search_provider = ProviderFactory.create_search_provider(self.config)
|
||||
return self._search_provider
|
||||
|
||||
@property
|
||||
def route_provider(self) -> RouteProvider:
|
||||
"""Get route provider, creating if needed."""
|
||||
if self._route_provider is None:
|
||||
# Import providers to trigger registration
|
||||
self._ensure_providers_imported()
|
||||
self._route_provider = ProviderFactory.create_route_provider(self.config)
|
||||
return self._route_provider
|
||||
|
||||
@property
|
||||
def tile_provider(self) -> TileProvider:
|
||||
"""Get tile provider, creating if needed."""
|
||||
if self._tile_provider is None:
|
||||
# Import providers to trigger registration
|
||||
self._ensure_providers_imported()
|
||||
self._tile_provider = ProviderFactory.create_tile_provider(self.config)
|
||||
return self._tile_provider
|
||||
|
||||
def _ensure_providers_imported(self):
|
||||
"""Import provider modules to trigger registration."""
|
||||
try:
|
||||
from . import search
|
||||
from . import routing
|
||||
from . import tiles
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
def get_tile_config_for_js(self) -> dict:
|
||||
"""
|
||||
Get tile configuration as dict for JavaScript frontend.
|
||||
|
||||
Returns dict suitable for sending to frontend via API.
|
||||
"""
|
||||
tile_config = self.tile_provider.get_tile_config()
|
||||
style = self.tile_provider.get_style_json()
|
||||
return {
|
||||
'provider': self.config.tile_provider.value,
|
||||
'url_template': tile_config.url_template,
|
||||
'style': style,
|
||||
'attribution': tile_config.attribution,
|
||||
'min_zoom': tile_config.min_zoom,
|
||||
'max_zoom': tile_config.max_zoom,
|
||||
}
|
||||
|
||||
def get_provider_info(self) -> dict:
|
||||
"""
|
||||
Get information about current providers.
|
||||
|
||||
Useful for debugging and UI display.
|
||||
"""
|
||||
return {
|
||||
'search': {
|
||||
'provider': self.config.search_provider.value,
|
||||
'name': self.search_provider.name,
|
||||
'requires_api_key': self.search_provider.requires_api_key,
|
||||
},
|
||||
'route': {
|
||||
'provider': self.config.route_provider.value,
|
||||
'name': self.route_provider.name,
|
||||
'requires_api_key': self.route_provider.requires_api_key,
|
||||
'supports_traffic': self.route_provider.supports_traffic,
|
||||
},
|
||||
'tile': {
|
||||
'provider': self.config.tile_provider.value,
|
||||
'name': self.tile_provider.name,
|
||||
'requires_api_key': self.tile_provider.requires_api_key,
|
||||
},
|
||||
}
|
||||
@@ -1,180 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Data models for map providers.
|
||||
|
||||
These models provide a common interface for search results, routes, and coordinates
|
||||
across different providers (OSRM, Mapbox, Google, AMap).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional
|
||||
from enum import Enum
|
||||
|
||||
|
||||
EARTH_MEAN_RADIUS = 6371007.2 # meters
|
||||
|
||||
|
||||
class CoordinateSystem(Enum):
|
||||
"""Coordinate reference system."""
|
||||
WGS84 = "wgs84" # Standard GPS coordinates (used globally)
|
||||
GCJ02 = "gcj02" # China encrypted coordinates (required for China maps)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Coordinate:
|
||||
"""
|
||||
Geographic coordinate with optional coordinate system awareness.
|
||||
|
||||
Supports WGS-84 (standard GPS) and GCJ-02 (China) coordinate systems.
|
||||
Provides distance and bearing calculations.
|
||||
"""
|
||||
latitude: float
|
||||
longitude: float
|
||||
system: CoordinateSystem = CoordinateSystem.WGS84
|
||||
annotations: dict = field(default_factory=dict)
|
||||
|
||||
def distance_to(self, other: Coordinate) -> float:
|
||||
"""Calculate Haversine distance to another coordinate in meters."""
|
||||
dlat = math.radians(other.latitude - self.latitude)
|
||||
dlon = math.radians(other.longitude - self.longitude)
|
||||
|
||||
haversine_dlat = math.sin(dlat / 2.0)
|
||||
haversine_dlat *= haversine_dlat
|
||||
haversine_dlon = math.sin(dlon / 2.0)
|
||||
haversine_dlon *= haversine_dlon
|
||||
|
||||
y = haversine_dlat + \
|
||||
math.cos(math.radians(self.latitude)) * \
|
||||
math.cos(math.radians(other.latitude)) * \
|
||||
haversine_dlon
|
||||
x = 2 * math.asin(math.sqrt(y))
|
||||
return x * EARTH_MEAN_RADIUS
|
||||
|
||||
def bearing_to(self, other: Coordinate) -> float:
|
||||
"""Calculate bearing to another coordinate in degrees (0-360)."""
|
||||
lat1, lat2 = math.radians(self.latitude), math.radians(other.latitude)
|
||||
dlon = math.radians(other.longitude - self.longitude)
|
||||
x = math.sin(dlon) * math.cos(lat2)
|
||||
y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
|
||||
bearing = math.degrees(math.atan2(x, y))
|
||||
return (bearing + 360) % 360
|
||||
|
||||
def to_wgs84(self) -> Coordinate:
|
||||
"""Convert to WGS-84 coordinate system."""
|
||||
if self.system == CoordinateSystem.WGS84:
|
||||
return self
|
||||
from .utils.coordinates import gcj02_to_wgs84
|
||||
lat, lon = gcj02_to_wgs84(self.latitude, self.longitude)
|
||||
return Coordinate(lat, lon, CoordinateSystem.WGS84, self.annotations.copy())
|
||||
|
||||
def to_gcj02(self) -> Coordinate:
|
||||
"""Convert to GCJ-02 coordinate system (for China maps)."""
|
||||
if self.system == CoordinateSystem.GCJ02:
|
||||
return self
|
||||
from .utils.coordinates import wgs84_to_gcj02
|
||||
lat, lon = wgs84_to_gcj02(self.latitude, self.longitude)
|
||||
return Coordinate(lat, lon, CoordinateSystem.GCJ02, self.annotations.copy())
|
||||
|
||||
def as_dict(self) -> dict:
|
||||
"""Convert to dictionary."""
|
||||
return {'latitude': self.latitude, 'longitude': self.longitude}
|
||||
|
||||
def __str__(self) -> str:
|
||||
return f'Coordinate({self.latitude:.6f}, {self.longitude:.6f})'
|
||||
|
||||
def __repr__(self) -> str:
|
||||
return self.__str__()
|
||||
|
||||
def __eq__(self, other) -> bool:
|
||||
if not isinstance(other, Coordinate):
|
||||
return False
|
||||
return self.latitude == other.latitude and self.longitude == other.longitude
|
||||
|
||||
def __hash__(self) -> int:
|
||||
return hash((self.latitude, self.longitude))
|
||||
|
||||
|
||||
@dataclass
|
||||
class SearchResult:
|
||||
"""
|
||||
Normalized search/geocoding result.
|
||||
|
||||
All search providers return results in this format.
|
||||
"""
|
||||
name: str # Display name (e.g., "Taipei 101")
|
||||
address: str # Full address string
|
||||
coordinate: Coordinate # Location
|
||||
distance: Optional[float] = None # Distance from search proximity (meters)
|
||||
place_id: Optional[str] = None # Provider-specific place ID
|
||||
provider: str = "" # Provider name (e.g., "photon", "mapbox")
|
||||
raw: dict = field(default_factory=dict) # Raw provider response for debugging
|
||||
|
||||
|
||||
@dataclass
|
||||
class Step:
|
||||
"""
|
||||
Single navigation step/maneuver in a route.
|
||||
|
||||
Represents one turn or road segment with its geometry.
|
||||
"""
|
||||
distance: float # Step distance in meters
|
||||
duration: float # Step duration in seconds
|
||||
duration_typical: Optional[float] = None # Typical duration (with traffic)
|
||||
name: str = "" # Road/street name
|
||||
maneuver_type: str = "" # Type: turn, fork, off ramp, merge, etc.
|
||||
maneuver_modifier: str = "" # Direction: left, right, slight left, etc.
|
||||
geometry: list[Coordinate] = field(default_factory=list) # Path coordinates
|
||||
speed_limit: Optional[float] = None # Speed limit in m/s
|
||||
speed_limit_sign: str = "vienna" # Sign type: vienna or mutcd
|
||||
maneuver_point: Optional[Coordinate] = None # Explicit maneuver location from OSRM
|
||||
|
||||
|
||||
@dataclass
|
||||
class Route:
|
||||
"""
|
||||
Complete navigation route.
|
||||
|
||||
Contains all steps from origin to destination with total distance/duration.
|
||||
"""
|
||||
steps: list[Step] # List of navigation steps
|
||||
distance: float # Total distance in meters
|
||||
duration: float # Total duration in seconds
|
||||
duration_typical: Optional[float] = None # Typical duration (with traffic)
|
||||
geometry: list[Coordinate] = field(default_factory=list) # Full route polyline
|
||||
provider: str = "" # Provider name
|
||||
has_traffic: bool = False # Whether duration includes traffic
|
||||
raw: dict = field(default_factory=dict) # Raw provider response
|
||||
|
||||
|
||||
@dataclass
|
||||
class TileConfig:
|
||||
"""
|
||||
Map tile configuration for frontend display.
|
||||
"""
|
||||
url_template: str # URL template with {z}/{x}/{y}
|
||||
style_url: Optional[str] = None # MapLibre style URL
|
||||
attribution: str = "" # Map attribution text
|
||||
min_zoom: int = 0
|
||||
max_zoom: int = 22
|
||||
@@ -1,5 +0,0 @@
|
||||
"""Routing providers."""
|
||||
|
||||
from .osrm import OSRMRouteProvider
|
||||
|
||||
__all__ = ['OSRMRouteProvider']
|
||||
@@ -1,190 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
OSRM Route Provider.
|
||||
|
||||
Uses the free OSRM (Open Source Routing Machine) API for routing.
|
||||
No API key required.
|
||||
|
||||
OSRM API Docs: http://project-osrm.org/docs/v5.24.0/api/
|
||||
"""
|
||||
|
||||
from typing import Optional
|
||||
import aiohttp
|
||||
import requests
|
||||
|
||||
from ..base import RouteProvider
|
||||
from ..models import Coordinate, Route, Step
|
||||
|
||||
OSRM_URL = 'https://router.project-osrm.org'
|
||||
|
||||
|
||||
class OSRMRouteProvider(RouteProvider):
|
||||
"""
|
||||
Free OSRM routing provider.
|
||||
|
||||
No API key required. Uses public OSRM demo server.
|
||||
Does not support traffic data.
|
||||
"""
|
||||
|
||||
name = "osrm"
|
||||
requires_api_key = False
|
||||
supports_traffic = False
|
||||
|
||||
def __init__(self):
|
||||
"""Initialize OSRM provider."""
|
||||
pass
|
||||
|
||||
async def get_route(
|
||||
self,
|
||||
origin: Coordinate,
|
||||
destination: Coordinate,
|
||||
waypoints: Optional[list[Coordinate]] = None,
|
||||
bearing: Optional[float] = None
|
||||
) -> Optional[Route]:
|
||||
"""Calculate route using OSRM API (async)."""
|
||||
url, params = self._build_request(origin, destination, waypoints, bearing)
|
||||
|
||||
try:
|
||||
async with aiohttp.ClientSession() as session:
|
||||
async with session.get(url, params=params, timeout=aiohttp.ClientTimeout(total=10)) as resp:
|
||||
if resp.status != 200:
|
||||
return None
|
||||
data = await resp.json()
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
return self._parse_response(data)
|
||||
|
||||
def get_route_sync(
|
||||
self,
|
||||
origin: Coordinate,
|
||||
destination: Coordinate,
|
||||
waypoints: Optional[list[Coordinate]] = None,
|
||||
bearing: Optional[float] = None
|
||||
) -> Optional[Route]:
|
||||
"""Calculate route using OSRM API (synchronous for maad.py)."""
|
||||
url, params = self._build_request(origin, destination, waypoints, bearing)
|
||||
|
||||
try:
|
||||
resp = requests.get(url, params=params, timeout=10)
|
||||
if resp.status_code != 200:
|
||||
return None
|
||||
data = resp.json()
|
||||
if data.get('code') != 'Ok':
|
||||
return None
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
return self._parse_response(data)
|
||||
|
||||
def _build_request(
|
||||
self,
|
||||
origin: Coordinate,
|
||||
destination: Coordinate,
|
||||
waypoints: Optional[list[Coordinate]],
|
||||
bearing: Optional[float]
|
||||
) -> tuple[str, dict]:
|
||||
"""Build OSRM API request URL and params."""
|
||||
# Convert to WGS-84 if needed (OSRM uses WGS-84)
|
||||
origin = origin.to_wgs84() if hasattr(origin, 'to_wgs84') and origin.system.value != 'wgs84' else origin
|
||||
destination = destination.to_wgs84() if hasattr(destination, 'to_wgs84') and destination.system.value != 'wgs84' else destination
|
||||
|
||||
# Build coordinate string: lon,lat;lon,lat;...
|
||||
all_coords = [(origin.longitude, origin.latitude)]
|
||||
if waypoints:
|
||||
for wp in waypoints:
|
||||
wp = wp.to_wgs84() if hasattr(wp, 'to_wgs84') and wp.system.value != 'wgs84' else wp
|
||||
all_coords.append((wp.longitude, wp.latitude))
|
||||
all_coords.append((destination.longitude, destination.latitude))
|
||||
|
||||
# Limit coordinate precision to 6 decimal places (about 0.1m accuracy)
|
||||
coords_str = ';'.join([f'{lon:.6f},{lat:.6f}' for lon, lat in all_coords])
|
||||
url = f"{OSRM_URL}/route/v1/driving/{coords_str}"
|
||||
|
||||
params = {
|
||||
'overview': 'full',
|
||||
'geometries': 'geojson',
|
||||
'steps': 'true',
|
||||
}
|
||||
|
||||
# Add bearing if provided (helps with route direction at start)
|
||||
# OSRM bearings format: bearing,range for each coord, separated by ;
|
||||
# Note: Disabled for now due to URL encoding issues with semicolons
|
||||
# TODO: Re-enable once we properly handle the bearings parameter
|
||||
# if bearing is not None:
|
||||
# bearing_parts = [f"{int(bearing) % 360},90"] + [''] * (len(all_coords) - 1)
|
||||
# url += f"?bearings={';'.join(bearing_parts)}"
|
||||
# return url, params
|
||||
|
||||
return url, params
|
||||
|
||||
def _parse_response(self, data: dict) -> Optional[Route]:
|
||||
"""Parse OSRM API response into Route object."""
|
||||
if data.get('code') != 'Ok' or not data.get('routes'):
|
||||
return None
|
||||
|
||||
route_data = data['routes'][0]
|
||||
steps = []
|
||||
full_geometry = []
|
||||
|
||||
for leg in route_data.get('legs', []):
|
||||
for step in leg.get('steps', []):
|
||||
maneuver = step.get('maneuver', {})
|
||||
|
||||
# Parse geometry coordinates
|
||||
geometry = []
|
||||
for coord in step.get('geometry', {}).get('coordinates', []):
|
||||
# OSRM uses [lon, lat] order
|
||||
c = Coordinate(coord[1], coord[0])
|
||||
geometry.append(c)
|
||||
full_geometry.append(c)
|
||||
|
||||
step_name = step.get('name', '')
|
||||
|
||||
# Extract explicit maneuver location from OSRM
|
||||
maneuver_location = maneuver.get('location') # [lon, lat]
|
||||
maneuver_point = None
|
||||
if maneuver_location and len(maneuver_location) == 2:
|
||||
maneuver_point = Coordinate(maneuver_location[1], maneuver_location[0])
|
||||
|
||||
steps.append(Step(
|
||||
distance=step.get('distance', 0),
|
||||
duration=step.get('duration', 0),
|
||||
name=step_name,
|
||||
maneuver_type=maneuver.get('type', ''),
|
||||
maneuver_modifier=maneuver.get('modifier', ''),
|
||||
geometry=geometry,
|
||||
speed_limit=None,
|
||||
speed_limit_sign='vienna',
|
||||
maneuver_point=maneuver_point,
|
||||
))
|
||||
|
||||
return Route(
|
||||
steps=steps,
|
||||
distance=route_data.get('distance', 0),
|
||||
duration=route_data.get('duration', 0),
|
||||
geometry=full_geometry,
|
||||
provider=self.name,
|
||||
has_traffic=False,
|
||||
raw=route_data,
|
||||
)
|
||||
@@ -1,5 +0,0 @@
|
||||
"""Search providers."""
|
||||
|
||||
from .photon import PhotonSearchProvider
|
||||
|
||||
__all__ = ['PhotonSearchProvider']
|
||||
@@ -1,198 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Photon Search Provider.
|
||||
|
||||
Uses the free Photon (Komoot) geocoding API for address search.
|
||||
No API key required, no rate limits.
|
||||
|
||||
Photon API: https://photon.komoot.io/
|
||||
"""
|
||||
|
||||
from typing import Optional
|
||||
import aiohttp
|
||||
import requests
|
||||
|
||||
from ..base import SearchProvider
|
||||
from ..models import Coordinate, SearchResult
|
||||
|
||||
PHOTON_URL = 'https://photon.komoot.io/api'
|
||||
|
||||
|
||||
class PhotonSearchProvider(SearchProvider):
|
||||
"""
|
||||
Free Photon geocoding provider.
|
||||
|
||||
No API key required. Fast, reliable, uses OpenStreetMap data.
|
||||
"""
|
||||
|
||||
name = "photon"
|
||||
requires_api_key = False
|
||||
|
||||
def __init__(self):
|
||||
"""Initialize Photon provider."""
|
||||
pass
|
||||
|
||||
async def search(
|
||||
self,
|
||||
query: str,
|
||||
proximity: Optional[Coordinate] = None,
|
||||
limit: int = 10
|
||||
) -> list[SearchResult]:
|
||||
"""Search for places using Photon API (async)."""
|
||||
if not query or len(query) < 2:
|
||||
return []
|
||||
|
||||
params = {'q': query, 'limit': min(limit + 5, 15)} # Request extra for filtering
|
||||
|
||||
if proximity:
|
||||
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||
params['lat'] = prox.latitude
|
||||
params['lon'] = prox.longitude
|
||||
|
||||
try:
|
||||
async with aiohttp.ClientSession() as session:
|
||||
async with session.get(PHOTON_URL, params=params, timeout=aiohttp.ClientTimeout(total=10)) as resp:
|
||||
if resp.status != 200:
|
||||
return []
|
||||
data = await resp.json()
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
return self._parse_results(data, proximity, limit)
|
||||
|
||||
def search_sync(
|
||||
self,
|
||||
query: str,
|
||||
proximity: Optional[Coordinate] = None,
|
||||
limit: int = 10
|
||||
) -> list[SearchResult]:
|
||||
"""Search for places using Photon API (synchronous)."""
|
||||
if not query or len(query) < 2:
|
||||
return []
|
||||
|
||||
params = {'q': query, 'limit': min(limit + 5, 15)}
|
||||
|
||||
if proximity:
|
||||
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||
params['lat'] = prox.latitude
|
||||
params['lon'] = prox.longitude
|
||||
|
||||
try:
|
||||
resp = requests.get(PHOTON_URL, params=params, timeout=10)
|
||||
if resp.status_code != 200:
|
||||
return []
|
||||
data = resp.json()
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
return self._parse_results(data, proximity, limit)
|
||||
|
||||
async def reverse_geocode(
|
||||
self,
|
||||
coord: Coordinate
|
||||
) -> Optional[SearchResult]:
|
||||
"""Get address from coordinates using Photon reverse API."""
|
||||
wgs_coord = coord.to_wgs84() if hasattr(coord, 'to_wgs84') else coord
|
||||
url = f"{PHOTON_URL}/reverse"
|
||||
params = {'lat': wgs_coord.latitude, 'lon': wgs_coord.longitude}
|
||||
|
||||
try:
|
||||
async with aiohttp.ClientSession() as session:
|
||||
async with session.get(url, params=params, timeout=aiohttp.ClientTimeout(total=10)) as resp:
|
||||
if resp.status != 200:
|
||||
return None
|
||||
data = await resp.json()
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
features = data.get('features', [])
|
||||
if not features:
|
||||
return None
|
||||
|
||||
feature = features[0]
|
||||
props = feature.get('properties', {})
|
||||
parts = [props.get('name'), props.get('street'), props.get('city')]
|
||||
address = ', '.join(filter(None, parts))
|
||||
|
||||
return SearchResult(
|
||||
name=address.split(',')[0] if address else 'Unknown',
|
||||
address=address or 'Unknown location',
|
||||
coordinate=coord,
|
||||
provider=self.name,
|
||||
raw=feature,
|
||||
)
|
||||
|
||||
def _parse_results(
|
||||
self,
|
||||
data: dict,
|
||||
proximity: Optional[Coordinate],
|
||||
limit: int
|
||||
) -> list[SearchResult]:
|
||||
"""Parse Photon GeoJSON response into SearchResult list."""
|
||||
results = []
|
||||
|
||||
for feature in data.get('features', []):
|
||||
try:
|
||||
coords = feature['geometry']['coordinates']
|
||||
props = feature.get('properties', {})
|
||||
|
||||
# Create coordinate (Photon uses [lon, lat] GeoJSON order)
|
||||
coord = Coordinate(coords[1], coords[0])
|
||||
|
||||
# Build address from properties
|
||||
parts = [
|
||||
props.get('name'),
|
||||
props.get('street'),
|
||||
props.get('city'),
|
||||
props.get('state'),
|
||||
props.get('country')
|
||||
]
|
||||
address = ', '.join(filter(None, parts))
|
||||
|
||||
# Calculate distance if proximity provided
|
||||
distance = None
|
||||
if proximity:
|
||||
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||
search_coord = Coordinate(coords[1], coords[0]) # Use WGS84 for distance
|
||||
distance = search_coord.distance_to(prox)
|
||||
|
||||
# Determine display name
|
||||
name = props.get('name') or props.get('street') or (address.split(',')[0] if address else 'Unknown')
|
||||
|
||||
results.append(SearchResult(
|
||||
name=name,
|
||||
address=address or 'Unknown location',
|
||||
coordinate=coord,
|
||||
distance=distance,
|
||||
place_id=props.get('osm_id'),
|
||||
provider=self.name,
|
||||
raw=feature,
|
||||
))
|
||||
except (KeyError, IndexError):
|
||||
continue
|
||||
|
||||
# Sort by distance if proximity provided
|
||||
if proximity:
|
||||
results.sort(key=lambda r: r.distance if r.distance is not None else float('inf'))
|
||||
|
||||
return results[:limit]
|
||||
@@ -1,6 +0,0 @@
|
||||
"""Tile providers."""
|
||||
|
||||
# Import providers to register them with the factory
|
||||
from .openfreemap import OpenFreeMapTileProvider
|
||||
|
||||
__all__ = ['OpenFreeMapTileProvider']
|
||||
@@ -1,88 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
OpenFreeMap Tile Provider.
|
||||
|
||||
Uses free OpenFreeMap tiles. No API key required.
|
||||
|
||||
OpenFreeMap: https://openfreemap.org/
|
||||
"""
|
||||
|
||||
from ..base import TileProvider
|
||||
from ..models import TileConfig
|
||||
from ..factory import ProviderFactory
|
||||
from ..config import TileProviderType
|
||||
|
||||
|
||||
@ProviderFactory.register_tile(TileProviderType.OPENFREEMAP)
|
||||
class OpenFreeMapTileProvider(TileProvider):
|
||||
"""
|
||||
Free OpenFreeMap tile provider.
|
||||
|
||||
No API key required. Uses OpenStreetMap data with nice styling.
|
||||
"""
|
||||
|
||||
name = "openfreemap"
|
||||
requires_api_key = False
|
||||
|
||||
def __init__(self, api_key: str = None):
|
||||
"""Initialize OpenFreeMap provider."""
|
||||
pass # No API key needed
|
||||
|
||||
def get_tile_config(self) -> TileConfig:
|
||||
"""Get tile configuration."""
|
||||
return TileConfig(
|
||||
url_template="https://tiles.openfreemap.org/planet/{z}/{x}/{y}.pbf",
|
||||
style_url="https://tiles.openfreemap.org/styles/liberty",
|
||||
attribution='<a href="https://openfreemap.org">OpenFreeMap</a> | <a href="https://www.openstreetmap.org/copyright">OpenStreetMap</a>',
|
||||
min_zoom=0,
|
||||
max_zoom=20,
|
||||
)
|
||||
|
||||
def get_style_json(self) -> dict:
|
||||
"""
|
||||
Get MapLibre GL style JSON.
|
||||
|
||||
This style is optimized for navigation display with good contrast
|
||||
and road visibility.
|
||||
"""
|
||||
return {
|
||||
"version": 8,
|
||||
"name": "OpenFreeMap Liberty",
|
||||
"sources": {
|
||||
"openmaptiles": {
|
||||
"type": "vector",
|
||||
"url": "https://tiles.openfreemap.org/planet"
|
||||
}
|
||||
},
|
||||
"glyphs": "https://tiles.openfreemap.org/fonts/{fontstack}/{range}.pbf",
|
||||
"sprite": "https://tiles.openfreemap.org/styles/liberty/sprite",
|
||||
"layers": [
|
||||
# Simplified layer config - the actual style is loaded from style_url
|
||||
# This is a fallback/reference
|
||||
{
|
||||
"id": "background",
|
||||
"type": "background",
|
||||
"paint": {"background-color": "#f8f4f0"}
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
"""Utility functions for map providers."""
|
||||
|
||||
from .coordinates import wgs84_to_gcj02, gcj02_to_wgs84
|
||||
|
||||
__all__ = ['wgs84_to_gcj02', 'gcj02_to_wgs84']
|
||||
@@ -1,114 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
WGS-84 <-> GCJ-02 coordinate transformations.
|
||||
|
||||
GCJ-02 (Chinese: 火星坐标系 "Mars Coordinates") is the coordinate system
|
||||
mandated for maps in China. All maps in China must use GCJ-02, including
|
||||
AMap (Gaode/高德) and Baidu Maps.
|
||||
|
||||
GPS devices output WGS-84 coordinates, which must be converted to GCJ-02
|
||||
for use with Chinese map services, and vice versa.
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
# Krasovsky 1940 ellipsoid parameters
|
||||
_A = 6378245.0 # Semi-major axis
|
||||
_EE = 0.00669342162296594323 # Eccentricity squared
|
||||
|
||||
|
||||
def _out_of_china(lat: float, lon: float) -> bool:
|
||||
"""Check if coordinates are outside China's approximate bounds."""
|
||||
return not (72.004 <= lon <= 137.8347 and 0.8293 <= lat <= 55.8271)
|
||||
|
||||
|
||||
def _transform_lat(x: float, y: float) -> float:
|
||||
"""Transform latitude offset."""
|
||||
ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * math.sqrt(abs(x))
|
||||
ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
|
||||
ret += (20.0 * math.sin(y * math.pi) + 40.0 * math.sin(y / 3.0 * math.pi)) * 2.0 / 3.0
|
||||
ret += (160.0 * math.sin(y / 12.0 * math.pi) + 320 * math.sin(y * math.pi / 30.0)) * 2.0 / 3.0
|
||||
return ret
|
||||
|
||||
|
||||
def _transform_lon(x: float, y: float) -> float:
|
||||
"""Transform longitude offset."""
|
||||
ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * math.sqrt(abs(x))
|
||||
ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
|
||||
ret += (20.0 * math.sin(x * math.pi) + 40.0 * math.sin(x / 3.0 * math.pi)) * 2.0 / 3.0
|
||||
ret += (150.0 * math.sin(x / 12.0 * math.pi) + 300.0 * math.sin(x / 30.0 * math.pi)) * 2.0 / 3.0
|
||||
return ret
|
||||
|
||||
|
||||
def wgs84_to_gcj02(lat: float, lon: float) -> tuple[float, float]:
|
||||
"""
|
||||
Convert WGS-84 coordinates to GCJ-02.
|
||||
|
||||
Args:
|
||||
lat: WGS-84 latitude
|
||||
lon: WGS-84 longitude
|
||||
|
||||
Returns:
|
||||
Tuple of (GCJ-02 latitude, GCJ-02 longitude)
|
||||
"""
|
||||
if _out_of_china(lat, lon):
|
||||
return lat, lon
|
||||
|
||||
dlat = _transform_lat(lon - 105.0, lat - 35.0)
|
||||
dlon = _transform_lon(lon - 105.0, lat - 35.0)
|
||||
|
||||
radlat = lat / 180.0 * math.pi
|
||||
magic = math.sin(radlat)
|
||||
magic = 1 - _EE * magic * magic
|
||||
sqrtmagic = math.sqrt(magic)
|
||||
|
||||
dlat = (dlat * 180.0) / ((_A * (1 - _EE)) / (magic * sqrtmagic) * math.pi)
|
||||
dlon = (dlon * 180.0) / (_A / sqrtmagic * math.cos(radlat) * math.pi)
|
||||
|
||||
return lat + dlat, lon + dlon
|
||||
|
||||
|
||||
def gcj02_to_wgs84(lat: float, lon: float) -> tuple[float, float]:
|
||||
"""
|
||||
Convert GCJ-02 coordinates to WGS-84 (approximate inverse).
|
||||
|
||||
Uses iterative approach for better accuracy (~0.5m error).
|
||||
|
||||
Args:
|
||||
lat: GCJ-02 latitude
|
||||
lon: GCJ-02 longitude
|
||||
|
||||
Returns:
|
||||
Tuple of (WGS-84 latitude, WGS-84 longitude)
|
||||
"""
|
||||
if _out_of_china(lat, lon):
|
||||
return lat, lon
|
||||
|
||||
# Iterative approach for better accuracy
|
||||
wgs_lat, wgs_lon = lat, lon
|
||||
for _ in range(5):
|
||||
gcj_lat, gcj_lon = wgs84_to_gcj02(wgs_lat, wgs_lon)
|
||||
wgs_lat += lat - gcj_lat
|
||||
wgs_lon += lon - gcj_lon
|
||||
|
||||
return wgs_lat, wgs_lon
|
||||
@@ -1,447 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2026, Rick Lan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||
for non-commercial purposes only, subject to the following conditions:
|
||||
|
||||
- The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||
generate revenue) is prohibited without explicit written permission from
|
||||
the copyright holder.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
Route Tracker - OsmAnd-inspired route tracking logic
|
||||
|
||||
Handles:
|
||||
- Step transition detection (finding closest segment among lookahead)
|
||||
- Reroute detection with time-based debounce
|
||||
- GPS accuracy-aware tolerances
|
||||
|
||||
References:
|
||||
- OsmAnd RoutingHelper.java: lookAheadFindMinOrthogonalDistance (line 562)
|
||||
- OsmAnd RoutingHelperUtils.java: bearing thresholds (lines 175, 200)
|
||||
- OsmAnd AnnounceTimeDistances.java: positioning tolerance (line 25)
|
||||
"""
|
||||
import time
|
||||
from typing import Optional, TYPE_CHECKING
|
||||
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from dragonpilot.dashy.maa.helpers import (
|
||||
Coordinate,
|
||||
minimum_distance,
|
||||
normalize_angle,
|
||||
project_onto_segment,
|
||||
)
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from dragonpilot.dashy.maa.maad import Step
|
||||
|
||||
|
||||
# Reroute parameters (OsmAnd-inspired)
|
||||
REROUTE_DISTANCE_BASE = 25 # meters off route before considering reroute
|
||||
REROUTE_DEBOUNCE_TIME = 3.0 # seconds of sustained deviation before reroute
|
||||
POSITIONING_TOLERANCE = 12 # meters GPS error buffer
|
||||
|
||||
# Bearing thresholds (OsmAnd: RoutingHelperUtils.java)
|
||||
WRONG_DIRECTION_THRESHOLD = 90.0 # degrees - wrong movement direction
|
||||
UTURN_THRESHOLD = 135.0 # degrees - U-turn needed
|
||||
|
||||
# Speed-based lookahead (OsmAnd: RoutingHelper.java:562)
|
||||
LOOKAHEAD_SLOW = 8 # segments for slow speed
|
||||
LOOKAHEAD_FAST = 15 # segments for fast speed (highway)
|
||||
FAST_SPEED_THRESHOLD = 20.0 # m/s (~72 km/h) to switch to fast lookahead
|
||||
|
||||
# Step transition - snap zone around maneuver point
|
||||
MANEUVER_SNAP_ZONE = 100.0 # meters before/after maneuver to check
|
||||
|
||||
# Maneuver point detection (bearing-based)
|
||||
MANEUVER_PROXIMITY_THRESHOLD = 50.0 # meters - must be this close to check bearing
|
||||
MANEUVER_BEHIND_ANGLE = 90.0 # degrees - point is "behind" if angle > this
|
||||
|
||||
|
||||
class RouteTracker:
|
||||
"""Tracks position along a route with snap-to-road logic."""
|
||||
|
||||
def __init__(self):
|
||||
self.step_idx: Optional[int] = None
|
||||
self.segment_idx: int = 0 # Current segment within step
|
||||
self.deviation_start_time: Optional[float] = None
|
||||
|
||||
def reset(self):
|
||||
"""Reset tracking state."""
|
||||
self.step_idx = None
|
||||
self.segment_idx = 0
|
||||
self.deviation_start_time = None
|
||||
|
||||
def set_step(self, idx: int):
|
||||
"""Set current step index."""
|
||||
self.step_idx = idx
|
||||
self.segment_idx = 0
|
||||
self.deviation_start_time = None
|
||||
|
||||
def get_lookahead(self, speed: float) -> int:
|
||||
"""Get lookahead distance based on speed (OsmAnd: RoutingHelper.java:562)."""
|
||||
return LOOKAHEAD_FAST if speed > FAST_SPEED_THRESHOLD else LOOKAHEAD_SLOW
|
||||
|
||||
def snap_to_route(
|
||||
self,
|
||||
route: list['Step'],
|
||||
position: Coordinate,
|
||||
bearing: Optional[float] = None,
|
||||
speed: float = 0.0,
|
||||
) -> tuple[int, int, float, float, float]:
|
||||
"""Snap position to route geometry.
|
||||
|
||||
Returns:
|
||||
(step_idx, segment_idx, t, distance_to_route, distance_along_step) where:
|
||||
- step_idx: which step we're on
|
||||
- segment_idx: which segment within the step
|
||||
- t: position along segment (0-1)
|
||||
- distance_to_route: perpendicular distance to route
|
||||
- distance_along_step: how far along the current step (meters)
|
||||
"""
|
||||
if not route or self.step_idx is None:
|
||||
return self.step_idx or 0, 0, 0.0, float('inf'), 0.0
|
||||
|
||||
lookahead = self.get_lookahead(speed)
|
||||
best_dist = float('inf')
|
||||
best_step = self.step_idx
|
||||
best_seg = 0
|
||||
best_t = 0.0
|
||||
|
||||
# Search current step and lookahead steps
|
||||
end_idx = min(self.step_idx + lookahead, len(route))
|
||||
for step_idx in range(self.step_idx, end_idx):
|
||||
step = route[step_idx]
|
||||
|
||||
for seg_idx in range(len(step.geometry) - 1):
|
||||
a = step.geometry[seg_idx]
|
||||
b = step.geometry[seg_idx + 1]
|
||||
seg_len = a.distance_to(b)
|
||||
if seg_len < 1.0:
|
||||
continue
|
||||
|
||||
_, t, dist = project_onto_segment(a, b, position)
|
||||
|
||||
# If we have bearing, validate direction for non-current steps
|
||||
if bearing is not None and step_idx > self.step_idx:
|
||||
route_bearing = a.bearing_to(b)
|
||||
bearing_diff = abs(normalize_angle(bearing - route_bearing))
|
||||
if bearing_diff > WRONG_DIRECTION_THRESHOLD:
|
||||
continue # Skip segments going wrong direction
|
||||
|
||||
if dist < best_dist:
|
||||
best_dist = dist
|
||||
best_step = step_idx
|
||||
best_seg = seg_idx
|
||||
best_t = t
|
||||
|
||||
# Calculate distance along the best step
|
||||
dist_along = 0.0
|
||||
if best_step < len(route):
|
||||
step = route[best_step]
|
||||
for i in range(best_seg):
|
||||
if i < len(step.geometry) - 1:
|
||||
dist_along += step.geometry[i].distance_to(step.geometry[i + 1])
|
||||
# Add partial distance in current segment
|
||||
if best_seg < len(step.geometry) - 1:
|
||||
seg_len = step.geometry[best_seg].distance_to(step.geometry[best_seg + 1])
|
||||
dist_along += seg_len * best_t
|
||||
|
||||
return best_step, best_seg, best_t, best_dist, dist_along
|
||||
|
||||
def find_closest_step(
|
||||
self,
|
||||
route: list['Step'],
|
||||
position: Coordinate,
|
||||
bearing: Optional[float],
|
||||
speed: float = 0.0,
|
||||
) -> int:
|
||||
"""Find closest step among current and lookahead steps (OsmAnd-style).
|
||||
|
||||
Uses orthogonal distance to segments with bearing validation.
|
||||
Returns the step index with minimum distance that matches travel direction.
|
||||
"""
|
||||
if not route or position is None or self.step_idx is None:
|
||||
return self.step_idx or 0
|
||||
|
||||
lookahead = self.get_lookahead(speed)
|
||||
|
||||
if bearing is None:
|
||||
return self._find_closest_by_distance(route, position, lookahead)
|
||||
|
||||
min_dist = float('inf')
|
||||
closest_idx = self.step_idx
|
||||
|
||||
end_idx = min(self.step_idx + lookahead, len(route))
|
||||
for step_idx in range(self.step_idx, end_idx):
|
||||
step = route[step_idx]
|
||||
|
||||
for i in range(len(step.geometry) - 1):
|
||||
a = step.geometry[i]
|
||||
b = step.geometry[i + 1]
|
||||
seg_len = a.distance_to(b)
|
||||
if seg_len < 1.0:
|
||||
continue
|
||||
|
||||
d = minimum_distance(a, b, position)
|
||||
if d < min_dist:
|
||||
# Check bearing match before accepting
|
||||
route_bearing = a.bearing_to(b)
|
||||
bearing_diff = abs(normalize_angle(bearing - route_bearing))
|
||||
|
||||
# Only accept if bearing within 90 degrees (not going backwards)
|
||||
if bearing_diff < WRONG_DIRECTION_THRESHOLD or step_idx == self.step_idx:
|
||||
min_dist = d
|
||||
closest_idx = step_idx
|
||||
|
||||
return closest_idx
|
||||
|
||||
def _find_closest_by_distance(
|
||||
self,
|
||||
route: list['Step'],
|
||||
position: Coordinate,
|
||||
lookahead: int,
|
||||
) -> int:
|
||||
"""Fallback: find closest step by distance only (no bearing check)."""
|
||||
min_dist = float('inf')
|
||||
closest_idx = self.step_idx
|
||||
|
||||
end_idx = min(self.step_idx + lookahead, len(route))
|
||||
for step_idx in range(self.step_idx, end_idx):
|
||||
step = route[step_idx]
|
||||
for i in range(len(step.geometry) - 1):
|
||||
a = step.geometry[i]
|
||||
b = step.geometry[i + 1]
|
||||
if a.distance_to(b) < 1.0:
|
||||
continue
|
||||
d = minimum_distance(a, b, position)
|
||||
if d < min_dist:
|
||||
min_dist = d
|
||||
closest_idx = step_idx
|
||||
|
||||
return closest_idx
|
||||
|
||||
def should_reroute(
|
||||
self,
|
||||
route: list['Step'],
|
||||
position: Coordinate,
|
||||
gps_accuracy: float = 0.0,
|
||||
) -> bool:
|
||||
"""Check if route should be recomputed (OsmAnd-style time-based debounce).
|
||||
|
||||
Returns True if vehicle has been off-route for REROUTE_DEBOUNCE_TIME seconds.
|
||||
Uses GPS accuracy-aware tolerance for distance threshold.
|
||||
"""
|
||||
if self.step_idx is None or not route:
|
||||
return True
|
||||
|
||||
# Don't reroute in last segment
|
||||
if self.step_idx == len(route) - 1:
|
||||
return False
|
||||
|
||||
# GPS accuracy-aware tolerance (OsmAnd: AnnounceTimeDistances.java:25)
|
||||
tolerance = POSITIONING_TOLERANCE / 2 + gps_accuracy
|
||||
reroute_threshold = max(REROUTE_DISTANCE_BASE, tolerance)
|
||||
|
||||
# Find minimum distance to current step geometry
|
||||
min_d = reroute_threshold + 1
|
||||
step = route[self.step_idx]
|
||||
|
||||
for i in range(len(step.geometry) - 1):
|
||||
a = step.geometry[i]
|
||||
b = step.geometry[i + 1]
|
||||
if a.distance_to(b) < 1.0:
|
||||
continue
|
||||
min_d = min(min_d, minimum_distance(a, b, position))
|
||||
|
||||
now = time.monotonic()
|
||||
|
||||
# Time-based debounce (OsmAnd: 10 seconds of sustained deviation)
|
||||
if min_d > reroute_threshold:
|
||||
if self.deviation_start_time is None:
|
||||
self.deviation_start_time = now
|
||||
cloudlog.info(f"maad: deviation detected, dist={min_d:.0f}m > threshold={reroute_threshold:.0f}m")
|
||||
elif now - self.deviation_start_time > REROUTE_DEBOUNCE_TIME:
|
||||
cloudlog.warning(f"maad: rerouting after {REROUTE_DEBOUNCE_TIME}s deviation")
|
||||
return True
|
||||
else:
|
||||
# Back on route - reset timer
|
||||
if self.deviation_start_time is not None:
|
||||
cloudlog.info("maad: back on route, resetting deviation timer")
|
||||
self.deviation_start_time = None
|
||||
|
||||
return False
|
||||
|
||||
def _get_geometry_tail(self, geometry: list[Coordinate], max_dist: float) -> list[Coordinate]:
|
||||
"""Get the last max_dist meters of geometry (before maneuver point)."""
|
||||
if len(geometry) < 2:
|
||||
return geometry
|
||||
|
||||
# Walk backwards from end
|
||||
result = [geometry[-1]]
|
||||
dist = 0.0
|
||||
for i in range(len(geometry) - 2, -1, -1):
|
||||
seg_dist = geometry[i].distance_to(geometry[i + 1])
|
||||
if dist + seg_dist > max_dist:
|
||||
break
|
||||
result.insert(0, geometry[i])
|
||||
dist += seg_dist
|
||||
|
||||
return result
|
||||
|
||||
def _get_geometry_head(self, geometry: list[Coordinate], max_dist: float) -> list[Coordinate]:
|
||||
"""Get the first max_dist meters of geometry (after maneuver point)."""
|
||||
if len(geometry) < 2:
|
||||
return geometry
|
||||
|
||||
# Walk forwards from start
|
||||
result = [geometry[0]]
|
||||
dist = 0.0
|
||||
for i in range(1, len(geometry)):
|
||||
seg_dist = geometry[i - 1].distance_to(geometry[i])
|
||||
if dist + seg_dist > max_dist:
|
||||
break
|
||||
result.append(geometry[i])
|
||||
dist += seg_dist
|
||||
|
||||
return result
|
||||
|
||||
def _snap_to_geometry(self, geometry: list[Coordinate], position: Coordinate) -> float:
|
||||
"""Find minimum distance from position to geometry segments."""
|
||||
min_dist = float('inf')
|
||||
for i in range(len(geometry) - 1):
|
||||
a, b = geometry[i], geometry[i + 1]
|
||||
if a.distance_to(b) < 1.0:
|
||||
continue
|
||||
_, _, dist = project_onto_segment(a, b, position)
|
||||
min_dist = min(min_dist, dist)
|
||||
return min_dist
|
||||
|
||||
def _check_passed_maneuver_point(
|
||||
self,
|
||||
maneuver_pt: Coordinate,
|
||||
position: Coordinate,
|
||||
bearing: Optional[float],
|
||||
) -> bool:
|
||||
"""Check if vehicle has passed the maneuver point using bearing.
|
||||
|
||||
A vehicle has "passed" a point when that point is behind it (>90° off heading).
|
||||
|
||||
Before fork: Vehicle → → → [Fork Point]
|
||||
bearing_to_fork ≈ 0° (ahead)
|
||||
|
||||
After fork: [Fork Point] Vehicle → → →
|
||||
bearing_to_fork ≈ 180° (behind)
|
||||
"""
|
||||
dist = position.distance_to(maneuver_pt)
|
||||
|
||||
if bearing is not None:
|
||||
# Calculate bearing FROM vehicle TO maneuver point
|
||||
bearing_to_pt = position.bearing_to(maneuver_pt)
|
||||
angle_diff = abs(normalize_angle(bearing - bearing_to_pt))
|
||||
|
||||
# Debug logging
|
||||
if dist < 500:
|
||||
cloudlog.info(f"maad: maneuver check step={self.step_idx} dist={dist:.1f}m "
|
||||
f"bearing={bearing:.1f} to_pt={bearing_to_pt:.1f} angle_diff={angle_diff:.1f}")
|
||||
|
||||
# If maneuver point is behind us (>90° off heading), we've passed it
|
||||
# This works regardless of distance - if it's behind us, we passed it
|
||||
if angle_diff > MANEUVER_BEHIND_ANGLE:
|
||||
cloudlog.info(f"maad: passed maneuver point, advancing step {self.step_idx} -> {self.step_idx + 1} "
|
||||
f"(dist={dist:.1f}m, angle_diff={angle_diff:.1f}°)")
|
||||
self.step_idx += 1
|
||||
self.segment_idx = 0
|
||||
self.deviation_start_time = None
|
||||
return True
|
||||
else:
|
||||
# No bearing - use distance only (very close = passed)
|
||||
if dist < 15.0:
|
||||
cloudlog.info(f"maad: passed maneuver point (no bearing), advancing step {self.step_idx} -> {self.step_idx + 1} "
|
||||
f"(dist={dist:.1f}m)")
|
||||
self.step_idx += 1
|
||||
self.segment_idx = 0
|
||||
self.deviation_start_time = None
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def update_step(
|
||||
self,
|
||||
route: list['Step'],
|
||||
position: Coordinate,
|
||||
bearing: Optional[float],
|
||||
speed: float = 0.0,
|
||||
) -> bool:
|
||||
"""Update current step by detecting when maneuver point is passed.
|
||||
|
||||
Uses explicit maneuver point with bearing-based detection if available,
|
||||
otherwise falls back to geometry comparison.
|
||||
"""
|
||||
if not route or self.step_idx is None:
|
||||
return False
|
||||
|
||||
# Need a next step to transition to
|
||||
if self.step_idx + 1 >= len(route):
|
||||
return False
|
||||
|
||||
current_step = route[self.step_idx]
|
||||
next_step = route[self.step_idx + 1]
|
||||
|
||||
# TODO: Re-enable maneuver point detection after fixing display issues
|
||||
# # Use explicit maneuver point if available (preferred - works at forks)
|
||||
# # Note: OSRM's maneuver.location is at the START of each step, so we check
|
||||
# # the NEXT step's maneuver_point (where the turn/fork happens)
|
||||
# if next_step.maneuver_point is not None:
|
||||
# return self._check_passed_maneuver_point(next_step.maneuver_point, position, bearing)
|
||||
# else:
|
||||
# cloudlog.warning(f"maad: step {self.step_idx + 1} has no maneuver_point, using geometry fallback")
|
||||
|
||||
# Geometry-based comparison (standard OSRM/OsmAnd approach)
|
||||
if not current_step.geometry or not next_step.geometry:
|
||||
return False
|
||||
|
||||
# Get geometry around the maneuver point
|
||||
# Before: last 100m of current step
|
||||
before_geom = self._get_geometry_tail(current_step.geometry, MANEUVER_SNAP_ZONE)
|
||||
# After: first 100m of next step
|
||||
after_geom = self._get_geometry_head(next_step.geometry, MANEUVER_SNAP_ZONE)
|
||||
|
||||
# Snap to both geometries
|
||||
dist_before = self._snap_to_geometry(before_geom, position)
|
||||
dist_after = self._snap_to_geometry(after_geom, position)
|
||||
|
||||
# Debug logging
|
||||
cloudlog.debug(f"maad: step transition check step={self.step_idx} "
|
||||
f"dist_before={dist_before:.1f}m dist_after={dist_after:.1f}m")
|
||||
|
||||
# If closer to "after" geometry, we've passed the maneuver
|
||||
if dist_after < dist_before:
|
||||
# Additional bearing check if available
|
||||
if bearing is not None and len(after_geom) >= 2:
|
||||
route_bearing = after_geom[0].bearing_to(after_geom[1])
|
||||
bearing_diff = abs(normalize_angle(bearing - route_bearing))
|
||||
if bearing_diff > WRONG_DIRECTION_THRESHOLD:
|
||||
# Going wrong direction on next step - don't transition
|
||||
return False
|
||||
|
||||
cloudlog.info(f"maad: maneuver passed {self.step_idx} -> {self.step_idx + 1} "
|
||||
f"(dist_before={dist_before:.1f}m, dist_after={dist_after:.1f}m)")
|
||||
self.step_idx += 1
|
||||
self.segment_idx = 0
|
||||
self.deviation_start_time = None
|
||||
return True
|
||||
|
||||
return False
|
||||
2
dragonpilot/dashy/web/dist/css/styles.css
vendored
|
Before Width: | Height: | Size: 40 KiB |
|
Before Width: | Height: | Size: 40 KiB |
|
Before Width: | Height: | Size: 30 KiB |
|
Before Width: | Height: | Size: 34 KiB |
|
Before Width: | Height: | Size: 34 KiB |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M10,5a2,2,0,1,1,2-2A2,2,0,0,1,10,5Zm4.91284,8.35114L10.00916,7.0083,5.10547,13.35114a0.38659,0.38659,0,0,0,.40942.62354l2.95184-1.34375A0.35542,0.35542,0,0,1,9.00769,13H9v5.50006A0.49992,0.49992,0,0,0,9.49994,19h1.00012A0.49992,0.49992,0,0,0,11,18.50006V13.0083h0.00916a0.35757,0.35757,0,0,1,.54242-0.37738l2.95184,1.34375A0.3866,0.3866,0,0,0,14.91284,13.35114Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 554 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M3,12a2,2,0,1,1,2-2A2,2,0,0,1,3,12Zm10.00293-.96332a4.05782,4.05782,0,0,1,3.98877,4.07324H17v1.37775A0.51232,0.51232,0,0,0,17.51233,17h0.97534A0.51232,0.51232,0,0,0,19,16.48767V15H18.98615a6.05607,6.05607,0,0,0-5.9834-5.96332l-0.011-.00183,0.00012,0.02008H12V9.04584a0.35757,0.35757,0,0,1-.37738-0.54242l1.34375-2.95184a0.38659,0.38659,0,0,0-.62354-0.40942L6,10.04584l6.34283,4.90369a0.3866,0.3866,0,0,0,.62354-0.40942l-1.34375-2.95184A0.35757,0.35757,0,0,1,12,11.04584v0.00909h1"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 672 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M15,10a2,2,0,1,1,2,2A2,2,0,0,1,15,10ZM7,11.05493H8V11.04584a0.35757,0.35757,0,0,1,.37738.54242L7.03363,14.5401a0.3866,0.3866,0,0,0,.62354.40942L14,10.04584,7.65717,5.14215a0.38659,0.38659,0,0,0-.62354.40942L8.37738,8.50342A0.35757,0.35757,0,0,1,8,9.04584V9.05493H7.00818L7.0083,9.03485l-0.011.00183A6.05607,6.05607,0,0,0,1.01385,15H1v1.48767A0.51232,0.51232,0,0,0,1.51233,17H2.48767A0.51232,0.51232,0,0,0,3,16.48767V15.10992H3.0083a4.05782,4.05782,0,0,1,3.98877-4.07324"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 662 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M10,5a2,2,0,1,1,2-2A2,2,0,0,1,10,5Zm4.91284,8.35114L10.00916,7.0083,5.10547,13.35114a0.38659,0.38659,0,0,0,.40942.62354l2.95184-1.34375A0.35542,0.35542,0,0,1,9.00769,13H9v5.50006A0.49992,0.49992,0,0,0,9.49994,19h1.00012A0.49992,0.49992,0,0,0,11,18.50006V13.0083h0.00916a0.35757,0.35757,0,0,1,.54242-0.37738l2.95184,1.34375A0.3866,0.3866,0,0,0,14.91284,13.35114Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 554 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M17.35551,4.05855L11.76558,9.64848a0.49713,0.49713,0,0,0,0,.703l5.58993,5.58993a0.49713,0.49713,0,0,1,0,.703l-0.711.711a0.49713,0.49713,0,0,1-.703,0l-5.58993-5.58993a0.49713,0.49713,0,0,0-.703,0L4.05855,17.35551a0.49713,0.49713,0,0,1-.703,0l-0.711-.711a0.49713,0.49713,0,0,1,0-.703l5.58993-5.58993a0.49713,0.49713,0,0,0,0-.703L2.64449,4.05855a0.49713,0.49713,0,0,1,0-.703l0.711-.711a0.49713,0.49713,0,0,1,.703,0L9.64848,8.23442a0.49713,0.49713,0,0,0,.703,0l5.58993-5.58993a0.49713,0.49713,0,0,1,.703,0l0.711,0.711A0.49713,0.49713,0,0,1,17.35551,4.05855Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 746 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M14.50342,8.96637L11.55157,7.62262A0.35755,0.35755,0,0,0,11.00916,8v9.49652A0.50346,0.50346,0,0,1,10.50568,18h-0.993a0.50346,0.50346,0,0,1-.50348-0.50348V8a0.35756,0.35756,0,0,0-.54242-0.37738L5.51489,8.96637a0.38659,0.38659,0,0,1-.40942-0.62354L10.00916,2l4.90369,6.34283A0.3866,0.3866,0,0,1,14.50342,8.96637Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 503 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M10,5.97986l0.011,0.00183A6.06019,6.06019,0,0,1,16,12.05493V16H15.99689l0.002,1.50317A0.49614,0.49614,0,0,1,15.50269,18H14.49622A0.49622,0.49622,0,0,1,14,17.50378V12.05493a4.05782,4.05782,0,0,0-3.98877-4.07324H8.01245a0.3576,0.3576,0,0,0-.37738.54248L8.97882,11.476a0.38659,0.38659,0,0,1-.62354.40942L2.0083,7l6.347-4.922a0.38659,0.38659,0,0,1,.62354.40942L7.63507,5.43927a0.35757,0.35757,0,0,0,.37738.54242H10"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 603 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M9.98877,7.98169A4.05782,4.05782,0,0,0,6,12.05493v5.44885A0.49622,0.49622,0,0,1,5.50378,18H4.49731a0.49614,0.49614,0,0,1-.49615-0.49683L4.00311,16H4V12.05493A6.06019,6.06019,0,0,1,9.989,5.98169L10,5.97986V5.98169h1.98755a0.35757,0.35757,0,0,0,.37738-0.54242L11.02118,2.48743A0.38659,0.38659,0,0,1,11.64471,2.078L17.9917,7l-6.347,4.88544a0.38659,0.38659,0,0,1-.62354-0.40942l1.34375-2.95184a0.3576,0.3576,0,0,0-.37738-0.54248H9.98877Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 626 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M14.9859,14.043v3.46082A0.49621,0.49621,0,0,1,14.48974,18H13.48321a0.49614,0.49614,0,0,1-.49615-0.49683l0.0047-3.60767a5.21819,5.21819,0,0,0-1.665-4.144L8.87854,7.68585a0.35758,0.35758,0,0,0-.6405.16266l-0.91821,3.1106A0.38663,0.38663,0,0,1,6.58044,10.86L5,3l8.00476,0.44965a0.38658,0.38658,0,0,1,.20294.71777L10.25878,5.51758a0.3576,0.3576,0,0,0-.07019.6571l2.45746,2.07385A7.25158,7.25158,0,0,1,14.9859,14.043Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 605 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M7.35395,8.24854L9.81141,6.17468a0.3576,0.3576,0,0,0-.07019-0.6571L6.7923,4.16742a0.38658,0.38658,0,0,1,.20294-0.71777L15,3l-1.58044,7.86a0.38663,0.38663,0,0,1-.73938.09912L11.762,7.84851a0.35758,0.35758,0,0,0-.6405-0.16266L8.67328,9.75146a5.21819,5.21819,0,0,0-1.665,4.144l0.0047,3.60767A0.49614,0.49614,0,0,1,6.51679,18H5.51026a0.49621,0.49621,0,0,1-.49615-0.49622V14.043A7.25157,7.25157,0,0,1,7.35395,8.24854Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 605 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M14.50342,8.96637L11.55157,7.62262A0.35755,0.35755,0,0,0,11.00916,8v9.49652A0.50346,0.50346,0,0,1,10.50568,18h-0.993a0.50346,0.50346,0,0,1-.50348-0.50348V8a0.35756,0.35756,0,0,0-.54242-0.37738L5.51489,8.96637a0.38659,0.38659,0,0,1-.40942-0.62354L10.00916,2l4.90369,6.34283A0.3866,0.3866,0,0,1,14.50342,8.96637Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 503 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M17.00909,8v9.49652A0.50346,0.50346,0,0,1,16.50562,18h-0.993a0.50346,0.50346,0,0,1-.50348-0.50348V8a3.5,3.5,0,1,0-7,0v4H8a0.35757,0.35757,0,0,0,.54242.37738l2.95184-1.34375a0.3866,0.3866,0,0,1,.40942.62354L7,18,2.09625,11.65717a0.3866,0.3866,0,0,1,.40942-0.62354l2.95184,1.34375A0.3576,0.3576,0,0,0,6,12H6.00909V8A5.5,5.5,0,1,1,17.00909,8Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 532 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" transform="translate(20,0) scale(-1,1)" d="M17.00909,8v9.49652A0.50346,0.50346,0,0,1,16.50562,18h-0.993a0.50346,0.50346,0,0,1-.50348-0.50348V8a3.5,3.5,0,1,0-7,0v4H8a0.35757,0.35757,0,0,0,.54242.37738l2.95184-1.34375a0.3866,0.3866,0,0,1,.40942.62354L7,18,2.09625,11.65717a0.3866,0.3866,0,0,1,.40942-0.62354l2.95184,1.34375A0.3576,0.3576,0,0,0,6,12H6.00909V8A5.5,5.5,0,1,1,17.00909,8Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 572 B |
@@ -1,5 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<svg fill="red" width="72" height="80" viewBox="0 0 15 15" xmlns="http://www.w3.org/2000/svg" id="marker">
|
||||
<path stroke="darkred" stroke-width="0.5" d="M7.5 1C5.42312 1 3 2.2883 3 5.56759C3 7.79276 6.46156 12.7117 7.5 14C8.42309 12.7117 12 7.90993 12 5.56759C12 2.2883 9.57688 1 7.5 1Z"/>
|
||||
<circle cx="7.5" cy="5.5" r="2" fill="darkred"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 389 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M9.98172,19a2,2,0,1,1,2-2A2,2,0,0,1,9.98172,19ZM14.89456,7.34283L9.99087,1,5.08719,7.34283a0.38659,0.38659,0,0,0,.40942.62354L8.44845,6.62262a0.35542,0.35542,0,0,1,.541.36908H8.98172v5.50006a0.49992,0.49992,0,0,0,.49994.49994h1.00012a0.49992,0.49992,0,0,0,.49994-0.49994V7h0.00916a0.35757,0.35757,0,0,1,.54242-0.37738l2.95184,1.34375A0.3866,0.3866,0,0,0,14.89456,7.34283Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 564 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M15,19a2,2,0,1,1,2-2A2,2,0,0,1,15,19ZM10.00293,8.03668a4.05782,4.05782,0,0,1,3.98877,4.07324H14v1.37775A0.51232,0.51232,0,0,0,14.51233,14h0.97534A0.51232,0.51232,0,0,0,16,13.48767V12H15.98615a6.05607,6.05607,0,0,0-5.9834-5.96332l-0.011-.00183L9.99182,6.05493H9V6.04584a0.35757,0.35757,0,0,1-.37738-0.54242L9.96637,2.55157a0.38659,0.38659,0,0,0-.62354-0.40942L3,7.04584l6.34283,4.90369a0.3866,0.3866,0,0,0,.62354-0.40942L8.62262,8.58826A0.35757,0.35757,0,0,1,9,8.04584V8.05493h1"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 670 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M3,17a2,2,0,1,1,2,2A2,2,0,0,1,3,17Zm7-8.94507h1V8.04584a0.35757,0.35757,0,0,1,.37738.54242L10.03363,11.5401a0.3866,0.3866,0,0,0,.62354.40942L17,7.04584,10.65717,2.14215a0.38659,0.38659,0,0,0-.62354.40942l1.34375,2.95184A0.35757,0.35757,0,0,1,11,6.04584V6.05493H10.00818l0.00012-.02008-0.011.00183A6.05607,6.05607,0,0,0,4.01385,12H4v1.48767A0.51232,0.51232,0,0,0,4.51233,14H5.48767A0.51232,0.51232,0,0,0,6,13.48767V12.10992H6.0083A4.05782,4.05782,0,0,1,9.99707,8.03668"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 660 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M10,19a2,2,0,1,1,2-2A2,2,0,0,1,10,19ZM14.91284,7.34283L10.00916,1,5.10547,7.34283a0.38659,0.38659,0,0,0,.40942.62354L8.46674,6.62262a0.35542,0.35542,0,0,1,.541.36908H9v5.50006a0.49992,0.49992,0,0,0,.49994.49994h1.00012A0.49992,0.49992,0,0,0,11,12.49176V7h0.00916a0.35757,0.35757,0,0,1,.54242-0.37738l2.95184,1.34375A0.3866,0.3866,0,0,0,14.91284,7.34283Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 546 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M16.50354,6.2A0.29678,0.29678,0,0,1,16.8,6.49646V7.50354A0.29678,0.29678,0,0,1,16.50354,7.8H7.2V6.2h9.30353m0-.2H7V8h9.50354A0.49645,0.49645,0,0,0,17,7.50354V6.49646A0.49645,0.49645,0,0,0,16.50354,6h0ZM10,5.98169H8.01245a0.35757,0.35757,0,0,1-.37738-0.54242L8.97882,2.48743A0.38659,0.38659,0,0,0,8.35529,2.078L2.0083,7l6.347,4.88544A0.38659,0.38659,0,0,0,8.97882,11.476L7.63507,8.52417a0.3576,0.3576,0,0,1,.37738-0.54248h1.99878A4.05782,4.05782,0,0,1,14,12.05493v5.44885A0.49622,0.49622,0,0,0,14.49622,18h1.00647a0.49614,0.49614,0,0,0,.49615-0.49683L15.99689,16H16V12.05493a6.06019,6.06019,0,0,0-5.989-6.07324L10,5.97986"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 813 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M12.8,6.2V7.8H2.50119A0.30152,0.30152,0,0,1,2.2,7.49882V6.50118A0.30152,0.30152,0,0,1,2.50119,6.2H12.8M13,6H2.50119A0.50119,0.50119,0,0,0,2,6.50118V7.49882A0.50119,0.50119,0,0,0,2.50119,8H13V6h0ZM10,7.98169h1.98755a0.3576,0.3576,0,0,1,.37738.54248L11.02118,11.476a0.38659,0.38659,0,0,0,.62354.40942L17.9917,7l-6.347-4.922a0.38659,0.38659,0,0,0-.62354.40942l1.34375,2.95184a0.35757,0.35757,0,0,1-.37738.54242H10V5.97986l-0.011.00183A6.06019,6.06019,0,0,0,4,12.05493V16H4.00311l-0.002,1.50317A0.49614,0.49614,0,0,0,4.49731,18H5.50378A0.49622,0.49622,0,0,0,6,17.50378V12.05493A4.05782,4.05782,0,0,1,9.98877,7.98169H10Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 808 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M3.5,18h0A0.5,0.5,0,0,1,3,17.5V3.5A0.5,0.5,0,0,1,3.5,3h0a0.5,0.5,0,0,1,.5.5v14A0.5,0.5,0,0,1,3.5,18ZM13.33333,7L17,3H6v8H17Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 317 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M19,3l-1.58044,7.86a0.38663,0.38663,0,0,1-.73938.09912L15.762,7.84851a0.35758,0.35758,0,0,0-.6405-0.16266L12.67328,9.75146a5.21819,5.21819,0,0,0-1.665,4.144l0.0047,3.60767A0.49614,0.49614,0,0,1,10.51678,18H9.48322a0.49614,0.49614,0,0,1-.49615-0.49683l0.0047-3.60767a5.21819,5.21819,0,0,0-1.665-4.144L4.87854,7.68585a0.35758,0.35758,0,0,0-.6405.16266l-0.91821,3.1106A0.38663,0.38663,0,0,1,2.58044,10.86L1,3,9.00476,3.44965a0.38658,0.38658,0,0,1,.20294.71777L6.25879,5.51758a0.3576,0.3576,0,0,0-.07019.6571L8.64606,8.24854A6.64064,6.64064,0,0,1,10,9.89148a6.64064,6.64064,0,0,1,1.35394-1.64294L13.8114,6.17468a0.3576,0.3576,0,0,0-.07019-0.6571L10.7923,4.16742a0.38658,0.38658,0,0,1,.20294-0.71777Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 888 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M17.73611,6.57682a0.30465,0.30465,0,0,1,.23032.10507l0.649,0.74859a0.30457,0.30457,0,0,1-.03369.43226L16.5213,9.60119a5.4089,5.4089,0,0,0-1.73306,4.29459l0.0047,3.60769A0.29612,0.29612,0,0,1,14.4968,17.8H13.49033a0.29657,0.29657,0,0,1-.29623-0.29622l-0.00058-3.47624A6.99129,6.99129,0,0,1,15.463,8.40138l2.077-1.75275a0.30475,0.30475,0,0,1,.1962-0.07182m0-.2a0.50269,0.50269,0,0,0-.32516.119L15.334,8.24854A7.25157,7.25157,0,0,0,12.99411,14.043v3.46082A0.49622,0.49622,0,0,0,13.49033,18H14.4968a0.49614,0.49614,0,0,0,.49615-0.49681l-0.0047-3.60767a5.2182,5.2182,0,0,1,1.665-4.14405L18.71066,8.0156a0.50455,0.50455,0,0,0,.05585-0.71613l-0.649-.74859a0.50345,0.50345,0,0,0-.38147-0.17406h0ZM9.00078,5.98169H7.01323a0.35757,0.35757,0,0,1-.37738-0.54242L7.9796,2.48743A0.38659,0.38659,0,0,0,7.35606,2.078L1.00908,7l6.347,4.88544A0.38659,0.38659,0,0,0,7.9796,11.476L6.63585,8.52417a0.3576,0.3576,0,0,1,.37738-0.54248H9.012a4.05782,4.05782,0,0,1,3.98877,4.07324v5.44885A0.49622,0.49622,0,0,0,13.497,18h1.00647a0.49614,0.49614,0,0,0,.49615-0.49683L14.99766,16h0.00311V12.05493a6.06019,6.06019,0,0,0-5.989-6.07324l-0.011-.00183"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 1.3 KiB |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M2.27386,6.57682A0.30457,0.30457,0,0,1,2.47,6.64864L4.544,8.39882A7.00218,7.00218,0,0,1,6.81586,14.043v3.46088A0.29651,0.29651,0,0,1,6.51968,17.8H5.51318A0.29613,0.29613,0,0,1,5.217,17.50343L5.22079,13.9146a5.42,5.42,0,0,0-1.73517-4.316L1.42825,7.86274a0.30461,0.30461,0,0,1-.03372-0.43226l0.649-.74859a0.30466,0.30466,0,0,1,.23035-0.10507m0-.2a0.50345,0.50345,0,0,0-.38147.17406l-0.649.74859a0.50458,0.50458,0,0,0,.05588.71613L3.35666,9.75146a5.2182,5.2182,0,0,1,1.665,4.14405L5.017,17.50317A0.49617,0.49617,0,0,0,5.51318,18h1.0065a0.49616,0.49616,0,0,0,.49615-0.49615V14.043A7.25157,7.25157,0,0,0,4.676,8.24854L2.599,6.49579a0.50264,0.50264,0,0,0-.32516-0.119h0ZM11.0083,7.98169h1.98755a0.3576,0.3576,0,0,1,.37738.54248L12.02948,11.476a0.38659,0.38659,0,0,0,.62354.40942L19,7,12.653,2.078a0.38659,0.38659,0,0,0-.62354.40942l1.34375,2.95184a0.35757,0.35757,0,0,1-.37738.54242H11.0083V5.97986l-0.011.00183a6.06019,6.06019,0,0,0-5.989,6.07324V16H5.01141l-0.002,1.50317A0.49614,0.49614,0,0,0,5.50562,18H6.51208a0.49622,0.49622,0,0,0,.49622-0.49622V12.05493a4.05782,4.05782,0,0,1,3.98877-4.07324H11.0083Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 1.3 KiB |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M15.75607,6.57682a0.30471,0.30471,0,0,1,.23035.10507l0.649,0.74859a0.30457,0.30457,0,0,1-.03369.43226L14.54129,9.60119a5.4089,5.4089,0,0,0-1.73306,4.29459l0.0047,3.60769a0.29613,0.29613,0,0,1-.29614.29654H11.51031a0.29657,0.29657,0,0,1-.29623-0.29622L11.2135,14.02754a6.99129,6.99129,0,0,1,2.26944-5.62616l2.077-1.75275a0.30461,0.30461,0,0,1,.19617-0.07182m0-.2a0.50263,0.50263,0,0,0-.32516.119l-2.077,1.75275A7.25157,7.25157,0,0,0,11.0141,14.043v3.46082A0.49622,0.49622,0,0,0,11.51031,18h1.00647a0.49616,0.49616,0,0,0,.49615-0.49681l-0.0047-3.60767a5.2182,5.2182,0,0,1,1.665-4.14405L16.73065,8.0156a0.50458,0.50458,0,0,0,.05585-0.71613l-0.649-.74859a0.50345,0.50345,0,0,0-.38147-0.17406h0Zm-5.11,1.87171L8.18859,6.17468a0.3576,0.3576,0,0,1,.07019-0.6571L11.2077,4.16742a0.38658,0.38658,0,0,0-.20294-0.71777L3,3l1.58044,7.86a0.38663,0.38663,0,0,0,.73938.09912L6.238,7.84851a0.35758,0.35758,0,0,1,.6405-0.16266L9.32672,9.75146a5.21819,5.21819,0,0,1,1.665,4.144l-0.0047,3.60767A0.49614,0.49614,0,0,0,11.48321,18h1.00653a0.49621,0.49621,0,0,0,.49615-0.49622V14.043A7.25158,7.25158,0,0,0,10.64605,8.24854Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 1.3 KiB |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M4.26865,6.57682a0.30457,0.30457,0,0,1,.19617.07182l2.074,1.75018A7.00218,7.00218,0,0,1,8.81064,14.043v3.46088A0.29651,0.29651,0,0,1,8.51447,17.8H7.50793a0.29612,0.29612,0,0,1-.29614-0.29655L7.21558,13.9146a5.42,5.42,0,0,0-1.73517-4.316L3.423,7.86274a0.30454,0.30454,0,0,1-.03372-0.43226l0.649-.74859a0.30466,0.30466,0,0,1,.23035-0.10507m0-.2a0.50345,0.50345,0,0,0-.38147.17406l-0.649.74859a0.50455,0.50455,0,0,0,.05588.71613L5.35144,9.75146a5.2182,5.2182,0,0,1,1.665,4.14405l-0.0047,3.60767A0.49614,0.49614,0,0,0,7.50793,18H8.51447a0.49616,0.49616,0,0,0,.49615-0.49615V14.043A7.25157,7.25157,0,0,0,6.67078,8.24854l-2.077-1.75275a0.50264,0.50264,0,0,0-.32516-0.119h0ZM7.0141,14.043v3.46082A0.49621,0.49621,0,0,0,7.51025,18H8.51678a0.49614,0.49614,0,0,0,.49615-0.49683l-0.0047-3.60767a5.21819,5.21819,0,0,1,1.665-4.144l2.44818-2.06561a0.35758,0.35758,0,0,1,.6405.16266l0.91821,3.1106A0.38663,0.38663,0,0,0,15.41956,10.86L17,3l-8.00476.44965a0.38658,0.38658,0,0,0-.20294.71777l2.94891,1.35016a0.3576,0.3576,0,0,1,.07019.6571L9.35394,8.24854A7.25157,7.25157,0,0,0,7.0141,14.043Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 1.2 KiB |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M17.75607,6.59766a0.30466,0.30466,0,0,1,.23035.10507l0.649,0.74859a0.30457,0.30457,0,0,1-.03369.43226L16.54129,9.622a5.4089,5.4089,0,0,0-1.73306,4.29459l0.0047,3.60769a0.29613,0.29613,0,0,1-.29614.29654H13.51031a0.29657,0.29657,0,0,1-.29623-0.29622L13.2135,14.04838a6.99129,6.99129,0,0,1,2.26944-5.62616l2.077-1.75275a0.30461,0.30461,0,0,1,.19617-0.07182m0-.2a0.50264,0.50264,0,0,0-.32516.119l-2.077,1.75275A7.25157,7.25157,0,0,0,13.0141,14.0638v3.46082a0.49622,0.49622,0,0,0,.49622.49622h1.00647a0.49616,0.49616,0,0,0,.49615-0.49681l-0.0047-3.60767a5.2182,5.2182,0,0,1,1.665-4.14405l2.05737-1.73586A0.50455,0.50455,0,0,0,18.7865,7.3203l-0.649-.74859a0.50345,0.50345,0,0,0-.38147-0.17406h0ZM2.24393,6.57682a0.30457,0.30457,0,0,1,.19617.07182l2.074,1.75018A7.00218,7.00218,0,0,1,6.78592,14.043v3.46088A0.29651,0.29651,0,0,1,6.48975,17.8H5.48322a0.29613,0.29613,0,0,1-.29614-0.29655L5.19086,13.9146a5.42,5.42,0,0,0-1.73517-4.316L1.39832,7.86274a0.30457,0.30457,0,0,1-.03369-0.43226l0.649-.74859a0.30466,0.30466,0,0,1,.23035-0.10507m0-.2a0.50345,0.50345,0,0,0-.38147.17406l-0.649.74859a0.50455,0.50455,0,0,0,.05585.71613L3.32672,9.75146a5.2182,5.2182,0,0,1,1.665,4.14405l-0.0047,3.60767A0.49616,0.49616,0,0,0,5.48322,18H6.48975a0.49616,0.49616,0,0,0,.49615-0.49615V14.043A7.25157,7.25157,0,0,0,4.64606,8.24854l-2.077-1.75275a0.50264,0.50264,0,0,0-.32516-0.119h0Zm12.66891,1.966L10.00916,2,5.10547,8.34283a0.38659,0.38659,0,0,0,.40942.62354L8.46674,7.62262A0.35756,0.35756,0,0,1,9.00916,8v9.49652A0.50346,0.50346,0,0,0,9.51263,18h0.993a0.50346,0.50346,0,0,0,.50348-0.50348V8a0.35755,0.35755,0,0,1,.54242-0.37738l2.95184,1.34375A0.3866,0.3866,0,0,0,14.91284,8.34283Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 1.8 KiB |
BIN
dragonpilot/dashy/web/dist/icons/navigation/home.png
vendored
|
Before Width: | Height: | Size: 6.0 KiB |
@@ -1,65 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<svg
|
||||
xmlns:dc="http://purl.org/dc/elements/1.1/"
|
||||
xmlns:cc="http://creativecommons.org/ns#"
|
||||
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
|
||||
xmlns:svg="http://www.w3.org/2000/svg"
|
||||
xmlns="http://www.w3.org/2000/svg"
|
||||
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
|
||||
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
|
||||
version="1.1"
|
||||
width="200"
|
||||
height="200"
|
||||
viewBox="0 0 200 200"
|
||||
id="home-15"
|
||||
sodipodi:docname="home.svg"
|
||||
inkscape:version="0.92.5 (2060ec1f9f, 2020-04-08)"
|
||||
inkscape:export-filename="/home/batman/openpilot/selfdrive/assets/navigation/home_inactive.png"
|
||||
inkscape:export-xdpi="96"
|
||||
inkscape:export-ydpi="96">
|
||||
<metadata
|
||||
id="metadata99">
|
||||
<rdf:RDF>
|
||||
<cc:Work
|
||||
rdf:about="">
|
||||
<dc:format>image/svg+xml</dc:format>
|
||||
<dc:type
|
||||
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
|
||||
<dc:title />
|
||||
</cc:Work>
|
||||
</rdf:RDF>
|
||||
</metadata>
|
||||
<defs
|
||||
id="defs97" />
|
||||
<sodipodi:namedview
|
||||
pagecolor="#ffffff"
|
||||
bordercolor="#666666"
|
||||
borderopacity="1"
|
||||
objecttolerance="10"
|
||||
gridtolerance="10"
|
||||
guidetolerance="10"
|
||||
inkscape:pageopacity="0"
|
||||
inkscape:pageshadow="2"
|
||||
inkscape:window-width="1278"
|
||||
inkscape:window-height="1418"
|
||||
id="namedview95"
|
||||
showgrid="false"
|
||||
inkscape:zoom="1.810488"
|
||||
inkscape:cx="100.16296"
|
||||
inkscape:cy="24.659346"
|
||||
inkscape:window-x="2480"
|
||||
inkscape:window-y="0"
|
||||
inkscape:window-maximized="0"
|
||||
inkscape:current-layer="home-15" />
|
||||
<path
|
||||
d="m 58.038143,147.98997 c 0,1.11009 0.899932,2.01003 2.010031,2.01003 H 90.19059 v -24.12037 h 24.12035 V 150 h 30.14242 c 1.1101,0 2.01003,-0.89994 2.01003,-2.01003 V 101.76651 H 58.038143 c 0,0 0,46.22346 0,46.22346 z m 95.918637,-57.647675 -7.49259,-6.58406 v -30.22199 c 0,-4.44041 -3.59964,-8.04012 -8.04013,-8.04012 -4.44047,0 -8.04012,3.59971 -8.04012,8.04012 v 16.08023 l -26.67147,-23.39675 c -0.75981,-0.80642 -2.02886,-0.84597 -2.83737,-0.0884 l -0.0884,0.0884 -50.243521,43.92317 c -0.758989,0.81013 -0.717501,2.08216 0.09263,2.84115 0.365504,0.34242 0.84574,0.53603 1.346559,0.54295 l 14.095938,0.201011 h 86.405544 c 1.11011,0.004 2.01302,-0.893251 2.01672,-2.003361 8.4e-4,-0.51306 -0.19282,-1.00727 -0.54377,-1.38155 z"
|
||||
id="path92"
|
||||
inkscape:connector-curvature="0"
|
||||
style="stroke-width:8.04012012;fill:#ffffff;fill-opacity:1" />
|
||||
<circle
|
||||
style="fill:none;stroke:#ffffff;stroke-width:3.26192951;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
id="path104"
|
||||
cx="100"
|
||||
cy="100"
|
||||
r="98.369034" />
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 2.6 KiB |
|
Before Width: | Height: | Size: 9.0 KiB |
@@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="100" height="100" fill="none"><g fill="#fff" clip-path="url(#a)" transform="matrix(2.3325 0 0 2.3825 -72.89 -63.797)"><path d="M31.25 53.12A15.62 15.62 0 0 1 46.88 37.5h9.37v-9.16a1.56 1.56 0 0 1 2.56-1.2l14.75 12.28c.75.63.75 1.78 0 2.4l-14.75 12.3a1.56 1.56 0 0 1-2.56-1.2v-9.17h-9.38a9.37 9.37 0 0 0-9.37 9.37v15.63h-6.25z"/></g><defs><clipPath id="a"><path fill="#fff" d="M0 0h100v100H0z"/></clipPath></defs></svg>
|
||||
|
Before Width: | Height: | Size: 466 B |
@@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="300" height="300" fill="none" viewBox="0 0 100 100"><g fill="#fff" clip-path="url(#a)"><path d="M31.25 53.12A15.62 15.62 0 0 1 46.88 37.5h9.37v-9.16a1.56 1.56 0 0 1 2.56-1.2l14.75 12.28c.75.63.75 1.78 0 2.4l-14.75 12.3a1.56 1.56 0 0 1-2.56-1.2v-9.17h-9.38a9.37 9.37 0 0 0-9.37 9.37v15.63h-6.25V53.12Z"/><path fill-rule="evenodd" d="M43.44 2.72a9.28 9.28 0 0 1 13.12 0l40.72 40.72a9.26 9.26 0 0 1 0 13.12L56.56 97.28a9.26 9.26 0 0 1-13.1 0L2.71 56.56a9.26 9.26 0 0 1 0-13.11L43.44 2.72Zm8.75 4.37a3.1 3.1 0 0 0-4.38 0L7.1 47.81a3.1 3.1 0 0 0 0 4.38L47.8 92.9a3.1 3.1 0 0 0 4.38 0L92.9 52.2a3.1 3.1 0 0 0 0-4.38L52.2 7.1Z" clip-rule="evenodd"/></g><defs><clipPath id="a"><path fill="#fff" d="M0 0h100v100H0z"/></clipPath></defs></svg>
|
||||
|
Before Width: | Height: | Size: 780 B |
@@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="100" height="100" fill="none"><g clip-path="url(#a)"><path fill="#fff" d="M49.1 0 36.35 31.82H0l27.27 27.27L13.64 100l35.45-23.64L86.36 100 72.73 59.1 100 31.81H63.64L49.09 0Z"/></g><defs><clipPath id="a"><path fill="#fff" d="M0 0h100v100H0z"/></clipPath></defs></svg>
|
||||
|
Before Width: | Height: | Size: 315 B |
@@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="100" height="100" fill="none"><g clip-path="url(#a)"><path fill="#fff" d="M7.7 98.08c0 1.06.85 1.92 1.92 1.92h28.84V76.92h23.08V100H90.4c1.06 0 1.92-.86 1.92-1.92V53.85H7.69v44.23ZM99.47 42.9l-7.17-6.3V7.7a7.7 7.7 0 0 0-15.39 0v15.4L51.4.68A1.92 1.92 0 0 0 48.68.6l-.08.08L.52 42.72a1.92 1.92 0 0 0 1.38 3.24l13.49.2h82.68a1.92 1.92 0 0 0 1.41-3.25Z"/></g><defs><clipPath id="a"><path fill="#fff" d="M0 0h100v100H0z"/></clipPath></defs></svg>
|
||||
|
Before Width: | Height: | Size: 489 B |
@@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="100" height="100" fill="none"><g fill="#fff" clip-path="url(#a)"><path d="M53.22 6.37a43.75 43.75 0 0 0-3.21-.12V0a50 50 0 0 1 3.68.14l-.47 6.23Zm12.53 2.81c-2-.77-4.06-1.4-6.16-1.87l1.37-6.1c2.4.54 4.75 1.25 7.04 2.14l-2.25 5.83Zm8.56 4.44c-.9-.6-1.8-1.16-2.74-1.69l3.08-5.44a50.15 50.15 0 0 1 6.12 4.09l-3.85 4.93a43.75 43.75 0 0 0-2.61-1.89ZM85.77 24.8a43.7 43.7 0 0 0-4.08-4.97l4.53-4.31a52.14 52.14 0 0 1 4.66 5.68l-5.1 3.6Zm4.65 8.45a36.5 36.5 0 0 0-1.34-2.92l5.59-2.81a48.6 48.6 0 0 1 2.8 6.8l-5.93 1.95a43.9 43.9 0 0 0-1.12-3.02Zm3.31 15.67a43.68 43.68 0 0 0-.62-6.4l6.15-1.07c.42 2.41.67 4.86.73 7.31l-6.25.16Zm-.81 9.61c.2-1.06.37-2.12.5-3.19l6.2.77a49.3 49.3 0 0 1-1.43 7.22l-6.02-1.67c.28-1.03.53-2.07.75-3.13ZM86.97 73.4A44.16 44.16 0 0 0 90 67.72l5.72 2.53c-1 2.25-2.16 4.42-3.47 6.49l-5.28-3.34Zm-6.03 7.53c.76-.77 1.5-1.55 2.19-2.36l4.74 4.08a49.2 49.2 0 0 1-2.51 2.7l-4.42-4.42Z"/><path d="M50 6.25a43.75 43.75 0 1 0 30.94 74.68l4.42 4.42A50 50 0 1 1 50 0v6.25Z"/><path d="M46.88 18.75a3.13 3.13 0 0 1 3.13 3.12v32.56l20.3 11.6a3.12 3.12 0 0 1-3.1 5.42l-21.88-12.5a3.12 3.12 0 0 1-1.57-2.7V21.86a3.12 3.12 0 0 1 3.12-3.12Z"/></g><defs><clipPath id="a"><path fill="#fff" d="M0 0h100v100H0z"/></clipPath></defs></svg>
|
||||
|
Before Width: | Height: | Size: 1.2 KiB |
@@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="128" height="128" fill="#fff" class="bi bi-gear-fill"><path d="M75.24 8.4c-3.304-11.2-19.176-11.2-22.48 0l-.8 2.72a11.712 11.712 0 0 1-16.84 6.976l-2.48-1.36c-10.264-5.584-21.488 5.64-15.896 15.896l1.352 2.488c3.568 6.56.184 14.728-6.976 16.84l-2.72.8c-11.2 3.304-11.2 19.176 0 22.48l2.72.8a11.712 11.712 0 0 1 6.976 16.84l-1.36 2.48c-5.584 10.264 5.64 21.488 15.896 15.896l2.488-1.352a11.712 11.712 0 0 1 16.84 6.976l.8 2.72c3.304 11.2 19.176 11.2 22.48 0l.8-2.72a11.712 11.712 0 0 1 16.84-6.976l2.48 1.36c10.264 5.584 21.488-5.64 15.896-15.896l-1.352-2.488a11.712 11.712 0 0 1 6.976-16.84l2.72-.8c11.2-3.304 11.2-19.176 0-22.48l-2.72-.8a11.712 11.712 0 0 1-6.976-16.84l1.36-2.48c5.584-10.264-5.64-21.488-15.896-15.896l-2.488 1.352a11.712 11.712 0 0 1-16.84-6.976ZM64 87.44a23.44 23.44 0 1 1 0-46.88 23.432 23.432 0 0 1 0 46.864z" style="stroke-width:8"/></svg>
|
||||
|
Before Width: | Height: | Size: 910 B |
@@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="100" height="100" fill="none"><g clip-path="url(#a)"><path fill="#fff" d="M77.78 22.22V6.42L70.67 0H29.33l-7.1 8.3v13.92H5.55L0 27.78v66.66L5.56 100h88.88l5.56-5.56V27.78l-5.56-5.56H77.78Zm-44.45-11.1h33.34v11.1H33.33v-11.1Z"/></g><defs><clipPath id="a"><path fill="#fff" d="M0 0h100v100H0z"/></clipPath></defs></svg>
|
||||
|
Before Width: | Height: | Size: 364 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M14.50342,8.96637L11.55157,7.62262A0.35755,0.35755,0,0,0,11.00916,8v9.49652A0.50346,0.50346,0,0,1,10.50568,18h-0.993a0.50346,0.50346,0,0,1-.50348-0.50348V8a0.35756,0.35756,0,0,0-.54242-0.37738L5.51489,8.96637a0.38659,0.38659,0,0,1-.40942-0.62354L10.00916,2l4.90369,6.34283A0.3866,0.3866,0,0,1,14.50342,8.96637Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 503 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M10,5.97986l0.011,0.00183A6.06019,6.06019,0,0,1,16,12.05493V16H15.99689l0.002,1.50317A0.49614,0.49614,0,0,1,15.50269,18H14.49622A0.49622,0.49622,0,0,1,14,17.50378V12.05493a4.05782,4.05782,0,0,0-3.98877-4.07324H8.01245a0.3576,0.3576,0,0,0-.37738.54248L8.97882,11.476a0.38659,0.38659,0,0,1-.62354.40942L2.0083,7l6.347-4.922a0.38659,0.38659,0,0,1,.62354.40942L7.63507,5.43927a0.35757,0.35757,0,0,0,.37738.54242H10"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 603 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M9.98877,7.98169A4.05782,4.05782,0,0,0,6,12.05493v5.44885A0.49622,0.49622,0,0,1,5.50378,18H4.49731a0.49614,0.49614,0,0,1-.49615-0.49683L4.00311,16H4V12.05493A6.06019,6.06019,0,0,1,9.989,5.98169L10,5.97986V5.98169h1.98755a0.35757,0.35757,0,0,0,.37738-0.54242L11.02118,2.48743A0.38659,0.38659,0,0,1,11.64471,2.078L17.9917,7l-6.347,4.88544a0.38659,0.38659,0,0,1-.62354-0.40942l1.34375-2.95184a0.3576,0.3576,0,0,0-.37738-0.54248H9.98877Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 626 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M14.9859,14.043v3.46082A0.49621,0.49621,0,0,1,14.48974,18H13.48321a0.49614,0.49614,0,0,1-.49615-0.49683l0.0047-3.60767a5.21819,5.21819,0,0,0-1.665-4.144L8.87854,7.68585a0.35758,0.35758,0,0,0-.6405.16266l-0.91821,3.1106A0.38663,0.38663,0,0,1,6.58044,10.86L5,3l8.00476,0.44965a0.38658,0.38658,0,0,1,.20294.71777L10.25878,5.51758a0.3576,0.3576,0,0,0-.07019.6571l2.45746,2.07385A7.25158,7.25158,0,0,1,14.9859,14.043Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 605 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M7.35395,8.24854L9.81141,6.17468a0.3576,0.3576,0,0,0-.07019-0.6571L6.7923,4.16742a0.38658,0.38658,0,0,1,.20294-0.71777L15,3l-1.58044,7.86a0.38663,0.38663,0,0,1-.73938.09912L11.762,7.84851a0.35758,0.35758,0,0,0-.6405-0.16266L8.67328,9.75146a5.21819,5.21819,0,0,0-1.665,4.144l0.0047,3.60767A0.49614,0.49614,0,0,1,6.51679,18H5.51026a0.49621,0.49621,0,0,1-.49615-0.49622V14.043A7.25157,7.25157,0,0,1,7.35395,8.24854Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 605 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M14.50342,8.96637L11.55157,7.62262A0.35755,0.35755,0,0,0,11.00916,8v9.49652A0.50346,0.50346,0,0,1,10.50568,18h-0.993a0.50346,0.50346,0,0,1-.50348-0.50348V8a0.35756,0.35756,0,0,0-.54242-0.37738L5.51489,8.96637a0.38659,0.38659,0,0,1-.40942-0.62354L10.00916,2l4.90369,6.34283A0.3866,0.3866,0,0,1,14.50342,8.96637Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 503 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M17.00909,8v9.49652A0.50346,0.50346,0,0,1,16.50562,18h-0.993a0.50346,0.50346,0,0,1-.50348-0.50348V8a3.5,3.5,0,1,0-7,0v4H8a0.35757,0.35757,0,0,0,.54242.37738l2.95184-1.34375a0.3866,0.3866,0,0,1,.40942.62354L7,18,2.09625,11.65717a0.3866,0.3866,0,0,1,.40942-0.62354l2.95184,1.34375A0.3576,0.3576,0,0,0,6,12H6.00909V8A5.5,5.5,0,1,1,17.00909,8Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 532 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" transform="translate(20,0) scale(-1,1)" d="M17.00909,8v9.49652A0.50346,0.50346,0,0,1,16.50562,18h-0.993a0.50346,0.50346,0,0,1-.50348-0.50348V8a3.5,3.5,0,1,0-7,0v4H8a0.35757,0.35757,0,0,0,.54242.37738l2.95184-1.34375a0.3866,0.3866,0,0,1,.40942.62354L7,18,2.09625,11.65717a0.3866,0.3866,0,0,1,.40942-0.62354l2.95184,1.34375A0.3576,0.3576,0,0,0,6,12H6.00909V8A5.5,5.5,0,1,1,17.00909,8Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 572 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M7.8,7.2V17.50252A0.2978,0.2978,0,0,1,7.50253,17.8H6.49747A0.2978,0.2978,0,0,1,6.2,17.50252V7.2H7.8M8,7H6V17.50252A0.49747,0.49747,0,0,0,6.49747,18H7.50253A0.49747,0.49747,0,0,0,8,17.50252V7H8ZM7.98169,10V8.01245a0.3576,0.3576,0,0,1,.54248-0.37738L11.476,8.97882a0.38659,0.38659,0,0,0,.40942-0.62354L7,2.0083l-4.922,6.347a0.38659,0.38659,0,0,0,.40942.62354L5.43927,7.63507a0.35757,0.35757,0,0,1,.54242.37738V10H5.97986l0.00183,0.011A6.06019,6.06019,0,0,0,12.05493,16H16V15.99689l1.50317,0.002A0.49614,0.49614,0,0,0,18,15.50269V14.49622A0.49622,0.49622,0,0,0,17.50378,14H12.05493a4.05782,4.05782,0,0,1-4.07324-3.98877V10Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 813 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M13.8,7.2V17.49921a0.30112,0.30112,0,0,1-.30078.3008H12.50079A0.30112,0.30112,0,0,1,12.2,17.49921V7.2h1.6M14,7H12V17.49921A0.50079,0.50079,0,0,0,12.50079,18h0.99841A0.50079,0.50079,0,0,0,14,17.49921V7h0Zm0.01831,3V8.01245a0.35757,0.35757,0,0,1,.54242-0.37738l2.95184,1.34375A0.38659,0.38659,0,0,0,17.922,8.35529L13,2.0083l-4.88544,6.347a0.38659,0.38659,0,0,0,.40942.62354l2.95184-1.34375a0.3576,0.3576,0,0,1,.54248.37738v1.99878A4.05782,4.05782,0,0,1,7.94507,14H2.49622A0.49622,0.49622,0,0,0,2,14.49622v1.00647a0.49614,0.49614,0,0,0,.49683.49615L4,15.99689V16H7.94507a6.06019,6.06019,0,0,0,6.07324-5.989L14.02014,10"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 808 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M9.82565,9.2v8.29515A0.3052,0.3052,0,0,1,9.52081,17.8H8.53052a0.3052,0.3052,0,0,1-.30484-0.30486V9.2h1.6m0.2-.2h-2v8.49515A0.50484,0.50484,0,0,0,8.53052,18h0.9903a0.50486,0.50486,0,0,0,.50485-0.50485V9h0ZM4.517,7.95947L7.46885,6.61572a0.35757,0.35757,0,0,1,.54242.37738h0.001V8.9585a6.60115,6.60115,0,0,0,3.38232,5.72021L15.70547,17.364a0.50277,0.50277,0,0,0,.69263-0.16089l0.52576-.84369a0.50285,0.50285,0,0,0-.16089-0.69275l-4.30485-2.6818a4.65641,4.65641,0,0,1-2.44824-3.957l0.00165-2.03607a0.35723,0.35723,0,0,1,.54224-0.376l2.95184,1.34375A0.38659,0.38659,0,0,0,13.915,7.33594L9.01127,0.9931,4.10758,7.33594A0.38659,0.38659,0,0,0,4.517,7.95947Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 842 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M11.79221,9.2v8.29515a0.3052,0.3052,0,0,1-.30484.30486h-0.9903a0.3052,0.3052,0,0,1-.30484-0.30486V9.2h1.6m0.2-.2h-2v8.49515A0.50484,0.50484,0,0,0,10.49707,18h0.9903a0.50486,0.50486,0,0,0,.50485-0.50485V9h0Zm3.91806-1.66406L11.00659,0.9931,6.10284,7.33594a0.38659,0.38659,0,0,0,.40942.62354L9.46411,6.61572a0.35723,0.35723,0,0,1,.54224.376L10.008,9.02783a4.65641,4.65641,0,0,1-2.44824,3.957L3.2549,15.66667a0.50285,0.50285,0,0,0-.16089.69275L3.61977,17.2031a0.50277,0.50277,0,0,0,.69263.16089l4.31083-2.68528A6.60115,6.60115,0,0,0,12.00555,8.9585V6.9931h0.001A0.35757,0.35757,0,0,1,12.549,6.61572l2.95184,1.34375A0.38659,0.38659,0,0,0,15.91028,7.33594Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 844 B |
@@ -1,3 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 118 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M10,5.97986l0.011,0.00183A6.06019,6.06019,0,0,1,16,12.05493V16H15.99689l0.002,1.50317A0.49614,0.49614,0,0,1,15.50269,18H14.49622A0.49622,0.49622,0,0,1,14,17.50378V12.05493a4.05782,4.05782,0,0,0-3.98877-4.07324H8.01245a0.3576,0.3576,0,0,0-.37738.54248L8.97882,11.476a0.38659,0.38659,0,0,1-.62354.40942L2.0083,7l6.347-4.922a0.38659,0.38659,0,0,1,.62354.40942L7.63507,5.43927a0.35757,0.35757,0,0,0,.37738.54242H10"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 603 B |
@@ -1,4 +0,0 @@
|
||||
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||
<title>direction</title>
|
||||
<path fill="#FFFFFF" d="M9.98877,7.98169A4.05782,4.05782,0,0,0,6,12.05493v5.44885A0.49622,0.49622,0,0,1,5.50378,18H4.49731a0.49614,0.49614,0,0,1-.49615-0.49683L4.00311,16H4V12.05493A6.06019,6.06019,0,0,1,9.989,5.98169L10,5.97986V5.98169h1.98755a0.35757,0.35757,0,0,0,.37738-0.54242L11.02118,2.48743A0.38659,0.38659,0,0,1,11.64471,2.078L17.9917,7l-6.347,4.88544a0.38659,0.38659,0,0,1-.62354-0.40942l1.34375-2.95184a0.3576,0.3576,0,0,0-.37738-0.54248H9.98877Z"/>
|
||||
</svg>
|
||||
|
Before Width: | Height: | Size: 626 B |