diff --git a/.gitignore b/.gitignore index 3fa3c4995..2585c1085 100644 --- a/.gitignore +++ b/.gitignore @@ -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~ diff --git a/RELEASES.md b/RELEASES.md index ede5e530c..7dd53cff8 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -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! diff --git a/cereal/car.capnp b/cereal/car.capnp index d7b90d873..377afbea9 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -114,6 +114,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { controlsdLagging @112; resumeBlocked @113; steerTimeLimit @115; + vehicleSensorsInvalid @116; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; diff --git a/cereal/dp.capnp b/cereal/dp.capnp new file mode 100644 index 000000000..42a50398f --- /dev/null +++ b/cereal/dp.capnp @@ -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; +} diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so index 0b148e871..5224fa634 100755 Binary files a/cereal/libcereal_shared.so and b/cereal/libcereal_shared.so differ diff --git a/cereal/log.capnp b/cereal/log.capnp index 327875797..b53ddf869 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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; diff --git a/cereal/messaging/bridge b/cereal/messaging/bridge index fc8247bf8..26e9e14f5 100755 Binary files a/cereal/messaging/bridge and b/cereal/messaging/bridge differ diff --git a/cereal/messaging/messaging_pyx.so b/cereal/messaging/messaging_pyx.so index aa6e1985e..561f7886d 100755 Binary files a/cereal/messaging/messaging_pyx.so and b/cereal/messaging/messaging_pyx.so differ diff --git a/cereal/services.h b/cereal/services.h index 6d7f371e9..34f970bbc 100644 --- a/cereal/services.h +++ b/cereal/services.h @@ -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 diff --git a/cereal/services.py b/cereal/services.py index 4b7f84a79..eb40d3de1 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -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())} diff --git a/cereal/visionipc/visionipc_pyx.so b/cereal/visionipc/visionipc_pyx.so index 5a376ecba..0baece6c4 100755 Binary files a/cereal/visionipc/visionipc_pyx.so and b/cereal/visionipc/visionipc_pyx.so differ diff --git a/common/params.h b/common/params.h index aecb3ee47..24b1bffeb 100644 --- a/common/params.h +++ b/common/params.h @@ -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 }; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 9d8933609..abb3199d0 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -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": diff --git a/common/params_pyx.so b/common/params_pyx.so index 6100c58e7..883cca828 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/common/version.h b/common/version.h index 3625e8686..5f8b29e7a 100644 --- a/common/version.h +++ b/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "2023.05.09" +#define COMMA_VERSION "2023.05.15" diff --git a/docs/CARS.md b/docs/CARS.md index 2febdd53b..f0ad95853 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -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)](##)|Ford Q3|| |Ford|Escape 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|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)](##)|Ford Q3|| -|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)](##)|Ford Q3|| |Ford|Explorer 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|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)](##)|Ford Q3|| |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)](##)|Ford Q3|| -|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)](##)|Ford Q3|| -|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)](##)|Ford Q3|| |Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| @@ -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[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| |Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| |Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Genesis|GV70 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| +|Genesis|GV70 (2.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| +|Genesis|GV70 (3.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai M|| |Genesis|GV80 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai M|| |GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| |GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| @@ -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)](##)|Toyota|| |Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Lincoln|Aviator 2021|Co-Pilot360 Plus|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|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)](##)|Ford Q3|| |MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 115e4fcdd..611719911 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -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 diff --git a/opendbc/can/libdbc.so b/opendbc/can/libdbc.so index 1772591ca..0a513f0b2 100755 Binary files a/opendbc/can/libdbc.so and b/opendbc/can/libdbc.so differ diff --git a/opendbc/can/parser_pyx.so b/opendbc/can/parser_pyx.so index 09b524d18..ed574ec4b 100755 Binary files a/opendbc/can/parser_pyx.so and b/opendbc/can/parser_pyx.so differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index 72dbc26b4..14ae3cd9a 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index a6196c220..90a12d8db 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/rednose/helpers/ekf_sym_pyx.so b/rednose/helpers/ekf_sym_pyx.so index 7a728eb32..53121e8e6 100755 Binary files a/rednose/helpers/ekf_sym_pyx.so and b/rednose/helpers/ekf_sym_pyx.so differ diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 245242336..0ac0437cb 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -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 diff --git a/selfdrive/boardd/boardd b/selfdrive/boardd/boardd index 2af4efc94..b2f817468 100755 Binary files a/selfdrive/boardd/boardd and b/selfdrive/boardd/boardd differ diff --git a/selfdrive/boardd/boardd_api_impl.so b/selfdrive/boardd/boardd_api_impl.so index dac8384d5..8307c7902 100755 Binary files a/selfdrive/boardd/boardd_api_impl.so and b/selfdrive/boardd/boardd_api_impl.so differ diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 23c31f856..83a674a32 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -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): diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index 6217561bd..c389ea488 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -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() diff --git a/selfdrive/camerad/camerad b/selfdrive/camerad/camerad index 80864b7cd..044e50ff6 100755 Binary files a/selfdrive/camerad/camerad and b/selfdrive/camerad/camerad differ diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 9b8e96622..1281b65d1 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -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 diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 81e08c367..f35a2d81a 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -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) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 01c5dd377..f74cafdd9 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -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 diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 00cbb7de0..1ec34b219 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -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"), diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 1ad8896b9..128dac1cd 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -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: { diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index a78b70608..103136135 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -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) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 3c9605399..174d7ffb0 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -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', diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 03311037d..95a5c8169 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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: diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ec9f7a347..dc3aa5803 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index c8c0057d8..c389f6354 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -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"), + }, + } diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json b/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json new file mode 100644 index 000000000..18c598000 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json @@ -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 + ] + } +} \ No newline at end of file diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.h b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.h new file mode 100644 index 000000000..c2f7a3771 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.h @@ -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_ diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h index afc630825..ef2522a8f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h @@ -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 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so index fa9889641..b510235a3 100755 Binary files a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so and b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so differ diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/make_sfun_long.m b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/make_sfun_long.m new file mode 100644 index 000000000..5675b71b7 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/make_sfun_long.m @@ -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) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 5fff2ec8d..10c447b18 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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' diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index dbcf4600a..01f866d13 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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) diff --git a/selfdrive/dragonpilot/gpx_uploader.py b/selfdrive/dragonpilot/gpx_uploader.py new file mode 100644 index 000000000..0b24eb8b0 --- /dev/null +++ b/selfdrive/dragonpilot/gpx_uploader.py @@ -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() diff --git a/selfdrive/dragonpilot/gpxd.py b/selfdrive/dragonpilot/gpxd.py new file mode 100644 index 000000000..f51877d26 --- /dev/null +++ b/selfdrive/dragonpilot/gpxd.py @@ -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 = [ + '', + '', + '', + f'{self.started_time}', + '', + ] + for trkpt in self.logs: + lines.append(self._trkpt_template(trkpt[0], trkpt[1], trkpt[2], trkpt[3])) + lines.extend([ + '', + '', + '', + ]) + 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'\n' \ + f'\n' \ + f'{alt}\n' \ + f'\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() diff --git a/selfdrive/legacy_modeld/_dmonitoringmodeld b/selfdrive/legacy_modeld/_dmonitoringmodeld index 74fb4503c..fb8692cfb 100755 Binary files a/selfdrive/legacy_modeld/_dmonitoringmodeld and b/selfdrive/legacy_modeld/_dmonitoringmodeld differ diff --git a/selfdrive/legacy_modeld/_modeld b/selfdrive/legacy_modeld/_modeld index 0584b59eb..6e5118937 100755 Binary files a/selfdrive/legacy_modeld/_modeld and b/selfdrive/legacy_modeld/_modeld differ diff --git a/selfdrive/legacy_modeld/models/supercombo.thneed b/selfdrive/legacy_modeld/models/supercombo.thneed index 5b2c7c28e..cbcca8325 100644 Binary files a/selfdrive/legacy_modeld/models/supercombo.thneed and b/selfdrive/legacy_modeld/models/supercombo.thneed differ diff --git a/selfdrive/locationd/locationd b/selfdrive/locationd/locationd index f15b30356..ae87803c8 100755 Binary files a/selfdrive/locationd/locationd and b/selfdrive/locationd/locationd differ diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index 4f0d1bb0c..523dfd598 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -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); diff --git a/selfdrive/locationd/models/generated/gnss.h b/selfdrive/locationd/models/generated/gnss.h index 765c984bd..0df3c3a59 100644 --- a/selfdrive/locationd/models/generated/gnss.h +++ b/selfdrive/locationd/models/generated/gnss.h @@ -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); } \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/libkf.so b/selfdrive/locationd/models/generated/libkf.so index e631ef413..44809b59d 100755 Binary files a/selfdrive/locationd/models/generated/libkf.so and b/selfdrive/locationd/models/generated/libkf.so differ diff --git a/selfdrive/locationd/models/generated/live.h b/selfdrive/locationd/models/generated/live.h index b0f12ad9d..5ac3abad2 100644 --- a/selfdrive/locationd/models/generated/live.h +++ b/selfdrive/locationd/models/generated/live.h @@ -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); } \ No newline at end of file diff --git a/selfdrive/loggerd/bootlog b/selfdrive/loggerd/bootlog index c3513f932..d2f9bc6d7 100755 Binary files a/selfdrive/loggerd/bootlog and b/selfdrive/loggerd/bootlog differ diff --git a/selfdrive/loggerd/deleter.py b/selfdrive/loggerd/deleter.py index e7006a08c..e17d38965 100644 --- a/selfdrive/loggerd/deleter.py +++ b/selfdrive/loggerd/deleter.py @@ -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 diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 9d48cb614..9f70ebaa1 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/loggerd/tests/test_uploader.py b/selfdrive/loggerd/tests/test_uploader.py index 54c65db73..b8a22ecd6 100755 --- a/selfdrive/loggerd/tests/test_uploader.py +++ b/selfdrive/loggerd/tests/test_uploader.py @@ -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 diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 2013a522b..288917637 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -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' diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index f99ca2ab8..437b19cb9 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -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) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 13f177970..201c52bae 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -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} diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/mapd/config.py b/selfdrive/mapd/config.py new file mode 100644 index 000000000..5d122422c --- /dev/null +++ b/selfdrive/mapd/config.py @@ -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. diff --git a/selfdrive/mapd/default_speeds.json b/selfdrive/mapd/default_speeds.json new file mode 100644 index 000000000..7126c27fc --- /dev/null +++ b/selfdrive/mapd/default_speeds.json @@ -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" +} diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py new file mode 100644 index 000000000..888c9e1df --- /dev/null +++ b/selfdrive/mapd/default_speeds_generator.py @@ -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() diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py new file mode 100644 index 000000000..2f30607df --- /dev/null +++ b/selfdrive/mapd/lib/NodesData.py @@ -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 diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py new file mode 100644 index 000000000..7aa502bb2 --- /dev/null +++ b/selfdrive/mapd/lib/Route.py @@ -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 diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py new file mode 100644 index 000000000..56ad0b60d --- /dev/null +++ b/selfdrive/mapd/lib/WayCollection.py @@ -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) diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py new file mode 100644 index 000000000..7c796eb40 --- /dev/null +++ b/selfdrive/mapd/lib/WayRelation.py @@ -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 ` @ ()` 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] diff --git a/selfdrive/mapd/lib/WayRelationIndex.py b/selfdrive/mapd/lib/WayRelationIndex.py new file mode 100644 index 000000000..e941dcd5b --- /dev/null +++ b/selfdrive/mapd/lib/WayRelationIndex.py @@ -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, []) diff --git a/selfdrive/mapd/lib/default_speeds.json b/selfdrive/mapd/lib/default_speeds.json new file mode 100644 index 000000000..a8db60880 --- /dev/null +++ b/selfdrive/mapd/lib/default_speeds.json @@ -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" +} diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py new file mode 100644 index 000000000..8f6232225 --- /dev/null +++ b/selfdrive/mapd/lib/geo.py @@ -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 diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py new file mode 100644 index 000000000..04c8163bc --- /dev/null +++ b/selfdrive/mapd/lib/osm.py @@ -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 diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py new file mode 100644 index 000000000..bf552c479 --- /dev/null +++ b/selfdrive/mapd/mapd.py @@ -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() diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py new file mode 100644 index 000000000..58a33ee41 --- /dev/null +++ b/selfdrive/mapd/mapd_helpers.py @@ -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 diff --git a/selfdrive/mapd/test/__init__.py b/selfdrive/mapd/test/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/mapd/test/mock_data.py b/selfdrive/mapd/test/mock_data.py new file mode 100644 index 000000000..b7da229bd --- /dev/null +++ b/selfdrive/mapd/test/mock_data.py @@ -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) diff --git a/selfdrive/mapd/test/mock_osm_response_01.xml b/selfdrive/mapd/test/mock_osm_response_01.xml new file mode 100644 index 000000000..d1e2a219c --- /dev/null +++ b/selfdrive/mapd/test/mock_osm_response_01.xml @@ -0,0 +1,9908 @@ + + + +The data included in this document is from www.openstreetmap.org. The data is made available under ODbL. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/selfdrive/mapd/test/mock_osm_response_02.xml b/selfdrive/mapd/test/mock_osm_response_02.xml new file mode 100644 index 000000000..bbcf72f3e --- /dev/null +++ b/selfdrive/mapd/test/mock_osm_response_02.xml @@ -0,0 +1,11529 @@ + + + +The data included in this document is from www.openstreetmap.org. The data is made available under ODbL. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/selfdrive/mapd/test/test_NodesData.py b/selfdrive/mapd/test/test_NodesData.py new file mode 100644 index 000000000..915f575ed --- /dev/null +++ b/selfdrive/mapd/test/test_NodesData.py @@ -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 diff --git a/selfdrive/mapd/test/test_WayRelation.py b/selfdrive/mapd/test/test_WayRelation.py new file mode 100644 index 000000000..9719c86f0 --- /dev/null +++ b/selfdrive/mapd/test/test_WayRelation.py @@ -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) diff --git a/selfdrive/mapd/test/test_WayRelationIndex.py b/selfdrive/mapd/test/test_WayRelationIndex.py new file mode 100644 index 000000000..faa70c141 --- /dev/null +++ b/selfdrive/mapd/test/test_WayRelationIndex.py @@ -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)) diff --git a/selfdrive/mapd/test/test_geo.py b/selfdrive/mapd/test/test_geo.py new file mode 100644 index 000000000..a18fe717b --- /dev/null +++ b/selfdrive/mapd/test/test_geo.py @@ -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) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 177cd9c43..71e1ba2d3 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -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) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 65fb45b67..61a575f14 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -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 diff --git a/selfdrive/ui/_ui b/selfdrive/ui/_ui index 8e87c3f3d..78db3ac6f 100755 Binary files a/selfdrive/ui/_ui and b/selfdrive/ui/_ui differ diff --git a/selfdrive/ui/qt/libpython_helpers.so b/selfdrive/ui/qt/libpython_helpers.so index 06296d82d..7725d6783 100755 Binary files a/selfdrive/ui/qt/libpython_helpers.so and b/selfdrive/ui/qt/libpython_helpers.so differ diff --git a/selfdrive/ui/qt/spinner b/selfdrive/ui/qt/spinner index 21c2f52b7..676827763 100755 Binary files a/selfdrive/ui/qt/spinner and b/selfdrive/ui/qt/spinner differ diff --git a/selfdrive/ui/qt/text b/selfdrive/ui/qt/text index 554538ca4..8395ef833 100755 Binary files a/selfdrive/ui/qt/text and b/selfdrive/ui/qt/text differ diff --git a/selfdrive/ui/soundd/_soundd b/selfdrive/ui/soundd/_soundd index 67067d3b0..f388b2df4 100755 Binary files a/selfdrive/ui/soundd/_soundd and b/selfdrive/ui/soundd/_soundd differ diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 5431f1562..ae7b07ab1 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -265,6 +265,14 @@ Review Überprüfen + + Debug Console + + + + Error displaying tmux output. + + DriveStats diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 871af031b..f82ea6d15 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -265,6 +265,14 @@ Review 確認 + + Debug Console + + + + Error displaying tmux output. + + DriveStats diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 46b51830b..b00c4165c 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -265,6 +265,14 @@ Review 다시보기 + + Debug Console + + + + Error displaying tmux output. + + DriveStats diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 428ecfec7..bb00b1878 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -265,6 +265,14 @@ Review Revisar + + Debug Console + + + + Error displaying tmux output. + + DriveStats diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 1fe12e55d..1a0aff8f8 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -265,6 +265,14 @@ Review 预览 + + Debug Console + + + + Error displaying tmux output. + + DriveStats diff --git a/selfdrive/ui/translations/main_zh-CHT.qm b/selfdrive/ui/translations/main_zh-CHT.qm index dea825a08..2bb956324 100644 Binary files a/selfdrive/ui/translations/main_zh-CHT.qm and b/selfdrive/ui/translations/main_zh-CHT.qm differ diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 804e64a68..5f8c18116 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -94,19 +94,19 @@ C2NetworkPanel Wi-Fi Settings - + 無線網路設置 OPEN - + 開啟 Tethering Settings - + 熱點設置 IP Address - IP 地址 + IP 地址 @@ -265,6 +265,14 @@ Review 回顧 + + Debug Console + 除錯視窗 + + + Error displaying tmux output. + 顯示 tmux 輸出時發生錯誤。 + DriveStats diff --git a/system/clocksd/clocksd b/system/clocksd/clocksd index 17dcf4073..826fc55bf 100755 Binary files a/system/clocksd/clocksd and b/system/clocksd/clocksd differ diff --git a/system/hardware/base.h b/system/hardware/base.h index 6cfc1d874..546009972 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -18,6 +18,10 @@ public: static std::string get_serial() { return "cccccc"; } + static std::map get_init_logs() { + return {}; + } + static void reboot() {} static void poweroff() {} static void set_brightness(int percent) {} diff --git a/system/hardware/eon/libs/opspline.tar.gz b/system/hardware/eon/libs/opspline.tar.gz new file mode 100644 index 000000000..95bd3f1f3 Binary files /dev/null and b/system/hardware/eon/libs/opspline.tar.gz differ diff --git a/system/hardware/eon/libs/overpy.tar.gz b/system/hardware/eon/libs/overpy.tar.gz new file mode 100644 index 000000000..c2d4977c4 Binary files /dev/null and b/system/hardware/eon/libs/overpy.tar.gz differ diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index 765b53ba6..fba67d96c 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -73,6 +73,31 @@ public: std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); } + + static std::map get_init_logs() { + std::map ret = { + {"/BUILD", util::read_file("/BUILD")}, + }; + + std::string bs = util::check_output("abctl --boot_slot"); + ret["boot slot"] = bs.substr(0, bs.find_first_of("\n")); + + std::string temp = util::read_file("/dev/disk/by-partlabel/ssd"); + temp.erase(temp.find_last_not_of(std::string("\0\r\n", 3))+1); + ret["boot temp"] = temp; + + // TODO: log something from system and boot + for (std::string part : {"xbl", "abl", "aop", "devcfg", "xbl_config"}) { + for (std::string slot : {"a", "b"}) { + std::string partition = part + "_" + slot; + std::string hash = util::check_output("sha256sum /dev/disk/by-partlabel/" + partition); + ret[partition] = hash.substr(0, hash.find_first_of(" ")); + } + } + + return ret; + } + static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }; static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }; }; diff --git a/system/logcatd/logcatd b/system/logcatd/logcatd index 36b40837e..eea9a9fc9 100755 Binary files a/system/logcatd/logcatd and b/system/logcatd/logcatd differ diff --git a/system/proclogd/proclogd b/system/proclogd/proclogd index b7371c563..c5abc3534 100755 Binary files a/system/proclogd/proclogd and b/system/proclogd/proclogd differ diff --git a/system/sensord/_sensord b/system/sensord/_sensord index 3894b37d4..99b1067b7 100755 Binary files a/system/sensord/_sensord and b/system/sensord/_sensord differ diff --git a/system/ubloxd/ubloxd b/system/ubloxd/ubloxd index 0ed3c73cd..10800ddf1 100755 Binary files a/system/ubloxd/ubloxd and b/system/ubloxd/ubloxd differ