mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-09-27 18:03:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

lp-dp 2023-05-15T02:15:54 for EON/C2

Browse Source 
version: lp-dp v0.9.2 for EON/C2
date: 2023-05-15T02:15:54
commit: 39446776e3af41df83540eaf68e84f8b0e2b5c9f
This commit is contained in:
Vehicle Researcher
2023-05-15 02:14:07 +00:00
parent c8440ab691
commit 687f549628
107 changed files with 27019 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2 -1
View File
@@ -32,7 +32,7 @@ a.out
*.class
*.pyxbldc
*.vcd
*.qm
config.json
clcache
compile_commands.json
@@ -93,3 +93,4 @@ selfdrive/legacy_modeld/models/supercombo.thneed
selfdrive/legacy_modeld/models/supercombo_badweights.thneed
selfdrive/legacy_modeld/thneed/compile
selfdrive/loggerd/bootlog
*.po~
RELEASES.md
-6
View File
@@ -8,19 +8,13 @@ Version 0.9.2 (2023-05-XX)
* Chevrolet Trailblazer 2021-22 support thanks to TurboCE!
* Ford Bronco Sport 2021-22 support
* Ford Escape 2020-22 support
* Ford Escape Hybrid 2020-22 support
* Ford Escape Plug-in Hybrid 2021-22 support
* Ford Explorer 2020-22 support
* Ford Explorer Hybrid 2020-22 support
* Ford Kuga 2020-22 support
* Ford Kuga Hybrid 2020-22 support
* Ford Kuga Plug-in Hybrid 2020-22 support
* Genesis GV80 2023 support thanks to JWingate80!
* Honda HR-V 2023 support thanks to AlexandreSato and galegozi!
* Kia Niro EV 2023 support thanks to JosselinLecocq!
* Lexus ES 2017-18 support
* Lincoln Aviator 2021 support
* Lincoln Aviator Plug-in Hybrid 2021 support
* Škoda Fabia 2022-23 support thanks to jyoung8607!
cereal/car.capnp
+1
View File
@@ -114,6 +114,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
controlsdLagging @112;
resumeBlocked @113;
steerTimeLimit @115;
vehicleSensorsInvalid @116;
radarCanErrorDEPRECATED @15;
communityFeatureDisallowedDEPRECATED @62;
cereal/dp.capnp
+14
View File
@@ -0,0 +1,14 @@
using Cxx = import "./include/c++.capnp";
$Cxx.namespace("cereal");
@0xbfa7e645486440c7;
# mapd
struct LiveMapData {
speedLimitValid @0 :Bool;
speedLimit @1 :Float32;
speedLimitAheadValid @2 :Bool;
speedLimitAhead @3 :Float32;
speedLimitAheadDistance @4 :Float32;
currentRoadName @5 :Text;
}
cereal/libcereal_shared.so
BIN
View File
Binary file not shown.
cereal/log.capnp
+2 -1
View File
@@ -3,6 +3,7 @@ $Cxx.namespace("cereal");
using Car = import "car.capnp";
using Legacy = import "legacy.capnp";
using Dp = import "dp.capnp";
@0xf3b1f17e25a4285b;
@@ -2206,7 +2207,7 @@ struct Event {
cellInfoDEPRECATED @28 :List(Legacy.CellInfo);
wifiScanDEPRECATED @29 :List(Legacy.WifiScan);
uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent;
liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED;
liveMapData @62 :Dp.LiveMapData;
gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints;
gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan;
applanixRawDEPRECATED @42 :Data;
cereal/messaging/bridge
BIN
View File
Binary file not shown.
cereal/messaging/messaging_pyx.so
BIN
View File
Binary file not shown.
cereal/services.h
+1
View File
@@ -70,6 +70,7 @@ static struct service services[] = {
{ "qRoadEncodeData", 8066, false, 20, -1 },
{ "driverState", 8067, true, 10, 5 },
{ "sensorEvents", 8068, true, 100, 100 },
{ "liveMapData", 8069, true, 0, -1 },
};
#endif
cereal/services.py
+2
View File
@@ -95,6 +95,8 @@ services = {
# legacy
"driverState": (True, 10, 5),
"sensorEvents": (True, 100., 100),
# mapd
"liveMapData": (True, 0.),
}
service_list = {name: Service(new_port(idx), *vals) for # type: ignore
idx, (name, vals) in enumerate(services.items())}
cereal/visionipc/visionipc_pyx.so
BIN
View File
Binary file not shown.
common/params.h
+2 -2
View File
@@ -7,8 +7,8 @@
enum ParamKeyType {
PERSISTENT = 0x02,
CLEAR_ON_MANAGER_START = 0x04,
CLEAR_ON_IGNITION_ON = 0x08,
CLEAR_ON_IGNITION_OFF = 0x10,
CLEAR_ON_ONROAD_TRANSITION = 0x08,
CLEAR_ON_OFFROAD_TRANSITION = 0x10,
DONT_LOG = 0x20,
ALL = 0xFFFFFFFF
};
common/params_pyx.pyx
+2 -2
View File
@@ -9,8 +9,8 @@ cdef extern from "common/params.h":
cpdef enum ParamKeyType:
PERSISTENT
CLEAR_ON_MANAGER_START
CLEAR_ON_IGNITION_ON
CLEAR_ON_IGNITION_OFF
CLEAR_ON_ONROAD_TRANSITION
CLEAR_ON_OFFROAD_TRANSITION
ALL
cdef cppclass c_Params "Params":
common/params_pyx.so
BIN
View File
Binary file not shown.
common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "2023.05.09"
#define COMMA_VERSION "2023.05.15"
docs/CARS.md
+3 -8
View File
@@ -4,7 +4,7 @@
A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
# 256 Supported Cars
# 251 Supported Cars
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -33,13 +33,8 @@ A supported vehicle is one that just works when you install a comma three. All s
|comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None||
|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Bronco Sport 2021-22">Ford Q3</a>||
|Ford|Escape 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Escape 2020-22">Ford Q3</a>||
|Ford|Escape Hybrid 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Escape Hybrid 2020-22">Ford Q3</a>||
|Ford|Escape Plug-in Hybrid 2021-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Escape Plug-in Hybrid 2021-22">Ford Q3</a>||
|Ford|Explorer 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Explorer 2020-22">Ford Q3</a>||
|Ford|Explorer Hybrid 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Explorer Hybrid 2020-22">Ford Q3</a>||
|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Kuga 2020-22">Ford Q3</a>||
|Ford|Kuga Hybrid 2020-22|Adaptive Cruise Control with Lane Centering|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Kuga Hybrid 2020-22">Ford Q3</a>||
|Ford|Kuga Plug-in Hybrid 2020-22|Adaptive Cruise Control with Lane Centering|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Kuga Plug-in Hybrid 2020-22">Ford Q3</a>||
|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G70 2018-19">Hyundai F</a>||
|Genesis|G70 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G70 2020">Hyundai F</a>||
|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G80 2017">Hyundai J</a>||
@@ -47,7 +42,8 @@ A supported vehicle is one that just works when you install a comma three. All s
|Genesis|G90 2017-18|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G90 2017-18">Hyundai C</a>||
|Genesis|GV60 (Advanced Trim) 2023[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV60 (Advanced Trim) 2023">Hyundai A</a>||
|Genesis|GV60 (Performance Trim) 2023[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV60 (Performance Trim) 2023">Hyundai K</a>||
|Genesis|GV70 2022-23[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 2022-23">Hyundai L</a>||
|Genesis|GV70 (2.5T Trim) 2022-23[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 (2.5T Trim) 2022-23">Hyundai L</a>||
|Genesis|GV70 (3.5T Trim) 2022-23[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 (3.5T Trim) 2022-23">Hyundai M</a>||
|Genesis|GV80 2023[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV80 2023">Hyundai M</a>||
|GMC|Acadia 2018[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=GMC&model=Acadia 2018">OBD-II</a>|<a href="https://www.youtube.com/watch?v=0ZN6DdsBUZo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=GMC&model=Sierra 1500 2020-21">GM</a>|<a href="https://youtu.be/5HbNoBLzRwE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
@@ -156,7 +152,6 @@ A supported vehicle is one that just works when you install a comma three. All s
|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2020-21">Toyota</a>||
|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=UX Hybrid 2019-22">Toyota</a>||
|Lincoln|Aviator 2021|Co-Pilot360 Plus|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lincoln&model=Aviator 2021">Ford Q3</a>||
|Lincoln|Aviator Plug-in Hybrid 2021|Co-Pilot360 Plus|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lincoln&model=Aviator Plug-in Hybrid 2021">Ford Q3</a>||
|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=MAN&model=eTGE 2020-23">J533</a>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=MAN&model=TGE 2017-23">J533</a>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Mazda&model=CX-5 2022-23">Mazda</a>||
launch_chffrplus.sh
+11
View File
@@ -139,6 +139,17 @@ function two_init {
LIB_PATH="/data/openpilot/system/hardware/eon/libs"
PY_LIB_DEST="/system/comma/usr/lib/python3.8/site-packages"
mount -o remount,rw /system
# mapd
MODULE="opspline"
if [ ! -d "$PY_LIB_DEST/$MODULE" ]; then
echo "Installing $MODULE..."
tar -zxvf "$LIB_PATH/$MODULE.tar.gz" -C "$PY_LIB_DEST/"
fi
MODULE="overpy"
if [ ! -d "$PY_LIB_DEST/$MODULE" ]; then
echo "Installing $MODULE..."
tar -zxvf "$LIB_PATH/$MODULE.tar.gz" -C "$PY_LIB_DEST/"
fi
# laika
MODULE="hatanaka"
if [ ! -d "$PY_LIB_DEST/$MODULE" ]; then
opendbc/can/libdbc.so
BIN
View File
Binary file not shown.
opendbc/can/parser_pyx.so
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.signed
BIN
View File
Binary file not shown.
rednose/helpers/ekf_sym_pyx.so
BIN
View File
Binary file not shown.
selfdrive/athena/athenad.py
+1 -1
View File
@@ -36,7 +36,7 @@ from common.file_helpers import CallbackReader
from common.params import Params
from common.realtime import sec_since_boot, set_core_affinity
from system.hardware import HARDWARE, PC, AGNOS
from system.loggerd.config import ROOT
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
from selfdrive.statsd import STATS_DIR
from system.swaglog import SWAGLOG_DIR, cloudlog
selfdrive/boardd/boardd
BIN
View File
Binary file not shown.
selfdrive/boardd/boardd_api_impl.so
BIN
View File
Binary file not shown.
selfdrive/boardd/pandad.py
+23 -7
View File
@@ -81,16 +81,18 @@ def main() -> NoReturn:
params = Params()
while True:
count += 1
cloudlog.event("pandad.flash_and_connect", count=count)
try:
count += 1
cloudlog.event("pandad.flash_and_connect", count=count)
params.remove("PandaSignatures")
# Flash all Pandas in DFU mode
for serial in PandaDFU.list():
cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}")
PandaDFU(serial).recover()
time.sleep(1)
dfu_serials = PandaDFU.list()
if len(dfu_serials) > 0:
for serial in dfu_serials:
cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}")
PandaDFU(serial).recover()
time.sleep(1)
panda_serials = Panda.list()
if len(panda_serials) == 0:
@@ -133,7 +135,21 @@ def main() -> NoReturn:
# log panda fw versions
params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas))
# close all pandas
# for panda in pandas:
# # check health for lost heartbeat
# health = panda.health()
# if health["heartbeat_lost"]:
# params.put_bool("PandaHeartbeatLost", True)
# cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
#
# if first_run:
# cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
# if panda.is_internal():
# HARDWARE.reset_internal_panda()
# else:
# panda.reset(reconnect=False)
#
for p in pandas:
p.close()
except (usb1.USBErrorNoDevice, usb1.USBErrorPipe):
selfdrive/boardd/tests/test_boardd_loopback.py
+4 -3
View File
@@ -6,7 +6,7 @@ import unittest
from collections import defaultdict
import cereal.messaging as messaging
from cereal import car
from cereal import car, log
from common.params import Params
from common.spinner import Spinner
from common.timeout import Timeout
@@ -36,7 +36,8 @@ class TestBoardd(unittest.TestCase):
with Timeout(60, "boardd didn't start"):
sm = messaging.SubMaster(['pandaStates'])
while sm.rcv_frame['pandaStates'] < 1 and len(sm['pandaStates']) == 0:
while sm.rcv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \
any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']):
sm.update(1000)
num_pandas = len(sm['pandaStates'])
@@ -44,7 +45,7 @@ class TestBoardd(unittest.TestCase):
self.assertEqual(num_pandas, expected_pandas, "connected pandas ({num_pandas}) doesn't match expected panda count ({expected_pandas}). \
connect another panda for multipanda tests.")
# boardd blocks on CarVin and CarParams
# boardd blocks on FirmwareQueryDone, ControlsReady, and CarParams
cp = car.CarParams.new_message()
safety_config = car.CarParams.SafetyConfig.new_message()
selfdrive/camerad/camerad
BIN
View File
Binary file not shown.
selfdrive/car/ford/carstate.py
+16 -4
View File
@@ -16,9 +16,21 @@ class CarState(CarStateBase):
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]
self.vehicle_sensors_valid = False
self.hybrid_platform = False
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
# Hybrid variants experience a bug where a message from the PCM sends invalid checksums,
# we do not support these cars at this time.
# TrnAin_Tq_Actl and its quality flag are only set on ICE platform variants
self.hybrid_platform = cp.vl["VehicleOperatingModes"]["TrnAinTq_D_Qf"] == 0
# Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
# The vehicle usually recovers out of this state within a minute of normal driving
self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3
# car speed
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
@@ -93,6 +105,8 @@ class CarState(CarStateBase):
def get_can_parser(CP):
signals = [
# sig_name, sig_address
("TrnAinTq_D_Qf", "VehicleOperatingModes"), # Used to detect hybrid or ICE platform variant
("Veh_V_ActlBrk", "BrakeSysFeatures"), # ABS vehicle speed (kph)
("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s)
("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped
@@ -106,6 +120,7 @@ class CarState(CarStateBase):
("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill
("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable
("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg)
("StePinCompAnEst_D_Qf", "SteeringPinion_Data"), # PSCM estimated steering angle (quality flag)
# Calculates steering angle (and offset) from pinion
# angle and driving measurements.
# StePinRelInit_An_Sns is the pinion angle, initialised
@@ -126,7 +141,6 @@ class CarState(CarStateBase):
("AccButtnGapIncPress", "Steering_Data_FD1"),
("AslButtnOnOffCnclPress", "Steering_Data_FD1"),
("AslButtnOnOffPress", "Steering_Data_FD1"),
("CcAslButtnCnclPress", "Steering_Data_FD1"),
("LaSwtchPos_D_Stat", "Steering_Data_FD1"),
("CcAslButtnCnclResPress", "Steering_Data_FD1"),
("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"),
@@ -140,7 +154,6 @@ class CarState(CarStateBase):
("CcAslButtnSetDecPress", "Steering_Data_FD1"),
("CcAslButtnSetIncPress", "Steering_Data_FD1"),
("CcAslButtnSetPress", "Steering_Data_FD1"),
("CcAsllButtnResPress", "Steering_Data_FD1"),
("CcButtnOffPress", "Steering_Data_FD1"),
("CcButtnOnOffCnclPress", "Steering_Data_FD1"),
("CcButtnOnOffPress", "Steering_Data_FD1"),
@@ -155,6 +168,7 @@ class CarState(CarStateBase):
checks = [
# sig_address, frequency
("VehicleOperatingModes", 100),
("BrakeSysFeatures", 50),
("Yaw_Data_FD1", 100),
("DesiredTorqBrk", 50),
@@ -234,9 +248,7 @@ class CarState(CarStateBase):
("FeatNoIpmaActl", "IPMA_Data"),
("PersIndexIpma_D_Actl", "IPMA_Data"),
("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping
("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines)
("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error
("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime
("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater?
("CamraStats_D_Dsply", "IPMA_Data"), # Camera status
("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level
selfdrive/car/ford/fordcan.py
+94 -15
View File
@@ -5,11 +5,15 @@ HUDControl = car.CarControl.HUDControl
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray):
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
checksum = mode + counter
checksum += dat[2] + ((dat[3] & 0xE0) >> 5) # curvature
checksum += dat[6] + ((dat[7] & 0xE0) >> 5) # curvature rate
checksum += (dat[3] & 0x1F) + ((dat[4] & 0xFC) >> 2) # path angle
checksum += (dat[4] & 0x3) + dat[5] # path offset
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
checksum += sig_val + (sig_val >> 8)
return 0xFF - (checksum & 0xFF)
@@ -146,10 +150,37 @@ def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_v
else:
status = 0 # Off
values = {
**stock_values,
"Tja_D_Stat": status,
}
values = {s: stock_values[s] for s in [
"HaDsply_No_Cs",
"HaDsply_No_Cnt",
"AccStopStat_D_Dsply", # ACC stopped status message
"AccTrgDist2_D_Dsply", # ACC target distance
"AccStopRes_B_Dsply",
"TjaWarn_D_Rq", # TJA warning
"TjaMsgTxt_D_Dsply", # TJA text
"IaccLamp_D_Rq", # iACC status icon
"AccMsgTxt_D2_Rq", # ACC text
"FcwDeny_B_Dsply", # FCW disabled
"FcwMemStat_B_Actl", # FCW enabled setting
"AccTGap_B_Dsply", # ACC time gap display setting
"CadsAlignIncplt_B_Actl",
"AccFllwMde_B_Dsply", # ACC follow mode display setting
"CadsRadrBlck_B_Actl",
"CmbbPostEvnt_B_Dsply", # AEB event status
"AccStopMde_B_Dsply", # ACC stop mode display setting
"FcwMemSens_D_Actl", # FCW sensitivity setting
"FcwMsgTxt_D_Rq", # FCW text
"AccWarn_D_Dsply", # ACC warning
"FcwVisblWarn_B_Rq", # FCW visible alert
"FcwAudioWarn_B_Rq", # FCW audio alert
"AccTGap_D_Dsply", # ACC time gap
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
"FdaMem_B_Stat", # FDA enabled setting
]}
values.update({
"Tja_D_Stat": status, # TJA status
})
return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)
@@ -196,11 +227,26 @@ def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool,
hands_on_wheel_dsply = 1 if steer_alert else 0
values = {
**stock_values,
values = {s: stock_values[s] for s in [
"FeatConfigIpmaActl",
"FeatNoIpmaActl",
"PersIndexIpma_D_Actl",
"AhbcRampingV_D_Rq", # AHB ramping
"LaDenyStats_B_Dsply", # LKAS error
"CamraDefog_B_Req", # Windshield heater?
"CamraStats_D_Dsply", # Camera status
"DasAlrtLvl_D_Dsply", # DAS alert level
"DasStats_D_Dsply", # DAS status
"DasWarn_D_Dsply", # DAS warning
"AhbHiBeam_D_Rq", # AHB status
"Passthru_63",
"Passthru_48",
]}
values.update({
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
}
})
return packer.make_can_msg("IPMA_Data", CANBUS.main, values)
@@ -214,10 +260,43 @@ def create_button_msg(packer, stock_values: dict, cancel=False, resume=False, tj
Frequency is 10Hz.
"""
values = {
**stock_values,
values = {s: stock_values[s] for s in [
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
"WiprFront_D_Stat",
"LghtAmb_D_Sns",
"AccButtnGapDecPress",
"AccButtnGapIncPress",
"AslButtnOnOffCnclPress",
"AslButtnOnOffPress",
"LaSwtchPos_D_Stat",
"CcAslButtnCnclResPress",
"CcAslButtnDeny_B_Actl",
"CcAslButtnIndxDecPress",
"CcAslButtnIndxIncPress",
"CcAslButtnOffCnclPress",
"CcAslButtnOnOffCncl",
"CcAslButtnOnPress",
"CcAslButtnResDecPress",
"CcAslButtnResIncPress",
"CcAslButtnSetDecPress",
"CcAslButtnSetIncPress",
"CcAslButtnSetPress",
"CcButtnOffPress",
"CcButtnOnOffCnclPress",
"CcButtnOnOffPress",
"CcButtnOnPress",
"HeadLghtHiFlash_D_Actl",
"HeadLghtHiOn_B_StatAhb",
"AhbStat_B_Dsply",
"AccButtnGapTogglePress",
"WiprFrontSwtch_D_Stat",
"HeadLghtHiCtrl_D_RqAhb",
]}
values.update({
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # TJA toggle button
}
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
})
return packer.make_can_msg("Steering_Data_FD1", bus, values)
selfdrive/car/ford/interface.py
+5
View File
@@ -76,6 +76,11 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam)
events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
if not self.CS.vehicle_sensors_valid:
events.add(car.CarEvent.EventName.vehicleSensorsInvalid)
if self.CS.hybrid_platform:
events.add(car.CarEvent.EventName.startupNoControl)
ret.events = events.to_msg()
return ret
selfdrive/car/ford/values.py
-6
View File
@@ -72,17 +72,11 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
CAR.BRONCO_SPORT_MK1: FordCarInfo("Ford Bronco Sport 2021-22"),
CAR.ESCAPE_MK4: [
FordCarInfo("Ford Escape 2020-22"),
FordCarInfo("Ford Escape Hybrid 2020-22"),
FordCarInfo("Ford Escape Plug-in Hybrid 2021-22"),
FordCarInfo("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering"),
FordCarInfo("Ford Kuga Hybrid 2020-22", "Adaptive Cruise Control with Lane Centering"),
FordCarInfo("Ford Kuga Plug-in Hybrid 2020-22", "Adaptive Cruise Control with Lane Centering"),
],
CAR.EXPLORER_MK6: [
FordCarInfo("Ford Explorer 2020-22"),
FordCarInfo("Ford Explorer Hybrid 2020-22"),
FordCarInfo("Lincoln Aviator 2021", "Co-Pilot360 Plus"),
FordCarInfo("Lincoln Aviator Plug-in Hybrid 2021", "Co-Pilot360 Plus"),
],
CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2018", "Adaptive Cruise Control with Lane Centering"),
CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022-23", "Co-Pilot360 Assist"),
selfdrive/car/hyundai/values.py
+6 -1
View File
@@ -248,7 +248,10 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
],
CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f),
CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f),
CAR.GENESIS_GV70_1ST_GEN: HyundaiCarInfo("Genesis GV70 2022-23", "All", harness=Harness.hyundai_l),
CAR.GENESIS_GV70_1ST_GEN: [
HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", harness=Harness.hyundai_l),
HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", harness=Harness.hyundai_m),
],
CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness=Harness.hyundai_h),
CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c),
CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", harness=Harness.hyundai_m),
@@ -1705,10 +1708,12 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.04 99211-AR000 210204',
b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.01 99211-AR200 220125',
b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.01 99211-AR300 220125',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ',
b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-AR200 ',
b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-AR300 ',
],
},
CAR.GENESIS_GV60_EV_1ST_GEN: {
selfdrive/car/toyota/toyotacan.py
+8 -1
View File
@@ -99,6 +99,13 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep
# lane sway functionality
# not all cars have LKAS_HUD — update with camera values if available
values.update(stock_lkas_hud)
if len(stock_lkas_hud):
values.update({s: stock_lkas_hud[s] for s in [
"LANE_SWAY_FLD",
"LANE_SWAY_BUZZER",
"LANE_SWAY_WARNING",
"LANE_SWAY_SENSITIVITY",
"LANE_SWAY_TOGGLE",
]})
return packer.make_can_msg("LKAS_HUD", 0, values)
selfdrive/car/volkswagen/values.py
+2
View File
@@ -631,6 +631,7 @@ FW_VERSIONS = {
b'\xf1\x870CW300042H \xf1\x891601',
b'\xf1\x870GC300042H \xf1\x891404',
b'\xf1\x870D9300018C \xf1\x895297',
b'\xf1\x870GC300043 \xf1\x892301',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111',
@@ -639,6 +640,7 @@ FW_VERSIONS = {
b'\xf1\x873Q0959655BA\xf1\x890195\xf1\x82\r56140056130012516612125111',
b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211',
b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\x0e5915005914001305701311052900',
b'\xf1\x873Q0959655BG\xf1\x890712\xf1\x82\x0e5915005914001305701311052900',
b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\0165915005914001344701311442900',
b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e5915005914001354701311542900',
b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e5915005914001305701311052900',
selfdrive/controls/controlsd.py
+1 -1
View File
@@ -36,7 +36,7 @@ LANE_DEPARTURE_THRESHOLD = 0.1
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
NOSENSOR = "NOSENSOR" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd", "mapd"}
NO_IR_CTRL = os.path.isfile('/data/media/0/no_ir_ctrl')
if NO_IR_CTRL:
selfdrive/controls/lib/drive_helpers.py
+11 -1
View File
@@ -1,6 +1,6 @@
import math
from cereal import car
from cereal import car, log
from common.conversions import Conversions as CV
from common.numpy_fast import clip, interp
from common.realtime import DT_MDL
@@ -23,6 +23,8 @@ CAR_ROTATION_RADIUS = 0.0
# EU guidelines
MAX_LATERAL_JERK = 5.0
MAX_VEL_ERR = 5.0
ButtonEvent = car.CarState.ButtonEvent
ButtonType = car.CarState.ButtonEvent.Type
CRUISE_LONG_PRESS = 50
@@ -200,3 +202,11 @@ def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, fric
)
friction = float(friction_interp) if friction_compensation else 0.0
return friction
def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
# ToDo: Try relative error, and absolute speed
if len(modelV2.temporalPose.trans):
vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
return float(vel_err)
return 0.0
selfdrive/controls/lib/events.py
+7
View File
@@ -363,6 +363,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# Car is recognized, but marked as dashcam only
EventName.startupNoControl: {
ET.PERMANENT: StartupAlert(_("Dashcam mode")),
ET.NO_ENTRY: NoEntryAlert(_("Dashcam mode")),
},
# Car is not recognized
@@ -950,4 +951,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.NO_ENTRY: NoEntryAlert(_("LKAS Disabled")),
},
EventName.vehicleSensorsInvalid: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"),
ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"),
ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"),
},
}
selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json
+628
View File
@@ -0,0 +1,628 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [],
"idxbx_0": [
0,
1,
2
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [
0,
1,
2,
3
],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [],
"lbx_0": [
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [
0.0,
0.0,
0.0,
0.0
],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [
0.0,
0.0,
0.0,
0.0
],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [],
"ubx_0": [
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [
10000.0,
10000.0,
10000.0,
10000.0
],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [
0.0,
0.0,
0.0,
0.0
],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vu_0": [
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vx": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"Vx_0": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"Vx_e": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"Zl": [
0.0,
0.0,
0.0,
0.0
],
"Zl_e": [],
"Zu": [
0.0,
0.0,
0.0,
0.0
],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0,
0.0,
0.0,
0.0
],
"zl": [
0.0,
0.0,
0.0,
0.0
],
"zl_e": [],
"zu": [
0.0,
0.0,
0.0,
0.0
],
"zu_e": []
},
"cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
"dims": {
"N": 12,
"nbu": 0,
"nbx": 0,
"nbx_0": 3,
"nbx_e": 0,
"nbxe_0": 3,
"ng": 0,
"ng_e": 0,
"nh": 4,
"nh_e": 0,
"np": 7,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 4,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 4,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 3,
"ny": 6,
"ny_0": 6,
"ny_e": 5,
"nz": 0
},
"model": {
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_source_discrete": null,
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "long"
},
"parameter_values": [
-1.2,
1.2,
0.0,
0.0,
1.45,
0.75,
6.0
],
"problem_class": "OCP",
"simulink_opts": {
"inputs": {
"cost_W": 0,
"cost_W_0": 0,
"cost_W_e": 0,
"lbu": 1,
"lbx": 1,
"lbx_0": 1,
"lbx_e": 1,
"lg": 1,
"lh": 1,
"parameter_traj": 1,
"reset_solver": 0,
"u_init": 0,
"ubu": 1,
"ubx": 1,
"ubx_0": 1,
"ubx_e": 1,
"ug": 1,
"uh": 1,
"x_init": 0,
"y_ref": 1,
"y_ref_0": 1,
"y_ref_e": 1
},
"outputs": {
"CPU_time": 1,
"CPU_time_lin": 0,
"CPU_time_qp": 0,
"CPU_time_sim": 0,
"KKT_residual": 1,
"solver_status": 1,
"sqp_iter": 1,
"u0": 1,
"utraj": 0,
"x1": 1,
"xtraj": 0
},
"samplingtime": "t0"
},
"solver_options": {
"Tsim": 0.06944444444444445,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_iter_max": 10,
"qp_solver_tol_comp": 0.001,
"qp_solver_tol_eq": 0.001,
"qp_solver_tol_ineq": 0.001,
"qp_solver_tol_stat": 0.001,
"qp_solver_warm_start": 0,
"regularize_method": null,
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 10.0,
"time_steps": [
0.06944444444444445,
0.20833333333333334,
0.3472222222222222,
0.48611111111111116,
0.6250000000000002,
0.7638888888888886,
0.9027777777777786,
1.041666666666666,
1.1805555555555554,
1.3194444444444455,
1.4583333333333313,
1.5972222222222232
]
}
}
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.h
+103
View File
@@ -0,0 +1,103 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_long_H_
#define ACADOS_SIM_long_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define LONG_NX 3
#define LONG_NZ 0
#define LONG_NU 1
#define LONG_NP 7
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int long_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int long_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int long_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int long_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * long_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * long_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int long_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_long_H_
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h
+1 -1
View File
@@ -42,7 +42,7 @@
#define LONG_NX 3
#define LONG_NZ 0
#define LONG_NU 1
#define LONG_NP 6
#define LONG_NP 7
#define LONG_NBX 0
#define LONG_NBX0 3
#define LONG_NBU 0
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/make_sfun_long.m
+128
View File
@@ -0,0 +1,128 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% This file is part of acados.
%
% The 2-Clause BSD License
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.;
%
SOURCES = { ...
'long_model/long_expl_ode_fun.c', ...
'long_model/long_expl_vde_forw.c',...
'long_cost/long_cost_y_0_fun.c',...
'long_cost/long_cost_y_0_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_0_hess.c',...
'long_cost/long_cost_y_fun.c',...
'long_cost/long_cost_y_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_hess.c',...
'long_cost/long_cost_y_e_fun.c',...
'long_cost/long_cost_y_e_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_e_hess.c',...
'long_constraints/long_constr_h_fun.c', ...
'long_constraints/long_constr_h_fun_jac_uxt_zt_hess.c', ...
'long_constraints/long_constr_h_fun_jac_uxt_zt.c', ...
'acados_solver_sfunction_long.c', ...
'acados_solver_long.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_long' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_long', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [91]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [6]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [66]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [5]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lh, size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') uh, size [4]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+20 -13
View File
@@ -25,7 +25,7 @@ SOURCES = ['lead0', 'lead1', 'cruise', 'e2e']
X_DIM = 3
U_DIM = 1
PARAM_DIM = 6
PARAM_DIM = 7
COST_E_DIM = 5
COST_DIM = COST_E_DIM + 1
CONSTR_DIM = 4
@@ -61,11 +61,11 @@ STOP_DISTANCE = 6.0
def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW):
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW, stop_distance=STOP_DISTANCE):
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance
def desired_follow_distance(v_ego, v_lead):
return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW, stop_distance=STOP_DISTANCE):
return get_safe_obstacle_distance(v_ego, t_follow, stop_distance) - get_stopped_equivalence_factor(v_lead)
def gen_long_model():
@@ -95,7 +95,8 @@ def gen_long_model():
prev_a = SX.sym('prev_a')
lead_t_follow = SX.sym('lead_t_follow')
lead_danger_factor = SX.sym('lead_danger_factor')
model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor)
stop_distance = SX.sym('stop_distance')
model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor, stop_distance)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
@@ -131,11 +132,12 @@ def gen_long_ocp():
prev_a = ocp.model.p[3]
lead_t_follow = ocp.model.p[4]
lead_danger_factor = ocp.model.p[5]
stop_distance = ocp.model.p[6]
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow)
desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow, stop_distance)
# The main cost in normal operation is how close you are to the "desired" distance
# from an obstacle at every timestep. This obstacle can be a lead car
@@ -161,7 +163,7 @@ def gen_long_ocp():
x0 = np.zeros(X_DIM)
ocp.constraints.x0 = x0
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_FOLLOW, LEAD_DANGER_FACTOR])
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_FOLLOW, LEAD_DANGER_FACTOR, STOP_DISTANCE])
# We put all constraint cost weights to 0 and only set them at runtime
cost_weights = np.zeros(CONSTR_DIM)
@@ -201,6 +203,8 @@ class LongitudinalMpc:
def __init__(self, mode='acc'):
self.mode = mode
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.t_follow = T_FOLLOW
self.stop_distance = STOP_DISTANCE
self.reset()
self.source = SOURCES[2]
@@ -307,13 +311,15 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j):
def update(self, radarstate, v_cruise, x, v, a, j, prev_accel_constraint, t_follow=T_FOLLOW, stop_distance=STOP_DISTANCE):
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
self.t_follow = t_follow
self.stop_distance = stop_distance
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
# and then treat that as a stopped car/obstacle at this new distance.
@@ -334,7 +340,7 @@ class LongitudinalMpc:
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
v_lower,
v_upper)
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped)
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, self.t_follow, self.stop_distance)
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]
@@ -368,7 +374,8 @@ class LongitudinalMpc:
self.params[:,2] = np.min(x_obstacles, axis=1)
self.params[:,3] = np.copy(self.prev_a)
self.params[:,4] = T_FOLLOW
self.params[:,4] = self.t_follow
self.params[:,6] = self.stop_distance
self.run()
if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
@@ -380,9 +387,9 @@ class LongitudinalMpc:
# Check if it got within lead comfort range
# TODO This should be done cleaner
if self.mode == 'blended':
if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], T_FOLLOW))- self.x_sol[:,0] < 0.0):
if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.t_follow, self.stop_distance))- self.x_sol[:,0] < 0.0):
self.source = 'lead0'
if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], T_FOLLOW))- self.x_sol[:,0] < 0.0) and \
if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.t_follow, self.stop_distance))- self.x_sol[:,0] < 0.0) and \
(lead_1_obstacle[0] - lead_0_obstacle[0]):
self.source = 'lead1'
selfdrive/controls/lib/longitudinal_planner.py
+4 -5
View File
@@ -9,9 +9,9 @@ from common.filter_simple import FirstOrderFilter
from common.realtime import DT_MDL
from selfdrive.legacy_modeld.constants import T_IDXS
from selfdrive.controls.lib.longcontrol import LongCtrlState
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, MIN_ACCEL, MAX_ACCEL, T_FOLLOW
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, MIN_ACCEL, MAX_ACCEL, T_FOLLOW, STOP_DISTANCE
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
from system.swaglog import cloudlog
from selfdrive.controls.lib.vision_turn_controller import VisionTurnController
@@ -217,8 +217,7 @@ class LongitudinalPlanner:
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
# Compute model v_ego error
if len(sm['modelV2'].temporalPose.trans):
self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego
self.v_model_error = get_speed_error(sm['modelV2'], v_ego)
# rick - vision turn controller from move-fast team
# https://github.com/move-fast/openpilot/blob/develop/selfdrive/controls/lib/vision_turn_controller.py
@@ -238,7 +237,7 @@ class LongitudinalPlanner:
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, prev_accel_constraint, T_FOLLOW, interp(sm['carState'].vEgo, [0., 2.78, 5.55, 22.], [3., 4., 5, STOP_DISTANCE]))
self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
selfdrive/dragonpilot/gpx_uploader.py
+151
View File
@@ -0,0 +1,151 @@
#!/usr/bin/env python3
# The MIT License
#
# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
#
# 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, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, 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.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
import os
import time
from common.params import Params
from system.version import get_version
# for uploader
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
import glob
import requests
import json
# customisable values
GPX_LOG_PATH = '/data/media/0/gpx_logs/'
LOG_HERTZ = 1/10 # 0.1 Hz = 10 sec, higher for higher accuracy, 10hz seems fine
# uploader
UPLOAD_ATTR_NAME = 'user.upload'
UPLOAD_ATTR_VALUE = b'1'
LOG_PATH = '/data/media/0/gpx_logs/'
# osm api
API_HEADER = {'Authorization': 'Bearer 2pvUyXfk9vizuh7PwQFSEYBtFWcM-Pu7vxApUjSA0fc'}
VERSION_URL = 'https://api.openstreetmap.org/api/versions'
UPLOAD_URL = 'https://api.openstreetmap.org/api/0.6/gpx/create'
_DEBUG = False
def _debug(msg):
if not _DEBUG:
return
print(msg, flush=True)
class GpxUploader():
def __init__(self):
self._delete_after_upload = True #not Params().get_bool('dp_gpxd')
self._car_model = "Unknown Vehicle"
def _identify_vehicle(self):
# read model from LiveParameters
params = Params().get("LiveParameters")
if params is not None:
params = json.loads(params)
self._car_model = params.get('carFingerprint', self._car_model)
self._dp_version = get_version()
_debug("GpxUploader init - _delete_after_upload = %s" % self._delete_after_upload)
_debug("GpxUploader init - _car_model = %s" % self._car_model)
def _is_online(self):
try:
r = requests.get(VERSION_URL, headers=API_HEADER)
_debug("is_online? status_code = %s" % r.status_code)
return r.status_code >= 200
except Exception:
return False
def _get_is_uploaded(self, filename):
result = getxattr(filename, UPLOAD_ATTR_NAME) is not None
_debug("%s is uploaded: %s" % (filename, result))
return result
def _set_is_uploaded(self, filename):
_debug("%s set to uploaded" % filename)
setxattr(filename, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE)
def _get_files(self):
return sorted( filter( os.path.isfile, glob.glob(LOG_PATH + '*') ) )
def _get_files_to_be_uploaded(self):
files = self._get_files()
files_to_be_uploaded = []
for file in files:
if not self._get_is_uploaded(file):
files_to_be_uploaded.append(file)
return files_to_be_uploaded
def _do_upload(self, filename):
fn = os.path.basename(filename)
data = {
'description': "Routes from dragonpilot %s (%s)." % (self._dp_version, self._car_model),
'visibility': 'identifiable'
}
files = {
"file": (fn, open(filename, 'rb'))
}
try:
r = requests.post(UPLOAD_URL, files=files, data=data, headers=API_HEADER)
_debug("do_upload - %s - %s" % (filename, r.status_code))
return r.status_code == 200
except Exception:
return False
def run(self):
# give it few seconds before we start runing the process
# only identify vehicle once
time.sleep(10)
self._identify_vehicle()
while True:
is_offroad = Params().get_bool("IsOffroad")
files = self._get_files_to_be_uploaded()
if len(files) == 0:
if is_offroad and self._delete_after_upload:
for file in self._get_files():
os.remove(file)
_debug("run - no files, clean stash")
elif not self._is_online() and self._delete_after_upload:
_debug("run - not online & delete_after_upload")
for file in files:
os.remove(file)
else:
for file in files:
if self._do_upload(file):
if self._delete_after_upload:
_debug("run - _delete_after_upload")
os.remove(file)
else:
_debug("run - set_is_uploaded")
self._set_is_uploaded(file)
time.sleep(60)
def gpx_uploader_thread():
gpx_uploader = GpxUploader()
gpx_uploader.run()
def main():
gpx_uploader_thread()
if __name__ == "__main__":
main()
selfdrive/dragonpilot/gpxd.py
+143
View File
@@ -0,0 +1,143 @@
#!/usr/bin/env python3
#pylint: skip-file
# The MIT License
#
# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
#
# 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, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, 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.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
import cereal.messaging as messaging
import os
import datetime
from common.realtime import set_core_affinity, set_realtime_priority
from system.swaglog import cloudlog
from pathlib import Path
# customisable values
GPX_LOG_PATH = '/data/media/0/gpx_logs/'
LOG_HERTZ = 5 # 5 hz = 0.2 sec, higher for higher accuracy, 10hz seems fine
LOG_LENGTH = 5 # mins, higher means it keeps more data in the memory, will take more time to write into a file too.
LOST_SIGNAL_COUNT_LENGTH = 10 # secs, output log file if we lost signal for this long
# do not change
LOST_SIGNAL_COUNT_MAX = LOST_SIGNAL_COUNT_LENGTH * LOG_HERTZ # secs,
LOGS_PER_FILE = LOG_LENGTH * 60 * LOG_HERTZ # e.g. 10 * 60 * 10 = 6000 points per file
_DEBUG = False
_CLOUDLOG_DEBUG = True
def _debug(msg, log_to_cloud=True):
if _CLOUDLOG_DEBUG and log_to_cloud:
cloudlog.debug(msg)
if _DEBUG:
print(msg)
class GpxD():
def __init__(self):
self.log_count = 0
self.logs = list()
self.lost_signal_count = 0
self.started_time = datetime.datetime.utcnow().isoformat()
self.pause = True
def log(self, sm):
gps = sm['gpsLocationExternal']
if gps.speed >= 0.1:
self.pause = False
location_not_valid = gps.flags % 2 == 0
if location_not_valid or self.pause:
if self.log_count > 0:
self.lost_signal_count += 1
else:
lat = gps.latitude
lon = gps.longitude
alt = gps.altitude
timestamp = gps.unixTimestampMillis*0.001
_debug("gpxd: logged - %s %s %s %s" % (timestamp, lat, lon, alt))
self.logs.append([timestamp, lat, lon, alt])
self.log_count += 1
self.lost_signal_count = 0
if not self.pause and gps.speed < 0.1:
_debug("gpxd: paused")
self.pause = True
def write_log(self, force=False):
if self.log_count == 0:
return
if force or (self.log_count >= LOGS_PER_FILE or self.lost_signal_count >= LOST_SIGNAL_COUNT_MAX):
_debug("gpxd: save to log")
self._write_gpx()
self.lost_signal_count = 0
self.log_count = 0
self.logs.clear()
self.started_time = datetime.datetime.utcnow().isoformat()
def _write_gpx(self):
if len(self.logs) > 1:
if not os.path.exists(GPX_LOG_PATH):
os.makedirs(GPX_LOG_PATH)
filename = f"{self.started_time.replace(':', '-')}.gpx"
lines = [
'<?xml version="1.0" encoding="utf-8" standalone="yes"?>',
'<gpx version="1.1" creator="dragonpilot https://github.com/dragonpilot-community/dragonpilot" xmlns="http://www.topografix.com/GPX/1/1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd">',
'<trk>',
f'<name>{self.started_time}</name>',
'<trkseg>',
]
for trkpt in self.logs:
lines.append(self._trkpt_template(trkpt[0], trkpt[1], trkpt[2], trkpt[3]))
lines.extend([
'</trkseg>',
'</trk>',
'</gpx>',
])
with open(Path(GPX_LOG_PATH) / filename, 'w') as f:
f.write('\n'.join(lines))
def _trkpt_template(self, time, lat, lon, alt):
return f'<trkpt lat="{lat}" lon="{lon}">\n' \
f'<time>{datetime.datetime.utcfromtimestamp(time).isoformat()}</time>\n' \
f'<ele>{alt}</ele>\n' \
f'</trkpt>\n'
def gpxd_thread(sm=None, pm=None):
set_core_affinity([1,])
set_realtime_priority(1)
if sm is None:
sm = messaging.SubMaster(['gpsLocationExternal'])
gpxd = GpxD()
while True:
sm.update(1000)
gpxd.log(sm)
gpxd.write_log()
def main(sm=None, pm=None):
gpxd_thread(sm, pm)
if __name__ == "__main__":
main()
selfdrive/legacy_modeld/_dmonitoringmodeld
BIN
View File
Binary file not shown.
selfdrive/legacy_modeld/_modeld
BIN
View File
Binary file not shown.
selfdrive/legacy_modeld/models/supercombo.thneed
BIN
View File
Binary file not shown.
selfdrive/locationd/locationd
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/car.h
+21 -21
View File
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_err_fun(double *nom_x, double *delta_x, double *out_5384428575304141493);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4549184669954132702);
void car_H_mod_fun(double *state, double *out_1711160830974719761);
void car_f_fun(double *state, double dt, double *out_4246157559450135369);
void car_F_fun(double *state, double dt, double *out_226785303060119004);
void car_h_25(double *state, double *unused, double *out_9112045252708408632);
void car_H_25(double *state, double *unused, double *out_4780486682676063011);
void car_h_24(double *state, double *unused, double *out_5255225383959719707);
void car_H_24(double *state, double *unused, double *out_5177131987029922506);
void car_h_30(double *state, double *unused, double *out_6164319262352923671);
void car_H_30(double *state, double *unused, double *out_2262153724168814384);
void car_h_26(double *state, double *unused, double *out_3127533210908590986);
void car_H_26(double *state, double *unused, double *out_8521990001550119235);
void car_h_27(double *state, double *unused, double *out_1083931870738253695);
void car_H_27(double *state, double *unused, double *out_38559652984871167);
void car_h_29(double *state, double *unused, double *out_1162645994692078026);
void car_H_29(double *state, double *unused, double *out_1751922379854422200);
void car_h_28(double *state, double *unused, double *out_6046311868419546927);
void car_H_28(double *state, double *unused, double *out_6834321396923952774);
void car_h_31(double *state, double *unused, double *out_2341210026358057696);
void car_H_31(double *state, double *unused, double *out_4749840720799102583);
void car_err_fun(double *nom_x, double *delta_x, double *out_2352872656412802824);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7914023550455204906);
void car_H_mod_fun(double *state, double *out_6232055486637493083);
void car_f_fun(double *state, double dt, double *out_7443095893193800780);
void car_F_fun(double *state, double dt, double *out_2098139970056789871);
void car_h_25(double *state, double *unused, double *out_8602721369322250957);
void car_H_25(double *state, double *unused, double *out_8912817928593951573);
void car_h_24(double *state, double *unused, double *out_3580444101429411878);
void car_H_24(double *state, double *unused, double *out_5354892329791628437);
void car_h_30(double *state, double *unused, double *out_3073187478036949723);
void car_H_30(double *state, double *unused, double *out_6394484970086702946);
void car_h_26(double *state, double *unused, double *out_4240931279113091460);
void car_H_26(double *state, double *unused, double *out_5792422826241543819);
void car_h_27(double *state, double *unused, double *out_3601048101661405312);
void car_H_27(double *state, double *unused, double *out_7229823886171935062);
void car_h_29(double *state, double *unused, double *out_260635374767096340);
void car_H_29(double *state, double *unused, double *out_5516461159302384029);
void car_h_28(double *state, double *unused, double *out_5291495936397086272);
void car_H_28(double *state, double *unused, double *out_7480091430867710280);
void car_h_31(double *state, double *unused, double *out_7755811163072584423);
void car_H_31(double *state, double *unused, double *out_5166214724008192343);
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
void car_set_mass(double x);
void car_set_rotational_inertia(double x);
selfdrive/locationd/models/generated/gnss.h
+13 -13
View File
@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_638760506292347052);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_1357695715425304307);
void gnss_H_mod_fun(double *state, double *out_8293051259792062881);
void gnss_f_fun(double *state, double dt, double *out_3680399526685225726);
void gnss_F_fun(double *state, double dt, double *out_3518510924285127846);
void gnss_h_6(double *state, double *sat_pos, double *out_6626517376083254528);
void gnss_H_6(double *state, double *sat_pos, double *out_7012552859857848193);
void gnss_h_20(double *state, double *sat_pos, double *out_1916374596602260347);
void gnss_H_20(double *state, double *sat_pos, double *out_8575710610156363526);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_8599846894541753100);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_562112177466942736);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_8599846894541753100);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_562112177466942736);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_7168960314819711533);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_1733150604893116259);
void gnss_H_mod_fun(double *state, double *out_371800311444603574);
void gnss_f_fun(double *state, double dt, double *out_312732959618460837);
void gnss_F_fun(double *state, double dt, double *out_8832414516263107828);
void gnss_h_6(double *state, double *sat_pos, double *out_4697182586436612919);
void gnss_H_6(double *state, double *sat_pos, double *out_7897284550397221958);
void gnss_h_20(double *state, double *sat_pos, double *out_5766309662114082818);
void gnss_H_20(double *state, double *sat_pos, double *out_4592986296223702715);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_5139465688239376923);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1926627125318281676);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_5139465688239376923);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1926627125318281676);
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/models/generated/libkf.so
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/live.h
+24 -24
View File
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_H(double *in_vec, double *out_8241352854636576825);
void live_err_fun(double *nom_x, double *delta_x, double *out_6956584107259677061);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_3735347823356468233);
void live_H_mod_fun(double *state, double *out_7215248881055738529);
void live_f_fun(double *state, double dt, double *out_7385605237598930115);
void live_F_fun(double *state, double dt, double *out_7056904015291554990);
void live_h_4(double *state, double *unused, double *out_8349449970116828970);
void live_H_4(double *state, double *unused, double *out_7188737480185803033);
void live_h_9(double *state, double *unused, double *out_3652459562989647923);
void live_H_9(double *state, double *unused, double *out_6947547833556212388);
void live_h_10(double *state, double *unused, double *out_8215662764779518370);
void live_H_10(double *state, double *unused, double *out_2094257637643028062);
void live_h_12(double *state, double *unused, double *out_8569197030249233888);
void live_H_12(double *state, double *unused, double *out_2169281072153841238);
void live_h_35(double *state, double *unused, double *out_1501917841267451246);
void live_H_35(double *state, double *unused, double *out_576281960171172471);
void live_h_32(double *state, double *unused, double *out_8996403947392874492);
void live_H_32(double *state, double *unused, double *out_6377586332378785029);
void live_h_13(double *state, double *unused, double *out_5387084151892622067);
void live_H_13(double *state, double *unused, double *out_1375958207698795133);
void live_h_14(double *state, double *unused, double *out_3652459562989647923);
void live_H_14(double *state, double *unused, double *out_6947547833556212388);
void live_h_33(double *state, double *unused, double *out_3390849479985008479);
void live_H_33(double *state, double *unused, double *out_3726838964810030075);
void live_H(double *in_vec, double *out_263842806597291);
void live_err_fun(double *nom_x, double *delta_x, double *out_8569159409576762549);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_284279922701101553);
void live_H_mod_fun(double *state, double *out_8714581148882225607);
void live_f_fun(double *state, double dt, double *out_1713867968062410626);
void live_F_fun(double *state, double dt, double *out_8009710216144762132);
void live_h_4(double *state, double *unused, double *out_5932591475428494127);
void live_H_4(double *state, double *unused, double *out_2730109072736447982);
void live_h_9(double *state, double *unused, double *out_5854075401467761119);
void live_H_9(double *state, double *unused, double *out_2488919426106857337);
void live_h_10(double *state, double *unused, double *out_1420303220039557508);
void live_H_10(double *state, double *unused, double *out_1532345507062168959);
void live_h_12(double *state, double *unused, double *out_95484120703134841);
void live_H_12(double *state, double *unused, double *out_2289347335295513813);
void live_h_35(double *state, double *unused, double *out_6264768899209819592);
void live_H_35(double *state, double *unused, double *out_636552984636159394);
void live_h_32(double *state, double *unused, double *out_2061377894861506384);
void live_H_32(double *state, double *unused, double *out_3582146253035749676);
void live_h_13(double *state, double *unused, double *out_65595433475112262);
void live_H_13(double *state, double *unused, double *out_5494855882448693637);
void live_h_14(double *state, double *unused, double *out_5854075401467761119);
void live_H_14(double *state, double *unused, double *out_2488919426106857337);
void live_h_33(double *state, double *unused, double *out_3796847141673179462);
void live_H_33(double *state, double *unused, double *out_3787109989275016998);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/loggerd/bootlog
BIN
View File
Binary file not shown.
selfdrive/loggerd/deleter.py
+1 -1
View File
@@ -2,7 +2,7 @@
import os
import shutil
import threading
from selfdrive.swaglog import cloudlog
from system.swaglog import cloudlog
from selfdrive.loggerd.config import ROOT, get_available_bytes, get_available_percent
from selfdrive.loggerd.uploader import listdir_by_creation
selfdrive/loggerd/loggerd
BIN
View File
Binary file not shown.
selfdrive/loggerd/tests/test_uploader.py
+1 -1
View File
@@ -6,7 +6,7 @@ import unittest
import logging
import json
from selfdrive.swaglog import cloudlog
from system.swaglog import cloudlog
import selfdrive.loggerd.uploader as uploader
from common.xattr import getxattr
selfdrive/loggerd/uploader.py
+1 -1
View File
@@ -15,7 +15,7 @@ from common.params import Params
from selfdrive.hardware import TICI
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
from selfdrive.loggerd.config import ROOT
from selfdrive.swaglog import cloudlog
from system.swaglog import cloudlog
NetworkType = log.DeviceState.NetworkType
UPLOAD_ATTR_NAME = 'user.upload'
selfdrive/manager/manager.py
+15
View File
@@ -140,10 +140,25 @@ def manager_thread() -> None:
ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore)
started_prev = False
while True:
sm.update()
started = sm['deviceState'].started
if started and not started_prev:
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
elif not started and started_prev:
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
# initialize and update onroad params, which drives boardd's safety setter thread
if started != started_prev or sm.frame == 0:
params.put_bool("IsOnroad", started)
params.put_bool("IsOffroad", not started)
started_prev = started
ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore)
running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
selfdrive/manager/process_config.py
+14 -7
View File
@@ -6,6 +6,7 @@ from system.hardware import PC, TICI, EON
from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
NO_IR_CTRL = os.path.isfile('/data/media/0/no_ir_ctrl')
log_on = os.path.isfile('/data/media/0/log_on')
WEBCAM = os.getenv("USE_WEBCAM") is not None
@@ -36,16 +37,16 @@ procs = [
# due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption
NativeProcess("camerad", "selfdrive/camerad", ["./camerad"], unkillable=True, callback=driverview),
NativeProcess("clocksd", "system/clocksd", ["./clocksd"]),
# NativeProcess("logcatd", "system/logcatd", ["./logcatd"]),
# NativeProcess("proclogd", "system/proclogd", ["./proclogd"]),
# PythonProcess("logmessaged", "system.logmessaged", offroad=True),
NativeProcess("logcatd", "system/logcatd", ["./logcatd"], enabled=log_on),
NativeProcess("proclogd", "system/proclogd", ["./proclogd"], enabled=log_on),
PythonProcess("logmessaged", "system.logmessaged", offroad=True, enabled=log_on),
# PythonProcess("micd", "system.micd"),
# PythonProcess("timezoned", "system.timezoned", enabled=not PC, offroad=True),
DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),
NativeProcess("dmonitoringmodeld", "selfdrive/legacy_modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM) and not NO_IR_CTRL, callback=driverview),
# NativeProcess("encoderd", "system/loggerd", ["./encoderd"]),
# NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging),
NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging, enabled=log_on),
NativeProcess("modeld", "selfdrive/legacy_modeld", ["./modeld"]),
# NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"], enabled=False),
# NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False),
@@ -57,7 +58,7 @@ procs = [
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"),
PythonProcess("torqued", "selfdrive.locationd.torqued"),
PythonProcess("controlsd", "selfdrive.controls.controlsd"),
# PythonProcess("deleter", "system.loggerd.deleter", offroad=True),
PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True, enabled=log_on),
PythonProcess("dmonitoringd", "selfdrive.legacy_monitoring.dmonitoringd", enabled=(not PC or WEBCAM) and not NO_IR_CTRL, callback=driverview),
# PythonProcess("laikad", "selfdrive.locationd.laikad"),
# PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", enabled=TICI, onroad=False, callback=qcomgps),
@@ -69,9 +70,9 @@ procs = [
PythonProcess("plannerd", "selfdrive.controls.plannerd"),
PythonProcess("radard", "selfdrive.controls.radard"),
PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True),
# PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True),
PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True),
PythonProcess("updated", "selfdrive.updated", enabled=not PC, onroad=False, offroad=True),
# PythonProcess("uploader", "system.loggerd.uploader", offroad=True),
# PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True),
# PythonProcess("statsd", "selfdrive.statsd", offroad=True),
# debug procs
@@ -82,6 +83,12 @@ procs = [
PythonProcess("rtshield", "selfdrive.rtshield", enabled=EON),
PythonProcess("shutdownd", "system.hardware.eon.shutdownd", enabled=EON),
PythonProcess("androidd", "system.hardware.eon.androidd", enabled=EON, offroad=True),
# mapd
PythonProcess("mapd", "selfdrive.mapd.mapd"),
# gpxd
PythonProcess("gpxd", "selfdrive.dragonpilot.gpxd"),
PythonProcess("gpx_uploader", "selfdrive.dragonpilot.gpx_uploader", offroad=True),
]
managed_processes = {p.name: p for p in procs}
selfdrive/mapd/__init__.py
View File
selfdrive/mapd/config.py
+7
View File
@@ -0,0 +1,7 @@
# Map query config
QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries.
MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts. Minimum distance to query area edge before issuing a new query.
FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped.
LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapData msg.
LANE_WIDTH = 3.7 # Lane width estimate. Used for detecting departures from way.
selfdrive/mapd/default_speeds.json
+106
View File
@@ -0,0 +1,106 @@
{
"_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped",
"AR:urban": "40",
"AR:urban:primary": "60",
"AR:urban:secondary": "60",
"AR:rural": "110",
"AT:urban": "50",
"AT:rural": "100",
"AT:trunk": "100",
"AT:motorway": "130",
"BE:urban": "50",
"BE-VLG:rural": "70",
"BE-WAL:rural": "90",
"BE:trunk": "120",
"BE:motorway": "120",
"CH:urban[1]": "50",
"CH:rural": "80",
"CH:trunk": "100",
"CH:motorway": "120",
"CZ:pedestrian_zone": "20",
"CZ:living_street": "20",
"CZ:urban": "50",
"CZ:urban_trunk": "80",
"CZ:urban_motorway": "80",
"CZ:rural": "90",
"CZ:trunk": "110",
"CZ:motorway": "130",
"DK:urban": "50",
"DK:rural": "80",
"DK:motorway": "130",
"DE:living_street": "7",
"DE:residential": "30",
"DE:urban": "50",
"DE:rural": "100",
"DE:trunk": "none",
"DE:motorway": "none",
"FI:urban": "50",
"FI:rural": "80",
"FI:trunk": "100",
"FI:motorway": "120",
"FR:urban": "50",
"FR:rural": "80",
"FR:trunk": "110",
"FR:motorway": "130",
"GR:urban": "50",
"GR:rural": "90",
"GR:trunk": "110",
"GR:motorway": "130",
"HU:urban": "50",
"HU:rural": "90",
"HU:trunk": "110",
"HU:motorway": "130",
"IT:urban": "50",
"IT:rural": "90",
"IT:trunk": "110",
"IT:motorway": "130",
"JP:national": "60",
"JP:motorway": "100",
"LT:living_street": "20",
"LT:urban": "50",
"LT:rural": "90",
"LT:trunk": "120",
"LT:motorway": "130",
"PL:living_street": "20",
"PL:urban": "50",
"PL:rural": "90",
"PL:trunk": "100",
"PL:motorway": "140",
"RO:urban": "50",
"RO:rural": "90",
"RO:trunk": "100",
"RO:motorway": "130",
"RU:living_street": "20",
"RU:urban": "60",
"RU:rural": "90",
"RU:motorway": "110",
"SK:urban": "50",
"SK:rural": "90",
"SK:trunk": "90",
"SK:motorway": "90",
"SI:urban": "50",
"SI:rural": "90",
"SI:trunk": "110",
"SI:motorway": "130",
"ES:living_street": "20",
"ES:urban": "50",
"ES:rural": "50",
"ES:trunk": "90",
"ES:motorway": "120",
"SE:urban": "50",
"SE:rural": "70",
"SE:trunk": "90",
"SE:motorway": "110",
"GB:nsl_restricted": "30 mph",
"GB:nsl_single": "60 mph",
"GB:nsl_dual": "70 mph",
"GB:motorway": "70 mph",
"UA:urban": "50",
"UA:rural": "90",
"UA:trunk": "110",
"UA:motorway": "130",
"UZ:living_street": "30",
"UZ:urban": "70",
"UZ:rural": "100",
"UZ:motorway": "110"
}
selfdrive/mapd/default_speeds_generator.py
+240
View File
@@ -0,0 +1,240 @@
#!/usr/bin/env python
import json
DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json"
def main(filename = DEFAULT_OUTPUT_FILENAME):
countries = []
"""
--------------------------------------------------
US - United State of America
--------------------------------------------------
"""
US = Country("US") # First step, create the country using the ISO 3166 two letter code
countries.append(US) # Second step, add the country to countries list
""" Default rules """
# Third step, add some default rules for the country
# Speed limit rules are based on OpenStreetMaps (OSM) tags.
# The dictionary {...} defines the tag_name: value
# if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied.
# The text at the end is the speed limit (use no unit for km/h)
# Rules apply in the order in which they are written for each country
# Rules for specific regions (states) take priority over country rules
# If you modify existing country rules, you must update all existing states without that rule to use the old rule
US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph
US.add_rule({"highway": "trunk"}, "55 mph")
US.add_rule({"highway": "primary"}, "55 mph")
US.add_rule({"highway": "secondary"}, "45 mph")
US.add_rule({"highway": "tertiary"}, "35 mph")
US.add_rule({"highway": "unclassified"}, "55 mph")
US.add_rule({"highway": "residential"}, "25 mph")
US.add_rule({"highway": "service"}, "25 mph")
US.add_rule({"highway": "motorway_link"}, "55 mph")
US.add_rule({"highway": "trunk_link"}, "55 mph")
US.add_rule({"highway": "primary_link"}, "55 mph")
US.add_rule({"highway": "secondary_link"}, "45 mph")
US.add_rule({"highway": "tertiary_link"}, "35 mph")
US.add_rule({"highway": "living_street"}, "15 mph")
""" States """
new_york = US.add_region("New York") # Fourth step, add a state/region to country
new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules
new_york.add_rule({"highway": "secondary"}, "55 mph")
new_york.add_rule({"highway": "tertiary"}, "55 mph")
new_york.add_rule({"highway": "residential"}, "30 mph")
new_york.add_rule({"highway": "primary_link"}, "45 mph")
new_york.add_rule({"highway": "secondary_link"}, "55 mph")
new_york.add_rule({"highway": "tertiary_link"}, "55 mph")
# All if not written by the state, the rules will default to the country rules
#california = US.add_region("California")
# California uses only the default US rules
michigan = US.add_region("Michigan")
michigan.add_rule({"highway": "motorway"}, "70 mph")
oregon = US.add_region("Oregon")
oregon.add_rule({"highway": "motorway"}, "55 mph")
oregon.add_rule({"highway": "secondary"}, "35 mph")
oregon.add_rule({"highway": "tertiary"}, "30 mph")
oregon.add_rule({"highway": "service"}, "15 mph")
oregon.add_rule({"highway": "secondary_link"}, "35 mph")
oregon.add_rule({"highway": "tertiary_link"}, "30 mph")
south_dakota = US.add_region("South Dakota")
south_dakota.add_rule({"highway": "motorway"}, "80 mph")
south_dakota.add_rule({"highway": "trunk"}, "70 mph")
south_dakota.add_rule({"highway": "primary"}, "65 mph")
south_dakota.add_rule({"highway": "trunk_link"}, "70 mph")
south_dakota.add_rule({"highway": "primary_link"}, "65 mph")
wisconsin = US.add_region("Wisconsin")
wisconsin.add_rule({"highway": "trunk"}, "65 mph")
wisconsin.add_rule({"highway": "tertiary"}, "45 mph")
wisconsin.add_rule({"highway": "unclassified"}, "35 mph")
wisconsin.add_rule({"highway": "trunk_link"}, "65 mph")
wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph")
"""
--------------------------------------------------
AU - Australia
--------------------------------------------------
"""
AU = Country("AU")
countries.append(AU)
""" Default rules """
AU.add_rule({"highway": "motorway"}, "100")
AU.add_rule({"highway": "trunk"}, "80")
AU.add_rule({"highway": "primary"}, "80")
AU.add_rule({"highway": "secondary"}, "50")
AU.add_rule({"highway": "tertiary"}, "50")
AU.add_rule({"highway": "unclassified"}, "80")
AU.add_rule({"highway": "residential"}, "50")
AU.add_rule({"highway": "service"}, "40")
AU.add_rule({"highway": "motorway_link"}, "90")
AU.add_rule({"highway": "trunk_link"}, "80")
AU.add_rule({"highway": "primary_link"}, "80")
AU.add_rule({"highway": "secondary_link"}, "50")
AU.add_rule({"highway": "tertiary_link"}, "50")
AU.add_rule({"highway": "living_street"}, "30")
"""
--------------------------------------------------
CA - Canada
--------------------------------------------------
"""
CA = Country("CA")
countries.append(CA)
""" Default rules """
CA.add_rule({"highway": "motorway"}, "100")
CA.add_rule({"highway": "trunk"}, "80")
CA.add_rule({"highway": "primary"}, "80")
CA.add_rule({"highway": "secondary"}, "50")
CA.add_rule({"highway": "tertiary"}, "50")
CA.add_rule({"highway": "unclassified"}, "80")
CA.add_rule({"highway": "residential"}, "40")
CA.add_rule({"highway": "service"}, "40")
CA.add_rule({"highway": "motorway_link"}, "90")
CA.add_rule({"highway": "trunk_link"}, "80")
CA.add_rule({"highway": "primary_link"}, "80")
CA.add_rule({"highway": "secondary_link"}, "50")
CA.add_rule({"highway": "tertiary_link"}, "50")
CA.add_rule({"highway": "living_street"}, "20")
"""
--------------------------------------------------
DE - Germany
--------------------------------------------------
"""
DE = Country("DE")
countries.append(DE)
""" Default rules """
DE.add_rule({"highway": "motorway"}, "none")
DE.add_rule({"highway": "living_street"}, "10")
DE.add_rule({"highway": "residential"}, "30")
DE.add_rule({"zone:traffic": "DE:rural"}, "100")
DE.add_rule({"zone:traffic": "DE:urban"}, "50")
DE.add_rule({"zone:maxspeed": "DE:30"}, "30")
DE.add_rule({"zone:maxspeed": "DE:urban"}, "50")
DE.add_rule({"zone:maxspeed": "DE:rural"}, "100")
DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none")
DE.add_rule({"bicycle_road": "yes"}, "30")
"""
--------------------------------------------------
EE - Estonia
--------------------------------------------------
"""
EE = Country("EE")
countries.append(EE)
""" Default rules """
EE.add_rule({"highway": "motorway"}, "90")
EE.add_rule({"highway": "trunk"}, "90")
EE.add_rule({"highway": "primary"}, "90")
EE.add_rule({"highway": "secondary"}, "50")
EE.add_rule({"highway": "tertiary"}, "50")
EE.add_rule({"highway": "unclassified"}, "90")
EE.add_rule({"highway": "residential"}, "40")
EE.add_rule({"highway": "service"}, "40")
EE.add_rule({"highway": "motorway_link"}, "90")
EE.add_rule({"highway": "trunk_link"}, "70")
EE.add_rule({"highway": "primary_link"}, "70")
EE.add_rule({"highway": "secondary_link"}, "50")
EE.add_rule({"highway": "tertiary_link"}, "50")
EE.add_rule({"highway": "living_street"}, "20")
""" --- DO NOT MODIFY CODE BELOW THIS LINE --- """
""" --- ADD YOUR COUNTRY OR STATE ABOVE --- """
# Final step
write_json(countries, filename)
def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME):
out_dict = {}
for country in countries:
out_dict.update(country.jsonify())
json_string = json.dumps(out_dict, indent=2)
with open(filename, "wb") as f:
f.write(json_string)
class Region(object):
ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"]
ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"]
def __init__(self, name):
self.name = name
self.rules = []
def add_rule(self, tag_conditions, speed):
new_rule = {}
if not isinstance(tag_conditions, dict):
raise TypeError("Rule tag conditions must be dictionary")
if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions):
raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS
if 'highway' in tag_conditions:
if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES:
raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"]))
new_rule['tags'] = tag_conditions
try:
new_rule['speed'] = str(speed)
except ValueError:
raise ValueError("Rule speed must be string")
self.rules.append(new_rule)
def jsonify(self):
ret_dict = {}
ret_dict[self.name] = self.rules
return ret_dict
class Country(Region):
ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"]
def __init__(self, ISO_3166_alpha_2):
Region.__init__(self, ISO_3166_alpha_2)
if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES:
raise ValueError("Not valid IOS 3166 country code")
self.regions = {}
def add_region(self, name):
self.regions[name] = Region(name)
return self.regions[name]
def jsonify(self):
ret_dict = {}
ret_dict[self.name] = {}
for r_name, region in self.regions.items():
ret_dict[self.name].update(region.jsonify())
ret_dict[self.name]['Default'] = self.rules
return ret_dict
if __name__ == '__main__':
main()
selfdrive/mapd/lib/NodesData.py
+393
View File
@@ -0,0 +1,393 @@
import numpy as np
from enum import Enum
from selfdrive.mapd.lib.geo import DIRECTION, R, vectors
# from scipy.interpolate import splev, splprep
from opspline import splev, splprep # pylint: disable=E0401
_TURN_CURVATURE_THRESHOLD = 0.002 # 1/mts. A curvature over this value will generate a speed limit section.
_MAX_LAT_ACC = 2.3 # Maximum lateral acceleration in turns.
_SPLINE_EVAL_STEP = 5 # mts for spline evaluation for curvature calculation
_MIN_SPEED_SECTION_LENGTH = 100. # mts. Sections below this value will not be split in smaller sections.
_MAX_CURV_DEVIATION_FOR_SPLIT = 2. # Split a speed section if the max curvature deviates from mean by this factor.
_MAX_CURV_SPLIT_ARC_ANGLE = 90. # degrees. Arc section to split into new speed section around max curvature.
_MIN_NODE_DISTANCE = 50. # mts. Minimum distance between nodes for spline evaluation. Data is enhanced if not met.
_ADDED_NODES_DIST = 15. # mts. Distance between added nodes when data is enhanced for spline evaluation.
_DIVERTION_SEARCH_RANGE = [-200., 50.] # mt. Range of distance to current location for divertion search.
def nodes_raw_data_array_for_wr(wr, drop_last=False):
"""Provides an array of raw node data (id, lat, lon, speed_limit) for all nodes in way relation
"""
sl = wr.speed_limit
data = np.array([(n.id, n.lat, n.lon, sl) for n in wr.way.nodes], dtype=float)
# reverse the order if way direction is backwards
if wr.direction == DIRECTION.BACKWARD:
data = np.flip(data, axis=0)
# drop last if requested
return data[:-1] if drop_last else data
def node_calculations(points):
"""Provides node calculations based on an array of (lat, lon) points in radians.
points is a (N x 1) array where N >= 3
"""
if len(points) < 3:
raise(IndexError)
# Get the vector representation of node points in cartesian plane.
# (N-1, 2) array. Not including (0., 0.)
v = vectors(points) * R
# Calculate the vector magnitudes (or distance)
# (N-1, 1) array. No distance for v[-1]
d = np.linalg.norm(v, axis=1)
# Calculate the bearing (from true north clockwise) for every node.
# (N-1, 1) array. No bearing for v[-1]
b = np.arctan2(v[:, 0], v[:, 1])
# Add origin to vector space. (i.e first node in list)
v = np.concatenate(([[0., 0.]], v))
# Provide distance to previous node and distance to next node
dp = np.concatenate(([0.], d))
dn = np.concatenate((d, [0.]))
# Provide cumulative distance on route
dr = np.cumsum(dp, axis=0)
# Bearing of last node should keep bearing from previous.
b = np.concatenate((b, [b[-1]]))
return v, dp, dn, dr, b
def spline_curvature_calculations(vect, dist_prev):
"""Provides an array of curvatures and its distances by applying a spline interpolation
to the path described by the nodes data.
"""
# We need to artificially enhance the data before applying spline interpolation to avoid getting
# inexistent curvature values close to irregularities on the road when the resolution of nodes data
# approaching the irregularity is low.
# - Find indexes where dist_prev is greater than threshold
too_far_idxs = np.nonzero(dist_prev >= _MIN_NODE_DISTANCE)[0]
# - Traversing in reverse order, enhance data by adding points at the found indexes.
for idx in too_far_idxs[::-1]:
dp = dist_prev[idx] # distance of vector that needs to be replaced by higher resolution vectors.
n = int(np.ceil(dp / _ADDED_NODES_DIST)) # number of vectors that need to be added.
new_v = vect[idx, :] / n # new relative vector to insert.
vect = np.delete(vect, idx, axis=0) # remove the relative vector to be replaced by the insertion of new vectors.
vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0) # insert n new relative vectors
# Data is now enhanced, we can proceed with curvature evaluation.
# - Create cumulative arrays for distance traveled and vector (x, y)
ds = np.cumsum(dist_prev, axis=0)
vs = np.cumsum(vect, axis=0)
# - spline interpolation
tck, u = splprep([vs[:, 0], vs[:, 1]]) # pylint: disable=unbalanced-tuple-unpacking
# - evaluate every _SPLINE_EVAL_STEP mts.
n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u))
unew = np.arange(0, n + 1) / n
# - get derivatives
d1 = splev(unew, tck, der=1)
d2 = splev(unew, tck, der=2)
# - calculate curvatures
num = d1[0] * d2[1] - d1[1] * d2[0]
den = (d1[0]**2 + d1[1]**2)**(1.5)
curv = num / den
curv_ds = unew * ds[-1]
return curv, curv_ds
def speed_section(curv_sec):
"""Map curvature section data into turn speed sections data.
Returns: [section start distance, section end distance, speed limit based on max curvature, sing of curvature]
"""
max_curv_idx = np.argmax(curv_sec[:, 0])
start = np.amin(curv_sec[:, 2])
end = np.amax(curv_sec[:, 2])
return np.array([start, end, np.sqrt(_MAX_LAT_ACC / curv_sec[max_curv_idx, 0]), curv_sec[max_curv_idx, 1]])
def split_speed_section_by_sign(curv_sec):
"""Will split the given curvature section in subsections if there is a change of sign on the curvature value
in the section.
"""
# Find the indexes where the curvatures change signs (if any).
c_idx = np.nonzero(np.diff(curv_sec[:, 1]))[0] + 1
# Split section base on change of sign.
return np.split(curv_sec, c_idx)
def split_speed_section_by_curv_degree(curv_sec):
"""Will split the given curvature section in subsections as to isolate peaks of turn with substantially
higher curvature values. This will aid on preventing having very long turn sections with low speed limit
that is only really necessary for a small region of the section.
"""
# Only consider spliting a section if long enough.
length = curv_sec[-1, 2] - curv_sec[0, 2]
if length <= _MIN_SPEED_SECTION_LENGTH:
return [curv_sec]
# Only split if max curvature deviates substantially from mean curvature.
max_curv_idx = np.argmax(curv_sec[:, 0])
max_curv = curv_sec[max_curv_idx, 0]
mean_curv = np.mean(curv_sec[:, 0])
if max_curv / mean_curv <= _MAX_CURV_DEVIATION_FOR_SPLIT:
return [curv_sec]
# Calcualate where to split as to isolate a curve section around the max curvature peak.
arc_side = (np.radians(_MAX_CURV_SPLIT_ARC_ANGLE) / max_curv) / 2.
arc_side_idx_lenght = int(np.ceil(arc_side / _SPLINE_EVAL_STEP))
split_idxs = [max_curv_idx - arc_side_idx_lenght, max_curv_idx + arc_side_idx_lenght]
split_idxs = list(filter(lambda idx: idx > 0 and idx < len(curv_sec) - 1, split_idxs))
# If the arc section to split extendes outside the section, then no need to split.
if len(split_idxs) == 0:
return [curv_sec]
# Create the splits and split the resulting sections recursevly.
splits = [split_speed_section_by_curv_degree(cs) for cs in np.split(curv_sec, split_idxs)]
# Flatten the results and return the new list of curvature sections.
curv_secs = [cs for split in splits for cs in split]
return curv_secs
def speed_limits_for_curvatures_data(curv, dist):
"""Provides the calculations for the speed limits from the curvatures array and distances,
by providing distances to curvature sections and correspoinding speed limit values as well as
curvature direction/sign.
"""
# Prepare a data array for processing with absolute curvature values, curvature sign and distances.
curv_abs = np.abs(curv)
data = np.column_stack((curv_abs, np.sign(curv), dist))
# Find where curvatures overshoot turn curvature threshold and define as section
is_section = curv_abs >= _TURN_CURVATURE_THRESHOLD
# Find the indexes where the sections start and end. i.e. change indexes.
c_idx = np.nonzero(np.diff(is_section))[0] + 1
# Create independent arrays for each split section base on change indexes.
splits = np.array(np.split(data, c_idx), dtype=object)
# Filter the splits to keep only the curvature section arrays by getting the odd or even split arrays depending
# on whether the first split is a curvature split or not.
curv_sec_idxs = np.arange(0 if is_section[0] else 1, len(splits), 2, dtype=int)
curv_secs = splits[curv_sec_idxs]
# Further split the curv sections by sign change
sub_secs = [split_speed_section_by_sign(cs) for cs in curv_secs]
curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec]
# Further split the curv sections by degree of curvature
sub_secs = [split_speed_section_by_curv_degree(cs) for cs in curv_secs]
curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec]
# Return an array where each row represents a turn speed limit section.
# [start, end, speed_limit, curvature_sign]
return np.array([speed_section(cs) for cs in curv_secs])
def is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids):
"""
Evaluates if the way relation `wr` is a valid divertion from node with id `node_id`.
A valid divertion is a way relation with an edge node with the given `node_id` that is not already included
in the list of way relations in the route (`wr_ids`) and that can be travaled in the direction as if starting
from node with id `node_id`
"""
if wr.id in wr_ids:
return False
wr.update_direction_from_starting_node(node_id)
return not wr.is_prohibited
class SpeedLimitSection():
"""And object representing a speed limited road section ahead.
provides the start and end distance and the speed limit value
"""
def __init__(self, start, end, value):
self.start = start
self.end = end
self.value = value
def __repr__(self):
return f'from: {self.start}, to: {self.end}, limit: {self.value}'
class TurnSpeedLimitSection(SpeedLimitSection):
def __init__(self, start, end, value, sign):
super().__init__(start, end, value)
self.curv_sign = sign
def __repr__(self):
return f'{super().__repr__()}, sign: {self.curv_sign}'
class NodeDataIdx(Enum):
"""Column index for data elements on NodesData underlying data store.
"""
node_id = 0
lat = 1
lon = 2
speed_limit = 3
x = 4 # x value of cartesian vector representing the section between last node and this node.
y = 5 # y value of cartesian vector representing the section between last node and this node.
dist_prev = 6 # distance to previous node.
dist_next = 7 # distance to next node
dist_route = 8 # cumulative distance on route
bearing = 9 # bearing of the vector departing from this node.
class NodesData:
"""Container for the list of node data from a ordered list of way relations to be used in a Route
"""
def __init__(self, way_relations, wr_index):
self._nodes_data = np.array([])
self._divertions = [[]]
self._curvature_speed_sections_data = np.array([])
way_count = len(way_relations)
if way_count == 0:
return
# We want all the nodes from the last way section
nodes_data = nodes_raw_data_array_for_wr(way_relations[-1])
# For the ways before the last in the route we want all the nodes but the last, as that one is the first on
# the next section. Collect them, append last way node data and concatenate the numpy arrays.
if way_count > 1:
wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]])
wrs_data += (nodes_data,)
nodes_data = np.concatenate(wrs_data)
# Get a subarray with lat, lon to compute the remaining node values.
lat_lon_array = nodes_data[:, [1, 2]]
points = np.radians(lat_lon_array)
# Ensure we have more than 3 points, if not calculations are not possible.
if len(points) <= 3:
return
vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points)
# append calculations to nodes_data
# nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, dist_route, bearing]
self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing))
# Build route divertion options data from the wr_index.
wr_ids = [wr.id for wr in way_relations]
self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id)
if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)]
for node_id in nodes_data[:, 0]]
# Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process.
# _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign]
if len(vect) > 3:
curv, curv_ds = spline_curvature_calculations(vect, dist_prev)
self._curvature_speed_sections_data = speed_limits_for_curvatures_data(curv, curv_ds)
@property
def count(self):
return len(self._nodes_data)
def get(self, node_data_idx):
"""Returns the array containing all the elements of a specific NodeDataIdx type.
"""
if len(self._nodes_data) == 0 or node_data_idx.value >= self._nodes_data.shape[1]:
return np.array([])
return self._nodes_data[:, node_data_idx.value]
def speed_limits_ahead(self, ahead_idx, distance_to_node_ahead):
"""Returns and array of SpeedLimitSection objects for the actual route ahead of current location
"""
if len(self._nodes_data) == 0 or ahead_idx is None:
return []
# Find the cumulative distances where speed limit changes. Build Speed limit sections for those.
dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:]))
dist = np.cumsum(dist, axis=0)
sl = self.get(NodeDataIdx.speed_limit)[ahead_idx - 1:]
sl_next = np.concatenate((sl[1:], [0.]))
# Create a boolean mask where speed limit changes and filter values
sl_change = sl != sl_next
distances = dist[sl_change]
speed_limits = sl[sl_change]
# Create speed limits sections combining all continious nodes that have same speed limit value.
start = 0.
limits_ahead = []
for idx, end in enumerate(distances):
limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx]))
start = end
return limits_ahead
def distance_to_end(self, ahead_idx, distance_to_node_ahead):
if len(self._nodes_data) == 0 or ahead_idx is None:
return None
return np.sum(np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])))
def curvatures_speed_limit_sections_ahead(self, ahead_idx, distance_to_node_ahead):
"""Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location for
speed limit sections due to curvatures in the road.
"""
if len(self._curvature_speed_sections_data) == 0 or ahead_idx is None:
return []
# Find the current distance traveled so far on the route.
dist_curr = self.get(NodeDataIdx.dist_route)[ahead_idx] - distance_to_node_ahead
# Filter the sections to get only those where the stop distance is ahead of current.
sec_filter = self._curvature_speed_sections_data[:, 1] > dist_curr
data = self._curvature_speed_sections_data[sec_filter]
# Offset distances to current distance.
data[:, [0, 1]] -= dist_curr
# Create speed limits sections
limits_ahead = [TurnSpeedLimitSection(max(0., d[0]), d[1], d[2], d[3]) for d in data]
return limits_ahead
def possible_divertions(self, ahead_idx, distance_to_node_ahead):
""" Returns and array with the way relations the route could possible divert to by finding
the alternative way divertions on the nodes in the vicinity of the current location.
"""
if len(self._nodes_data) == 0 or ahead_idx is None:
return []
dist_route = self.get(NodeDataIdx.dist_route)
rel_dist = dist_route - dist_route[ahead_idx] + distance_to_node_ahead
valid_idxs = np.nonzero(np.logical_and(rel_dist >= _DIVERTION_SEARCH_RANGE[0],
rel_dist <= _DIVERTION_SEARCH_RANGE[1]))[0]
valid_divertions = [self._divertions[i] for i in valid_idxs]
return [wr for wrs in valid_divertions for wr in wrs] # flatten.
def distance_to_node(self, node_id, ahead_idx, distance_to_node_ahead):
"""
Provides the distance to a specific node in the route identified by `node_id` in reference to the node ahead
(`ahead_idx`) and the distance from current location to the node ahead (`distance_to_node_ahead`).
"""
node_ids = self.get(NodeDataIdx.node_id)
node_idxs = np.nonzero(node_ids == node_id)[0]
if len(self._nodes_data) == 0 or ahead_idx is None or len(node_idxs) == 0:
return None
return self.get(NodeDataIdx.dist_route)[node_idxs[0]] - self.get(NodeDataIdx.dist_route)[ahead_idx] + \
distance_to_node_ahead
selfdrive/mapd/lib/Route.py
+340
View File
@@ -0,0 +1,340 @@
from selfdrive.mapd.lib.NodesData import NodesData, NodeDataIdx
from selfdrive.mapd.config import QUERY_RADIUS
from selfdrive.mapd.lib.geo import ref_vectors, R, distance_to_points
from itertools import compress
import numpy as np
_ACCEPTABLE_BEARING_DELTA_COSINE = -0.7 # Continuation paths with a bearing of 180 +/- 45 degrees.
_MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE = -0.3420 # bearing delta at route edge must be 180 +/- 70 degrees.
_MAP_DATA_EDGE_DISTANCE = 50 # mts. Consider edge of map data from this distance to edge of query radius.
class Route():
"""A set of consecutive way relations forming a default driving route.
"""
def __init__(self, current, wr_index, way_collection_id, query_center):
"""Create a Route object from a given `wr_index` (Way relation index)
Args:
current (WayRelation): The Way Relation that is currently located. It must be active.
wr_index (WayRelationIndex): The indexes of WayRelations by node id.
way_collection_id (UUID): The id of the Way Collection that created this Route.
query_center (Numpy Array): lat, lon] numpy array in radians indicating the center of the data query.
"""
self.way_collection_id = way_collection_id
self._ordered_way_relations = []
self._nodes_data = None
self._reset()
# An active current way is needed to be able to build a route
if not current.active:
return
# Build the route by finding iteratavely the best matching ways continuing after the end of the
# current (last_wr) way. Use the index to find the continuation posibilities on each iteration.
last_wr = current
ordered_way_ids = []
split_wrs = []
while True:
# - Append current element to the route list of ordered way relations.
self._ordered_way_relations.append(last_wr)
ordered_way_ids.append(last_wr.id)
# - Get the id of the node at the end of the way and then fetch the way relations that share the end node id.
last_node_id = last_wr.last_node.id
way_relations = wr_index.way_relations_with_edge_node_id(last_node_id)
# - Add split way relations when necessary and remove parent way relations.
split_wrs_to_add = [wr for wr in split_wrs if last_node_id in wr.edge_nodes_ids]
way_relations.extend(split_wrs_to_add)
parent_ids = [wr.parent_wr_id for wr in split_wrs_to_add]
way_relations = [wr for wr in way_relations if wr.id not in parent_ids]
# - If no more way_relations than last_wr, we have to check if we join another wr on an internal node, and
# if we do, we replace such way relation with the split of it and continue.
if len(way_relations) == 1:
way_relations = wr_index.way_relations_with_node_id(last_node_id)
# If no more way_relations than last_wr or its parent, we got to the end.
if len(way_relations) == 1:
break
# If last_wr is a split, replace its parent with last_wr
way_relations = [last_wr if wr is last_wr.parent else wr for wr in way_relations]
# If we join a wr on an internal node, then we artificially split the wr in two and pass both wrs as
# candidates to the wr selection code below.
wr_to_split = [wr for wr in way_relations if wr is not last_wr][0]
next_split_way_id = -len(split_wrs) - 1 # Keep split wrs ids unique on Route
new_wrs = wr_to_split.split(last_node_id, [next_split_way_id, next_split_way_id - 1])
# If it could not be splited, we are done.
if len(new_wrs) != 2:
break
# Replace the original way relation for the splitted version on way_relations and track splited wrs.
split_wrs.extend(new_wrs)
way_relations.remove(wr_to_split)
way_relations.extend(new_wrs)
# - Get the coordinates for the edge node and build the array of coordinates for the nodes before the edge node
# on each of the common way relations, then get the vectors in cartesian plane for the end sections of each way.
ref_point = last_wr.last_node_coordinates
points = np.array([wr.node_before_edge_coordinates(last_node_id) for wr in way_relations])
v = ref_vectors(ref_point, points) * R
# - Calculate the bearing (from true north clockwise) for every end section of each way.
b = np.arctan2(v[:, 0], v[:, 1])
# - Find index of las_wr section and calculate deltas of bearings to the other sections.
last_wr_idx = way_relations.index(last_wr)
b_ref = b[last_wr_idx]
delta = b - b_ref
# - Update the direction of the possible route continuation ways as starting from last_node_id.
# Make sure to exclude any ways already included in the ordered list as to not modify direction when there
# are looping roads (like roundabouts). A way will never be included twice in a route anyway.
for wr in way_relations:
if wr.id not in ordered_way_ids:
wr.update_direction_from_starting_node(last_node_id)
# - Filter the possible route continuation way relations:
# - exclude any way already added to the ordered list.
# - exclude all way relations that are prohibited due to traffic direction.
mask = [wr.id not in ordered_way_ids and not wr.is_prohibited for wr in way_relations]
way_relations = list(compress(way_relations, mask))
delta = delta[mask]
# if no options left, we got to the end.
if len(way_relations) == 0:
break
# - The cosine of the bearing delta will aid us in choosing the way that continues. The cosine is
# minimum (-1) for a perfect straight continuation as delta would be pi or -pi.
cos_delta = np.cos(delta)
def pick_best_idx(cos_delta):
"""Selects the best index on `cos_delta` array for a way that continues the route.
In principle we want to choose the way that continues as straight as possible.
Bue we need to make sure that if there are 2 or more ways continuing relatively straight, then we
need to disambiguate, either by matching the `ref` or `name` value of the continuing way with the
last way selected.
This can prevent cases where the chosen route could be for instance an exit ramp of a way due to the fact
that the ramp has a better match on bearing to previous way. We choose to stay on the road with the same `ref`
or `name` value if available.
If there is no ambiguity or there are no `name` or `ref` values to disambiguate, then we pick the one with
the straightest following direction.
"""
# Find the indexes of the cosine of the deltas that are considered straight enough to continue.
idxs = np.nonzero(cos_delta < _ACCEPTABLE_BEARING_DELTA_COSINE)[0]
# If no amiguity or no way to break it, just return the straightest line.
if len(idxs) <= 1 or (last_wr.ref is None and last_wr.name is None):
# The section with the best continuation is the one with a bearing delta closest to pi. This is equivalent
# to taking the one with the smallest cosine of the bearing delta, as cosine is minimum (-1) on both pi
# and -pi.
return np.argmin(cos_delta)
wrs = [way_relations[idx] for idx in idxs]
# If we find a continuation way with the same reference we just choose it.
refs = list(map(lambda wr: wr.ref, wrs))
if last_wr.ref is not None:
idx = next((idx for idx, ref in enumerate(refs) if ref == last_wr.ref), None)
if idx is not None:
return idxs[idx]
# If we find a continuation way with the same name we just choose it.
names = list(map(lambda wr: wr.name, wrs))
if last_wr.name is not None:
idx = next((idx for idx, name in enumerate(names) if name == last_wr.name), None)
if idx is not None:
return idxs[idx]
# We did not manage to deambiguate, choose straightest path.
return np.argmin(cos_delta)
# Get the index of the continuation way.
best_idx = pick_best_idx(cos_delta)
# - Make sure to not select as route continuation a way that turns too much if we are close to the border of
# map data queried. This is to avoid building a route that takes a sharp turn just because we do not have the
# data for the way that actually continues straight.
if cos_delta[best_idx] > _MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE:
dist_to_center = distance_to_points(query_center, np.array([ref_point]))[0]
if dist_to_center > QUERY_RADIUS - _MAP_DATA_EDGE_DISTANCE:
break
# - Select next way.
last_wr = way_relations[best_idx]
# Build the node data from the ordered list of way relations
self._nodes_data = NodesData(self._ordered_way_relations, wr_index)
# Locate where we are in the route node list.
self._locate()
def __repr__(self):
count = self._nodes_data.count if self._nodes_data is not None else None
return f'Route: {self.way_collection_id}, idx ahead: {self._ahead_idx} of {count}'
def _reset(self):
self._limits_ahead = None
self._cuvature_limits_ahead = None
self._curvatures_ahead = None
self._ahead_idx = None
self._distance_to_node_ahead = None
@property
def located(self):
return self._ahead_idx is not None
def _locate(self):
"""Will resolve the index in the nodes_data list for the node ahead of the current location.
It updates as well the distance from the current location to the node ahead.
"""
current = self.current_wr
if current is None:
return
node_ahead_id = current.node_ahead.id
self._distance_to_node_ahead = current.distance_to_node_ahead
start_idx = self._ahead_idx if self._ahead_idx is not None else 1
self._ahead_idx = None
ids = self._nodes_data.get(NodeDataIdx.node_id)
for idx in range(start_idx, len(ids)):
if ids[idx] == node_ahead_id:
self._ahead_idx = idx
break
@property
def current_wr(self):
return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None
def update(self, location_rad, bearing_rad, location_stdev):
"""Will update the route structure based on the given `location_rad` and `bearing_rad` assuming progress on the
route on the original direction. If direction has changed or active point on the route can not be found, the route
will become invalid.
"""
if len(self._ordered_way_relations) == 0 or location_rad is None or bearing_rad is None:
return
# Skip if no update on location or bearing.
if np.array_equal(self.current_wr.location_rad, location_rad) and self.current_wr.bearing_rad == bearing_rad:
return
# Transverse the way relations on the actual order until we find an active one. From there, rebuild the route
# with the way relations remaining ahead.
for idx, wr in enumerate(self._ordered_way_relations):
active_direction = wr.direction
wr.update(location_rad, bearing_rad, location_stdev)
if not wr.active:
continue
if wr.direction != active_direction:
# Driving direction on the route has changed. stop.
break
# We have now the current wr. Repopulate from here till the end and locate
self._ordered_way_relations = self._ordered_way_relations[idx:]
self._reset()
self._locate()
# If the active way is diverting, check whether there are posibilities to divert from the route in the
# vecinity of the current location. If there are possibilities, then stop here to loose the route as we are
# most likely driving away. If there are no possibilites, then stick to the route as the diversion is probably
# just a matter of GPS accuracy. (It can happen after driving under a bridge)
if wr.diverting and len(self._nodes_data.possible_divertions(self._ahead_idx, self._distance_to_node_ahead)) > 0:
break
# The current location in route is valid, return.
return
# if we got here, there is no new active way relation or driving direction has changed. Reset.
self._reset()
@property
def speed_limits_ahead(self):
"""Returns and array of SpeedLimitSection objects for the actual route ahead of current location
"""
if self._limits_ahead is not None:
return self._limits_ahead
if self._nodes_data is None or self._ahead_idx is None:
return []
self._limits_ahead = self._nodes_data.speed_limits_ahead(self._ahead_idx, self._distance_to_node_ahead)
return self._limits_ahead
@property
def curvature_speed_limits_ahead(self):
"""Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location due
to curvatures
"""
if self._cuvature_limits_ahead is not None:
return self._cuvature_limits_ahead
if self._nodes_data is None or self._ahead_idx is None:
return []
self._cuvature_limits_ahead = self._nodes_data. \
curvatures_speed_limit_sections_ahead(self._ahead_idx, self._distance_to_node_ahead)
return self._cuvature_limits_ahead
@property
def current_speed_limit(self):
if not self.located:
return None
limits_ahead = self.speed_limits_ahead
if len(limits_ahead) == 0 or limits_ahead[0].start != 0:
return None
return limits_ahead[0].value
@property
def current_curvature_speed_limit_section(self):
if not self.located:
return None
limits_ahead = self.curvature_speed_limits_ahead
if len(limits_ahead) == 0 or limits_ahead[0].start != 0:
return None
return limits_ahead[0]
@property
def next_speed_limit_section(self):
if not self.located:
return None
limits_ahead = self.speed_limits_ahead
if len(limits_ahead) == 0:
return None
# Find the first section that does not start in 0. i.e. the next section
for section in limits_ahead:
if section.start > 0:
return section
return None
def next_curvature_speed_limit_sections(self, horizon_mts):
if not self.located:
return []
# Provide the curvature speed sections that start ahead (> 0) and up to horizon
return list(filter(lambda la: la.start > 0 and la.start <= horizon_mts, self.curvature_speed_limits_ahead))
@property
def distance_to_end(self):
if not self.located:
return None
return self._nodes_data.distance_to_end(self._ahead_idx, self._distance_to_node_ahead)
@property
def current_road_name(self):
return self.current_wr.road_name if self.located else None
selfdrive/mapd/lib/WayCollection.py
+85
View File
@@ -0,0 +1,85 @@
from selfdrive.mapd.lib.WayRelation import WayRelation
from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex
from selfdrive.mapd.lib.Route import Route
from selfdrive.mapd.config import LANE_WIDTH
import uuid
_ACCEPTABLE_BEARING_DELTA_IND = 0.7071067811865475 # sin(pi/4) | 45 degrees acceptable bearing delta
class WayCollection():
"""A collection of WayRelations to use for maps data analysis.
"""
def __init__(self, ways, query_center):
"""Creates a WayCollection with a set of OSM way objects.
Args:
ways (Array): Collection of Way objects fetched from OSM in a radius around `query_center`
query_center (Numpy Array): [lat, lon] numpy array in radians indicating the center of the data query.
"""
self.id = uuid.uuid4()
self.way_relations = [WayRelation(way) for way in ways]
self.query_center = query_center
self.wr_index = WayRelationIndex(self.way_relations)
def get_route(self, location_rad, bearing_rad, location_stdev):
"""Provides the best route found in the way collection based on current location and bearing.
"""
if location_rad is None or bearing_rad is None or location_stdev is None:
return None
# Update all way relations in collection to the provided location and bearing.
for wr in self.way_relations:
wr.update(location_rad, bearing_rad, location_stdev)
# Get the way relations where a match was found. i.e. those now marked as active as long as the direction of
# travel is valid.
valid_way_relations = [wr for wr in self.way_relations if wr.active and not wr.is_prohibited]
# If no active, then we could not find a current way to build a route.
if len(valid_way_relations) == 0:
return None
# If only one valid, then pick it as current.
if len(valid_way_relations) == 1:
current = valid_way_relations[0]
# If more than one is valid, filter out any valid way relation where the bearing delta indicator is too high.
else:
wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND,
valid_way_relations))
# If delta bearing indicator is too high for all, then use as current the one that has the shorter one.
if len(wr_acceptable_bearing) == 0:
valid_way_relations.sort(key=lambda wr: wr.active_bearing_delta)
current = valid_way_relations[0]
# If only one with acceptable bearing, use it.
elif len(wr_acceptable_bearing) == 1:
current = wr_acceptable_bearing[0]
else:
# If more than one with acceptable bearing, filter the ones with distance to way lower than 2 standard
# deviation from GPS accuracy (95%) + half the road width estimate.
wr_accurate_distance = [wr for wr in wr_acceptable_bearing
if wr.distance_to_way <= 2. * location_stdev + wr.lanes * LANE_WIDTH / 2.]
# If none with accurate distance to way, then select the closest to the way
if len(wr_accurate_distance) == 0:
wr_acceptable_bearing.sort(key=lambda wr: wr.distance_to_way)
current = wr_acceptable_bearing[0]
# If only one with distance under accuracy, select this one.
elif len(wr_accurate_distance) == 1:
current = wr_accurate_distance[0]
# If more than one with distance under accuracy. Then select the one with lowest highway rank.
# i.e. prefere motorways over other roads and so on. This is to prevent selecting a small paralel
# road to a main road when the accuracy is poor.
else:
wr_accurate_distance.sort(key=lambda wr: wr.highway_rank)
current = wr_accurate_distance[0]
return Route(current, self.wr_index, self.id, self.query_center)
selfdrive/mapd/lib/WayRelation.py
+422
View File
@@ -0,0 +1,422 @@
from selfdrive.mapd.lib.geo import DIRECTION, R, vectors, bearing_to_points, distance_to_points
from selfdrive.mapd.lib.osm import create_way
from common.conversions import Conversions as CV
from selfdrive.mapd.config import LANE_WIDTH
from common.basedir import BASEDIR
from datetime import datetime as dt
import numpy as np
import re
import json
_WAY_BBOX_PADING = 80. / R # 80 mts of pading to bounding box. (expressed in radians)
with open(BASEDIR + "/selfdrive/mapd/lib/default_speeds.json", "rb") as f:
_COUNTRY_LIMITS = json.loads(f.read())
_WD = {
'Mo': 0,
'Tu': 1,
'We': 2,
'Th': 3,
'Fr': 4,
'Sa': 5,
'Su': 6
}
_HIGHWAY_RANK = {
'motorway': 0,
'motorway_link': 1,
'trunk': 10,
'trunk_link': 11,
'primary': 20,
'primary_link': 21,
'secondary': 30,
'secondary_link': 31,
'tertiary': 40,
'tertiary_link': 41,
'unclassified': 50,
'residential': 60,
'living_street': 61
}
def is_osm_time_condition_active(condition_string):
"""
Will indicate if a time condition for a restriction as described
@ https://wiki.openstreetmap.org/wiki/Conditional_restrictions
is active for the current date and time of day.
"""
now = dt.now().astimezone()
today = now.date()
week_days = []
# Look for days of week matched and validate if today matches criteria.
dr = re.findall(r'(Mo|Tu|We|Th|Fr|Sa|Su[-,\s]*?)', condition_string)
if len(dr) == 1:
week_days = [_WD[dr[0]]]
# If two or more matches condider it a range of days between 1st and 2nd element.
elif len(dr) > 1:
week_days = list(range(_WD[dr[0]], _WD[dr[1]] + 1))
# If valid week days list is not empy and today day is not in the list, then the time-date range is not active.
if len(week_days) > 0 and now.weekday() not in week_days:
return False
# Look for time ranges on the day. No time range, means all day
tr = re.findall(r'([0-9]{1,2}:[0-9]{2})\s*?-\s*?([0-9]{1,2}:[0-9]{2})', condition_string)
# if no time range but there were week days set, consider it active during the whole day
if len(tr) == 0:
return len(dr) > 0
# Search among time ranges matched, one where now time belongs too. If found range is active.
for times_tup in tr:
times = list(map(lambda tt: dt.
combine(today, dt.strptime(tt, '%H:%M').time().replace(tzinfo=now.tzinfo)), times_tup))
if now >= times[0] and now <= times[1]:
return True
return False
def speed_limit_value_for_limit_string(limit_string):
# Look for matches of speed by default in kph, or in mph when explicitly noted.
v = re.match(r'^\s*([0-9]{1,3})\s*?(mph)?\s*$', limit_string)
if v is None:
return None
conv = CV.MPH_TO_MS if v[2] is not None and v[2] == "mph" else CV.KPH_TO_MS
return conv * float(v[1])
def speed_limit_for_osm_tag_limit_string(limit_string):
# https://wiki.openstreetmap.org/wiki/Key:maxspeed
if limit_string is None:
# When limit is set to 0. is considered not existing.
return 0.
# Attempt to parse limit as simple numeric value considering units.
limit = speed_limit_value_for_limit_string(limit_string)
if limit is not None:
return limit
# Look for matches of speed with country implicit values.
v = re.match(r'^\s*([A-Z]{2}):([a-z_]+):?([0-9]{1,3})?(\s+)?(mph)?\s*', limit_string)
if v is None:
return 0.
if v[2] == "zone" and v[3] is not None:
conv = CV.MPH_TO_MS if v[5] is not None and v[5] == "mph" else CV.KPH_TO_MS
limit = conv * float(v[3])
elif f'{v[1]}:{v[2]}' in _COUNTRY_LIMITS:
limit = speed_limit_value_for_limit_string(_COUNTRY_LIMITS[f'{v[1]}:{v[2]}'])
return limit if limit is not None else 0.
def conditional_speed_limit_for_osm_tag_limit_string(limit_string):
if limit_string is None:
# When limit is set to 0. is considered not existing.
return 0.
# Look for matches of the `<restriction-value> @ (<condition>)` format
v = re.match(r'^(.*)@\s*\((.*)\).*$', limit_string)
if v is None:
return 0. # No valid format match
value = speed_limit_for_osm_tag_limit_string(v[1])
if value == 0.:
return 0. # Invalid speed limit value
# Look for date-time conditions separated by semicolon
v = re.findall(r'(?:;|^)([^;]*)', v[2])
for datetime_condition in v:
if is_osm_time_condition_active(datetime_condition):
return value
# If we get here, no current date-time conditon is active.
return 0.
class WayRelation():
"""A class that represent the relationship of an OSM way and a given `location` and `bearing` of a driving vehicle.
"""
def __init__(self, way, parent=None):
self.way = way
self.parent = parent
self.parent_wr_id = parent.id if parent is not None else None # For WRs created as splits of other WRs
self.reset_location_variables()
self.direction = DIRECTION.NONE
self._speed_limit = None
self._one_way = way.tags.get("oneway")
self.name = way.tags.get('name')
self.ref = way.tags.get('ref')
self.highway_type = way.tags.get("highway")
self.highway_rank = _HIGHWAY_RANK.get(self.highway_type, 1000)
try:
self.lanes = int(way.tags.get('lanes'))
except Exception:
self.lanes = 2
# Create numpy arrays with nodes data to support calculations.
self._nodes_np = np.radians(np.array([[nd.lat, nd.lon] for nd in way.nodes], dtype=float))
self._nodes_ids = np.array([nd.id for nd in way .nodes], dtype=int)
# Get the vectors representation of the segments betwheen consecutive nodes. (N-1, 2)
v = vectors(self._nodes_np) * R
# Calculate the vector magnitudes (or distance) between nodes. (N-1)
self._way_distances = np.linalg.norm(v, axis=1)
# Calculate the bearing (from true north clockwise) for every section of the way (vectors between nodes). (N-1)
self._way_bearings = np.arctan2(v[:, 0], v[:, 1])
# Define bounding box to ease the process of locating a node in a way.
# [[min_lat, min_lon], [max_lat, max_lon]]
self.bbox = np.row_stack((np.amin(self._nodes_np, 0) - _WAY_BBOX_PADING,
np.amax(self._nodes_np, 0) + _WAY_BBOX_PADING))
# Get the edge nodes ids.
self.edge_nodes_ids = [way.nodes[0].id, way.nodes[-1].id]
def __repr__(self):
return f'(id: {self.id}, between {self.behind_idx} and {self.ahead_idx}, {self.direction}, active: {self.active})'
def __eq__(self, other):
if isinstance(other, WayRelation):
return self.id == other.id
return False
def reset_location_variables(self):
self.distance_to_node_ahead = 0.
self.location_rad = None
self.bearing_rad = None
self.active = False
self.diverting = False
self.ahead_idx = None
self.behind_idx = None
self._active_bearing_delta = None
self._distance_to_way = None
@property
def id(self):
return self.way.id
@property
def road_name(self):
if self.name is not None:
return self.name
return self.ref
def update(self, location_rad, bearing_rad, location_stdev):
"""Will update and validate the associated way with a given `location_rad` and `bearing_rad`.
Specifically it will find the nodes behind and ahead of the current location and bearing.
If no proper fit to the way geometry, the way relation is marked as invalid.
"""
self.reset_location_variables()
# Ignore if location not in way bounding box
if not self.is_location_in_bbox(location_rad):
return
# - Get the distance and bearings from location to all nodes. (N)
bearings = bearing_to_points(location_rad, self._nodes_np)
distances = distance_to_points(location_rad, self._nodes_np)
# - Get absolute bearing delta to current driving bearing. (N)
delta = np.abs(bearing_rad - bearings)
# - Nodes are ahead if the cosine of the delta is positive (N)
is_ahead = np.cos(delta) >= 0.
# - Possible locations on the way are those where adjacent nodes change from ahead to behind or viceversa.
possible_idxs = np.nonzero(np.diff(is_ahead))[0]
# - when no possible locations found, then the location is not in this way.
if len(possible_idxs) == 0:
return
# - Find then angle formed between the vectors from the current location to consecutive nodes. This is the
# value of the difference in the bearings of the vectors.
teta = np.diff(bearings)
# - When two consecutive nodes will be ahead and behind, they will form a triangle with the current location.
# We find the closest distance to the way by solving the area of the triangle and finding the height (h).
# We must use the abolute value of the sin of the angle in the formula, which is equivalent to ensure we
# are considering the smallest of the two angles formed between the two vectors.
# https://www.mathsisfun.com/algebra/trig-area-triangle-without-right-angle.html
h = distances[:-1] * distances[1:] * np.abs(np.sin(teta)) / self._way_distances
# - Calculate the delta between driving bearing and way bearings. (N-1)
bw_delta = self._way_bearings - bearing_rad
# - The absolut value of the sin of `bw_delta` indicates how close the bearings match independent of direction.
# We will use this value along the distance to the way to aid on way selection. (N-1)
abs_sin_bw_delta = np.abs(np.sin(bw_delta))
# - Get the delta to way bearing indicators and the distance to the way for the possible locations.
abs_sin_bw_delta_possible = abs_sin_bw_delta[possible_idxs]
h_possible = h[possible_idxs]
# - Get the index where the distance to the way is minimum. That is the chosen location.
min_h_possible_idx = np.argmin(h_possible)
min_delta_idx = possible_idxs[min_h_possible_idx]
# - If the distance to the way is over 4 standard deviations of the gps accuracy + half the maximum road width
# estimate, then we are way too far to stick to this way (i.e. we are not on this way anymore)
half_road_width_estimate = self.lanes * LANE_WIDTH / 2.
if h_possible[min_h_possible_idx] > 4. * location_stdev + half_road_width_estimate:
return
# - If the distance to the road is greater than 2 standard deviations of the gps accuracy + half the maximum road
# width estimate then we are most likely diverting from this route.
diverting = h_possible[min_h_possible_idx] > 2. * location_stdev + half_road_width_estimate
# Populate location variables with result
if is_ahead[min_delta_idx]:
self.direction = DIRECTION.BACKWARD
self.ahead_idx = min_delta_idx
self.behind_idx = min_delta_idx + 1
else:
self.direction = DIRECTION.FORWARD
self.ahead_idx = min_delta_idx + 1
self.behind_idx = min_delta_idx
self._distance_to_way = h[min_delta_idx]
self._active_bearing_delta = abs_sin_bw_delta_possible[min_h_possible_idx]
# TODO: The distance to node ahead currently represent the distance from the GPS fix location.
# It would be perhaps more accurate to use the distance on the projection over the direct line between
# the two nodes.
self.distance_to_node_ahead = distances[self.ahead_idx]
self.active = True
self.diverting = diverting
self.location_rad = location_rad
self.bearing_rad = bearing_rad
self._speed_limit = None
def update_direction_from_starting_node(self, start_node_id):
self._speed_limit = None
if self.edge_nodes_ids[0] == start_node_id:
self.direction = DIRECTION.FORWARD
elif self.edge_nodes_ids[-1] == start_node_id:
self.direction = DIRECTION.BACKWARD
else:
self.direction = DIRECTION.NONE
def is_location_in_bbox(self, location_rad):
"""Indicates if a given location is contained in the bounding box surrounding the way.
self.bbox = [[min_lat, min_lon], [max_lat, max_lon]]
"""
is_g = np.greater_equal(location_rad, self.bbox[0, :])
is_l = np.less_equal(location_rad, self.bbox[1, :])
return np.all(np.concatenate((is_g, is_l)))
@property
def speed_limit(self):
if self._speed_limit is not None:
return self._speed_limit
# Get string from corresponding tag, consider conditional limits first.
limit_string = self.way.tags.get("maxspeed:conditional")
if limit_string is None:
if self.direction == DIRECTION.FORWARD:
limit_string = self.way.tags.get("maxspeed:forward:conditional")
elif self.direction == DIRECTION.BACKWARD:
limit_string = self.way.tags.get("maxspeed:backward:conditional")
limit = conditional_speed_limit_for_osm_tag_limit_string(limit_string)
# When no conditional limit set, attempt to get from regular speed limit tags.
if limit == 0.:
limit_string = self.way.tags.get("maxspeed")
if limit_string is None:
if self.direction == DIRECTION.FORWARD:
limit_string = self.way.tags.get("maxspeed:forward")
elif self.direction == DIRECTION.BACKWARD:
limit_string = self.way.tags.get("maxspeed:backward")
limit = speed_limit_for_osm_tag_limit_string(limit_string)
self._speed_limit = limit
return self._speed_limit
@property
def active_bearing_delta(self):
"""Returns the sine of the delta between the current location bearing and the exact
bearing of the portion of way we are currentluy located at.
"""
return self._active_bearing_delta
@property
def is_one_way(self):
return self._one_way in ['yes'] or self.highway_type in ["motorway"]
@property
def is_prohibited(self):
# Direction must be defined to asses this property. Default to `True` if not.
if self.direction == DIRECTION.NONE:
return True
return self.is_one_way and self.direction == DIRECTION.BACKWARD
@property
def distance_to_way(self):
"""Returns the perpendicular (i.e. minimum) distance between current location and the way
"""
return self._distance_to_way
@property
def node_ahead(self):
return self.way.nodes[self.ahead_idx] if self.ahead_idx is not None else None
@property
def last_node(self):
"""Returns the last node on the way considering the traveling direction
"""
if self.direction == DIRECTION.FORWARD:
return self.way.nodes[-1]
if self.direction == DIRECTION.BACKWARD:
return self.way.nodes[0]
return None
@property
def last_node_coordinates(self):
"""Returns the coordinates for the last node on the way considering the traveling direction. (in radians)
"""
if self.direction == DIRECTION.FORWARD:
return self._nodes_np[-1]
if self.direction == DIRECTION.BACKWARD:
return self._nodes_np[0]
return None
def node_before_edge_coordinates(self, node_id):
"""Returns the coordinates of the node before the edge node identifeid with `node_id`. (in radians)
"""
if self.edge_nodes_ids[0] == node_id:
return self._nodes_np[1]
if self.edge_nodes_ids[-1] == node_id:
return self._nodes_np[-2]
return np.array([0., 0.])
def split(self, node_id, way_ids=None):
""" Returns and array with the way relations resulting from spliting the current way relation at node_id
"""
idxs = np.nonzero(self._nodes_ids == node_id)[0]
if len(idxs) == 0:
return []
idx = idxs[0]
if idx == 0 or idx == len(self._nodes_ids) - 1:
return [self]
if not isinstance(way_ids, list):
way_ids = [-1, -2] # Default id values.
ways = [create_way(way_ids[0], node_ids=self._nodes_ids[:idx + 1], from_way=self.way),
create_way(way_ids[1], node_ids=self._nodes_ids[idx:], from_way=self.way)]
return [WayRelation(way, parent=self) for way in ways]
selfdrive/mapd/lib/WayRelationIndex.py
+34
View File
@@ -0,0 +1,34 @@
class WayRelationIndex():
"""
A class containing an index of WayRelations by node ids of internal nodes and edge nodes.
"""
def __init__(self, way_relations):
self._edge_nodes_index_dict = {}
self._full_nodes_index_dict = {}
for wr in way_relations:
self.add(wr)
def add(self, way_relation):
for node in way_relation.way.nodes:
node_id = node.id
self._full_nodes_index_dict[node_id] = self._full_nodes_index_dict.get(node_id, []) + [way_relation]
if node_id in way_relation.edge_nodes_ids:
self._edge_nodes_index_dict[node_id] = self._edge_nodes_index_dict.get(node_id, []) + [way_relation]
def remove(self, way_relation):
for node in way_relation.way.nodes:
node_id = node.id
self._full_nodes_index_dict[node_id] = [wr for wr in self._full_nodes_index_dict.get(node_id, [])
if wr is not way_relation]
if node_id in way_relation.edge_nodes_ids:
self._edge_nodes_index_dict[node_id] = [wr for wr in self._edge_nodes_index_dict.get(node_id, [])
if wr is not way_relation]
def way_relations_with_edge_node_id(self, node_id):
return self._edge_nodes_index_dict.get(node_id, [])
def way_relations_with_node_id(self, node_id):
return self._full_nodes_index_dict.get(node_id, [])
selfdrive/mapd/lib/default_speeds.json
+111
View File
@@ -0,0 +1,111 @@
{
"_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped",
"AR:urban": "40",
"AR:urban:primary": "60",
"AR:urban:secondary": "60",
"AR:rural": "110",
"AT:urban": "50",
"AT:rural": "100",
"AT:trunk": "100",
"AT:motorway": "130",
"BE:urban": "50",
"BE-VLG:rural": "70",
"BE-WAL:rural": "90",
"BE:trunk": "120",
"BE:motorway": "120",
"CH:urban[1]": "50",
"CH:rural": "80",
"CH:trunk": "100",
"CH:motorway": "120",
"CZ:pedestrian_zone": "20",
"CZ:living_street": "20",
"CZ:urban": "50",
"CZ:urban_trunk": "80",
"CZ:urban_motorway": "80",
"CZ:rural": "90",
"CZ:trunk": "110",
"CZ:motorway": "130",
"DK:urban": "50",
"DK:rural": "80",
"DK:motorway": "130",
"DE:living_street": "7",
"DE:residential": "30",
"DE:urban": "50",
"DE:rural": "100",
"DE:trunk": "none",
"DE:motorway": "none",
"FI:urban": "50",
"FI:rural": "80",
"FI:trunk": "100",
"FI:motorway": "120",
"FR:urban": "50",
"FR:rural": "80",
"FR:trunk": "110",
"FR:motorway": "130",
"GR:urban": "50",
"GR:rural": "90",
"GR:trunk": "110",
"GR:motorway": "130",
"HU:urban": "50",
"HU:rural": "90",
"HU:trunk": "110",
"HU:motorway": "130",
"IT:urban": "50",
"IT:rural": "90",
"IT:trunk": "110",
"IT:motorway": "130",
"JP:national": "60",
"JP:motorway": "100",
"LT:living_street": "20",
"LT:urban": "50",
"LT:rural": "90",
"LT:trunk": "120",
"LT:motorway": "130",
"PL:living_street": "20",
"PL:urban": "50",
"PL:rural": "90",
"PL:trunk": "100",
"PL:motorway": "140",
"RO:urban": "50",
"RO:rural": "90",
"RO:trunk": "100",
"RO:motorway": "130",
"RU:living_street": "20",
"RU:urban": "60",
"RU:rural": "90",
"RU:motorway": "110",
"SK:urban": "50",
"SK:rural": "90",
"SK:trunk": "90",
"SK:motorway": "90",
"SI:urban": "50",
"SI:rural": "90",
"SI:trunk": "110",
"SI:motorway": "130",
"ES:living_street": "20",
"ES:urban": "50",
"ES:rural": "50",
"ES:trunk": "90",
"ES:motorway": "120",
"SE:urban": "50",
"SE:rural": "70",
"SE:trunk": "90",
"SE:motorway": "110",
"GB:nsl_restricted": "30 mph",
"GB:nsl_single": "60 mph",
"GB:nsl_dual": "70 mph",
"GB:motorway": "70 mph",
"UA:urban": "50",
"UA:rural": "90",
"UA:trunk": "110",
"UA:motorway": "130",
"UZ:living_street": "30",
"UZ:urban": "70",
"UZ:rural": "100",
"UZ:motorway": "110",
"ZA:trunk": "120",
"ZA:residential": "60",
"ZA:rural": "100",
"ZA:urban": "60",
"ZA:motorway": "120"
}
selfdrive/mapd/lib/geo.py
+66
View File
@@ -0,0 +1,66 @@
from enum import Enum
import numpy as np
R = 6373000.0 # approximate radius of earth in mts
def vectors(points):
"""Provides a array of vectors on cartesian space (x, y).
Each vector represents the path from a point in `points` to the next.
`points` must by a (N, 2) array of [lat, lon] pairs in radians.
"""
latA = points[:-1, 0]
latB = points[1:, 0]
delta = np.diff(points, axis=0)
dlon = delta[:, 1]
x = np.sin(dlon) * np.cos(latB)
y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon))
return np.column_stack((x, y))
def ref_vectors(ref, points):
"""Provides a array of vectors on cartesian space (x, y).
Each vector represents the path from ref to a point in `points`.
`points` must by a (N, 2) array of [lat, lon] pairs in radians.
"""
latA = ref[0]
latB = points[:, 0]
delta = points - ref
dlon = delta[:, 1]
x = np.sin(dlon) * np.cos(latB)
y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon))
return np.column_stack((x, y))
def bearing_to_points(point, points):
"""Calculate the bearings (angle from true north clockwise) of the vectors between `point` and each
one of the entries in `points`. Both `point` and `points` elements are 2 element arrays containing a latitud,
longitude pair in radians.
"""
delta = points - point
x = np.sin(delta[:, 1]) * np.cos(points[:, 0])
y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1]))
return np.arctan2(x, y)
def distance_to_points(point, points):
"""Calculate the distance of the vectors between `point` and each one of the entries in `points`.
Both `point` and `points` elements are 2 element arrays containing a latitud, longitude pair in radians.
"""
delta = points - point
a = np.sin(delta[:, 0] / 2)**2 + np.cos(point[0]) * np.cos(points[:, 0]) * np.sin(delta[:, 1] / 2)**2
c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
return c * R
class DIRECTION(Enum):
NONE = 0
AHEAD = 1
BEHIND = 2
FORWARD = 3
BACKWARD = 4
selfdrive/mapd/lib/osm.py
+37
View File
@@ -0,0 +1,37 @@
import overpy
import numpy as np
from selfdrive.mapd.lib.geo import R
def create_way(way_id, node_ids, from_way):
"""
Creates and OSM Way with the given `way_id` and list of `node_ids`, copying attributes and tags from `from_way`
"""
return overpy.Way(way_id, node_ids=node_ids, attributes={}, result=from_way._result,
tags=from_way.tags)
class OSM():
def __init__(self):
self.api = overpy.Overpass()
# self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter')
def fetch_road_ways_around_location(self, lat, lon, radius):
# Calculate the bounding box coordinates for the bbox containing the circle around location.
bbox_angle = np.degrees(radius / R)
# fetch all ways and nodes on this ways in bbox
bbox_str = f'{str(lat - bbox_angle)},{str(lon - bbox_angle)},{str(lat + bbox_angle)},{str(lon + bbox_angle)}'
q = """
way(""" + bbox_str + """)
[highway]
[highway!~"^(footway|path|corridor|bridleway|steps|cycleway|construction|bus_guideway|escape|service|track)$"];
(._;>;);
out;
"""
try:
ways = self.api.query(q).ways
except Exception as e:
print(f'Exception while querying OSM:\n{e}')
ways = []
return ways
selfdrive/mapd/mapd.py
+229
View File
@@ -0,0 +1,229 @@
#!/usr/bin/env python3
import threading
from traceback import print_exception
import numpy as np
import cereal.messaging as messaging
from common.realtime import Ratekeeper, set_core_affinity, set_realtime_priority
from selfdrive.mapd.lib.osm import OSM
from selfdrive.mapd.lib.geo import distance_to_points
from selfdrive.mapd.lib.WayCollection import WayCollection
from selfdrive.mapd.config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME
from system.swaglog import cloudlog
from cereal import log
from selfdrive.navd.helpers import Coordinate
import math
_DEBUG = False
_CLOUDLOG_DEBUG = True
def _debug(msg, log_to_cloud=True):
if _CLOUDLOG_DEBUG and log_to_cloud:
cloudlog.debug(msg)
if _DEBUG:
print(msg)
def excepthook(args):
_debug(f'MapD: Threading exception:\n{args}')
print_exception(args.exc_type, args.exc_value, args.exc_traceback)
threading.excepthook = excepthook
class MapD():
def __init__(self):
self.osm = OSM()
self.way_collection = None
self.route = None
self.last_gps_fix_timestamp = 0
self.last_gps = None
self.location_deg = None # The current location in degrees.
self.location_rad = None # The current location in radians as a Numpy array.
self.bearing_rad = None
self.location_stdev = None # The current location accuracy in mts. 1 standard devitation.
self.gps_speed = 0.
self.last_fetch_location = None
self.last_route_update_fix_timestamp = 0
self.last_publish_fix_timestamp = 0
self._query_thread = None
self._lock = threading.RLock()
def update_location(self, sm):
sock = 'liveLocationKalman'
if not sm.updated[sock] or not sm.valid[sock]:
return
location = sm[sock]
locationd_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
if not locationd_valid:
return
self.last_gps_fix_timestamp = location.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970.
position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
self.location_rad = np.radians(np.array([position.latitude, position.longitude], dtype=float))
self.location_deg = (position.latitude, position.longitude)
self.bearing_rad = np.radians(math.degrees(location.calibratedOrientationNED.value[2]), dtype=float)
self.gps_speed = location.velocityCalibrated.value[0]
self.location_stdev = 1.
def _query_osm_not_blocking(self):
def query(osm, location_deg, location_rad, radius):
_debug(f'Mapd: Start query for OSM map data at {location_deg}')
lat, lon = location_deg
ways = osm.fetch_road_ways_around_location(lat, lon, radius)
_debug(f'Mapd: Query to OSM finished with {len(ways)} ways')
# Only issue an update if we received some ways. Otherwise it is most likely a conectivity issue.
# Will retry on next loop.
if len(ways) > 0:
new_way_collection = WayCollection(ways, location_rad)
# Use the lock to update the way_collection as it might be being used to update the route.
_debug('Mapd: Locking to write results from osm.', log_to_cloud=False)
with self._lock:
self.way_collection = new_way_collection
self.last_fetch_location = location_rad
_debug(f'Mapd: Updated map data @ {location_deg} - got {len(ways)} ways')
_debug('Mapd: Releasing Lock to write results from osm', log_to_cloud=False)
# Ignore if we have a query thread already running.
if self._query_thread is not None and self._query_thread.is_alive():
return
self._query_thread = threading.Thread(target=query, args=(self.osm, self.location_deg, self.location_rad,
QUERY_RADIUS))
self._query_thread.start()
def updated_osm_data(self):
if self.route is not None:
distance_to_end = self.route.distance_to_end
if distance_to_end is not None and distance_to_end >= MIN_DISTANCE_FOR_NEW_QUERY:
# do not query as long as we have a route with enough distance ahead.
return
if self.location_rad is None:
return
if self.last_fetch_location is not None:
distance_since_last = distance_to_points(self.last_fetch_location, np.array([self.location_rad]))[0]
if distance_since_last < QUERY_RADIUS - MIN_DISTANCE_FOR_NEW_QUERY:
# do not query if are still not close to the border of previous query area
return
self._query_osm_not_blocking()
def update_route(self):
def update_proc():
# Ensure we clear the route on op disengage, this way we can correct possible incorrect map data due
# to wrongly locating or picking up the wrong route.
# if self._disengaging:
# self.route = None
# _debug('Mapd *****: Clearing Route as system is disengaging. ********')
if self.way_collection is None or self.location_rad is None or self.bearing_rad is None:
_debug('Mapd *****: Can not update route. Missing WayCollection, location or bearing ********')
return
if self.route is not None and self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp:
_debug('Mapd *****: Skipping route update. No new fix since last update ********')
return
self.last_route_update_fix_timestamp = self.last_gps_fix_timestamp
# Create the route if not existent or if it was generated by an older way collection
if self.route is None or self.route.way_collection_id != self.way_collection.id:
self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev)
_debug(f'Mapd *****: Route created: \n{self.route}\n********')
return
# Do not attempt to update the route if the car is going close to a full stop, as the bearing can start
# jumping and creating unnecesary loosing of the route. Since the route update timestamp has been updated
# a new liveMapData message will be published with the current values (which is desirable)
if self.gps_speed < FULL_STOP_MAX_SPEED:
_debug('Mapd *****: Route Not updated as car has Stopped ********')
return
self.route.update(self.location_rad, self.bearing_rad, self.location_stdev)
if self.route.located:
_debug(f'Mapd *****: Route updated: \n{self.route}\n********')
return
# if an old route did not mange to locate, attempt to regenerate form way collection.
self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev)
_debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********')
# We use the lock when updating the route, as it reads `way_collection` which can ben updated by
# a new query result from the _query_thread.
_debug('Mapd: Locking to update route.', log_to_cloud=False)
with self._lock:
update_proc()
_debug('Mapd: Releasing Lock to update route', log_to_cloud=False)
def publish(self, pm, sm):
# Ensure we have a route currently located
if self.route is None or not self.route.located:
_debug('Mapd: Skipping liveMapData message as there is no route or is not located.')
return
# Ensure we have a route update since last publish
if self.last_publish_fix_timestamp == self.last_route_update_fix_timestamp:
_debug('Mapd: Skipping liveMapData since there is no new gps fix.')
return
self.last_publish_fix_timestamp = self.last_route_update_fix_timestamp
speed_limit = self.route.current_speed_limit
next_speed_limit_section = self.route.next_speed_limit_section
current_road_name = self.route.current_road_name
map_data_msg = messaging.new_message('liveMapData')
map_data_msg.valid = sm.all_alive(service_list=['liveLocationKalman']) and \
sm.all_valid(service_list=['liveLocationKalman'])
map_data_msg.liveMapData.speedLimitValid = bool(speed_limit is not None)
map_data_msg.liveMapData.speedLimit = float(speed_limit if speed_limit is not None else 0.0)
map_data_msg.liveMapData.speedLimitAheadValid = bool(next_speed_limit_section is not None)
map_data_msg.liveMapData.speedLimitAhead = float(next_speed_limit_section.value
if next_speed_limit_section is not None else 0.0)
map_data_msg.liveMapData.speedLimitAheadDistance = float(next_speed_limit_section.start
if next_speed_limit_section is not None else 0.0)
map_data_msg.liveMapData.currentRoadName = str(current_road_name if current_road_name is not None else "")
pm.send('liveMapData', map_data_msg)
_debug(f'Mapd *****: Publish: \n{map_data_msg}\n********', log_to_cloud=False)
# provides live map data information
def mapd_thread(sm=None, pm=None):
set_core_affinity([1,])
set_realtime_priority(1)
mapd = MapD()
rk = Ratekeeper(1., print_delay_threshold=None) # Keeps rate at 1 hz
# *** setup messaging
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman'])
if pm is None:
pm = messaging.PubMaster(['liveMapData'])
while True:
sm.update()
mapd.update_location(sm)
mapd.updated_osm_data()
mapd.update_route()
mapd.publish(pm, sm)
rk.keep_time()
def main(sm=None, pm=None):
mapd_thread(sm, pm)
if __name__ == "__main__":
main()
selfdrive/mapd/mapd_helpers.py
+364
View File
@@ -0,0 +1,364 @@
import math
import json
import numpy as np
from datetime import datetime
from common.basedir import BASEDIR
from selfdrive.config import Conversions as CV
from common.transformations.coordinates import LocalCoord, geodetic2ecef
LOOKAHEAD_TIME = 10.
MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME
DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json"
DEFAULT_SPEEDS = {}
with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f:
DEFAULT_SPEEDS = json.loads(f.read())
DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json"
DEFAULT_SPEEDS_BY_REGION = {}
with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f:
DEFAULT_SPEEDS_BY_REGION = json.loads(f.read())
def circle_through_points(p1, p2, p3):
"""Fits a circle through three points
Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm"""
x1, y1, _ = p1
x2, y2, _ = p2
x3, y3, _ = p3
A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2
B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1)
C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2)
D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2)
return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
def parse_speed_unit(max_speed):
"""Converts a maxspeed string to m/s based on the unit present in the input.
OpenStreetMap defaults to kph if no unit is present. """
if not max_speed:
return None
conversion = CV.KPH_TO_MS
if 'mph' in max_speed:
max_speed = max_speed.replace(' mph', '')
conversion = CV.MPH_TO_MS
try:
return float(max_speed) * conversion
except ValueError:
return None
def parse_speed_tags(tags):
"""Parses tags on a way to find the maxspeed string"""
max_speed = None
if 'maxspeed' in tags:
max_speed = tags['maxspeed']
if 'maxspeed:conditional' in tags:
try:
max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ')
cond = cond[1:-1]
start, end = cond.split('-')
now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays
start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
if start <= now <= end:
max_speed = max_speed_cond
except ValueError:
pass
if not max_speed and 'source:maxspeed' in tags:
max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None)
if not max_speed and 'maxspeed:type' in tags:
max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None)
max_speed = parse_speed_unit(max_speed)
return max_speed
def geocode_maxspeed(tags, location_info):
max_speed = None
try:
geocode_country = location_info.get('country', '')
geocode_region = location_info.get('region', '')
country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {})
country_defaults = country_rules.get('Default', [])
for rule in country_defaults:
rule_valid = all(
tag_name in tags
and tags[tag_name] == value
for tag_name, value in rule['tags'].items()
)
if rule_valid:
max_speed = rule['speed']
break #stop searching country
region_rules = country_rules.get(geocode_region, [])
for rule in region_rules:
rule_valid = all(
tag_name in tags
and tags[tag_name] == value
for tag_name, value in rule['tags'].items()
)
if rule_valid:
max_speed = rule['speed']
break #stop searching region
except KeyError:
pass
max_speed = parse_speed_unit(max_speed)
return max_speed
class Way:
def __init__(self, way, query_results):
self.id = way.id
self.way = way
self.query_results = query_results
points = list()
for node in self.way.get_nodes(resolve_missing=False):
points.append((float(node.lat), float(node.lon), 0.))
self.points = np.asarray(points)
@classmethod
def closest(cls, query_results, lat, lon, heading, prev_way=None):
results, tree, real_nodes, node_to_way, location_info = query_results
cur_pos = geodetic2ecef((lat, lon, 0))
nodes = tree.query_ball_point(cur_pos, 500)
# If no nodes within 500m, choose closest one
if not nodes:
nodes = [tree.query(cur_pos)[1]]
ways = []
for n in nodes:
real_node = real_nodes[n]
ways += node_to_way[real_node.id]
ways = set(ways)
closest_way = None
best_score = None
for way in ways:
way = Way(way, query_results)
points = way.points_in_car_frame(lat, lon, heading)
on_way = way.on_way(lat, lon, heading, points)
if not on_way:
continue
# Create mask of points in front and behind
x = points[:, 0]
y = points[:, 1]
angles = np.arctan2(y, x)
front = np.logical_and((-np.pi / 2) < angles,
angles < (np.pi / 2))
behind = np.logical_not(front)
dists = np.linalg.norm(points, axis=1)
# Get closest point behind the car
dists_behind = np.copy(dists)
dists_behind[front] = np.NaN
closest_behind = points[np.nanargmin(dists_behind)]
# Get closest point in front of the car
dists_front = np.copy(dists)
dists_front[behind] = np.NaN
closest_front = points[np.nanargmin(dists_front)]
# fit line: y = a*x + b
x1, y1, _ = closest_behind
x2, y2, _ = closest_front
a = (y2 - y1) / max((x2 - x1), 1e-5)
b = y1 - a * x1
# With a factor of 60 a 20m offset causes the same error as a 20 degree heading error
# (A 20 degree heading offset results in an a of about 1/3)
score = abs(a) * 60. + abs(b)
# Prefer same type of road
if prev_way is not None:
if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''):
score *= 0.5
if closest_way is None or score < best_score:
closest_way = way
best_score = score
# Normal score is < 5
if best_score > 50:
return None
return closest_way
def __str__(self):
return "%s %s" % (self.id, self.way.tags)
def max_speed(self):
"""Extracts the (conditional) speed limit from a way"""
if not self.way:
return None
max_speed = parse_speed_tags(self.way.tags)
if not max_speed:
location_info = self.query_results[4]
max_speed = geocode_maxspeed(self.way.tags, location_info)
return max_speed
def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
"""Look ahead for a max speed"""
if not self.way:
return None
speed_ahead = None
speed_ahead_dist = None
lookahead_ways = 5
way = self
for i in range(lookahead_ways):
way_pts = way.points_in_car_frame(lat, lon, heading)
# Check current lookahead distance
max_dist = np.linalg.norm(way_pts[-1, :])
if max_dist > 2 * lookahead:
break
if 'maxspeed' in way.way.tags:
spd = parse_speed_tags(way.way.tags)
if not spd:
location_info = self.query_results[4]
spd = geocode_maxspeed(way.way.tags, location_info)
if spd < current_speed_limit:
speed_ahead = spd
min_dist = np.linalg.norm(way_pts[1, :])
speed_ahead_dist = min_dist
break
# Find next way
way = way.next_way()
if not way:
break
return speed_ahead, speed_ahead_dist
def advisory_max_speed(self):
if not self.way:
return None
tags = self.way.tags
adv_speed = None
if 'maxspeed:advisory' in tags:
adv_speed = tags['maxspeed:advisory']
adv_speed = parse_speed_unit(adv_speed)
return adv_speed
def on_way(self, lat, lon, heading, points=None):
if points is None:
points = self.points_in_car_frame(lat, lon, heading)
x = points[:, 0]
return np.min(x) < 0. and np.max(x) > 0.
def closest_point(self, lat, lon, heading, points=None):
if points is None:
points = self.points_in_car_frame(lat, lon, heading)
i = np.argmin(np.linalg.norm(points, axis=1))
return points[i]
def distance_to_closest_node(self, lat, lon, heading, points=None):
if points is None:
points = self.points_in_car_frame(lat, lon, heading)
return np.min(np.linalg.norm(points, axis=1))
def points_in_car_frame(self, lat, lon, heading):
lc = LocalCoord.from_geodetic([lat, lon, 0.])
# Build rotation matrix
heading = math.radians(-heading + 90)
c, s = np.cos(heading), np.sin(heading)
rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
# Convert to local coordinates
points_carframe = lc.geodetic2ned(self.points).T
# Rotate with heading of car
points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T
return points_carframe
def next_way(self, backwards=False):
results, tree, real_nodes, node_to_way, location_info = self.query_results
if backwards:
node = self.way.nodes[0]
else:
node = self.way.nodes[-1]
ways = node_to_way[node.id]
way = None
try:
# Simple heuristic to find next way
ways = [w for w in ways if w.id != self.id]
ways = [w for w in ways if w.nodes[0] == node]
# Filter on highway tag
acceptable_tags = list()
cur_tag = self.way.tags['highway']
acceptable_tags.append(cur_tag)
if cur_tag == 'motorway_link':
acceptable_tags.append('motorway')
acceptable_tags.append('trunk')
acceptable_tags.append('primary')
ways = [w for w in ways if w.tags['highway'] in acceptable_tags]
# Filter on number of lanes
cur_num_lanes = int(self.way.tags['lanes'])
if len(ways) > 1:
ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes]
if len(ways_same_lanes) == 1:
ways = ways_same_lanes
if len(ways) > 1:
ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes]
if len(ways) == 1:
way = Way(ways[0], self.query_results)
except (KeyError, ValueError):
pass
return way
def get_lookahead(self, lat, lon, heading, lookahead):
pnts = None
way = self
valid = False
for i in range(5):
# Get new points and append to list
new_pnts = way.points_in_car_frame(lat, lon, heading)
if pnts is None:
pnts = new_pnts
else:
pnts = np.vstack([pnts, new_pnts])
# Check current lookahead distance
max_dist = np.linalg.norm(pnts[-1, :])
if max_dist > lookahead:
valid = True
if max_dist > 2 * lookahead:
break
# Find next way
way = way.next_way()
if not way:
break
return pnts, valid
selfdrive/mapd/test/__init__.py
View File
selfdrive/mapd/test/mock_data.py
+266
View File
@@ -0,0 +1,266 @@
from selfdrive.mapd.lib.WayCollection import WayCollection
from selfdrive.mapd.lib.geo import vectors, R
from selfdrive.mapd.lib.NodesData import _MIN_NODE_DISTANCE, _ADDED_NODES_DIST, _SPLINE_EVAL_STEP, \
_MIN_SPEED_SECTION_LENGTH, nodes_raw_data_array_for_wr, node_calculations, is_wr_a_valid_divertion_from_node, \
spline_curvature_calculations, speed_limits_for_curvatures_data
from scipy.interpolate import splev, splprep
import numpy as np
import overpy
class MockNodesData():
def __init__(self, way_coords):
self.degrees = np.array(way_coords)
self.radians = np.radians(self.degrees)
# *****************
# Expected code implementation nodes_data
self.v = vectors(self.radians) * R
self.d = np.linalg.norm(self.v, axis=1)
self.b = np.arctan2(self.v[:, 0], self.v[:, 1])
self.v = np.concatenate(([[0., 0.]], self.v))
self.dp = np.concatenate(([0.], self.d))
self.dn = np.concatenate((self.d, [0.]))
self.dr = np.cumsum(self.dp, axis=0)
self.b = np.concatenate((self.b, [self.b[-1]]))
# Expected code implementation spline_curvature_calculations
vect = self.v
dist_prev = self.dp
too_far_idxs = np.nonzero(self.dp >= _MIN_NODE_DISTANCE)[0]
for idx in too_far_idxs[::-1]:
dp = dist_prev[idx] # distance of vector that needs to be replaced by higher resolution vectors.
n = int(np.ceil(dp / _ADDED_NODES_DIST)) # number of vectors that need to be added.
new_v = vect[idx, :] / n # new relative vector to insert.
vect = np.delete(vect, idx, axis=0) # remove the relative vector to be replaced by the insertion of new vectors.
vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0) # insert n new relative vectors
ds = np.cumsum(dist_prev, axis=0)
vs = np.cumsum(vect, axis=0)
tck, u = splprep([vs[:, 0], vs[:, 1]]) # pylint: disable=W0632
n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u))
unew = np.arange(0, n + 1) / n
d1 = splev(unew, tck, der=1)
d2 = splev(unew, tck, der=2)
num = d1[0] * d2[1] - d1[1] * d2[0]
den = (d1[0]**2 + d1[1]**2)**(1.5)
self.curv = num / den
self.curv_ds = unew * ds[-1]
# *****************
class MockCurveSection():
def __init__(self, func, di=0., df=1000., step=10.):
self.di = di
self.df = df
self.n = (df - di) // step
self.u = np.arange(0, self.n + 1) / self.n
self.curv_ds = self.u * (df - di) + di
self.curv = func(self.u)
self.curv_abs = np.abs(self.curv)
self.curv_sec = np.column_stack((self.curv_abs, np.sign(self.curv), self.curv_ds))
class MockOSMQueryResponse():
def __init__(self, xml_path, query_center):
self.api = overpy.Overpass()
self.query_center = np.radians(np.array(query_center))
with open(xml_path, 'r') as f:
overpass_xml = f.read()
self.ways = self.api.parse_xml(overpass_xml).ways
self.wayCollection = WayCollection(self.ways, self.query_center)
class MockRouteData():
def __init__(self, way_ids, way_collection, first_node_id): # way)ids must be in order forming a route.
self.wrs = [next(wr for wr in way_collection.way_relations if wr.id == way_id) for way_id in way_ids]
self.way_collection = way_collection
self.first_node_id = first_node_id
def reset(self):
way_relations = self.wrs
wr_index = self.way_collection.wr_index
# Nodes Data processing expects way relations to be updated with direction before running.
for idx, wr in enumerate(way_relations):
if idx == 0:
wr.update_direction_from_starting_node(self.first_node_id)
else:
wr.update_direction_from_starting_node(way_relations[idx - 1].last_node.id)
# ***** Expected calculations
self._nodes_data = np.array([])
self._divertions = [[]]
self._curvature_speed_sections_data = np.array([])
way_count = len(way_relations)
if way_count == 0:
return
# We want all the nodes from the last way section
nodes_data = nodes_raw_data_array_for_wr(way_relations[-1])
# For the ways before the last in the route we want all the nodes but the last, as that one is the first on
# the next section. Collect them, append last way node data and concatenate the numpy arrays.
if way_count > 1:
wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]])
wrs_data += (nodes_data,)
nodes_data = np.concatenate(wrs_data)
# Get a subarray with lat, lon to compute the remaining node values.
lat_lon_array = nodes_data[:, [1, 2]]
points = np.radians(lat_lon_array)
# Ensure we have more than 3 points, if not calculations are not possible.
if len(points) <= 3:
return
vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points)
# append calculations to nodes_data
# nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, dist_route, bearing]
self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing))
# Build route divertion options data from the wr_index.
wr_ids = [wr.id for wr in way_relations]
self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id)
if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)]
for node_id in nodes_data[:, 0]]
# Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process.
# _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign]
if len(vect) > 3:
self._curv, self._curv_ds = spline_curvature_calculations(vect, dist_prev)
self._curvature_speed_sections_data = speed_limits_for_curvatures_data(self._curv, self._curv_ds)
# *****
# Test data in degrees from this road:
# https://www.google.de/maps/@52.209263,13.8723137,13z
_WAY_NODES_COORDS_01 = [
[52.1933703, 13.8723799],
[52.1939477, 13.8711273],
[52.1942004, 13.8705818],
[52.1945408, 13.8698496],
[52.1948447, 13.8691873],
[52.1950772, 13.8685726],
[52.1951168, 13.8684641],
[52.1956681, 13.8670323],
[52.1958716, 13.8664936],
[52.1964366, 13.8649875],
[52.1969283, 13.8636040],
[52.1970203, 13.8634430],
[52.1975486, 13.8626307],
[52.1976354, 13.8624971],
[52.1977827, 13.8621795],
[52.1978564, 13.8619220],
[52.1981843, 13.8604497],
[52.1982614, 13.8602140],
[52.1983351, 13.8600595],
[52.1992768, 13.8579824],
[52.1995107, 13.8574321],
[52.1995948, 13.8572604],
[52.1996818, 13.8571155],
[52.1998000, 13.8570029],
[52.2000659, 13.8568236],
[52.2003868, 13.8566005],
[52.2007182, 13.8564460],
[52.2008760, 13.8564117],
[52.2009865, 13.8564117],
[52.2011390, 13.8564202],
[52.2012267, 13.8564496],
[52.2012544, 13.8564577],
[52.2013179, 13.8564803],
[52.2020491, 13.8571756],
[52.2026014, 13.8576991],
[52.2027592, 13.8578879],
[52.2027960, 13.8579309],
[52.2028960, 13.8580939],
[52.2030170, 13.8583343],
[52.2036587, 13.8597076],
[52.2052946, 13.8633039],
[52.2064332, 13.8658435],
[52.2067856, 13.8666332],
[52.2068961, 13.8668477],
[52.2070777, 13.8670890],
[52.2073723, 13.8674409],
[52.2077457, 13.8679387],
[52.2083874, 13.8687455],
[52.2093341, 13.8699214],
[52.2099652, 13.8707540],
[52.2102282, 13.8712089],
[52.2104228, 13.8715694],
[52.2106122, 13.8718955],
[52.2107619, 13.8721756],
[52.2108695, 13.8723771],
[52.2110747, 13.8727610],
[52.2111514, 13.8729047],
[52.2114010, 13.8733718],
[52.2114694, 13.8735006],
[52.2115430, 13.8736636],
[52.2116086, 13.8737571],
[52.2116770, 13.8738172],
[52.2117611, 13.8738515],
[52.2118664, 13.8738566],
[52.2119322, 13.8738439],
[52.2121058, 13.8737924],
[52.2122583, 13.8737495],
[52.2123265, 13.8737260],
[52.2124213, 13.8736894],
[52.2127466, 13.8734888],
[52.2128263, 13.8734491],
[52.2131313, 13.8733117],
[52.2133943, 13.8731830],
[52.2136625, 13.8731057],
[52.2139465, 13.8730456],
[52.2143619, 13.8730113],
[52.2148773, 13.8729942],
[52.2152275, 13.8730325],
[52.2153110, 13.8730398],
[52.2157442, 13.8730848],
[52.2158833, 13.8731036]]
mockNodesData01 = MockNodesData(_WAY_NODES_COORDS_01)
# OSM Query around B96 south of Berlin
mockOSMResponse01 = MockOSMQueryResponse('selfdrive/mapd/test/mock_osm_response_01.xml',
[52.31400353586984, 13.447158941786366])
# OSM Query on curvy town area south of Germany.
mockOSMResponse02 = MockOSMQueryResponse('selfdrive/mapd/test/mock_osm_response_02.xml',
[48.16573269276522, 9.81418473659117])
mockWayCollection01 = WayCollection(mockOSMResponse01.ways, mockOSMResponse01.query_center)
mockWayCollection02 = WayCollection(mockOSMResponse02.ways, mockOSMResponse02.query_center)
# Normal curvy Way. way id: 179532213 with 35 Nodes.
mockOSMWay_01_01_LongCurvy = next(way for way in mockOSMResponse01.ways if way.id == 179532213)
# Looped way. way id: 29233907
mockOSMWay_01_02_Loop = next(way for way in mockOSMResponse01.ways if way.id == 29233907)
# Complex curvy road through town with intersections. way id:178450395
mockOSMWay_02_01_CurvyTownWithIntersections = next(way for way in mockOSMResponse02.ways if way.id == 178450395)
# Valid divertion for way 02_01 at node: 34785115. way id: 27955186
mockOSMWay_02_02_Divertion_34785115 = next(way for way in mockOSMResponse02.ways if way.id == 27955186)
# 3 node way. way id: 807781992
mockOSMWay_02_03_Short_3_node_way = next(way for way in mockOSMResponse02.ways if way.id == 807781992)
# data composing route 01 in way collection 02
mockRouteData_02_01 = MockRouteData([60890967, 737120246, 601406617, 60890971, 178450395], mockWayCollection02,
first_node_id=201962346)
# data composing route 02 in way collection 02. Single WR
mockRouteData_02_02_single_wr = MockRouteData([178450395], mockWayCollection02, first_node_id=762086638)
# data composing route 03 in way collection 02. Multiple speed limits
mockRouteData_02_03 = MockRouteData([158799549, 798805532, 28707704, 158797898, 602249535, 602249536, 825823509,
178449088, 916462523, 158796386], mockWayCollection02,
first_node_id=252601829)
# 1000mt section with one full sin cycle as curv values.
mockCurveSectionSin = MockCurveSection(lambda x: np.sin(x * 2 * np.pi))
# 200mt section with changing curvature rate.
mockCurveSteepCurvChange = MockCurveSection(lambda x: 0.05 * x**3 - 0.007 * x**2 + 0.001 * x, df=200)
# _MIN_SPEED_SECTION_LENGTH section with changing curvature rate.
mockCurveSteepCurvChangeShort = MockCurveSection(
lambda x: 0.05 * x**3 - 0.007 * x**2 + 0.001 * x, df=_MIN_SPEED_SECTION_LENGTH)
# 200mt section with smooth changing curvature rate. no deviation over 2.
mockCurveSmoothCurveChange = MockCurveSection(lambda x: 0.0002 * x**3 - 0.001 * x**2 + 0.6 * x, df=200)
selfdrive/mapd/test/mock_osm_response_01.xml
+9908
View File
File diff suppressed because it is too large Load Diff
selfdrive/mapd/test/mock_osm_response_02.xml
+11529
View File
File diff suppressed because it is too large Load Diff
selfdrive/mapd/test/test_NodesData.py
+354
View File
@@ -0,0 +1,354 @@
import unittest
import numpy as np
from selfdrive.mapd.lib.geo import DIRECTION
from common.conversions import Conversions as CV
from selfdrive.mapd.lib.WayRelation import WayRelation
from selfdrive.mapd.lib.NodesData import nodes_raw_data_array_for_wr, node_calculations, \
spline_curvature_calculations, split_speed_section_by_sign, split_speed_section_by_curv_degree, speed_section, \
speed_limits_for_curvatures_data, is_wr_a_valid_divertion_from_node, SpeedLimitSection, TurnSpeedLimitSection, \
NodesData, NodeDataIdx
from selfdrive.mapd.test.mock_data import mockOSMWay_01_01_LongCurvy, mockNodesData01, mockCurveSectionSin, \
mockCurveSteepCurvChange, mockCurveSteepCurvChangeShort, mockCurveSmoothCurveChange, \
mockOSMWay_02_01_CurvyTownWithIntersections, mockOSMWay_02_02_Divertion_34785115, mockOSMWay_02_03_Short_3_node_way, \
mockRouteData_02_01, mockRouteData_02_02_single_wr, mockRouteData_02_03
from numpy.testing import assert_array_almost_equal
class TestNodesDataFileFunctions(unittest.TestCase):
def test_nodes_raw_data_array_for_wr(self):
wr = WayRelation(mockOSMWay_01_01_LongCurvy)
data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float)
data = nodes_raw_data_array_for_wr(wr)
assert_array_almost_equal(data, data_e)
def test_nodes_raw_data_array_for_wr_flips_when_backwards(self):
wr = WayRelation(mockOSMWay_01_01_LongCurvy)
wr.direction = DIRECTION.BACKWARD
data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float)
data_e = np.flip(data_e, axis=0)
data = nodes_raw_data_array_for_wr(wr)
assert_array_almost_equal(data, data_e)
def test_nodes_raw_data_array_for_wr_drops_last(self):
wr = WayRelation(mockOSMWay_01_01_LongCurvy)
data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float)[:-1]
data = nodes_raw_data_array_for_wr(wr, drop_last=True)
assert_array_almost_equal(data, data_e)
def test_node_calculations(self):
points = mockNodesData01.radians
v, dp, dn, dr, b = node_calculations(points)
assert_array_almost_equal(v, mockNodesData01.v)
assert_array_almost_equal(dp, mockNodesData01.dp)
assert_array_almost_equal(dn, mockNodesData01.dn)
assert_array_almost_equal(dr, mockNodesData01.dr)
assert_array_almost_equal(b, mockNodesData01.b)
def test_node_calculations_index_error(self):
points = mockNodesData01.radians[:2]
with self.assertRaises(IndexError):
node_calculations(points)
def test_spline_curvature_calculations(self):
vect = mockNodesData01.v
dist_prev = mockNodesData01.dp
curv, curv_ds = spline_curvature_calculations(vect, dist_prev)
assert_array_almost_equal(curv, mockNodesData01.curv)
assert_array_almost_equal(curv_ds, mockNodesData01.curv_ds)
def test_spline_curvature_calculations_with_route_data(self):
mockRouteData_02_01.reset()
nodes_data = mockRouteData_02_01._nodes_data
vect = np.column_stack((nodes_data[:, 4], nodes_data[:, 5]))
dist_prev = nodes_data[:, 6]
curv, curv_ds = spline_curvature_calculations(vect, dist_prev)
assert_array_almost_equal(curv, mockRouteData_02_01._curv)
assert_array_almost_equal(curv_ds, mockRouteData_02_01._curv_ds)
def test_split_speed_section_by_sign(self):
curv_sec = mockCurveSectionSin.curv_sec
new_secs = split_speed_section_by_sign(curv_sec)
# 3 sections with matching initial and final distance
self.assertEqual(len(new_secs), 3)
self.assertEqual(new_secs[0][0][2], mockCurveSectionSin.di)
self.assertEqual(new_secs[2][-1][2], mockCurveSectionSin.df)
# All new sections has same sign internally
for sec in new_secs:
self.assertEqual(np.average(sec, axis=0)[1], sec[0][1])
# Sections change sign
for idx in range(2):
self.assertNotEqual(new_secs[idx][0][1], new_secs[idx + 1][0][1])
# total items consistency
lenghts = [len(sec) for sec in new_secs]
self.assertEqual(len(curv_sec), sum(lenghts))
def test_split_speed_section_by_curv_degree(self):
curv_sec = mockCurveSteepCurvChange.curv_sec
new_secs = split_speed_section_by_curv_degree(curv_sec)
# 3 sections with matching initial and final distance
self.assertEqual(len(new_secs), 3)
self.assertEqual(new_secs[0][0][2], mockCurveSteepCurvChange.di)
self.assertEqual(new_secs[2][-1][2], mockCurveSteepCurvChange.df)
# Sections split at the right points
split_dist = [sec[-1][2] for sec in new_secs]
self.assertListEqual(split_dist, [50., 150., 200.])
def test_split_speed_section_by_curv_degree_does_nothing_if_short(self):
curv_sec = mockCurveSteepCurvChangeShort.curv_sec
new_secs = split_speed_section_by_curv_degree(curv_sec)
self.assertEqual(len(new_secs), 1)
assert_array_almost_equal(curv_sec, new_secs[0])
def test_split_speed_section_by_curv_degree_does_nothing_if_no_substantial_change(self):
curv_sec = mockCurveSmoothCurveChange.curv_sec
new_secs = split_speed_section_by_curv_degree(curv_sec)
self.assertEqual(len(new_secs), 1)
assert_array_almost_equal(curv_sec, new_secs[0])
def test_speed_section(self):
curv_sec = mockCurveSectionSin.curv_sec
speed_secs = speed_section(curv_sec)
expected = np.array([0., 1000., 1.51657509, 1.])
assert_array_almost_equal(speed_secs, expected)
def test_speed_limits_for_curvatures_data(self):
curv = mockCurveSectionSin.curv
curv_ds = mockCurveSectionSin.curv_ds
expected = np.array([
[10., 490., 1.51657509, 1.],
[510., 990., 1.51657509, -1.]])
limits = speed_limits_for_curvatures_data(curv, curv_ds)
assert_array_almost_equal(limits, expected)
def test_is_wr_a_valid_divertion_from_node(self):
wr = WayRelation(mockOSMWay_02_01_CurvyTownWithIntersections)
mockOSMWay_02_02_Divertion_34785115.tags['oneway'] = 'yes'
wr_div = WayRelation(mockOSMWay_02_02_Divertion_34785115)
# False if id already in route
wr_ids = [wr.id, wr_div.id]
self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 34785115, wr_ids))
# True if id not in route, node_id is edge and not prohibited
wr_ids = [wr.id, 11111, 22222]
self.assertTrue(is_wr_a_valid_divertion_from_node(wr_div, 34785115, wr_ids))
# False if id not in route, node_id is edge but prohibited (wrong direction from node 319503453)
self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 319503453, wr_ids))
# False if id not in route, node_id is not edge
self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 44444, wr_ids))
class TestSpeedLimitSection(unittest.TestCase):
def test_speed_limit_section_init(self):
section = SpeedLimitSection(10., 20., 50.)
self.assertEqual(section.start, 10.)
self.assertEqual(section.end, 20.)
self.assertEqual(section.value, 50.)
class TestTurnSpeedLimitSection(unittest.TestCase):
def test_turn_speed_limit_section_init(self):
section = TurnSpeedLimitSection(10., 20., 50., -1.)
self.assertEqual(section.start, 10.)
self.assertEqual(section.end, 20.)
self.assertEqual(section.value, 50.)
self.assertEqual(section.curv_sign, -1.)
class TestNodesData(unittest.TestCase):
def test_init_with_empty_list(self):
nd = NodesData([], {})
self.assertEqual(len(nd._nodes_data), 0)
num_diverstions = sum([len(d) for d in nd._divertions])
self.assertEqual(num_diverstions, 0)
self.assertEqual(len(nd._curvature_speed_sections_data), 0)
def test_init_with_single_wr_includes_all_wr_nodes(self):
mockRouteData_02_02_single_wr.reset()
way_relations = mockRouteData_02_02_single_wr.wrs
wr_index = mockRouteData_02_02_single_wr.way_collection.wr_index
nd = NodesData(way_relations, wr_index)
assert_array_almost_equal(nd._nodes_data, mockRouteData_02_02_single_wr._nodes_data)
assert_array_almost_equal(nd._curvature_speed_sections_data,
mockRouteData_02_02_single_wr._curvature_speed_sections_data)
self.assertListEqual(nd._divertions, mockRouteData_02_02_single_wr._divertions)
self.assertEqual(len(nd._nodes_data), len(way_relations[0].way.nodes))
self.assertEqual(len(nd._curvature_speed_sections_data), 6)
num_diverstions = sum([len(d) for d in nd._divertions])
self.assertEqual(num_diverstions, 6)
def test_init_with_less_than_4_nodes(self):
wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way)
nd = NodesData([wr_t], {})
self.assertEqual(len(nd._nodes_data), 0)
num_diverstions = sum([len(d) for d in nd._divertions])
self.assertEqual(num_diverstions, 0)
self.assertEqual(len(nd._curvature_speed_sections_data), 0)
def test_init_with_multiple_wr(self):
mockRouteData_02_01.reset()
way_relations = mockRouteData_02_01.wrs
wr_index = mockRouteData_02_01.way_collection.wr_index
nd = NodesData(way_relations, wr_index)
assert_array_almost_equal(nd._nodes_data, mockRouteData_02_01._nodes_data)
assert_array_almost_equal(nd._curvature_speed_sections_data, mockRouteData_02_01._curvature_speed_sections_data)
self.assertListEqual(nd._divertions, mockRouteData_02_01._divertions)
self.assertEqual(len(nd._curvature_speed_sections_data), 9)
num_diverstions = sum([len(d) for d in nd._divertions])
self.assertEqual(num_diverstions, 14)
def test_count(self):
mockRouteData_02_01.reset()
way_relations = mockRouteData_02_01.wrs
wr_index = mockRouteData_02_01.way_collection.wr_index
num_n = sum([len(wr.way.nodes) for wr in way_relations]) - len(way_relations) + 1
nd = NodesData(way_relations, wr_index)
self.assertEqual(nd.count, num_n)
def test_get_on_empty(self):
wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way)
nd = NodesData([wr_t], {})
assert_array_almost_equal(nd.get(NodeDataIdx.node_id), np.array([]))
def test_get_values(self):
mockRouteData_02_01.reset()
way_relations = mockRouteData_02_01.wrs
wr_index = mockRouteData_02_01.way_collection.wr_index
nd = NodesData(way_relations, wr_index)
assert_array_almost_equal(nd.get(NodeDataIdx.node_id), mockRouteData_02_01._nodes_data[:, 0])
assert_array_almost_equal(nd.get(NodeDataIdx.lat), mockRouteData_02_01._nodes_data[:, 1])
assert_array_almost_equal(nd.get(NodeDataIdx.lon), mockRouteData_02_01._nodes_data[:, 2])
assert_array_almost_equal(nd.get(NodeDataIdx.speed_limit), mockRouteData_02_01._nodes_data[:, 3])
assert_array_almost_equal(nd.get(NodeDataIdx.x), mockRouteData_02_01._nodes_data[:, 4])
assert_array_almost_equal(nd.get(NodeDataIdx.y), mockRouteData_02_01._nodes_data[:, 5])
assert_array_almost_equal(nd.get(NodeDataIdx.dist_prev), mockRouteData_02_01._nodes_data[:, 6])
assert_array_almost_equal(nd.get(NodeDataIdx.dist_next), mockRouteData_02_01._nodes_data[:, 7])
assert_array_almost_equal(nd.get(NodeDataIdx.dist_route), mockRouteData_02_01._nodes_data[:, 8])
assert_array_almost_equal(nd.get(NodeDataIdx.bearing), mockRouteData_02_01._nodes_data[:, 9])
def test_speed_limits_ahead_from_empty(self):
wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way)
nd = NodesData([wr_t], {})
self.assertEqual(len(nd.speed_limits_ahead(1, 10.)), 0)
def test_speed_limits_ahead(self):
mockRouteData_02_03.reset()
way_relations = mockRouteData_02_03.wrs
wr_index = mockRouteData_02_03.way_collection.wr_index
nd = NodesData(way_relations, wr_index)
# empty when ahead_idx is none.
self.assertEqual(len(nd.speed_limits_ahead(None, 10.)), 0)
# All limist from 0
all_limits = nd.speed_limits_ahead(1, nd.get(NodeDataIdx.dist_next)[0])
self.assertEqual(len(all_limits), 4) # 4 limits on this mock road.
self.assertListEqual([sl.value for sl in all_limits], [v * CV.KPH_TO_MS for v in [50, 100, 50, 100]])
for idx, sl in enumerate(all_limits):
self.assertTrue(sl.end > sl.start)
self.assertTrue(sl.value > 0.)
if idx == 0:
self.assertEqual(sl.start, 0.)
else:
self.assertEqual(sl.start, all_limits[idx - 1].end)
self.assertNotEqual(sl.value, all_limits[idx - 1].value)
def test_distance_to_end_from_empty(self):
wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way)
nd = NodesData([wr_t], {})
self.assertIsNone(nd.distance_to_end(1, 10.))
def test_distance_to_end(self):
mockRouteData_02_03.reset()
way_relations = mockRouteData_02_03.wrs
wr_index = mockRouteData_02_03.way_collection.wr_index
nd = NodesData(way_relations, wr_index)
# none when ahead_idx is none.
self.assertIsNone(nd.distance_to_end(None, 10.))
# From the begining
expected = np.sum(nd.get(NodeDataIdx.dist_next))
self.assertAlmostEqual(nd.distance_to_end(1, nd.get(NodeDataIdx.dist_next)[0]), expected)
self.assertAlmostEqual(nd.get(NodeDataIdx.dist_route)[-1], expected)
# From the node next to last
expected = nd.get(NodeDataIdx.dist_next)[-2]
self.assertAlmostEqual(nd.distance_to_end(nd.count - 2, 0.), expected)
def test_distance_to_node(self):
mockRouteData_02_03.reset()
way_relations = mockRouteData_02_03.wrs
wr_index = mockRouteData_02_03.way_collection.wr_index
nd = NodesData(way_relations, wr_index)
dist_to_node_ahead = 10.
node_id = 1887995486 # Some node id in the middle of the way. idx 50
node_idx = np.nonzero(nd.get(NodeDataIdx.node_id) == node_id)[0][0]
# none when ahead_idx is none.
self.assertIsNone(nd.distance_to_node(node_id, None, dist_to_node_ahead))
# From the begining
expected = nd.get(NodeDataIdx.dist_route)[node_idx]
self.assertAlmostEqual(nd.distance_to_node(node_id, 1, nd.get(NodeDataIdx.dist_next)[0]), expected)
# From the end
expected = -np.sum(nd.get(NodeDataIdx.dist_next)[node_idx:])
self.assertAlmostEqual(nd.distance_to_node(node_id, len(nd.get(NodeDataIdx.node_id)) - 1, 0.), expected)
# From some node behind including dist to node ahead
ahead_idx = node_idx - 10
expected = np.sum(nd.get(NodeDataIdx.dist_next)[ahead_idx:node_idx]) + dist_to_node_ahead
self.assertAlmostEqual(nd.distance_to_node(node_id, ahead_idx, dist_to_node_ahead), expected)
# From some node ahead including dist to node ahead
ahead_idx = node_idx + 10
expected = -np.sum(nd.get(NodeDataIdx.dist_next)[node_idx:ahead_idx]) + dist_to_node_ahead
self.assertAlmostEqual(nd.distance_to_node(node_id, ahead_idx, dist_to_node_ahead), expected)
# TODO: Missing tests for curvatures_speed_limit_sections_ahead and possible_divertions
selfdrive/mapd/test/test_WayRelation.py
+651
View File
@@ -0,0 +1,651 @@
import copy
import unittest
import numpy as np
from unittest import mock
from numpy.testing import assert_array_almost_equal
from datetime import datetime as dt, timezone, timedelta
from common.conversions import Conversions as CV
from selfdrive.mapd.lib.WayRelation import WayRelation, is_osm_time_condition_active, \
conditional_speed_limit_for_osm_tag_limit_string, speed_limit_for_osm_tag_limit_string
from selfdrive.mapd.config import LANE_WIDTH
from selfdrive.mapd.lib.geo import DIRECTION, R, vectors
from selfdrive.mapd.test.mock_data import mockOSMWay_01_01_LongCurvy, mockOSMWay_01_02_Loop, \
mockOSMWay_02_01_CurvyTownWithIntersections
class TestWayRelationFileFunctions(unittest.TestCase):
def test_speed_limit_for_osm_tag_limit_string(self):
values = [
None, # Invalid
"1000", # Invalid
"60 kph", # Invalid
"100",
"30 mph",
"DE:zone:40",
"DE:zone:50 mph",
"AR:urban",
"CZ:pedestrian_zone",
"DK:urban",
"DK:rural",
"DK:motorway",
"DE:living_street",
"DE:residential",
"DE:urban",
"DE:rural",
"DE:trunk", # No limit
"DE:motorway", # No limit
"GB:nsl_restricted",
"GB:nsl_single",
"GB:nsl_dual",
"GB:motorway",
"GB:invalid", # Invalid
]
expected = [
0.,
0.,
0.,
100. * CV.KPH_TO_MS,
30. * CV.MPH_TO_MS,
40. * CV.KPH_TO_MS,
50. * CV.MPH_TO_MS,
40. * CV.KPH_TO_MS,
20. * CV.KPH_TO_MS,
50. * CV.KPH_TO_MS,
80. * CV.KPH_TO_MS,
130. * CV.KPH_TO_MS,
7. * CV.KPH_TO_MS,
30. * CV.KPH_TO_MS,
50. * CV.KPH_TO_MS,
100. * CV.KPH_TO_MS,
0.,
0.,
30. * CV.MPH_TO_MS,
60. * CV.MPH_TO_MS,
70. * CV.MPH_TO_MS,
70. * CV.MPH_TO_MS,
0.,
]
result = [speed_limit_for_osm_tag_limit_string(sls) for sls in values]
self.assertEqual(result, expected)
@mock.patch('selfdrive.mapd.lib.WayRelation.dt')
def test_is_osm_time_condition_active(self, mock_dt):
tz = timezone(timedelta(hours=1), 'berlin')
wed_10_10_am = dt(2021, 9, 1, 10, 10, 0)
mock_dt.now.return_value = wed_10_10_am
mock_dt.tzinfo = tz
mock_dt.combine = dt.combine
mock_dt.strptime = dt.strptime
values = [
"WE", # Invalid
"We",
"Mo",
"Fr",
"Tu-Th",
"10:00", # Invalid
"10:00-10:30",
"We 10:00-10:30",
"SU 10:00-10:30", # Valid, SU string not considered a day string.
"Sa 10:00-10:30",
"Tu-Th 10:00-10:30",
]
expected = [
False, # Invalid
True,
False,
False,
True,
False, # Invalid
True,
True,
True,
False,
True,
]
result = [is_osm_time_condition_active(cs) for cs in values]
self.assertEqual(result, expected)
@mock.patch('selfdrive.mapd.lib.WayRelation.dt')
def test_conditional_speed_limit_for_osm_tag_limit_string(self, mock_dt):
tz = timezone(timedelta(hours=1), 'berlin')
wed_10_10_am = dt(2021, 9, 1, 10, 10, 0)
mock_dt.now.return_value = wed_10_10_am
mock_dt.tzinfo = tz
mock_dt.combine = dt.combine
mock_dt.strptime = dt.strptime
values = [
None, # Invalid
"Hola", # Invalid
"100 @ (WE)", # Invalid
"x @ (We)", # Invalid
"100 @ (We)",
"100 @ (Mo)",
"100 @ (Fr)",
"100 @ (Tu-Th)",
"100 @ (10:00)", # Invalid
"100 @ (10:00-10:30)",
"100 @ (We 10:00-10:30)",
"100 @ (SU 10:00-10:30)", # Valid, SU string not considered a day string.
"100 @ (Sa 10:00-10:30)",
"100 @ (Tu-Th 10:00-10:30)",
"100 @ (Mo-Th;Su)",
"100 @ (Mo Th;Fr-Sa)",
"100 @ (Fr-Su;Mo-Tu)",
"100 @ (10:00-10:30;15:00-16:00)",
"100 @ (We;Mo-Tu)",
"100 @ (We 10:00-10:30;Th 15:00-16:00)",
"100 @ (Tu 10:00-10:30;Th 15:00-16:00)",
]
_100 = 100. * CV.KPH_TO_MS
expected = [
0., # Invalid
0., # Invalid
0., # Invalid
0., # Invalid
_100,
0.,
0.,
_100,
0., # Invalid
_100,
_100,
_100,
0.,
_100,
_100,
_100,
0.,
_100,
_100,
_100,
0.
]
result = [conditional_speed_limit_for_osm_tag_limit_string(ls) for ls in values]
self.assertEqual(result, expected)
class TestWayRelation(unittest.TestCase):
def test_way_relation_init(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
nodes_np_expected = np.radians(np.array([[nd.lat, nd.lon] for nd in wayRelation.way.nodes], dtype=float))
v = vectors(wayRelation._nodes_np)
way_distances_expected = np.linalg.norm(v * R, axis=1)
way_bearings_expected = np.arctan2(v[:, 0], v[:, 1])
bbox_expected = np.array([
[0.91321784, 0.2346417],
[0.91344672, 0.23475751]])
self.assertEqual(wayRelation.way.id, 179532213)
self.assertIsNone(wayRelation.parent_wr_id)
self.assertEqual(wayRelation.direction, DIRECTION.NONE)
self.assertEqual(wayRelation._speed_limit, None)
self.assertEqual(wayRelation._one_way, 'yes')
self.assertEqual(wayRelation.name, None)
self.assertEqual(wayRelation.ref, 'B 96')
self.assertEqual(wayRelation.highway_type, 'trunk')
self.assertEqual(wayRelation.highway_rank, 10)
self.assertEqual(wayRelation.lanes, 2)
assert_array_almost_equal(wayRelation._nodes_np, nodes_np_expected)
assert_array_almost_equal(wayRelation._way_distances, way_distances_expected)
assert_array_almost_equal(wayRelation._way_bearings, way_bearings_expected)
assert_array_almost_equal(wayRelation.bbox, bbox_expected)
self.assertEqual(wayRelation.edge_nodes_ids, [wayRelation.way.nodes[0].id, wayRelation.way.nodes[-1].id])
def test_way_relation_init_with_parent(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy, parent=WayRelation(mockOSMWay_01_02_Loop))
self.assertEqual(wayRelation.way.id, 179532213)
self.assertEqual(wayRelation.parent_wr_id, 29233907)
def test_way_relation_equality(self):
wayRelation1 = WayRelation(mockOSMWay_01_01_LongCurvy)
wayRelation2 = copy.copy(wayRelation1)
wayRelation3 = copy.deepcopy(wayRelation1)
wayRelation3.way.id = 123
self.assertEqual(wayRelation1, wayRelation2)
self.assertNotEqual(wayRelation1, wayRelation3)
def test_way_relation_reset_location_variables(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
self.make_wayRelation_location_dirty(wayRelation)
wayRelation.reset_location_variables()
self.assert_wayRelation_variables_reset(wayRelation)
def test_way_relation_id(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
self.assertEqual(wayRelation.id, 179532213)
def test_way_relation_road_name(self):
# road name when no tag for name or ref
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
self.assertIsNone(wayRelation.road_name)
# road name based on ref tag
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
self.assertEqual(wayRelation.road_name, "B 96")
# road name based on name tag
wayRelation = WayRelation(mockOSMWay_02_01_CurvyTownWithIntersections)
self.assertEqual(wayRelation.road_name, "Hauptstraße")
def test_way_relation_update_resets_on_update(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
self.make_wayRelation_location_dirty(wayRelation)
location_rad = np.array([0., 0.]) # Location outside bbox
wayRelation.update(location_rad, 0., 10.)
self.assertFalse(wayRelation.is_location_in_bbox(location_rad))
self.assert_wayRelation_variables_reset(wayRelation)
def test_way_relation_update_only_resets_if_no_possible_found(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
location_rad = wayRelation.bbox[0] # Location inside bbox but outside actual way (due to padding)
wayRelation.update(location_rad, 0., 10.)
self.assertTrue(wayRelation.is_location_in_bbox(location_rad))
self.assert_wayRelation_variables_reset(wayRelation)
def test_way_relation_updates_in_the_correct_direction_with_correct_property_values(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
location_rad = np.radians(np.array([52.32855593146639, 13.445320150125069]))
bearing_rad = 0.
wayRelation.update(location_rad, bearing_rad, 10.)
self.assertTrue(wayRelation.is_location_in_bbox(location_rad))
self.assertEqual(wayRelation.direction, DIRECTION.FORWARD)
self.assertEqual(wayRelation.ahead_idx, 17)
self.assertEqual(wayRelation.behind_idx, 16)
self.assertAlmostEqual(wayRelation._distance_to_way, 3.43290781621360)
self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.320717420388962)
self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 25.4998961709014)
self.assertTrue(wayRelation.active)
self.assertFalse(wayRelation.diverting)
assert_array_almost_equal(wayRelation.location_rad, location_rad)
self.assertEqual(wayRelation.bearing_rad, bearing_rad)
self.assertIsNone(wayRelation._speed_limit)
bearing_rad = 180.
wayRelation.update(location_rad, bearing_rad, 10.)
self.assertTrue(wayRelation.is_location_in_bbox(location_rad))
self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD)
self.assertEqual(wayRelation.ahead_idx, 16)
self.assertEqual(wayRelation.behind_idx, 17)
self.assertAlmostEqual(wayRelation._distance_to_way, 3.43290781621360)
self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.9507682562504284)
self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 11.11623371145368)
self.assertTrue(wayRelation.active)
self.assertFalse(wayRelation.diverting)
assert_array_almost_equal(wayRelation.location_rad, location_rad)
self.assertEqual(wayRelation.bearing_rad, bearing_rad)
self.assertIsNone(wayRelation._speed_limit)
def test_way_relation_updates_with_location_closest_to_way_when_multiple_possible(self):
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
location_rad = np.radians(np.array([52.313303275461564, 13.437729236325788]))
bearing_rad = np.radians(10.)
wayRelation.update(location_rad, bearing_rad, 10.)
self.assertTrue(wayRelation.is_location_in_bbox(location_rad))
self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD)
self.assertEqual(wayRelation.ahead_idx, 26)
self.assertEqual(wayRelation.behind_idx, 27)
self.assertAlmostEqual(wayRelation._distance_to_way, 10.151775235257011)
self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.06371131069242782)
self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 10.174073707120915)
self.assertTrue(wayRelation.active)
self.assertFalse(wayRelation.diverting)
assert_array_almost_equal(wayRelation.location_rad, location_rad)
self.assertEqual(wayRelation.bearing_rad, bearing_rad)
self.assertIsNone(wayRelation._speed_limit)
def test_way_relation_updates_will_become_inactive_if_too_far_from_way(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
# Location is 24.9 mts away from the way. There are 2 Lanes in this way.
location_rad = np.radians(np.array([52.328634560607746, 13.445609877522788]))
location_stdev = 5.5 # threshold is 4 * location_stdev + LANE_WIDTH
distance_threshold = 4. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2.
wayRelation.update(location_rad, 0., location_stdev)
self.assertTrue(wayRelation.active)
self.assertLess(wayRelation._distance_to_way, distance_threshold)
location_stdev = 5.
wayRelation.update(location_rad, 0., location_stdev)
self.assertFalse(wayRelation.active)
def test_way_relation_updates_will_update_diverting_correctly(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
# Location is 24.9 mts away from the way. There are 2 Lanes in this way.
location_rad = np.radians(np.array([52.328634560607746, 13.445609877522788]))
location_stdev = 11.
distance_threshold = 2. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2.
wayRelation.update(location_rad, 0., location_stdev)
self.assertLess(wayRelation._distance_to_way, distance_threshold)
self.assertFalse(wayRelation.diverting)
location_stdev = 10.
distance_threshold = 2. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2.
wayRelation.update(location_rad, 0., location_stdev)
self.assertGreater(wayRelation._distance_to_way, distance_threshold)
self.assertTrue(wayRelation.diverting)
def test_way_relation_update_direction_from_starting_node_resets_speed_limit(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
wayRelation._speed_limit = 10.
wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[0].id)
self.assertIsNone(wayRelation._speed_limit)
def test_way_relation_update_direction_from_starting_node_updates_correctly(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[0].id)
self.assertEqual(wayRelation.direction, DIRECTION.FORWARD)
wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[-1].id)
self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD)
wayRelation.update_direction_from_starting_node(0)
self.assertEqual(wayRelation.direction, DIRECTION.NONE)
def test_way_relation_is_location_in_bbox(self):
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
bbox = wayRelation.bbox
loc_avg = np.average(bbox, axis=0)
loc_min = np.min(bbox, axis=0)
loc_max = np.max(bbox, axis=0)
locations = [
loc_avg,
loc_min,
loc_max,
[loc_avg[0], loc_min[1]],
[loc_avg[0], loc_max[1]],
[loc_min[0], loc_avg[1]],
[loc_max[0], loc_avg[1]],
loc_min - 0.1,
loc_max + 0.1,
[loc_avg[0], loc_min[1] - 0.1],
[loc_avg[0], loc_max[1] + 0.1],
[loc_min[0] - 0.1, loc_avg[1]],
[loc_max[0] + 0.1, loc_avg[1]],
]
is_in = [wayRelation.is_location_in_bbox(loc) for loc in locations]
self.assertEqual(is_in, [True, True, True, True, True, True, True, False, False, False, False, False, False])
def test_way_relation_speed_limit_when_set(self):
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
wayRelation._speed_limit = 10.
self.assertEqual(wayRelation.speed_limit, 10.)
@mock.patch('selfdrive.mapd.lib.WayRelation.dt')
def test_way_relation_speed_limit_conditional(self, mock_dt):
tz = timezone(timedelta(hours=1), 'berlin')
wed_10_10_am = dt(2021, 9, 1, 10, 10, 0)
mock_dt.now.return_value = wed_10_10_am
mock_dt.tzinfo = tz
mock_dt.combine = dt.combine
mock_dt.strptime = dt.strptime
# Reset all tags before teting
mockOSMWay_01_02_Loop.tags = {}
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
# No Value
self.assertEqual(wayRelation.speed_limit, 0.)
# Value on both directions
wayRelation._speed_limit = None
wayRelation.way.tags["maxspeed:conditional"] = "100 @ (We 10:00-10:30)"
self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
# Value on forward
wayRelation.way.tags.pop("maxspeed:conditional")
wayRelation._speed_limit = None
wayRelation.direction = DIRECTION.FORWARD
self.assertEqual(wayRelation.speed_limit, 0.)
wayRelation._speed_limit = None
wayRelation.way.tags["maxspeed:forward:conditional"] = "100 @ (We 10:00-10:30)"
self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
# Value on backward
wayRelation._speed_limit = None
wayRelation.direction = DIRECTION.BACKWARD
self.assertEqual(wayRelation.speed_limit, 0.)
wayRelation._speed_limit = None
wayRelation.way.tags["maxspeed:backward:conditional"] = "100 @ (We 10:00-10:30)"
self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
def test_way_relation_speed_limit_maxspeed(self):
# Reset all tags before teting
mockOSMWay_01_02_Loop.tags = {}
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
# No Value
self.assertEqual(wayRelation.speed_limit, 0.)
# Value on both directions
wayRelation._speed_limit = None
wayRelation.way.tags["maxspeed"] = "100"
self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
# Value on forward
wayRelation.way.tags.pop("maxspeed")
wayRelation._speed_limit = None
wayRelation.direction = DIRECTION.FORWARD
self.assertEqual(wayRelation.speed_limit, 0.)
wayRelation._speed_limit = None
wayRelation.way.tags["maxspeed:forward"] = "100"
self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
# Value on backward
wayRelation._speed_limit = None
wayRelation.direction = DIRECTION.BACKWARD
self.assertEqual(wayRelation.speed_limit, 0.)
wayRelation._speed_limit = None
wayRelation.way.tags["maxspeed:backward"] = "100"
self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
def test_way_relation_active_bearing_delta_reflects_internal_value(self):
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
wayRelation._active_bearing_delta = 10.
self.assertEqual(wayRelation.active_bearing_delta, 10.)
def test_way_relation_is_one_way(self):
# Setup initial tags
mockOSMWay_01_02_Loop.tags = {
'oneway': 'yes',
'highway': 'unclassified'
}
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
# oneway = yes
self.assertTrue(wayRelation.is_one_way)
# oneway non existing
wayRelation._one_way = None
self.assertFalse(wayRelation.is_one_way)
# highway = motorway
wayRelation.highway_type = 'motorway'
self.assertTrue(wayRelation.is_one_way)
def test_way_relation_is_prohibited(self):
# Setup initial tags
mockOSMWay_01_02_Loop.tags = {
'oneway': 'yes'
}
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
# Direction undefined
wayRelation.direction = DIRECTION.NONE
self.assertTrue(wayRelation.is_prohibited)
# oneway = yes
wayRelation.direction = DIRECTION.BACKWARD
self.assertTrue(wayRelation.is_prohibited)
wayRelation.direction = DIRECTION.FORWARD
self.assertFalse(wayRelation.is_prohibited)
# oneway non existing
wayRelation._one_way = None
self.assertFalse(wayRelation.is_one_way)
wayRelation.direction = DIRECTION.BACKWARD
self.assertFalse(wayRelation.is_prohibited)
def test_way_relation_distance_to_way_reflects_internal_value(self):
wayRelation = WayRelation(mockOSMWay_01_02_Loop)
wayRelation._distance_to_way = 10.
self.assertEqual(wayRelation.distance_to_way, 10.)
def test_way_relation_node_ahead(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
# ahead_ids is None on init
self.assertIsNone(wayRelation.node_ahead)
wayRelation.ahead_idx = 15
self.assertEqual(wayRelation.node_ahead, wayRelation.way.nodes[15])
def test_way_relation_last_node(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
# direction is NONE on init
self.assertIsNone(wayRelation.last_node)
# forward
wayRelation.direction = DIRECTION.FORWARD
self.assertEqual(wayRelation.last_node, wayRelation.way.nodes[-1])
# backward
wayRelation.direction = DIRECTION.BACKWARD
self.assertEqual(wayRelation.last_node, wayRelation.way.nodes[0])
def test_way_relation_last_node_coordinates(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
# direction is NONE on init
self.assertIsNone(wayRelation.last_node_coordinates)
# forward
wayRelation.direction = DIRECTION.FORWARD
coords = np.radians(np.array([wayRelation.way.nodes[-1].lat, wayRelation.way.nodes[-1].lon], dtype=float))
assert_array_almost_equal(wayRelation.last_node_coordinates, coords)
# backward
wayRelation.direction = DIRECTION.BACKWARD
coords = np.radians(np.array([wayRelation.way.nodes[0].lat, wayRelation.way.nodes[0].lon], dtype=float))
assert_array_almost_equal(wayRelation.last_node_coordinates, coords)
def test_way_relation_node_before_edge_coordinates(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
coords = wayRelation.node_before_edge_coordinates(0)
assert_array_almost_equal(coords, np.array([0., 0.]))
coords = wayRelation.node_before_edge_coordinates(wayRelation.way.nodes[0].id)
coords_e = np.radians(np.array([wayRelation.way.nodes[1].lat, wayRelation.way.nodes[1].lon], dtype=float))
assert_array_almost_equal(coords, coords_e)
coords = wayRelation.node_before_edge_coordinates(wayRelation.way.nodes[-1].id)
coords_e = np.radians(np.array([wayRelation.way.nodes[-2].lat, wayRelation.way.nodes[-2].lon], dtype=float))
assert_array_almost_equal(coords, coords_e)
def test_way_relation_split_no_matching_node(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
wrs = wayRelation.split(0)
self.assertEqual(len(wrs), 0)
def test_way_relation_split_use_correct_ids(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
wrs = wayRelation.split(wayRelation._nodes_ids[5], [-100, -200])
self.assertEqual(wrs[0].id, -100)
self.assertEqual(wrs[1].id, -200)
def test_way_relation_split_on_edge_node(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
edge_node_ids = wayRelation.edge_nodes_ids
for edge_node_id in edge_node_ids:
wrs = wayRelation.split(edge_node_id)
self.assertEqual(len(wrs), 1)
self.assertEqual(wrs[0], wayRelation)
self.assertEqual(wrs[0].way.tags, wayRelation.way.tags)
def test_way_relation_split_on_internal_node(self):
wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
way_ids = [-10, -20]
for idx, node_id in enumerate(wayRelation._nodes_ids):
if idx == 0 or idx == len(wayRelation._nodes_ids) - 1:
continue
wrs = wayRelation.split(node_id, way_ids)
self.assertEqual(len(wrs), 2)
assert_array_almost_equal(wrs[0]._nodes_ids, wayRelation._nodes_ids[:idx + 1])
assert_array_almost_equal(wrs[1]._nodes_ids, wayRelation._nodes_ids[idx:])
self.assertIn(node_id, wrs[0].edge_nodes_ids)
self.assertIn(node_id, wrs[1].edge_nodes_ids)
self.assertEqual(wrs[0].way.tags, wayRelation.way.tags)
self.assertEqual(wrs[1].way.tags, wayRelation.way.tags)
self.assertEqual(way_ids, [wr.id for wr in wrs])
# Helpers
def make_wayRelation_location_dirty(self, wayRelation):
wayRelation.distance_to_node_ahead = 10.
wayRelation.location_rad = 0.8
wayRelation.bearing_rad = 2.
wayRelation.active = True
wayRelation.diverting = True
wayRelation.ahead_idx = 5
wayRelation.behind_idx = 4
wayRelation._active_bearing_delta = 3.
wayRelation._distance_to_way = 20.
def assert_wayRelation_variables_reset(self, wayRelation):
self.assertEqual(wayRelation.distance_to_node_ahead, 0.)
self.assertIsNone(wayRelation.location_rad)
self.assertIsNone(wayRelation.bearing_rad)
self.assertFalse(wayRelation.active)
self.assertFalse(wayRelation.diverting)
self.assertIsNone(wayRelation.ahead_idx)
self.assertIsNone(wayRelation.behind_idx)
self.assertIsNone(wayRelation._active_bearing_delta)
self.assertIsNone(wayRelation._distance_to_way)
def wayRelation_mid_point_rad(self, wayRelation):
return np.average(wayRelation.bbox, axis=0)
selfdrive/mapd/test/test_WayRelationIndex.py
+74
View File
@@ -0,0 +1,74 @@
import unittest
from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex
from selfdrive.mapd.test.mock_data import mockWayCollection01
class TestWayRelationIndex(unittest.TestCase):
def test_init_and_add(self):
wrs = mockWayCollection01.way_relations
wr_index = WayRelationIndex(wrs)
# exptected init logic, including add logic.
edge_nodes_index_dict = {}
full_nodes_index_dict = {}
for wr in wrs:
for node in wr.way.nodes:
node_id = node.id
full_nodes_index_dict[node_id] = full_nodes_index_dict.get(node_id, []) + [wr]
if node_id in wr.edge_nodes_ids:
edge_nodes_index_dict[node_id] = edge_nodes_index_dict.get(node_id, []) + [wr]
# assert logic delivers same result
self.assertDictEqual(edge_nodes_index_dict, wr_index._edge_nodes_index_dict)
self.assertDictEqual(full_nodes_index_dict, wr_index._full_nodes_index_dict)
self.assertEqual(len(wr_index._edge_nodes_index_dict), 586)
self.assertEqual(len(wr_index._full_nodes_index_dict), 2342)
def test_remove(self):
wrs = mockWayCollection01.way_relations
wr_index = WayRelationIndex(wrs)
wr_to_remove = wrs[0]
affected_full_node_ids = [nd.id for nd in wr_to_remove.way.nodes]
affected_edge_node_ids = wr_to_remove.edge_nodes_ids
initial_full_lists = [wr_index._full_nodes_index_dict[ndid] for ndid in affected_full_node_ids]
initial_edge_lists = [wr_index._edge_nodes_index_dict[ndid] for ndid in affected_edge_node_ids]
expected_final_full_lists = [[wr for wr in li if wr is not wr_to_remove] for li in initial_full_lists]
expected_final_edge_lists = [[wr for wr in li if wr is not wr_to_remove] for li in initial_edge_lists]
wr_index.remove(wr_to_remove)
final_full_lists = [wr_index._full_nodes_index_dict[ndid] for ndid in affected_full_node_ids]
final_edge_lists = [wr_index._edge_nodes_index_dict[ndid] for ndid in affected_edge_node_ids]
for idx, li in enumerate(final_full_lists):
self.assertListEqual(li, expected_final_full_lists[idx])
for idx, li in enumerate(final_edge_lists):
self.assertListEqual(li, expected_final_edge_lists[idx])
def test_way_relations_with_edge_node_id(self):
wr_index = WayRelationIndex([])
ref_dict = {
0: ["fake_wr1", "fake_wr2"],
1: ["fake_wr3"],
3: ["fake_wr4", "fake_wr5", "fake_wr6"],
}
wr_index._edge_nodes_index_dict = ref_dict
for key, li in ref_dict.items():
self.assertListEqual(li, wr_index.way_relations_with_edge_node_id(key))
def test_way_relations_with_node_id(self):
wr_index = WayRelationIndex([])
ref_dict = {
0: ["fake_wr1", "fake_wr2"],
1: ["fake_wr3"],
3: ["fake_wr4", "fake_wr5", "fake_wr6"],
}
wr_index._full_nodes_index_dict = ref_dict
for key, li in ref_dict.items():
self.assertListEqual(li, wr_index.way_relations_with_node_id(key))
selfdrive/mapd/test/test_geo.py
+234
View File
@@ -0,0 +1,234 @@
import unittest
from selfdrive.mapd.lib.geo import vectors, ref_vectors, bearing_to_points, distance_to_points
import numpy as np
from numpy.testing import assert_array_almost_equal
from selfdrive.mapd.test.mock_data import mockNodesData01
class TestMapsdGeoLibrary(unittest.TestCase):
def test_vectors(self):
points = mockNodesData01.radians
expected = np.array([
[-1.34011951e-05, 1.00776468e-05],
[-5.83610920e-06, 4.41046897e-06],
[-7.83348567e-06, 5.94114032e-06],
[-7.08560788e-06, 5.30408795e-06],
[-6.57632550e-06, 4.05791838e-06],
[-1.16077872e-06, 6.91151252e-07],
[-1.53178098e-05, 9.62215139e-06],
[-5.76314175e-06, 3.55176643e-06],
[-1.61124141e-05, 9.86127759e-06],
[-1.48006628e-05, 8.58192512e-06],
[-1.72237209e-06, 1.60570482e-06],
[-8.68985228e-06, 9.22062311e-06],
[-1.42922812e-06, 1.51494711e-06],
[-3.39761486e-06, 2.57087743e-06],
[-2.75467373e-06, 1.28631255e-06],
[-1.57501989e-05, 5.72309451e-06],
[-2.52143954e-06, 1.34565295e-06],
[-1.65278643e-06, 1.28630942e-06],
[-2.22196114e-05, 1.64360838e-05],
[-5.88675934e-06, 4.08234746e-06],
[-1.83673390e-06, 1.46782408e-06],
[-1.55004206e-06, 1.51843800e-06],
[-1.20451533e-06, 2.06298011e-06],
[-1.91801338e-06, 4.64083285e-06],
[-2.38653483e-06, 5.60076524e-06],
[-1.65269781e-06, 5.78402290e-06],
[-3.66908309e-07, 2.75412965e-06],
[0.00000000e+00, 1.92858882e-06],
[9.09242615e-08, 2.66162711e-06],
[3.14490354e-07, 1.53065382e-06],
[8.66452477e-08, 4.83456208e-07],
[2.41750593e-07, 1.10828411e-06],
[7.43745228e-06, 1.27618831e-05],
[5.59968054e-06, 9.63947367e-06],
[2.01951467e-06, 2.75413219e-06],
[4.59952643e-07, 6.42281301e-07],
[1.74353749e-06, 1.74533121e-06],
[2.57144338e-06, 2.11185266e-06],
[1.46893187e-05, 1.11999169e-05],
[3.84659229e-05, 2.85527952e-05],
[2.71627936e-05, 1.98727946e-05],
[8.44632540e-06, 6.15058628e-06],
[2.29420323e-06, 1.92859222e-06],
[2.58083439e-06, 3.16952222e-06],
[3.76373643e-06, 5.14174911e-06],
[5.32416098e-06, 6.51707770e-06],
[8.62890928e-06, 1.11998258e-05],
[1.25762497e-05, 1.65231340e-05],
[8.90452991e-06, 1.10148240e-05],
[4.86505726e-06, 4.59023120e-06],
[3.85545276e-06, 3.39642031e-06],
[3.48753893e-06, 3.30566145e-06],
[2.99557303e-06, 2.61276368e-06],
[2.15496788e-06, 1.87797727e-06],
[4.10564937e-06, 3.58142649e-06],
[1.53680853e-06, 1.33866906e-06],
[4.99540175e-06, 4.35635790e-06],
[1.37744970e-06, 1.19380643e-06],
[1.74319821e-06, 1.28456429e-06],
[9.99931238e-07, 1.14493663e-06],
[6.42735560e-07, 1.19380547e-06],
[3.66818436e-07, 1.46782199e-06],
[5.45413874e-08, 1.83783170e-06],
[-1.35818548e-07, 1.14842666e-06],
[-5.50758101e-07, 3.02989178e-06],
[-4.58785270e-07, 2.66162724e-06],
[-2.51315555e-07, 1.19031459e-06],
[-3.91409773e-07, 1.65457223e-06],
[-2.14525206e-06, 5.67755902e-06],
[-4.24558096e-07, 1.39102753e-06],
[-1.46936730e-06, 5.32325561e-06],
[-1.37632061e-06, 4.59021715e-06],
[-8.26642899e-07, 4.68097349e-06],
[-6.42702724e-07, 4.95673534e-06],
[-3.66796960e-07, 7.25009780e-06],
[-1.82861669e-07, 8.99542699e-06],
[4.09564134e-07, 6.11214315e-06],
[7.80629912e-08, 1.45734993e-06],
[4.81205526e-07, 7.56076647e-06],
[2.01036346e-07, 2.42775302e-06]])
v = vectors(points)
assert_array_almost_equal(v, expected)
def test_ref_vectors(self):
points = mockNodesData01.radians
expected = np.array([
[1.59924145e-04, -1.07153714e-04],
[1.46520873e-04, -9.70788297e-05],
[1.40683931e-04, -9.26694631e-05],
[1.32849368e-04, -8.67297434e-05],
[1.25762852e-04, -8.14268689e-05],
[1.19185869e-04, -7.73700167e-05],
[1.18024984e-04, -7.66790438e-05],
[1.02705711e-04, -6.70592230e-05],
[9.69420991e-05, -6.35082196e-05],
[8.08284530e-05, -5.36489556e-05],
[6.60268961e-05, -4.50685727e-05],
[6.43043874e-05, -4.34630144e-05],
[5.56137708e-05, -3.42431117e-05],
[5.41844341e-05, -3.27282671e-05],
[5.07866397e-05, -3.01576270e-05],
[4.80318817e-05, -2.88714948e-05],
[3.22813286e-05, -2.31493755e-05],
[2.97598330e-05, -2.18038275e-05],
[2.81069973e-05, -2.05175815e-05],
[5.88679032e-06, -4.08230278e-06],
[0.00000000e+00, 0.00000000e+00],
[-1.83673390e-06, 1.46782408e-06],
[-3.38677236e-06, 2.98626574e-06],
[-4.59127869e-06, 5.04925111e-06],
[-6.50926460e-06, 9.69009532e-06],
[-8.89575243e-06, 1.52908806e-05],
[-1.05483839e-05, 2.10749224e-05],
[-1.09152548e-05, 2.38290571e-05],
[-1.09152276e-05, 2.57576459e-05],
[-1.08242659e-05, 2.84192717e-05],
[-1.05097542e-05, 2.99499212e-05],
[-1.04231024e-05, 3.04333762e-05],
[-1.01813369e-05, 3.15416571e-05],
[-2.74371711e-06, 4.43034426e-05],
[2.85599752e-06, 5.39428964e-05],
[4.87550206e-06, 5.66970360e-05],
[5.33545066e-06, 5.73393202e-05],
[7.07897615e-06, 5.90846634e-05],
[9.65040026e-06, 6.11965396e-05],
[2.43395796e-05, 7.23966392e-05],
[6.28046063e-05, 1.00950641e-04],
[8.99657904e-05, 1.20825635e-04],
[9.84114021e-05, 1.26977201e-04],
[1.00705361e-04, 1.28906084e-04],
[1.03285783e-04, 1.32075942e-04],
[1.07048835e-04, 1.37218192e-04],
[1.12372096e-04, 1.43736004e-04],
[1.20999382e-04, 1.54937080e-04],
[1.33573053e-04, 1.71462176e-04],
[1.42475686e-04, 1.82478533e-04],
[1.47339899e-04, 1.87069658e-04],
[1.51194707e-04, 1.90466811e-04],
[1.54681601e-04, 1.93773152e-04],
[1.57676653e-04, 1.96386513e-04],
[1.59831239e-04, 1.98264929e-04],
[1.63936150e-04, 2.01847201e-04],
[1.65472675e-04, 2.03186195e-04],
[1.70467147e-04, 2.07543619e-04],
[1.71844334e-04, 2.08737728e-04],
[1.73587247e-04, 2.10022678e-04],
[1.74586922e-04, 2.11167839e-04],
[1.75229389e-04, 2.12361789e-04],
[1.75595876e-04, 2.13829694e-04],
[1.75650001e-04, 2.15667538e-04],
[1.75513922e-04, 2.16815933e-04],
[1.74962478e-04, 2.19845700e-04],
[1.74503092e-04, 2.22507224e-04],
[1.74251509e-04, 2.23697482e-04],
[1.73859727e-04, 2.25351966e-04],
[1.71713202e-04, 2.31029044e-04],
[1.71288336e-04, 2.32419977e-04],
[1.69817793e-04, 2.37742908e-04],
[1.68440467e-04, 2.42332824e-04],
[1.67612807e-04, 2.47013617e-04],
[1.66969033e-04, 2.51970213e-04],
[1.66600674e-04, 2.59220232e-04],
[1.66415880e-04, 2.68215619e-04],
[1.66824132e-04, 2.74327850e-04],
[1.66901881e-04, 2.75785216e-04],
[1.67381459e-04, 2.83346086e-04],
[1.67581971e-04, 2.85773882e-04]])
v = ref_vectors(points[20], points)
assert_array_almost_equal(v, expected)
def test_bearing_to_points(self):
points = mockNodesData01.radians
expected = np.array([
2.16112265, 2.15595027, 2.15326799, 2.14916735, 2.14538642,
2.14657678, 2.14694997, 2.1492257, 2.1507589, 2.15676899,
2.16973441, 2.1651606, 2.12270237, 2.11416356, 2.10665211,
2.11201708, 2.19291574, 2.2031069, 2.20136186, 2.17712517,
0., -0.8965745, -0.84815954, -0.73792895, -0.59150953,
-0.5269061, -0.46406215, -0.42954043, -0.4008254, -0.36391371,
-0.33748609, -0.32996807, -0.31223189, -0.06185112, 0.05289544,
0.08578116, 0.0927833, 0.11924233, 0.15640718, 0.32432622,
0.55653415, 0.64003094, 0.6593301, 0.66319086, 0.66367982,
0.66251077, 0.66354137, 0.66302176, 0.66181884, 0.66291139,
0.66714676, 0.67095594, 0.67367984, 0.6765003, 0.67847961,
0.68212344, 0.68345356, 0.68762778, 0.68876073, 0.69070183,
0.69085143, 0.68988665, 0.68753177, 0.68348884, 0.68051081,
0.67220053, 0.66506824, 0.66177969, 0.65712162, 0.63916951,
0.6351146, 0.62025347, 0.60741567, 0.59618923, 0.58521935,
0.57122582, 0.55532475, 0.54636839, 0.54422542, 0.53357655,
0.53037033])
v = bearing_to_points(points[20], points)
assert_array_almost_equal(v, expected)
def test_distance_to_points(self):
points = mockNodesData01.radians
expected = np.array([
1226.82569068, 1120.13820773, 1073.61121415, 1011.10016574,
954.81557436, 905.58045038, 896.97734399, 781.7102819,
738.58271117, 618.26145463, 509.47052142, 494.6403804,
416.22483123, 403.42108699, 376.42615499, 357.15106681,
253.15957483, 235.11572972, 221.77439728, 45.65465979,
0., 14.98414, 28.77606056, 43.49299446,
74.39463425, 112.74005248, 150.19482607, 167.03665191,
178.28443483, 193.80834084, 202.28154097, 205.01173833,
211.22777104, 282.88676739, 344.25957352, 362.66370657,
367.00206795, 379.23951996, 394.82505328, 486.76073331,
757.70254732, 960.03439155, 1023.81434529, 1042.49401713,
1068.53770096, 1109.12696535, 1162.74555108, 1252.847351,
1385.17179405, 1475.42502599, 1517.57849916, 1549.79838056,
1580.12405964, 1605.05483058, 1622.98937809, 1657.19268821,
1669.99157205, 1711.63883132, 1723.09133393, 1736.47655688,
1746.16073119, 1754.63481838, 1763.34186103, 1772.62691273,
1777.76189094, 1790.62024447, 1802.11488235, 1807.1040605,
1813.90756815, 1834.49265566, 1840.00708445, 1861.96087374,
1880.81678093, 1902.42091191, 1926.37194131, 1963.78301115,
2011.62679077, 2046.18028824, 2054.37811294, 2097.30347724,
2111.28586072])
v = distance_to_points(points[20], points)
assert_array_almost_equal(v, expected)
selfdrive/thermald/thermald.py
-3
View File
@@ -321,9 +321,6 @@ def thermald_thread(end_event, hw_queue):
should_start = should_start and all(startup_conditions.values())
if should_start != should_start_prev or (count == 0):
params.put_bool("IsOnroad", should_start)
params.put_bool("IsOffroad", not should_start)
params.put_bool("IsEngaged", False)
engaged_prev = False
HARDWARE.set_power_save(not should_start)
selfdrive/tombstoned.py
+1 -1
View File
@@ -10,7 +10,7 @@ import glob
from typing import NoReturn
from common.file_helpers import mkdirs_exists_ok
from system.loggerd.config import ROOT
from selfdrive.loggerd.config import ROOT
import selfdrive.sentry as sentry
from system.swaglog import cloudlog
from system.version import get_commit
selfdrive/ui/_ui
BIN
View File
Binary file not shown.
selfdrive/ui/qt/libpython_helpers.so
BIN
View File
Binary file not shown.
selfdrive/ui/qt/spinner
BIN
View File
Binary file not shown.
selfdrive/ui/qt/text
BIN
View File
Binary file not shown.
selfdrive/ui/soundd/_soundd
BIN
View File
Binary file not shown.
selfdrive/ui/translations/main_de.ts
+8
View File
@@ -265,6 +265,14 @@
<source>Review</source>
<translation>Überprüfen</translation>
</message>
<message>
<source>Debug Console</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Error displaying tmux output.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DriveStats</name>
selfdrive/ui/translations/main_ja.ts
+8
View File
@@ -265,6 +265,14 @@
<source>Review</source>
<translation>確認</translation>
</message>
<message>
<source>Debug Console</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Error displaying tmux output.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DriveStats</name>
selfdrive/ui/translations/main_ko.ts
+8
View File
@@ -265,6 +265,14 @@
<source>Review</source>
<translation>다시보기</translation>
</message>
<message>
<source>Debug Console</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Error displaying tmux output.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DriveStats</name>
selfdrive/ui/translations/main_pt-BR.ts
+8
View File
@@ -265,6 +265,14 @@
<source>Review</source>
<translation>Revisar</translation>
</message>
<message>
<source>Debug Console</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Error displaying tmux output.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DriveStats</name>
selfdrive/ui/translations/main_zh-CHS.ts
+8
View File
@@ -265,6 +265,14 @@
<source>Review</source>
<translation>预览</translation>
</message>
<message>
<source>Debug Console</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Error displaying tmux output.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DriveStats</name>
selfdrive/ui/translations/main_zh-CHT.qm
BIN
View File
Binary file not shown.
selfdrive/ui/translations/main_zh-CHT.ts
+12 -4
View File
@@ -94,19 +94,19 @@
<name>C2NetworkPanel</name>
<message>
<source>Wi-Fi Settings</source>
<translation type="unfinished"></translation>
<translation>無線網路設置</translation>
</message>
<message>
<source>OPEN</source>
<translation type="unfinished"></translation>
<translation>開啟</translation>
</message>
<message>
<source>Tethering Settings</source>
<translation type="unfinished"></translation>
<translation>熱點設置</translation>
</message>
<message>
<source>IP Address</source>
<translation type="unfinished">IP 地址</translation>
<translation>IP 地址</translation>
</message>
</context>
<context>
@@ -265,6 +265,14 @@
<source>Review</source>
<translation>回顧</translation>
</message>
<message>
<source>Debug Console</source>
<translation>除錯視窗</translation>
</message>
<message>
<source>Error displaying tmux output.</source>
<translation>顯示 tmux 輸出時發生錯誤。</translation>
</message>
</context>
<context>
<name>DriveStats</name>
system/clocksd/clocksd
BIN
View File
Binary file not shown.
system/hardware/base.h
+4
View File
@@ -18,6 +18,10 @@ public:
static std::string get_serial() { return "cccccc"; }
static std::map<std::string, std::string> get_init_logs() {
return {};
}
static void reboot() {}
static void poweroff() {}
static void set_brightness(int percent) {}

Some files were not shown because too many files have changed in this diff Show More