mirror of
https://github.com/herizon1054/openpilot.git
synced 2026-06-08 12:14:14 +08:00
feat: Squash all min-features into full
This commit is contained in:
Rick Lan
3
.gitignore
vendored
3
.gitignore
vendored
@@ -95,3 +95,6 @@ Pipfile
|
|||||||
# Ignore all local history of files
|
# Ignore all local history of files
|
||||||
.history
|
.history
|
||||||
.ionide
|
.ionide
|
||||||
|
|
||||||
|
# rick - keep panda_tici standalone
|
||||||
|
panda_tici/
|
||||||
|
|||||||
140
ALKA_DESIGN.md
Normal file
140
ALKA_DESIGN.md
Normal file
@@ -0,0 +1,140 @@
|
|||||||
|
# ALKA (Always-on Lane Keeping Assist) Design v3
|
||||||
|
|
||||||
|
## Overview
|
||||||
|
|
||||||
|
ALKA enables lateral control (steering) when ACC Main is ON, without requiring cruise to be engaged. This allows lane keeping assist to function independently of longitudinal control.
|
||||||
|
|
||||||
|
**Simplified Behavior (v3):**
|
||||||
|
- All brands use direct tracking: `lkas_on = acc_main_on`
|
||||||
|
- No button/toggle tracking (removed TJA, LKAS button, LKAS HUD)
|
||||||
|
- ACC Main ON = ALKA enabled, ACC Main OFF = ALKA disabled
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Per-Brand Summary
|
||||||
|
|
||||||
|
| Brand | Status | ACC Main Source | Notes |
|
||||||
|
|-------|--------|-----------------|-------|
|
||||||
|
| Body | Disabled | - | No steering capability |
|
||||||
|
| Chrysler | Disabled | - | Needs special handling |
|
||||||
|
| Ford | Enabled | EngBrakeData (0x165) CcStat | |
|
||||||
|
| GM | Disabled | - | No ACC Main signal |
|
||||||
|
| Honda Nidec | Enabled | SCM_FEEDBACK (0x326) MAIN_ON | |
|
||||||
|
| Honda Bosch | Enabled | SCM_FEEDBACK (0x326) MAIN_ON | |
|
||||||
|
| Hyundai | Enabled | SCC11 (0x420) bit 0 | |
|
||||||
|
| Hyundai CAN-FD | Enabled | SCC_CONTROL (0x1A0) bit 66 | |
|
||||||
|
| Hyundai Legacy | Enabled | SCC11 (0x420) bit 0 | |
|
||||||
|
| Mazda | Enabled | CRZ_CTRL (0x21C) bit 17 | |
|
||||||
|
| Nissan | Enabled | CRUISE_THROTTLE (0x239) bit 17 | |
|
||||||
|
| PSA | Disabled | - | Not implemented |
|
||||||
|
| Rivian | Disabled | - | Different architecture |
|
||||||
|
| Subaru | Enabled | CruiseControl (0x240) bit 40 | |
|
||||||
|
| Subaru Preglobal | Enabled | CruiseControl (0x144) bit 48 | |
|
||||||
|
| Tesla | Disabled | - | Different architecture |
|
||||||
|
| Toyota | Enabled | PCM_CRUISE_2 (0x1D3) bit 15 | |
|
||||||
|
| Toyota (UNSUPPORTED_DSU) | Enabled | DSU_CRUISE (0x365) bit 0 | |
|
||||||
|
| VW MQB | Enabled | TSK_06 TSK_Status (>=2) | |
|
||||||
|
| VW PQ | Enabled | Motor_5 (0x480) bit 50 (long) | |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Permission Model
|
||||||
|
|
||||||
|
Lateral control requires checks at both layers. Normal path uses `controls_allowed`, ALKA path uses additional checks.
|
||||||
|
|
||||||
|
| Check | Panda | openpilot | Notes |
|
||||||
|
|-------|:-----:|:---------:|-------|
|
||||||
|
| **Normal Path** |
|
||||||
|
| `controls_allowed` (cruise engaged) | ✓ | ✓ | Either this OR ALKA path |
|
||||||
|
| **ALKA Path** |
|
||||||
|
| `alka_allowed` (brand supports) | ✓ | ✓ | Set per brand in safety init |
|
||||||
|
| `ALT_EXP_ALKA` (user enabled) | ✓ | ✓ | alternativeExperience flag |
|
||||||
|
| `lkas_on` (ACC Main ON) | ✓ | ✓ | Tracked via CAN messages |
|
||||||
|
| `vehicle_moving` / `!standstill` | ✓ | ✓ | |
|
||||||
|
| **openpilot Additional** |
|
||||||
|
| `gear_ok` (not P/N/R) | ✗ | ✓ | Python layer only |
|
||||||
|
| `calibrated` | ✗ | ✓ | Python layer only |
|
||||||
|
| `seatbelt latched` | ✗ | ✓ | Python layer only |
|
||||||
|
| `doors closed` | ✗ | ✓ | Python layer only |
|
||||||
|
| `!steerFaultTemporary` | ✗ | ✓ | Python layer only |
|
||||||
|
| `!steerFaultPermanent` | ✗ | ✓ | Python layer only |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Data Flow
|
||||||
|
|
||||||
|
```
|
||||||
|
┌─────────────────────────────────────────────────────────────────────┐
|
||||||
|
│ CAN Bus │
|
||||||
|
└─────────────────────────────────────────────────────────────────────┘
|
||||||
|
│ │
|
||||||
|
▼ ▼
|
||||||
|
┌─────────────────────────────────┐ ┌─────────────────────────────────┐
|
||||||
|
│ Safety Layer (panda C code) │ │ Python Layer │
|
||||||
|
│ │ │ │
|
||||||
|
│ rx_hook: │ │ carstate.py: │
|
||||||
|
│ - Parse ACC Main signal │ │ - Parse cruiseState.available │
|
||||||
|
│ - Set lkas_on = acc_main_on │ │ - Set self.lkas_on │
|
||||||
|
│ │ │ │
|
||||||
|
│ lat_control_allowed(): │ └─────────────┬───────────────────┘
|
||||||
|
│ - Check lkas_on + other flags │ │
|
||||||
|
│ - Gate steering commands │ ▼
|
||||||
|
└─────────────────────────────────┘ ┌─────────────────────────────────┐
|
||||||
|
│ card.py: │
|
||||||
|
│ - Publish carStateExt.lkasOn │
|
||||||
|
└─────────────┬───────────────────┘
|
||||||
|
│
|
||||||
|
▼
|
||||||
|
┌─────────────────────────────────┐
|
||||||
|
│ controlsd.py: │
|
||||||
|
│ - Read carStateExt.lkasOn │
|
||||||
|
│ - Check ALKA conditions │
|
||||||
|
│ - Set CC.latActive │
|
||||||
|
└─────────────────────────────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
### Key Files
|
||||||
|
|
||||||
|
| File | Purpose |
|
||||||
|
|------|---------|
|
||||||
|
| `custom.capnp` | Defines `CarStateExt` struct with `lkasOn` field |
|
||||||
|
| `log.capnp` | Includes `carStateExt` in event union |
|
||||||
|
| `interfaces.py` | Defines `self.lkas_on = False` default in `CarStateBase` |
|
||||||
|
| `carstate.py` (per brand) | Tracks `lkas_on` based on ACC Main |
|
||||||
|
| `card.py` | Publishes `carStateExt.lkasOn` from `CI.CS.lkas_on` |
|
||||||
|
| `controlsd.py` | Reads `carStateExt.lkasOn` to determine `alka_active` |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## ACC Main Tracking
|
||||||
|
|
||||||
|
All brands use simple direct tracking:
|
||||||
|
|
||||||
|
```c
|
||||||
|
// Panda (C code)
|
||||||
|
if (alka_allowed && (alternative_experience & ALT_EXP_ALKA)) {
|
||||||
|
lkas_on = acc_main_on; // or GET_BIT(msg, bit_position)
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
```python
|
||||||
|
# Python carstate.py
|
||||||
|
self.lkas_on = ret.cruiseState.available
|
||||||
|
```
|
||||||
|
|
||||||
|
This guard ensures:
|
||||||
|
1. Brand supports ALKA (`alka_allowed`)
|
||||||
|
2. User enabled ALKA (`ALT_EXP_ALKA`)
|
||||||
|
|
||||||
|
Without both conditions, no ACC Main tracking occurs, and ALKA remains disabled.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Testing
|
||||||
|
|
||||||
|
Safety tests verify:
|
||||||
|
- `alka_allowed` flag set correctly per brand
|
||||||
|
- ACC Main tracking updates `lkas_on` directly
|
||||||
|
- `lat_control_allowed()` returns true only when all conditions met
|
||||||
|
- Steering TX blocked when ALKA conditions not met
|
||||||
|
- Bus routing variants (camera_scc, unsupported_dsu)
|
||||||
@@ -196,6 +196,7 @@ Export('messaging')
|
|||||||
|
|
||||||
# Build other submodules
|
# Build other submodules
|
||||||
SConscript(['panda/SConscript'])
|
SConscript(['panda/SConscript'])
|
||||||
|
SConscript(['panda_tici/SConscript'])
|
||||||
|
|
||||||
# Build rednose library
|
# Build rednose library
|
||||||
SConscript(['rednose/SConscript'])
|
SConscript(['rednose/SConscript'])
|
||||||
|
|||||||
@@ -10,25 +10,101 @@ $Cxx.namespace("cereal");
|
|||||||
# DO rename the structs
|
# DO rename the structs
|
||||||
# DON'T change the identifier (e.g. @0x81c2f05a394cf4af)
|
# DON'T change the identifier (e.g. @0x81c2f05a394cf4af)
|
||||||
|
|
||||||
struct CustomReserved0 @0x81c2f05a394cf4af {
|
struct ControlsStateExt @0x81c2f05a394cf4af {
|
||||||
|
alkaActive @0 :Bool;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct CustomReserved1 @0xaedffd8f31e7b55d {
|
struct CarStateExt @0xaedffd8f31e7b55d {
|
||||||
|
# dp - ALKA: lkasOn state from carstate (mirrors panda's lkas_on)
|
||||||
|
lkasOn @0 :Bool;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct CustomReserved2 @0xf35cc4560bbf6ec2 {
|
struct ModelExt @0xf35cc4560bbf6ec2 {
|
||||||
|
leftEdgeDetected @0 :Bool;
|
||||||
|
rightEdgeDetected @1 :Bool;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct CustomReserved3 @0xda96579883444c35 {
|
struct LiveGPS @0xda96579883444c35 {
|
||||||
|
# Position
|
||||||
|
latitude @0 :Float64; # degrees
|
||||||
|
longitude @1 :Float64; # degrees
|
||||||
|
altitude @2 :Float64; # meters (WGS84)
|
||||||
|
|
||||||
|
# Motion
|
||||||
|
speed @3 :Float32; # m/s (horizontal speed)
|
||||||
|
bearingDeg @4 :Float32; # degrees (heading)
|
||||||
|
|
||||||
|
# Accuracy
|
||||||
|
horizontalAccuracy @5 :Float32; # meters
|
||||||
|
verticalAccuracy @6 :Float32; # meters
|
||||||
|
|
||||||
|
# Status
|
||||||
|
gpsOK @7 :Bool; # livePose valid + GPS fresh
|
||||||
|
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
|
||||||
}
|
}
|
||||||
|
|
||||||
struct CustomReserved4 @0x80ae746ee2596b11 {
|
# 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 CustomReserved5 @0xa5cd762cd951a455 {
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct CustomReserved6 @0xf98d843bfd7004a3 {
|
enum ManeuverType {
|
||||||
|
none @0;
|
||||||
|
turn @1; # intersection turn - use turnLeft/Right desire
|
||||||
|
laneChange @2; # highway exit/fork - use laneChangeLeft/Right desire
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
struct DashyState @0xa5cd762cd951a455 {
|
||||||
|
# Pre-serialized JSON bytes for dashy UI
|
||||||
|
# Aggregates all topics needed by dashy into single message
|
||||||
|
json @0 :Data;
|
||||||
|
}
|
||||||
|
|
||||||
|
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 CustomReserved7 @0xb86e6369214c01c8 {
|
struct CustomReserved7 @0xb86e6369214c01c8 {
|
||||||
|
|||||||
@@ -2625,13 +2625,13 @@ struct Event {
|
|||||||
# DO change the name of the field and struct
|
# DO change the name of the field and struct
|
||||||
# DON'T change the ID (e.g. @107)
|
# DON'T change the ID (e.g. @107)
|
||||||
# DON'T change which struct it points to
|
# DON'T change which struct it points to
|
||||||
customReserved0 @107 :Custom.CustomReserved0;
|
controlsStateExt @107 :Custom.ControlsStateExt;
|
||||||
customReserved1 @108 :Custom.CustomReserved1;
|
carStateExt @108 :Custom.CarStateExt;
|
||||||
customReserved2 @109 :Custom.CustomReserved2;
|
modelExt @109 :Custom.ModelExt;
|
||||||
customReserved3 @110 :Custom.CustomReserved3;
|
liveGPS @110 :Custom.LiveGPS;
|
||||||
customReserved4 @111 :Custom.CustomReserved4;
|
maaControl @111 :Custom.MaaControl;
|
||||||
customReserved5 @112 :Custom.CustomReserved5;
|
dashyState @112 :Custom.DashyState;
|
||||||
customReserved6 @113 :Custom.CustomReserved6;
|
navInstructionExt @113 :Custom.NavInstructionExt;
|
||||||
customReserved7 @114 :Custom.CustomReserved7;
|
customReserved7 @114 :Custom.CustomReserved7;
|
||||||
customReserved8 @115 :Custom.CustomReserved8;
|
customReserved8 @115 :Custom.CustomReserved8;
|
||||||
customReserved9 @116 :Custom.CustomReserved9;
|
customReserved9 @116 :Custom.CustomReserved9;
|
||||||
|
|||||||
@@ -75,7 +75,7 @@ _services: dict[str, tuple] = {
|
|||||||
"modelV2": (True, 20., None, QueueSize.BIG),
|
"modelV2": (True, 20., None, QueueSize.BIG),
|
||||||
"managerState": (True, 2., 1),
|
"managerState": (True, 2., 1),
|
||||||
"uploaderState": (True, 0., 1),
|
"uploaderState": (True, 0., 1),
|
||||||
"navInstruction": (True, 1., 10),
|
# "navInstruction": (True, 1., 10), # dp - make it 0 hz
|
||||||
"navRoute": (True, 0.),
|
"navRoute": (True, 0.),
|
||||||
"navThumbnail": (True, 0.),
|
"navThumbnail": (True, 0.),
|
||||||
"qRoadEncodeIdx": (False, 20.),
|
"qRoadEncodeIdx": (False, 20.),
|
||||||
@@ -102,6 +102,15 @@ _services: dict[str, tuple] = {
|
|||||||
"customReservedRawData0": (True, 0.),
|
"customReservedRawData0": (True, 0.),
|
||||||
"customReservedRawData1": (True, 0.),
|
"customReservedRawData1": (True, 0.),
|
||||||
"customReservedRawData2": (True, 0.),
|
"customReservedRawData2": (True, 0.),
|
||||||
|
"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)
|
||||||
}
|
}
|
||||||
SERVICE_LIST = {name: Service(*vals) for
|
SERVICE_LIST = {name: Service(*vals) for
|
||||||
idx, (name, vals) in enumerate(_services.items())}
|
idx, (name, vals) in enumerate(_services.items())}
|
||||||
|
|||||||
@@ -131,4 +131,35 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"Version", {PERSISTENT, STRING}},
|
{"Version", {PERSISTENT, STRING}},
|
||||||
{"dp_dev_last_log", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
|
{"dp_dev_last_log", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
|
||||||
{"dp_dev_reset_conf", {CLEAR_ON_MANAGER_START, BOOL, "0"}},
|
{"dp_dev_reset_conf", {CLEAR_ON_MANAGER_START, BOOL, "0"}},
|
||||||
|
{"dp_dev_beep", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_dev_is_rhd", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lat_alka", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_ui_display_mode", {PERSISTENT, INT, "0"}},
|
||||||
|
{"dp_dev_model_selected", {PERSISTENT, STRING}},
|
||||||
|
{"dp_dev_model_list", {PERSISTENT, STRING}},
|
||||||
|
{"dp_lat_lca_speed", {PERSISTENT, INT, "20"}},
|
||||||
|
{"dp_lat_lca_auto_sec", {PERSISTENT, FLOAT, "0.0"}},
|
||||||
|
{"dp_dev_go_off_road", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||||
|
{"dp_ui_hide_hud_speed_kph", {PERSISTENT, INT, "0"}},
|
||||||
|
{"dp_lon_ext_radar", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lat_road_edge_detection", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_ui_rainbow", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"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"}},
|
||||||
|
{"dp_ui_mici", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lat_offset_cm", {PERSISTENT, INT, "0"}},
|
||||||
};
|
};
|
||||||
|
|||||||
598
dragonpilot/dashy/dashyd.py
Executable file
598
dragonpilot/dashy/dashyd.py
Executable file
@@ -0,0 +1,598 @@
|
|||||||
|
#!/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()
|
||||||
423
dragonpilot/dashy/gpsd.py
Executable file
423
dragonpilot/dashy/gpsd.py
Executable file
@@ -0,0 +1,423 @@
|
|||||||
|
#!/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()
|
||||||
30
dragonpilot/dashy/maa/__init__.py
Normal file
30
dragonpilot/dashy/maa/__init__.py
Normal file
@@ -0,0 +1,30 @@
|
|||||||
|
"""
|
||||||
|
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
|
||||||
|
"""
|
||||||
657
dragonpilot/dashy/maa/helpers.py
Normal file
657
dragonpilot/dashy/maa/helpers.py
Normal file
@@ -0,0 +1,657 @@
|
|||||||
|
"""
|
||||||
|
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))
|
||||||
47
dragonpilot/dashy/maa/lib/__init__.py
Normal file
47
dragonpilot/dashy/maa/lib/__init__.py
Normal file
@@ -0,0 +1,47 @@
|
|||||||
|
"""
|
||||||
|
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',
|
||||||
|
]
|
||||||
238
dragonpilot/dashy/maa/lib/accel_limiter.py
Normal file
238
dragonpilot/dashy/maa/lib/accel_limiter.py
Normal file
@@ -0,0 +1,238 @@
|
|||||||
|
#!/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)
|
||||||
360
dragonpilot/dashy/maa/lib/longitudinal_helper.py
Normal file
360
dragonpilot/dashy/maa/lib/longitudinal_helper.py
Normal file
@@ -0,0 +1,360 @@
|
|||||||
|
#!/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
|
||||||
119
dragonpilot/dashy/maa/lib/maa_desire.py
Normal file
119
dragonpilot/dashy/maa/lib/maa_desire.py
Normal file
@@ -0,0 +1,119 @@
|
|||||||
|
#!/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
|
||||||
118
dragonpilot/dashy/maa/lib/model_helper.py
Normal file
118
dragonpilot/dashy/maa/lib/model_helper.py
Normal file
@@ -0,0 +1,118 @@
|
|||||||
|
#!/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
|
||||||
919
dragonpilot/dashy/maa/maa_controld.py
Executable file
919
dragonpilot/dashy/maa/maa_controld.py
Executable file
@@ -0,0 +1,919 @@
|
|||||||
|
#!/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()
|
||||||
566
dragonpilot/dashy/maa/maad.py
Executable file
566
dragonpilot/dashy/maa/maad.py
Executable file
@@ -0,0 +1,566 @@
|
|||||||
|
#!/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()
|
||||||
54
dragonpilot/dashy/maa/providers/__init__.py
Normal file
54
dragonpilot/dashy/maa/providers/__init__.py
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
"""
|
||||||
|
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',
|
||||||
|
]
|
||||||
177
dragonpilot/dashy/maa/providers/base.py
Normal file
177
dragonpilot/dashy/maa/providers/base.py
Normal file
@@ -0,0 +1,177 @@
|
|||||||
|
"""
|
||||||
|
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
|
||||||
45
dragonpilot/dashy/maa/providers/config.py
Normal file
45
dragonpilot/dashy/maa/providers/config.py
Normal file
@@ -0,0 +1,45 @@
|
|||||||
|
"""
|
||||||
|
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()
|
||||||
32
dragonpilot/dashy/maa/providers/dragonpilot/__init__.py
Normal file
32
dragonpilot/dashy/maa/providers/dragonpilot/__init__.py
Normal file
@@ -0,0 +1,32 @@
|
|||||||
|
"""
|
||||||
|
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']
|
||||||
123
dragonpilot/dashy/maa/providers/dragonpilot/client.py
Normal file
123
dragonpilot/dashy/maa/providers/dragonpilot/client.py
Normal file
@@ -0,0 +1,123 @@
|
|||||||
|
"""
|
||||||
|
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
|
||||||
269
dragonpilot/dashy/maa/providers/dragonpilot/routing.py
Normal file
269
dragonpilot/dashy/maa/providers/dragonpilot/routing.py
Normal file
@@ -0,0 +1,269 @@
|
|||||||
|
"""
|
||||||
|
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,
|
||||||
|
)
|
||||||
191
dragonpilot/dashy/maa/providers/dragonpilot/search.py
Normal file
191
dragonpilot/dashy/maa/providers/dragonpilot/search.py
Normal file
@@ -0,0 +1,191 @@
|
|||||||
|
"""
|
||||||
|
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]
|
||||||
56
dragonpilot/dashy/maa/providers/factory.py
Normal file
56
dragonpilot/dashy/maa/providers/factory.py
Normal file
@@ -0,0 +1,56 @@
|
|||||||
|
"""
|
||||||
|
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()
|
||||||
188
dragonpilot/dashy/maa/providers/map_service.py
Normal file
188
dragonpilot/dashy/maa/providers/map_service.py
Normal file
@@ -0,0 +1,188 @@
|
|||||||
|
"""
|
||||||
|
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,
|
||||||
|
},
|
||||||
|
}
|
||||||
180
dragonpilot/dashy/maa/providers/models.py
Normal file
180
dragonpilot/dashy/maa/providers/models.py
Normal file
@@ -0,0 +1,180 @@
|
|||||||
|
"""
|
||||||
|
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
|
||||||
5
dragonpilot/dashy/maa/providers/routing/__init__.py
Normal file
5
dragonpilot/dashy/maa/providers/routing/__init__.py
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
"""Routing providers."""
|
||||||
|
|
||||||
|
from .osrm import OSRMRouteProvider
|
||||||
|
|
||||||
|
__all__ = ['OSRMRouteProvider']
|
||||||
190
dragonpilot/dashy/maa/providers/routing/osrm.py
Normal file
190
dragonpilot/dashy/maa/providers/routing/osrm.py
Normal file
@@ -0,0 +1,190 @@
|
|||||||
|
"""
|
||||||
|
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,
|
||||||
|
)
|
||||||
5
dragonpilot/dashy/maa/providers/search/__init__.py
Normal file
5
dragonpilot/dashy/maa/providers/search/__init__.py
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
"""Search providers."""
|
||||||
|
|
||||||
|
from .photon import PhotonSearchProvider
|
||||||
|
|
||||||
|
__all__ = ['PhotonSearchProvider']
|
||||||
198
dragonpilot/dashy/maa/providers/search/photon.py
Normal file
198
dragonpilot/dashy/maa/providers/search/photon.py
Normal file
@@ -0,0 +1,198 @@
|
|||||||
|
"""
|
||||||
|
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]
|
||||||
6
dragonpilot/dashy/maa/providers/tiles/__init__.py
Normal file
6
dragonpilot/dashy/maa/providers/tiles/__init__.py
Normal file
@@ -0,0 +1,6 @@
|
|||||||
|
"""Tile providers."""
|
||||||
|
|
||||||
|
# Import providers to register them with the factory
|
||||||
|
from .openfreemap import OpenFreeMapTileProvider
|
||||||
|
|
||||||
|
__all__ = ['OpenFreeMapTileProvider']
|
||||||
88
dragonpilot/dashy/maa/providers/tiles/openfreemap.py
Normal file
88
dragonpilot/dashy/maa/providers/tiles/openfreemap.py
Normal file
@@ -0,0 +1,88 @@
|
|||||||
|
"""
|
||||||
|
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"}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
5
dragonpilot/dashy/maa/providers/utils/__init__.py
Normal file
5
dragonpilot/dashy/maa/providers/utils/__init__.py
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
"""Utility functions for map providers."""
|
||||||
|
|
||||||
|
from .coordinates import wgs84_to_gcj02, gcj02_to_wgs84
|
||||||
|
|
||||||
|
__all__ = ['wgs84_to_gcj02', 'gcj02_to_wgs84']
|
||||||
114
dragonpilot/dashy/maa/providers/utils/coordinates.py
Normal file
114
dragonpilot/dashy/maa/providers/utils/coordinates.py
Normal file
@@ -0,0 +1,114 @@
|
|||||||
|
"""
|
||||||
|
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
|
||||||
447
dragonpilot/dashy/maa/route_tracker.py
Normal file
447
dragonpilot/dashy/maa/route_tracker.py
Normal file
@@ -0,0 +1,447 @@
|
|||||||
|
"""
|
||||||
|
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
|
||||||
998
dragonpilot/dashy/serverd.py
Executable file
998
dragonpilot/dashy/serverd.py
Executable file
@@ -0,0 +1,998 @@
|
|||||||
|
#!/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 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
|
||||||
|
"""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import asyncio
|
||||||
|
import json
|
||||||
|
import os
|
||||||
|
import logging
|
||||||
|
import time
|
||||||
|
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 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")
|
||||||
|
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."""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self._params = None
|
||||||
|
self._car_params = None
|
||||||
|
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)."""
|
||||||
|
if self._params is None:
|
||||||
|
try:
|
||||||
|
self._params = Params()
|
||||||
|
except Exception as e:
|
||||||
|
logger.warning(f"Params unavailable, using mock: {e}")
|
||||||
|
self._params = MockParams()
|
||||||
|
return self._params
|
||||||
|
|
||||||
|
def get_car_params(self):
|
||||||
|
"""Get cached CarParams data (brand, longitudinal control)."""
|
||||||
|
now = time.time()
|
||||||
|
if self._car_params is None or (now - self._car_params_time) > CAR_PARAMS_CACHE_TTL:
|
||||||
|
self._car_params = self._parse_car_params()
|
||||||
|
self._car_params_time = now
|
||||||
|
return self._car_params
|
||||||
|
|
||||||
|
def _parse_car_params(self):
|
||||||
|
"""Parse CarParams from Params store."""
|
||||||
|
result = {'brand': '', 'openpilot_longitudinal_control': False}
|
||||||
|
try:
|
||||||
|
car_params_bytes = self.params.get("CarParams")
|
||||||
|
if car_params_bytes:
|
||||||
|
from cereal import car
|
||||||
|
with car.CarParams.from_bytes(car_params_bytes) as cp:
|
||||||
|
result['brand'] = cp.brand
|
||||||
|
result['openpilot_longitudinal_control'] = cp.openpilotLongitudinalControl
|
||||||
|
except Exception as e:
|
||||||
|
logger.debug(f"Could not parse CarParams: {e}")
|
||||||
|
return result
|
||||||
|
|
||||||
|
def get_settings_context(self):
|
||||||
|
"""Get context dict for settings condition evaluation."""
|
||||||
|
now = time.time()
|
||||||
|
if self._context is None or (now - self._context_time) > CAR_PARAMS_CACHE_TTL:
|
||||||
|
car_params = self.get_car_params()
|
||||||
|
self._context = {
|
||||||
|
'brand': car_params['brand'],
|
||||||
|
'openpilotLongitudinalControl': car_params['openpilot_longitudinal_control'],
|
||||||
|
'LITE': os.getenv("LITE") is not None,
|
||||||
|
'MICI': self._check_mici()
|
||||||
|
}
|
||||||
|
self._context_time = now
|
||||||
|
return self._context
|
||||||
|
|
||||||
|
def _check_mici(self):
|
||||||
|
"""Check if device is MICI type."""
|
||||||
|
try:
|
||||||
|
return HARDWARE.get_device_type() == "mici"
|
||||||
|
except Exception:
|
||||||
|
return False
|
||||||
|
|
||||||
|
def get_bool_safe(self, key, default=False):
|
||||||
|
"""Safely get a boolean param with default."""
|
||||||
|
try:
|
||||||
|
return self.params.get_bool(key)
|
||||||
|
except Exception:
|
||||||
|
return default
|
||||||
|
|
||||||
|
def invalidate(self):
|
||||||
|
"""Invalidate all caches."""
|
||||||
|
self._car_params = None
|
||||||
|
self._context = None
|
||||||
|
self._settings_cache = None
|
||||||
|
|
||||||
|
|
||||||
|
# --- Helper Functions ---
|
||||||
|
def api_handler(func):
|
||||||
|
"""Decorator for API handlers with consistent error handling."""
|
||||||
|
@wraps(func)
|
||||||
|
async def wrapper(request):
|
||||||
|
try:
|
||||||
|
return await func(request)
|
||||||
|
except web.HTTPException:
|
||||||
|
raise
|
||||||
|
except Exception as e:
|
||||||
|
logger.error(f"{func.__name__} error: {e}", exc_info=True)
|
||||||
|
return web.json_response({'error': str(e)}, status=500)
|
||||||
|
return wrapper
|
||||||
|
|
||||||
|
|
||||||
|
def get_safe_path(requested_path):
|
||||||
|
"""Ensures the requested path is within DEFAULT_DIR."""
|
||||||
|
combined_path = os.path.join(DEFAULT_DIR, requested_path.lstrip('/'))
|
||||||
|
safe_path = os.path.realpath(combined_path)
|
||||||
|
if os.path.commonpath((safe_path, DEFAULT_DIR)) == DEFAULT_DIR:
|
||||||
|
return safe_path
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def eval_condition(condition, context):
|
||||||
|
"""Safely evaluate a condition string."""
|
||||||
|
if not condition:
|
||||||
|
return True
|
||||||
|
try:
|
||||||
|
return eval(condition, {"__builtins__": {}}, context)
|
||||||
|
except Exception as e:
|
||||||
|
logger.debug(f"Condition evaluation failed: {condition}, error: {e}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_value(value):
|
||||||
|
"""Resolve callable values (lambdas) for JSON serialization."""
|
||||||
|
return value() if callable(value) else value
|
||||||
|
|
||||||
|
|
||||||
|
# --- API Endpoints ---
|
||||||
|
@api_handler
|
||||||
|
async def init_api(request):
|
||||||
|
"""Provide initial data to the client."""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
return web.json_response({
|
||||||
|
'dp_dev_dashy': cache.get_bool_safe("dp_dev_dashy", True),
|
||||||
|
})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def list_files_api(request):
|
||||||
|
"""List files and folders."""
|
||||||
|
path_param = request.query.get('path', '/')
|
||||||
|
safe_path = get_safe_path(path_param)
|
||||||
|
|
||||||
|
if not safe_path or not os.path.isdir(safe_path):
|
||||||
|
return web.json_response({'error': 'Invalid or Not Found Path'}, status=404)
|
||||||
|
|
||||||
|
items = []
|
||||||
|
for entry in os.listdir(safe_path):
|
||||||
|
full_path = os.path.join(safe_path, entry)
|
||||||
|
try:
|
||||||
|
stat = os.stat(full_path)
|
||||||
|
is_dir = os.path.isdir(full_path)
|
||||||
|
items.append({
|
||||||
|
'name': entry,
|
||||||
|
'is_dir': is_dir,
|
||||||
|
'mtime': datetime.fromtimestamp(stat.st_mtime).strftime('%Y-%m-%d %H:%M'),
|
||||||
|
'size': stat.st_size if not is_dir else 0
|
||||||
|
})
|
||||||
|
except FileNotFoundError:
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Sort: directories first (by mtime desc), then files (by mtime desc)
|
||||||
|
dirs = sorted([i for i in items if i['is_dir']], key=lambda x: x['mtime'], reverse=True)
|
||||||
|
files = sorted([i for i in items if not i['is_dir']], key=lambda x: x['mtime'], reverse=True)
|
||||||
|
|
||||||
|
relative_path = os.path.relpath(safe_path, DEFAULT_DIR)
|
||||||
|
return web.json_response({
|
||||||
|
'path': '' if relative_path == '.' else relative_path,
|
||||||
|
'files': dirs + files
|
||||||
|
})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def serve_player_api(request):
|
||||||
|
"""Serve the HLS player page."""
|
||||||
|
file_path = request.query.get('file')
|
||||||
|
if not file_path:
|
||||||
|
return web.Response(text="File parameter is required.", status=400)
|
||||||
|
|
||||||
|
player_html_path = os.path.join(WEB_DIST_PATH, 'pages', 'player.html')
|
||||||
|
try:
|
||||||
|
with open(player_html_path, 'r') as f:
|
||||||
|
html_template = f.read()
|
||||||
|
except FileNotFoundError:
|
||||||
|
return web.Response(text="Player HTML not found.", status=500)
|
||||||
|
|
||||||
|
html = html_template.replace('{{FILE_PATH}}', quote(file_path))
|
||||||
|
return web.Response(text=html, content_type='text/html')
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def serve_manifest_api(request):
|
||||||
|
"""Dynamically generate m3u8 playlist."""
|
||||||
|
file_path = request.query.get('file', '').lstrip('/')
|
||||||
|
if not file_path:
|
||||||
|
return web.Response(text="File parameter is required.", status=400)
|
||||||
|
|
||||||
|
encoded_path = quote(file_path)
|
||||||
|
manifest = f"#EXTM3U\n#EXT-X-VERSION:3\n#EXT-X-TARGETDURATION:60\n#EXT-X-PLAYLIST-TYPE:VOD\n#EXTINF:60.0,\n/media/{encoded_path}\n#EXT-X-ENDLIST\n"
|
||||||
|
return web.Response(text=manifest, content_type='application/vnd.apple.mpegurl')
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
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
|
||||||
|
current_lang = params.get("LanguageSetting")
|
||||||
|
if current_lang:
|
||||||
|
lang_str = current_lang.decode() if isinstance(current_lang, bytes) else str(current_lang)
|
||||||
|
lang_str = lang_str.removeprefix("main_")
|
||||||
|
if lang_str != base_multilang.language and lang_str in base_multilang.languages.values():
|
||||||
|
base_multilang._language = lang_str
|
||||||
|
base_multilang.setup()
|
||||||
|
|
||||||
|
context = cache.get_settings_context()
|
||||||
|
settings_with_values = []
|
||||||
|
|
||||||
|
for section in SETTINGS:
|
||||||
|
if not eval_condition(section.get('condition'), context):
|
||||||
|
continue
|
||||||
|
|
||||||
|
section_copy = section.copy()
|
||||||
|
settings_list = []
|
||||||
|
|
||||||
|
for setting in section.get('settings', []):
|
||||||
|
if not eval_condition(setting.get('condition'), context):
|
||||||
|
continue
|
||||||
|
|
||||||
|
setting_copy = setting.copy()
|
||||||
|
key = setting['key']
|
||||||
|
|
||||||
|
# Resolve callable values
|
||||||
|
for field in ['title', 'description', 'suffix', 'special_value_text']:
|
||||||
|
if field in setting_copy:
|
||||||
|
setting_copy[field] = resolve_value(setting_copy[field])
|
||||||
|
if 'options' in setting_copy:
|
||||||
|
setting_copy['options'] = [resolve_value(opt) for opt in setting_copy['options']]
|
||||||
|
|
||||||
|
# Get current value based on type
|
||||||
|
setting_copy['current_value'] = _get_setting_value(params, setting)
|
||||||
|
settings_list.append(setting_copy)
|
||||||
|
|
||||||
|
if settings_list:
|
||||||
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
def _get_setting_value(params, setting):
|
||||||
|
"""Get current value for a setting from Params."""
|
||||||
|
key = setting['key']
|
||||||
|
setting_type = setting['type']
|
||||||
|
default = setting.get('default', 0)
|
||||||
|
|
||||||
|
try:
|
||||||
|
if setting_type == 'toggle_item':
|
||||||
|
return params.get_bool(key)
|
||||||
|
elif setting_type == 'double_spin_button_item':
|
||||||
|
value = params.get(key)
|
||||||
|
return float(value) if value is not None else float(default)
|
||||||
|
else: # spin_button_item, text_spin_button_item
|
||||||
|
value = params.get(key)
|
||||||
|
return int(value) if value is not None else int(default)
|
||||||
|
except Exception as e:
|
||||||
|
logger.warning(f"Error getting value for {key}: {e}")
|
||||||
|
if setting_type == 'toggle_item':
|
||||||
|
return False
|
||||||
|
elif setting_type == 'double_spin_button_item':
|
||||||
|
return float(default)
|
||||||
|
return int(default)
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def save_param_api(request):
|
||||||
|
"""Save a single param value.
|
||||||
|
|
||||||
|
Usage: POST /api/settings/params/{name}
|
||||||
|
Body: { "value": <value> }
|
||||||
|
"""
|
||||||
|
param_name = request.match_info.get('param_name')
|
||||||
|
if not param_name:
|
||||||
|
return web.json_response({'error': 'param_name is required'}, status=400)
|
||||||
|
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
params = cache.params
|
||||||
|
data = await request.json()
|
||||||
|
|
||||||
|
if 'value' not in data:
|
||||||
|
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']})
|
||||||
|
|
||||||
|
|
||||||
|
def _save_param(params, key, value):
|
||||||
|
"""Save a single param value with proper type handling."""
|
||||||
|
try:
|
||||||
|
param_type = params.get_type(key)
|
||||||
|
|
||||||
|
if param_type == 1: # BOOL
|
||||||
|
params.put_bool(key, bool(value))
|
||||||
|
elif param_type == 2: # INT
|
||||||
|
params.put(key, int(value))
|
||||||
|
elif param_type == 3: # FLOAT
|
||||||
|
params.put(key, float(value))
|
||||||
|
elif isinstance(value, bool):
|
||||||
|
params.put_bool(key, value)
|
||||||
|
else:
|
||||||
|
params.put(key, str(value) if not isinstance(value, str) else value)
|
||||||
|
|
||||||
|
logger.debug(f"Saved {key}={value} (type={param_type})")
|
||||||
|
except Exception as e:
|
||||||
|
logger.error(f"Error saving param {key}={value}: {e}")
|
||||||
|
raise
|
||||||
|
|
||||||
|
|
||||||
|
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
|
||||||
|
async def get_param_api(request):
|
||||||
|
"""Get a single param value."""
|
||||||
|
param_name = request.match_info.get('param_name')
|
||||||
|
if not param_name:
|
||||||
|
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
|
||||||
|
|
||||||
|
return web.json_response({'key': param_name, 'value': value})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def get_model_list_api(request):
|
||||||
|
"""Get the model list and current selection."""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
params = cache.params
|
||||||
|
|
||||||
|
# Get model list
|
||||||
|
model_list = {}
|
||||||
|
try:
|
||||||
|
model_list_raw = params.get("dp_dev_model_list")
|
||||||
|
if model_list_raw:
|
||||||
|
model_list = json.loads(model_list_raw)
|
||||||
|
except Exception as e:
|
||||||
|
logger.debug(f"Could not parse dp_dev_model_list: {e}")
|
||||||
|
|
||||||
|
# Get current selection
|
||||||
|
selected_model = ""
|
||||||
|
try:
|
||||||
|
selected_raw = params.get("dp_dev_model_selected")
|
||||||
|
if selected_raw:
|
||||||
|
selected_model = selected_raw.decode('utf-8') if isinstance(selected_raw, bytes) else str(selected_raw)
|
||||||
|
except Exception as e:
|
||||||
|
logger.debug(f"Could not get dp_dev_model_selected: {e}")
|
||||||
|
|
||||||
|
return web.json_response({
|
||||||
|
'model_list': model_list,
|
||||||
|
'selected_model': selected_model
|
||||||
|
})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def save_model_selection_api(request):
|
||||||
|
"""Save the selected model."""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
params = cache.params
|
||||||
|
data = await request.json()
|
||||||
|
|
||||||
|
selected_model = data.get('selected_model', '')
|
||||||
|
|
||||||
|
if not selected_model or selected_model == "[AUTO]":
|
||||||
|
params.put("dp_dev_model_selected", "")
|
||||||
|
logger.info("Model selection cleared (AUTO mode)")
|
||||||
|
else:
|
||||||
|
params.put("dp_dev_model_selected", selected_model)
|
||||||
|
logger.info(f"Model selection saved: {selected_model}")
|
||||||
|
|
||||||
|
return web.json_response({'status': 'success'})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def webrtc_stream_proxy(request):
|
||||||
|
"""Proxy WebRTC stream requests to webrtcd."""
|
||||||
|
host = request.host.split(':')[0]
|
||||||
|
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
|
||||||
|
)
|
||||||
|
|
||||||
|
# --- 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
|
||||||
|
async def cors_middleware(request, handler):
|
||||||
|
response = await handler(request)
|
||||||
|
response.headers['Access-Control-Allow-Origin'] = '*'
|
||||||
|
response.headers['Access-Control-Allow-Methods'] = 'GET, POST, PUT, DELETE, OPTIONS'
|
||||||
|
response.headers['Access-Control-Allow-Headers'] = 'Content-Type, Authorization'
|
||||||
|
|
||||||
|
# Disable caching for web assets
|
||||||
|
path = request.path.lower()
|
||||||
|
if path.endswith(('.html', '.js', '.css')) or path == '/':
|
||||||
|
response.headers['Cache-Control'] = 'no-cache, no-store, must-revalidate'
|
||||||
|
response.headers['Pragma'] = 'no-cache'
|
||||||
|
response.headers['Expires'] = '0'
|
||||||
|
|
||||||
|
return response
|
||||||
|
|
||||||
|
|
||||||
|
async def handle_cors_preflight(request):
|
||||||
|
return web.Response(status=200, headers={
|
||||||
|
'Access-Control-Allow-Origin': '*',
|
||||||
|
'Access-Control-Allow-Methods': 'GET, POST, PUT, DELETE, OPTIONS',
|
||||||
|
'Access-Control-Allow-Headers': 'Content-Type, Authorization',
|
||||||
|
'Access-Control-Max-Age': '86400',
|
||||||
|
})
|
||||||
|
|
||||||
|
|
||||||
|
# --- Application Setup ---
|
||||||
|
async def on_startup(app):
|
||||||
|
"""Initialize app-level resources."""
|
||||||
|
app['cache'] = AppCache()
|
||||||
|
app['http_session'] = ClientSession(timeout=WEBRTC_TIMEOUT)
|
||||||
|
logger.info("Dashy server started")
|
||||||
|
|
||||||
|
|
||||||
|
async def on_cleanup(app):
|
||||||
|
"""Cleanup app-level resources."""
|
||||||
|
await app['http_session'].close()
|
||||||
|
logger.info("Dashy server stopped")
|
||||||
|
|
||||||
|
|
||||||
|
def setup_aiohttp_app(host: str, port: int, debug: bool):
|
||||||
|
logging.basicConfig(
|
||||||
|
level=logging.DEBUG if debug else logging.INFO,
|
||||||
|
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
|
||||||
|
)
|
||||||
|
|
||||||
|
app = web.Application(middlewares=[cors_middleware])
|
||||||
|
app['port'] = port
|
||||||
|
|
||||||
|
# API routes
|
||||||
|
app.router.add_get("/api/init", init_api)
|
||||||
|
app.router.add_get("/api/files", list_files_api)
|
||||||
|
app.router.add_get("/api/play", serve_player_api)
|
||||||
|
app.router.add_get("/api/manifest.m3u8", serve_manifest_api)
|
||||||
|
app.router.add_get("/api/settings", get_settings_config_api)
|
||||||
|
app.router.add_get("/api/settings/params/{param_name}", get_param_api)
|
||||||
|
app.router.add_post("/api/settings/params/{param_name}", save_param_api)
|
||||||
|
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
|
||||||
|
app.router.add_static('/media', path=DEFAULT_DIR, name='media', show_index=False, follow_symlinks=False)
|
||||||
|
app.router.add_static('/download', path=DEFAULT_DIR, name='download', show_index=False, follow_symlinks=False)
|
||||||
|
app.router.add_get("/", lambda r: web.FileResponse(os.path.join(WEB_DIST_PATH, "index.html")))
|
||||||
|
app.router.add_static("/", path=WEB_DIST_PATH)
|
||||||
|
|
||||||
|
app.on_startup.append(on_startup)
|
||||||
|
app.on_cleanup.append(on_cleanup)
|
||||||
|
|
||||||
|
return app
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
parser = argparse.ArgumentParser(description="Dashy Server")
|
||||||
|
parser.add_argument("--host", type=str, default="0.0.0.0", help="Host to listen on")
|
||||||
|
parser.add_argument("--port", type=int, default=5088, help="Port to listen on")
|
||||||
|
parser.add_argument("--debug", action="store_true", help="Enable debug mode")
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
app = setup_aiohttp_app(args.host, args.port, args.debug)
|
||||||
|
web.run_app(app, host=args.host, port=args.port)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
2
dragonpilot/dashy/web/dist/css/styles.css
vendored
Normal file
2
dragonpilot/dashy/web/dist/css/styles.css
vendored
Normal file
File diff suppressed because one or more lines are too long
BIN
dragonpilot/dashy/web/dist/icons/blinker/blind_spot_left.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/blinker/blind_spot_left.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 40 KiB |
BIN
dragonpilot/dashy/web/dist/icons/blinker/blind_spot_right.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/blinker/blind_spot_right.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 40 KiB |
BIN
dragonpilot/dashy/web/dist/icons/blinker/turn_signal_left.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/blinker/turn_signal_left.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 30 KiB |
BIN
dragonpilot/dashy/web/dist/icons/blinker/turn_signal_right.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/blinker/turn_signal_right.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 34 KiB |
BIN
dragonpilot/dashy/web/dist/icons/dashy.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/dashy.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.1 MiB |
BIN
dragonpilot/dashy/web/dist/icons/exp/experimental.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/exp/experimental.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 34 KiB |
BIN
dragonpilot/dashy/web/dist/icons/icon-192x192.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/icon-192x192.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 145 KiB |
4
dragonpilot/dashy/web/dist/icons/navigation/arrive.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/arrive.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 554 B |
4
dragonpilot/dashy/web/dist/icons/navigation/arrive_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/arrive_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 672 B |
4
dragonpilot/dashy/web/dist/icons/navigation/arrive_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/arrive_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 662 B |
4
dragonpilot/dashy/web/dist/icons/navigation/arrive_straight.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/arrive_straight.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 554 B |
4
dragonpilot/dashy/web/dist/icons/navigation/close.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/close.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 746 B |
4
dragonpilot/dashy/web/dist/icons/navigation/continue.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/continue.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 503 B |
4
dragonpilot/dashy/web/dist/icons/navigation/continue_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/continue_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 603 B |
4
dragonpilot/dashy/web/dist/icons/navigation/continue_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/continue_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 626 B |
4
dragonpilot/dashy/web/dist/icons/navigation/continue_slight_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/continue_slight_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 605 B |
4
dragonpilot/dashy/web/dist/icons/navigation/continue_slight_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/continue_slight_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 605 B |
4
dragonpilot/dashy/web/dist/icons/navigation/continue_straight.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/continue_straight.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 503 B |
4
dragonpilot/dashy/web/dist/icons/navigation/continue_uturn_anticlockwise.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/continue_uturn_anticlockwise.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 532 B |
4
dragonpilot/dashy/web/dist/icons/navigation/continue_uturn_clockwise.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/continue_uturn_clockwise.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 572 B |
5
dragonpilot/dashy/web/dist/icons/navigation/default_marker.svg
vendored
Normal file
5
dragonpilot/dashy/web/dist/icons/navigation/default_marker.svg
vendored
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
<?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>
|
||||||
|
After Width: | Height: | Size: 389 B |
4
dragonpilot/dashy/web/dist/icons/navigation/depart.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/depart.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 564 B |
4
dragonpilot/dashy/web/dist/icons/navigation/depart_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/depart_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 670 B |
4
dragonpilot/dashy/web/dist/icons/navigation/depart_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/depart_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 660 B |
4
dragonpilot/dashy/web/dist/icons/navigation/depart_straight.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/depart_straight.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 546 B |
4
dragonpilot/dashy/web/dist/icons/navigation/end_of_road_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/end_of_road_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 813 B |
4
dragonpilot/dashy/web/dist/icons/navigation/end_of_road_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/end_of_road_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 808 B |
4
dragonpilot/dashy/web/dist/icons/navigation/flag.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/flag.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 317 B |
4
dragonpilot/dashy/web/dist/icons/navigation/fork.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/fork.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 888 B |
4
dragonpilot/dashy/web/dist/icons/navigation/fork_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/fork_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 1.3 KiB |
4
dragonpilot/dashy/web/dist/icons/navigation/fork_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/fork_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 1.3 KiB |
4
dragonpilot/dashy/web/dist/icons/navigation/fork_slight_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/fork_slight_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 1.3 KiB |
4
dragonpilot/dashy/web/dist/icons/navigation/fork_slight_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/fork_slight_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 1.2 KiB |
4
dragonpilot/dashy/web/dist/icons/navigation/fork_straight.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/fork_straight.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 1.8 KiB |
BIN
dragonpilot/dashy/web/dist/icons/navigation/home.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/navigation/home.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.0 KiB |
65
dragonpilot/dashy/web/dist/icons/navigation/home.svg
vendored
Normal file
65
dragonpilot/dashy/web/dist/icons/navigation/home.svg
vendored
Normal file
@@ -0,0 +1,65 @@
|
|||||||
|
<?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>
|
||||||
|
After Width: | Height: | Size: 2.6 KiB |
BIN
dragonpilot/dashy/web/dist/icons/navigation/home_inactive.png
vendored
Normal file
BIN
dragonpilot/dashy/web/dist/icons/navigation/home_inactive.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 9.0 KiB |
1
dragonpilot/dashy/web/dist/icons/navigation/icon_directions.svg
vendored
Normal file
1
dragonpilot/dashy/web/dist/icons/navigation/icon_directions.svg
vendored
Normal file
@@ -0,0 +1 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 466 B |
1
dragonpilot/dashy/web/dist/icons/navigation/icon_directions_outlined.svg
vendored
Normal file
1
dragonpilot/dashy/web/dist/icons/navigation/icon_directions_outlined.svg
vendored
Normal file
@@ -0,0 +1 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 780 B |
1
dragonpilot/dashy/web/dist/icons/navigation/icon_favorite.svg
vendored
Normal file
1
dragonpilot/dashy/web/dist/icons/navigation/icon_favorite.svg
vendored
Normal file
@@ -0,0 +1 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 315 B |
1
dragonpilot/dashy/web/dist/icons/navigation/icon_home.svg
vendored
Normal file
1
dragonpilot/dashy/web/dist/icons/navigation/icon_home.svg
vendored
Normal file
@@ -0,0 +1 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 489 B |
1
dragonpilot/dashy/web/dist/icons/navigation/icon_recent.svg
vendored
Normal file
1
dragonpilot/dashy/web/dist/icons/navigation/icon_recent.svg
vendored
Normal file
@@ -0,0 +1 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 1.2 KiB |
1
dragonpilot/dashy/web/dist/icons/navigation/icon_settings.svg
vendored
Normal file
1
dragonpilot/dashy/web/dist/icons/navigation/icon_settings.svg
vendored
Normal file
@@ -0,0 +1 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 910 B |
1
dragonpilot/dashy/web/dist/icons/navigation/icon_work.svg
vendored
Normal file
1
dragonpilot/dashy/web/dist/icons/navigation/icon_work.svg
vendored
Normal file
@@ -0,0 +1 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 364 B |
4
dragonpilot/dashy/web/dist/icons/navigation/invalid.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/invalid.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 503 B |
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 603 B |
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 626 B |
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_slight_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_slight_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 605 B |
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_slight_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_slight_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 605 B |
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_straight.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_straight.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 503 B |
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_uturn_anticlockwise.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_uturn_anticlockwise.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 532 B |
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_uturn_clockwise.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/invalid_uturn_clockwise.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 572 B |
4
dragonpilot/dashy/web/dist/icons/navigation/merge_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/merge_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 813 B |
4
dragonpilot/dashy/web/dist/icons/navigation/merge_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/merge_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 808 B |
4
dragonpilot/dashy/web/dist/icons/navigation/merge_slight_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/merge_slight_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 842 B |
4
dragonpilot/dashy/web/dist/icons/navigation/merge_slight_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/merge_slight_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 844 B |
3
dragonpilot/dashy/web/dist/icons/navigation/merge_straight.svg
vendored
Normal file
3
dragonpilot/dashy/web/dist/icons/navigation/merge_straight.svg
vendored
Normal file
@@ -0,0 +1,3 @@
|
|||||||
|
<svg xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||||
|
<title>direction</title>
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 118 B |
4
dragonpilot/dashy/web/dist/icons/navigation/new_name_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/new_name_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 603 B |
4
dragonpilot/dashy/web/dist/icons/navigation/new_name_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/new_name_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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>
|
||||||
|
After Width: | Height: | Size: 626 B |
4
dragonpilot/dashy/web/dist/icons/navigation/new_name_sharp_left.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/new_name_sharp_left.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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.49771,18a0.49779,0.49779,0,0,1-.49779-0.49779V5.00391l-0.27979-.00635a2.56758,2.56758,0,0,0-2.0957.79L7.22917,10.40041a0.34918,0.34918,0,0,0,.08252.63177l2.92877,1.39331a0.38658,0.38658,0,0,1-.21344.71472l-8.0105.33209,1.69568-7.836a0.38661,0.38661,0,0,1,.74072-0.0882l0.8725,3.12372a0.35757,0.35757,0,0,0,.638.17206L5.9672,8.84835,11.3593,4.23926A4.46634,4.46634,0,0,1,14.74406,2.998L15.00822,3a1.92935,1.92935,0,0,1,1.43408.56885,2.10247,2.10247,0,0,1,.55713,1.46045L16.9999,17.50219A0.49779,0.49779,0,0,1,16.50211,18h-1.0044Z"/>
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 725 B |
4
dragonpilot/dashy/web/dist/icons/navigation/new_name_sharp_right.svg
vendored
Normal file
4
dragonpilot/dashy/web/dist/icons/navigation/new_name_sharp_right.svg
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
<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.49789,18a0.49779,0.49779,0,0,1-.49779-0.49781L3.00057,5.0293A2.10247,2.10247,0,0,1,3.5577,3.56885,1.92935,1.92935,0,0,1,4.99178,3l0.26416-.002A4.46634,4.46634,0,0,1,8.6407,4.23926L14.0328,8.84835l0.00378-.00446a0.35757,0.35757,0,0,0,.638-0.17206l0.8725-3.12372a0.38661,0.38661,0,0,1,.74072.0882l1.69568,7.836L9.973,13.14022a0.38658,0.38658,0,0,1-.21344-0.71472l2.92877-1.39331a0.34918,0.34918,0,0,0,.08252-0.63177L7.37557,5.7876a2.56758,2.56758,0,0,0-2.0957-.79l-0.27979.00635v12.4983A0.49779,0.49779,0,0,1,4.50229,18H3.49789Z"/>
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 722 B |
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user