diff --git a/cereal/log.capnp b/cereal/log.capnp index 6128f788164d29..bc2d5de6f236bb 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1811,6 +1811,13 @@ struct GPSPlannerPlan { xLookahead @7 :Float32; } +struct LiveTrafficData { + speedLimitValid @0 :Bool; + speedLimit @1 :Float32; + speedAdvisoryValid @2 :Bool; + speedAdvisory @3 :Float32; +} + struct TrafficEvent @0xacfa74a094e62626 { type @0 :Type; distance @1 :Float32; @@ -2142,6 +2149,7 @@ struct Event { frontEncodeIdx @76 :EncodeIndex; # driver facing camera wideEncodeIdx @77 :EncodeIndex; dragonConf @78 :DragonConf; + liveTrafficData @79:LiveTrafficData; } } @@ -2221,4 +2229,4 @@ struct DragonConf { dpDischargingAt @72 :UInt8; dpIsUpdating @73 :Bool; dpTimebombAssist @74 :Bool; -} \ No newline at end of file +} diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index b95ce6251e635d..c0af1611f48d1a 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -67,7 +67,7 @@ frontEncodeIdx: [8061, true, 5.] # should be 20fps on tici orbFeaturesSummary: [8062, true, 0.] driverState: [8063, true, 5., 1] liveParameters: [8064, true, 20., 2] -liveMapData: [8065, true, 0.] +liveMapData: [8065, false, 0.] cameraOdometry: [8066, true, 20., 5] pathPlan: [8067, true, 20., 2] kalmanOdometry: [8068, true, 0.] @@ -87,6 +87,9 @@ testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] testJoystick: [8056, false, 0.] +liveTrafficData: [8208, false, 100.] + + # 8080 is reserved for slave testing daemon # 8762 is reserved for logserver diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index cb1753a04d18a9..af147697d3a01d 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -45,7 +45,10 @@ keys = { b"LastAthenaPingTime": [TxType.PERSISTENT], b"LastUpdateTime": [TxType.PERSISTENT], b"LastUpdateException": [TxType.PERSISTENT], + b"LimitSetSpeed": [TxType.PERSISTENT], + b"LimitSetSpeedNeural": [TxType.PERSISTENT], b"LiveParameters": [TxType.PERSISTENT], + b"LongitudinalControl": [TxType.PERSISTENT], b"OpenpilotEnabledToggle": [TxType.PERSISTENT], b"LaneChangeEnabled": [TxType.PERSISTENT], b"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], @@ -56,6 +59,7 @@ keys = { b"ReleaseNotes": [TxType.PERSISTENT], b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], b"SubscriberInfo": [TxType.PERSISTENT], + b"SpeedLimitOffset": [TxType.PERSISTENT], b"TermsVersion": [TxType.PERSISTENT], b"TrainingVersion": [TxType.PERSISTENT], b"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], diff --git a/selfdrive/assets/img_trafficSign_speedahead.png b/selfdrive/assets/img_trafficSign_speedahead.png new file mode 100644 index 00000000000000..8357ff8a9112fc Binary files /dev/null and b/selfdrive/assets/img_trafficSign_speedahead.png differ diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 8ea8c4a7b38c7c..83c35592ee1728 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -4,6 +4,9 @@ from common.params import Params from common.numpy_fast import interp +from datetime import datetime +import time + import cereal.messaging as messaging from cereal import car from common.realtime import sec_since_boot @@ -15,7 +18,11 @@ from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +offset = 4.47 +osm = True + MAX_SPEED = 255.0 +NO_CURVATURE_SPEED = 90.0 LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted @@ -179,6 +186,66 @@ def update(self, sm, pm, CP, VM, PP): enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 + v_speedlimit = NO_CURVATURE_SPEED + v_curvature_map = NO_CURVATURE_SPEED + v_speedlimit_ahead = NO_CURVATURE_SPEED + now = datetime.now() + try: + if sm['liveMapData'].speedLimitValid and osm and self.osm and (sm['liveMapData'].lastGps.timestamp -time.mktime(now.timetuple()) * 1000) < 10000 and (smart_speed or smart_speed_max_vego > v_ego): + speed_limit = sm['liveMapData'].speedLimit + if speed_limit is not None: + v_speedlimit = speed_limit + # offset is in percentage,. + if v_ego > offset_limit: + v_speedlimit = v_speedlimit * (1. + self.offset/100.0) + if v_speedlimit > fixed_offset: + v_speedlimit = v_speedlimit + fixed_offset + else: + speed_limit = None + if sm['liveMapData'].speedLimitAheadValid and osm and self.osm and sm['liveMapData'].speedLimitAheadDistance < speed_ahead_distance and (sm['liveMapData'].lastGps.timestamp -time.mktime(now.timetuple()) * 1000) < 10000 and (smart_speed or smart_speed_max_vego > v_ego): + distanceatlowlimit = 50 + if sm['liveMapData'].speedLimitAhead < 21/3.6: + distanceatlowlimit = speed_ahead_distance = (v_ego - sm['liveMapData'].speedLimitAhead)*3.6*2 + if distanceatlowlimit < 50: + distanceatlowlimit = 0 + distanceatlowlimit = min(distanceatlowlimit,100) + speed_ahead_distance = (v_ego - sm['liveMapData'].speedLimitAhead)*3.6*5 + speed_ahead_distance = min(speed_ahead_distance,300) + speed_ahead_distance = max(speed_ahead_distance,50) + if speed_limit is not None and sm['liveMapData'].speedLimitAheadDistance > distanceatlowlimit and v_ego + 3 < sm['liveMapData'].speedLimitAhead + (speed_limit - sm['liveMapData'].speedLimitAhead)*sm['liveMapData'].speedLimitAheadDistance/speed_ahead_distance: + speed_limit_ahead = sm['liveMapData'].speedLimitAhead + (speed_limit - sm['liveMapData'].speedLimitAhead)*(sm['liveMapData'].speedLimitAheadDistance - distanceatlowlimit)/(speed_ahead_distance - distanceatlowlimit) + else: + speed_limit_ahead = sm['liveMapData'].speedLimitAhead + if speed_limit_ahead is not None: + v_speedlimit_ahead = speed_limit_ahead + if v_ego > offset_limit: + v_speedlimit_ahead = v_speedlimit_ahead * (1. + self.offset/100.0) + if v_speedlimit_ahead > fixed_offset: + v_speedlimit_ahead = v_speedlimit_ahead + fixed_offset + if sm['liveMapData'].curvatureValid and sm['liveMapData'].distToTurn < speed_ahead_distance and osm and self.osm and (sm['liveMapData'].lastGps.timestamp -time.mktime(now.timetuple()) * 1000) < 10000: + curvature = abs(sm['liveMapData'].curvature) + radius = 1/max(1e-4, curvature) * curvature_factor + if gas_button_status == 1: + radius = radius * 2.0 + elif gas_button_status == 2: + radius = radius * 1.0 + else: + radius = radius * 1.5 + if radius > 500: + c=0.9 # 0.9 at 1000m = 108 kph + elif radius > 250: + c = 3.5-13/2500*radius # 2.2 at 250m 84 kph + else: + c= 3.0 - 2/625 *radius # 3.0 at 15m 24 kph + v_curvature_map = math.sqrt(c*radius) + v_curvature_map = min(NO_CURVATURE_SPEED, v_curvature_map) + except KeyError: + pass + + decel_for_turn = bool(v_curvature_map < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.])) + + speed_ahead_distance = 250 + # dp self.dp_profile = sm['dragonConf'].dpAccelProfile self.dp_slow_on_curve = sm['dragonConf'].dpSlowOnCurve @@ -217,6 +284,21 @@ def update(self, sm, pm, CP, VM, PP): accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) + if decel_for_turn and sm['liveMapData'].distToTurn < speed_ahead_distance and not following: + time_to_turn = max(1.0, sm['liveMapData'].distToTurn / max((v_ego + v_curvature_map)/2, 1.)) + required_decel = min(0, (v_curvature_map - v_ego) / time_to_turn) + accel_limits[0] = max(accel_limits[0], required_decel) + if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm['liveMapData'].speedLimitAheadDistance > 1.0 and not following: + required_decel = min(0, (v_speedlimit_ahead*v_speedlimit_ahead - v_ego*v_ego)/(sm['liveMapData'].speedLimitAheadDistance*2)) + required_decel = max(required_decel, -3.0) + decel_for_turn = True + accel_limits[0] = required_decel + accel_limits[1] = required_decel + self.a_acc_start = required_decel + v_speedlimit_ahead = v_ego + + v_cruise_setpoint = min([v_cruise_setpoint, v_curvature_map, v_speedlimit, v_speedlimit_ahead]) + self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], @@ -290,6 +372,10 @@ def update(self, sm, pm, CP, VM, PP): plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource + plan_send.plan.vCurvature = float(v_curvature_map) + plan_send.plan.decelForTurn = bool(decel_for_turn) + plan_send.plan.mapValid = True + radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 7e20fb8c0dfd07..2f1e9467622f9f 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -23,7 +23,7 @@ def plannerd_thread(sm=None, pm=None): VM = VehicleModel(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf'], + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData'], poll=['radarState', 'model']) if pm is None: diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 857830d5ece91b..50f4975120b284 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -5,7 +5,7 @@ import fcntl import errno import signal -import shutil +#import shutil import subprocess import datetime import textwrap @@ -75,6 +75,9 @@ def unblock_stdout(): from common.spinner import Spinner from common.text_window import TextWindow +if not (os.system("python3 -m pip list | grep 'scipy' ") == 0): + os.system("cd /data/openpilot/installer/scipy_installer/ && ./scipy_installer") + import importlib import traceback from multiprocessing import Process @@ -128,9 +131,9 @@ def unblock_stdout(): for i in range(3, -1, -1): print("....%d" % i) time.sleep(1) - subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) - shutil.rmtree("/tmp/scons_cache", ignore_errors=True) - shutil.rmtree("/data/scons_cache", ignore_errors=True) + #subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) + #shutil.rmtree("/tmp/scons_cache", ignore_errors=True) + #shutil.rmtree("/data/scons_cache", ignore_errors=True) else: print("scons build failed after retry") sys.exit(1) @@ -198,6 +201,7 @@ def unblock_stdout(): "updated": "selfdrive.updated", "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]), + "mapd": ("selfdrive/mapd", ["./mapd.py"]), "rtshield": "selfdrive.rtshield", "systemd": "selfdrive.dragonpilot.systemd", "appd": "selfdrive.dragonpilot.appd", @@ -248,6 +252,7 @@ def get_running(): 'paramsd', 'camerad', 'proclogd', + 'mapd', 'locationd', 'clocksd', 'gpxd', @@ -353,9 +358,10 @@ def prepare_managed_process(p): subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) except subprocess.CalledProcessError: # make clean if the build failed - cloudlog.warning("building %s failed, make clean" % (proc, )) - subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0])) - subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) + if proc[0] != 'selfdrive/mapd': + cloudlog.warning("building %s failed, make clean" % (proc, )) + subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0])) + subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) def join_process(process, timeout): @@ -574,6 +580,10 @@ def main(): ("HasCompletedSetup", "0"), ("IsUploadRawEnabled", "1"), ("IsLdwEnabled", "1"), + ("SpeedLimitOffset", "0"), + ("LongitudinalControl", "1"), + ("LimitSetSpeed", "1"), + ("LimitSetSpeedNeural", "1"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), ("LaneChangeEnabled", "1"), diff --git a/selfdrive/mapd/README.md b/selfdrive/mapd/README.md new file mode 100644 index 00000000000000..f155db518376d0 --- /dev/null +++ b/selfdrive/mapd/README.md @@ -0,0 +1,104 @@ +Instructions to get cached offline maps: + +Android Root: just install Termux and install arch linux and intall docker + +Android Non Root: +Intall Termux from the play store +run +pkg install termux-tools proot util-linux net-tools openssh git coreutils wget + +wget https://raw.githubusercontent.com/xeffyr/termux-x-repository/master/enablerepo.sh +bash enablerepo.sh + +and + +pkg install qemu-system-x86_64 --fix-missing + +to intall qemu + +run + +termux-setup-storage + +to setup storage access + +mkdir termux-docker && cd termux-docker +git clone https://github.com/pwdonald/chromeos-qemu-docker + +cd storage/external-1 + +to get to the external storage + +pkg install qemu-utils + +qemu-img create -f qcow2 virtual_drive 16G + +curl -sL http://dl-cdn.alpinelinux.org/alpine/v3.7/releases/x86_64/alpine-virt-3.7.3-x86_64.iso -o alpine_x86_64.iso + +to download the alpine virutal distro + +qemu-system-x86_64 -nographic -m 512m -cdrom alpine_x86_64.iso -hda virtual_drive -boot d -net nic -net user + +to start the intallation. Use mbr sys + +run setup-alpine + +If you encounter internet related errors, use google dns withecho 'nameserver 8.8.8.8' > /etc/resolv.conf and run setup-alpine again. + +Once done, exit the virtual machine with Ctrl-A X. + +create a swap file. + +qemu-img create -f qcow2 virtual_swap 8G + +to run the VM use this command + +qemu-system-x86_64 -nographic -hda virtual_drive -boot c -net user,hostfwd=tcp::10022-:22,hostfwd=tcp::10080-:80,hostfwd=tcp::12345-:12345 -net nic -hdb virtual_swap -m 2048M -smp 3 + +This can take a few minutes to start up so just be patient + +apk add vim + +apk update + +vim /etc/apk/repositories +to enable community features + +apk add wget + +apk add docker + +in +/etc/update-extlinux.conf +add +default_kernel_opts="... cgroup_enable=memory swapaccount=1" +and +update-extlinux + +check +docker info +to see that everything is working correctly + +setup hdb as a linux swap partition with fdisk. That should give you 10GB of total RAM. + +service docker start + +use the https://github.com/wiktorn/Overpass-API + +docker pull wiktorn/overpass-api + +docker run \ +-e OVERPASS_META=yes \ +-e OVERPASS_MODE=init \ +-e OVERPASS_PLANET_URL=http://download.geofabrik.de/europe/germany/baden-wuerttemberg/tuebingen-regbez-latest.osm.bz2 \ +-e OVERPASS_DIFF_URL=http://download.geofabrik.de/europe/germany/baden-wuerttemberg/tuebingen-regbez-updates/ \ +-e OVERPASS_RULES_LOAD=10 \ +-v /big/docker/overpass_db/:/db \ +-p 12345:80 \ +-i -t \ +--name overpass_bw wiktorn/overpass-api + +docker start overpass_bw and docker stop overpass_bw can now start the docker process + +This is a 100MB file and lands up using about 6GB of RAM and HDD space after the inital install it only requires 1GB to actively runn + diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py new file mode 100644 index 00000000000000..8b137891791fe9 --- /dev/null +++ b/selfdrive/mapd/__init__.py @@ -0,0 +1 @@ + diff --git a/selfdrive/mapd/default_speeds.json b/selfdrive/mapd/default_speeds.json new file mode 100644 index 00000000000000..a8db6088077d9c --- /dev/null +++ b/selfdrive/mapd/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/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py new file mode 100644 index 00000000000000..e1a906bf82af83 --- /dev/null +++ b/selfdrive/mapd/default_speeds_generator.py @@ -0,0 +1,257 @@ +#!/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": "motorway"}, "65 mph") + 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") + new_york.add_rule({"highway": "living_street"}, "20 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({"highway": "service"}, "10") + 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") + + """ + -------------------------------------------------- + ZA - South Africa + -------------------------------------------------- + """ + ZA = Country("ZA") + countries.append(ZA) + + """ Default rules """ + ZA.add_rule({"highway": "motorway"}, "120") + ZA.add_rule({"highway": "motorway_link"}, "120") + ZA.add_rule({"highway": "primary"}, "100") + ZA.add_rule({"highway": "secondary"}, "100") + ZA.add_rule({"highway": "residential"}, "60") + + """ + -------------------------------------------------- + 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","ZA","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/mapd.py b/selfdrive/mapd/mapd.py new file mode 100755 index 00000000000000..1bc7c3073ddc31 --- /dev/null +++ b/selfdrive/mapd/mapd.py @@ -0,0 +1,600 @@ +#!/usr/bin/env python3 + +import time +import math +import overpy +import socket +import requests +import threading +import numpy as np +# setup logging +import logging +import logging.handlers +from scipy import spatial +import selfdrive.crash as crash +from common.params import Params +from collections import defaultdict +import cereal.messaging as messaging +#import cereal.messaging_arne as messaging_arne +from selfdrive.version import version, dirty +from common.transformations.coordinates import geodetic2ecef +from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points, rate_curvature_points + +#DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +#from selfdrive.mapd import default_speeds_generator +#default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE) + +# define LoggerThread class to implement logging functionality +class LoggerThread(threading.Thread): + def __init__(self, threadID, name): + threading.Thread.__init__(self) + self.threadID = threadID + self.name = name + self.logger = logging.getLogger(name) + h = logging.handlers.RotatingFileHandler(str(name)+'-Thread.log', 'a', 10*1024*1024, 5) + f = logging.Formatter('%(asctime)s %(processName)-10s %(name)s %(levelname)-8s %(message)s') + h.setFormatter(f) + self.logger.addHandler(h) + self.logger.setLevel(logging.CRITICAL) # set to logging.DEBUG to enable logging + # self.logger.setLevel(logging.DEBUG) # set to logging.CRITICAL to disable logging + + def save_gps_data(self, gps, osm_way_id): + try: + location = [gps.speed, gps.bearing, gps.latitude, gps.longitude, gps.altitude, gps.accuracy, time.time(), osm_way_id] + with open("/data/openpilot/selfdrive/data_collection/gps-data", "a") as f: + f.write("{}\n".format(location)) + except: + self.logger.error("Unable to write gps data to external file") + + def run(self): + pass # will be overridden in the child class + +class QueryThread(LoggerThread): + def __init__(self, threadID, name, sharedParams={}): # sharedParams is dict of params shared between two threads + # invoke parent constructor https://stackoverflow.com/questions/2399307/how-to-invoke-the-super-constructor-in-python + LoggerThread.__init__(self, threadID, name) + self.sharedParams = sharedParams + # memorize some parameters + self.OVERPASS_API_LOCAL = "http://192.168.43.1:12345/api/interpreter" + socket.setdefaulttimeout(15) + self.distance_to_edge = 500 + self.OVERPASS_API_URL = "https://z.overpass-api.de/api/interpreter" + self.OVERPASS_API_URL2 = "https://lz4.overpass-api.de/api/interpreter" + self.OVERPASS_HEADERS = { + 'User-Agent': 'NEOS (comma.ai)', + 'Accept-Encoding': 'gzip' + } + self.prev_ecef = None + + def is_connected_to_local(self, timeout=3.0): + try: + requests.get(self.OVERPASS_API_LOCAL, timeout=timeout) + self.logger.debug("connection local active") + return True + except: + self.logger.error("No local server available.") + return False + + def is_connected_to_internet(self, timeout=1.0): + try: + requests.get(self.OVERPASS_API_URL, timeout=timeout) + self.logger.debug("connection 1 active") + return True + except: + self.logger.error("No internet connection available.") + return False + + def is_connected_to_internet2(self, timeout=1.0): + try: + requests.get(self.OVERPASS_API_URL2, timeout=timeout) + self.logger.debug("connection 2 active") + return True + except: + self.logger.error("No internet connection available.") + return False + + def build_way_query(self, lat, lon, heading, radius=50): + """Builds a query to find all highways within a given radius around a point""" + a = 111132.954*math.cos(float(lat)/180*3.141592) + b = 111132.954 - 559.822 * math.cos( 2 * float(lat)/180*3.141592) + 1.175 * math.cos( 4 * float(lat)/180*3.141592) + heading = math.radians(-heading + 90) + lat = lat+math.sin(heading)*radius/2/b + lon = lon+math.cos(heading)*radius/2/a + pos = " (around:%f,%f,%f)" % (radius, lat, lon) + lat_lon = "(%f,%f)" % (lat, lon) + q = """( + way + """ + pos + """ + [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"]; + >;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"]; + convert area ::id = id(), admin_level = t['admin_level'], + name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out; + """ + self.logger.debug("build_way_query : %s" % str(q)) + return q, lat, lon + + def run(self): + self.logger.debug("run method started for thread %s" % self.name) + + # for now we follow old logic, will be optimized later + start = time.time() + radius = 3000 + while True: + if time.time() - start > 2.0: + print("Mapd QueryThread lagging by: %s" % str(time.time() - start - 1.0)) + if time.time() - start < 1.0: + time.sleep(0.1) + continue + else: + start = time.time() + + self.logger.debug("Starting after sleeping for 1 second ...") + last_gps = self.sharedParams.get('last_gps', None) + self.logger.debug("last_gps = %s" % str(last_gps)) + + if last_gps is not None: + fix_ok = last_gps.flags & 1 + if not fix_ok: + continue + else: + continue + + last_query_pos = self.sharedParams.get('last_query_pos', None) + if last_query_pos is not None: + cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) + if self.prev_ecef is None: + self.prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) + + dist = np.linalg.norm(cur_ecef - self.prev_ecef) + if dist < radius - self.distance_to_edge: #updated when we are close to the edge of the downloaded circle + continue + self.logger.debug("parameters, cur_ecef = %s, prev_ecef = %s, dist=%s" % (str(cur_ecef), str(self.prev_ecef), str(dist))) + + if dist > radius: + query_lock = self.sharedParams.get('query_lock', None) + if query_lock is not None: + query_lock.acquire() + self.sharedParams['cache_valid'] = False + query_lock.release() + else: + self.logger.error("There is no query_lock") + + if last_gps is not None and last_gps.accuracy < 5.0: + q, lat, lon = self.build_way_query(last_gps.latitude, last_gps.longitude, last_gps.bearing, radius=radius) + try: + if self.is_connected_to_local(): + api = overpy.Overpass(url=self.OVERPASS_API_LOCAL) + api.timeout = 15.0 + self.distance_to_edge = radius * 3 / 8 + elif self.is_connected_to_internet(): + api = overpy.Overpass(url=self.OVERPASS_API_URL) + self.logger.error("Using origional Server") + self.distance_to_edge = radius/4 + elif self.is_connected_to_internet2(): + api = overpy.Overpass(url=self.OVERPASS_API_URL2) + api.timeout = 10.0 + self.logger.error("Using backup Server") + self.distance_to_edge = radius/4 + else: + continue + new_result = api.query(q) + self.logger.debug("new_result = %s" % str(new_result)) + # Build kd-tree + nodes = [] + real_nodes = [] + node_to_way = defaultdict(list) + location_info = {} + + for n in new_result.nodes: + nodes.append((float(n.lat), float(n.lon), 0)) + real_nodes.append(n) + + for way in new_result.ways: + for n in way.nodes: + node_to_way[n.id].append(way) + + for area in new_result.areas: + if area.tags.get('admin_level', '') == "2": + location_info['country'] = area.tags.get('ISO3166-1:alpha2', '') + elif area.tags.get('admin_level', '') == "4": + location_info['region'] = area.tags.get('name', '') + + nodes = np.asarray(nodes) + nodes = geodetic2ecef(nodes) + tree = spatial.KDTree(nodes) + self.logger.debug("query thread, ... %s %s" % (str(nodes), str(tree))) + + # write result + query_lock = self.sharedParams.get('query_lock', None) + if query_lock is not None: + query_lock.acquire() + last_gps_mod = last_gps.as_builder() + last_gps_mod.latitude = lat + last_gps_mod.longitude = lon + last_gps = last_gps_mod.as_reader() + self.sharedParams['last_query_result'] = new_result, tree, real_nodes, node_to_way, location_info + self.prev_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) + self.sharedParams['last_query_pos'] = last_gps + self.sharedParams['cache_valid'] = True + query_lock.release() + else: + self.logger.error("There is not query_lock") + + except Exception as e: + self.logger.error("ERROR :" + str(e)) + print(str(e)) + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['last_query_result'] = None + query_lock.release() + else: + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['last_query_result'] = None + query_lock.release() + + self.logger.debug("end of one cycle in endless loop ...") + +class MapsdThread(LoggerThread): + def __init__(self, threadID, name, sharedParams={}): + # invoke parent constructor + LoggerThread.__init__(self, threadID, name) + self.sharedParams = sharedParams + self.pm = messaging.PubMaster(['liveMapData']) + self.logger.debug("entered mapsd_thread, ... %s" % ( str(self.pm))) + def run(self): + self.logger.debug("Entered run method for thread :" + str(self.name)) + cur_way = None + curvature_valid = False + curvature = None + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + max_speed = None + max_speed_ahead = None + max_speed_ahead_dist = None + max_speed_prev = 0 + had_good_gps = False + start = time.time() + while True: + if time.time() - start > 0.2: + print("Mapd MapsdThread lagging by: %s" % str(time.time() - start - 0.1)) + if time.time() - start < 0.1: + time.sleep(0.01) + continue + else: + start = time.time() + self.logger.debug("starting new cycle in endless loop") + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + gps = self.sharedParams['last_gps'] + traffic_status = self.sharedParams['traffic_status'] + traffic_confidence = self.sharedParams['traffic_confidence'] + last_not_none_signal = self.sharedParams['last_not_none_signal'] + speedLimittraffic = self.sharedParams['speedLimittraffic'] + speedLimittrafficvalid = self.sharedParams['speedLimittrafficvalid'] + speedLimittrafficAdvisory = self.sharedParams['speedLimittrafficAdvisory'] + speedLimittrafficAdvisoryvalid = self.sharedParams['speedLimittrafficAdvisoryvalid'] + query_lock.release() + if gps is None: + continue + fix_ok = gps.flags & 1 + self.logger.debug("fix_ok = %s" % str(fix_ok)) + + if gps.accuracy > 2.5: + if gps.accuracy > 5.0: + if not speedLimittrafficvalid: + if had_good_gps: + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['speedLimittrafficvalid'] = True + if max_speed is not None: + speedLimittraffic = max_speed * 3.6 + else: + speedLimittraffic = 130 + query_lock.release() + else: + fix_ok = False + had_good_gps = False + if not speedLimittrafficvalid and not had_good_gps: + fix_ok = False + elif not had_good_gps: + had_good_gps = True + if not fix_ok or self.sharedParams['last_query_result'] is None or not self.sharedParams['cache_valid']: + self.logger.debug("fix_ok %s" % fix_ok) + self.logger.error("Error in fix_ok logic") + cur_way = None + curvature = None + max_speed_ahead = None + max_speed_ahead_dist = None + curvature_valid = False + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + map_valid = False + else: + map_valid = True + lat = gps.latitude + lon = gps.longitude + heading = gps.bearing + speed = gps.speed + + query_lock.acquire() + cur_way = Way.closest(self.sharedParams['last_query_result'], lat, lon, heading, cur_way) + query_lock.release() + + if cur_way is not None: + self.logger.debug("cur_way is not None ...") + pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + if pnts is not None: + xs = pnts[:, 0] + ys = pnts[:, 1] + road_points = [float(x) for x in xs], [float(y) for y in ys] + + if speed < 5: + curvature_valid = False + if curvature_valid and pnts.shape[0] <= 3: + curvature_valid = False + else: + curvature_valid = False + upcoming_curvature = 0. + curvature = None + dist_to_turn = 0. + # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found + if curvature_valid: + # Compute the curvature for each point + with np.errstate(divide='ignore'): + circles = [circle_through_points(*p, direction=True) for p in zip(pnts, pnts[1:], pnts[2:])] + circles = np.asarray(circles) + radii = np.nan_to_num(circles[:, 2]) + radii[abs(radii) < 15.] = 10000 + + if cur_way.way.tags['highway'] == 'trunk' or cur_way.way.tags['highway'] == 'motorway_link': + radii = radii*1.6 # https://media.springernature.com/lw785/springer-static/image/chp%3A10.1007%2F978-3-658-01689-0_21/MediaObjects/298553_35_De_21_Fig65_HTML.gif + elif cur_way.way.tags['highway'] == 'motorway': + radii = radii*2.8 + + curvature = 1. / radii + rate = [rate_curvature_points(*p) for p in zip(pnts[1:], pnts[2:],curvature[0:],curvature[1:])] + rate = ([0] + rate) + + curvature = np.abs(curvature) + curvature = np.multiply(np.minimum(np.multiply(rate,4000)+0.7,1.1),curvature) + # Index of closest point + closest = np.argmin(np.linalg.norm(pnts, axis=1)) + dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close + + # Compute distance along path + dists = list() + dists.append(0) + for p, p_prev in zip(pnts, pnts[1:, :]): + dists.append(dists[-1] + np.linalg.norm(p - p_prev)) + dists = np.asarray(dists) + dists = dists - dists[closest] + dist_to_closest + dists = dists[1:-1] + + close_idx = np.logical_and(dists > 0, dists < 500) + dists = dists[close_idx] + curvature = curvature[close_idx] + + if len(curvature): + curvature = np.nan_to_num(curvature) + upcoming_curvature = np.amax(curvature) + dist_to_turn =np.amin(dists[np.logical_and(curvature >= upcoming_curvature, curvature <= upcoming_curvature)]) + else: + upcoming_curvature = 0. + dist_to_turn = 999 + + dat = messaging.new_message() + dat.init('liveMapData') + + last_gps = self.sharedParams.get('last_gps', None) + + if last_gps is not None: + dat.liveMapData.lastGps = last_gps + + if cur_way is not None: + dat.liveMapData.wayId = cur_way.id + self.sharedParams['osm_way_id'] = cur_way.id + # Speed limit + max_speed = cur_way.max_speed(heading) + max_speed_ahead = None + max_speed_ahead_dist = None + if max_speed is not None: + max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE, traffic_status, traffic_confidence, last_not_none_signal) + else: + max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(speed*1.1, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE, traffic_status, traffic_confidence, last_not_none_signal) + # TODO: anticipate T junctions and right and left hand turns based on indicator + + if max_speed_ahead is not None and max_speed_ahead_dist is not None: + dat.liveMapData.speedLimitAheadValid = True + dat.liveMapData.speedLimitAhead = float(max_speed_ahead) + dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) + if max_speed is not None: + if abs(max_speed - max_speed_prev) > 0.1: + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['speedLimittrafficvalid'] = False + query_lock.release() + max_speed_prev = max_speed + advisory_max_speed = cur_way.advisory_max_speed() + if speedLimittrafficAdvisoryvalid: + dat.liveMapData.speedAdvisoryValid = True + dat.liveMapData.speedAdvisory = speedLimittrafficAdvisory / 3.6 + else: + if advisory_max_speed is not None: + dat.liveMapData.speedAdvisoryValid = True + dat.liveMapData.speedAdvisory = advisory_max_speed + + # Curvature + dat.liveMapData.curvatureValid = curvature_valid + dat.liveMapData.curvature = float(upcoming_curvature) + dat.liveMapData.distToTurn = float(dist_to_turn) + if road_points is not None: + dat.liveMapData.roadX, dat.liveMapData.roadY = road_points + if curvature is not None: + dat.liveMapData.roadCurvatureX = [float(x) for x in dists] + dat.liveMapData.roadCurvature = [float(x) for x in curvature] + else: + self.sharedParams['osm_way_id'] = 0 + if self.sharedParams['speedLimittrafficvalid']: + if speedLimittraffic > 0.1: + dat.liveMapData.speedLimitValid = True + dat.liveMapData.speedLimit = speedLimittraffic / 3.6 + map_valid = False + else: + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['speedLimittrafficvalid'] = False + query_lock.release() + else: + if max_speed is not None and map_valid: + dat.liveMapData.speedLimitValid = True + dat.liveMapData.speedLimit = max_speed + + dat.liveMapData.mapValid = map_valid + self.logger.debug("Sending ... liveMapData ... %s", str(dat)) + self.pm.send('liveMapData', dat) + +class MessagedGPSThread(LoggerThread): + def __init__(self, threadID, name, sharedParams={}): + # invoke parent constructor + LoggerThread.__init__(self, threadID, name) + self.sharedParams = sharedParams + self.sm = messaging.SubMaster(['gpsLocationExternal']) + self.logger.debug("entered messagedGPS_thread, ... %s" % (str(self.sm))) + def run(self): + self.logger.debug("Entered run method for thread :" + str(self.name)) + gps = None + start = time.time() + while True: + if time.time() - start > 0.2: + print("Mapd MessagedGPSThread lagging by: %s" % str(time.time() - start - 0.1)) + if time.time() - start < 0.1: + time.sleep(0.01) + continue + else: + start = time.time() + self.logger.debug("starting new cycle in endless loop") + self.sm.update(0) + if self.sm.updated['gpsLocationExternal']: + gps = self.sm['gpsLocationExternal'] + self.save_gps_data(gps, self.sharedParams['osm_way_id']) + + query_lock = self.sharedParams.get('query_lock', None) + + query_lock.acquire() + self.sharedParams['last_gps'] = gps + query_lock.release() + self.logger.debug("setting last_gps to %s" % str(gps)) + +class MessagedThread(LoggerThread): + def __init__(self, threadID, name, sharedParams={}): + # invoke parent constructor + LoggerThread.__init__(self, threadID, name) + self.sharedParams = sharedParams + self.sm = messaging.SubMaster(['liveTrafficData'])#,'trafficModelEvent']) + #self.logger.debug("entered messageArned_thread, ... %s" % str(self.arne_sm)) + def run(self): + self.logger.debug("Entered run method for thread :" + str(self.name)) + last_not_none_signal = 'NONE' + last_not_none_signal_counter = 0 + traffic_confidence = 0 + traffic_status = 'NONE' + speedLimittraffic = 0 + speedLimittraffic_prev = 0 + speedLimittrafficAdvisoryvalid = False + speedLimittrafficAdvisory = 0 + start = time.time() + while True: + if time.time() - start > 0.2: + print("Mapd MessagedArneThread lagging by: %s" % str(time.time() - start - 0.1)) + if time.time() - start < 0.1: + time.sleep(0.01) + continue + else: + start = time.time() + self.logger.debug("starting new cycle in endless loop") + #self.arne_sm.update(0) + #if self.arne_sm.updated['trafficModelEvent']: + # traffic_status = self.arne_sm['trafficModelEvent'].status + #traffic_confidence = round(self.arne_sm['trafficModelEvent'].confidence * 100, 2) + #if traffic_confidence >= 50 and (traffic_status == 'GREEN' or traffic_status == 'SLOW'): + #last_not_none_signal = traffic_status + #last_not_none_signal_counter = 0 + #elif traffic_confidence >= 50 and traffic_status == 'NONE' and last_not_none_signal != 'NONE': + #if last_not_none_signal_counter < 25: + #last_not_none_signal_counter = last_not_none_signal_counter + 1 + #print("self.last_not_none_signal_counter") + #print(self.last_not_none_signal_counter) + #print("self.last_not_none_signal") + #print(self.last_not_none_signal) + #else: + #last_not_none_signal = 'NONE' + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + speedLimittrafficvalid = self.sharedParams['speedLimittrafficvalid'] + query_lock.release() + traffic = self.sm['liveTrafficData'] + if traffic.speedLimitValid: + speedLimittraffic = traffic.speedLimit + if abs(speedLimittraffic_prev - speedLimittraffic) > 0.1: + speedLimittrafficvalid = True + speedLimittraffic_prev = speedLimittraffic + else: + speedLimittrafficvalid = False + if traffic.speedAdvisoryValid: + speedLimittrafficAdvisory = traffic.speedAdvisory + speedLimittrafficAdvisoryvalid = True + else: + speedLimittrafficAdvisoryvalid = False + + query_lock.acquire() + self.sharedParams['traffic_status'] = traffic_status + self.sharedParams['traffic_confidence'] = traffic_confidence + self.sharedParams['last_not_none_signal'] = last_not_none_signal + self.sharedParams['speedLimittraffic'] = speedLimittraffic + self.sharedParams['speedLimittrafficvalid'] = speedLimittrafficvalid + self.sharedParams['speedLimittrafficAdvisory'] = speedLimittrafficAdvisory + self.sharedParams['speedLimittrafficAdvisoryvalid'] = speedLimittrafficAdvisoryvalid + query_lock.release() + +def main(): + params = Params() + dongle_id = params.get("DongleId") + crash.bind_user(id=dongle_id) + crash.bind_extra(version=version, dirty=dirty, is_eon=True) + crash.install() + + # setup shared parameters + last_gps = None + query_lock = threading.Lock() + last_query_result = None + last_query_pos = None + cache_valid = False + traffic_status = 'None' + traffic_confidence = 100 + last_not_none_signal = 'None' + speedLimittraffic = 0 + speedLimittrafficvalid = False + speedLimittrafficAdvisory = 0 + osm_way_id = 0 + speedLimittrafficAdvisoryvalid = False + sharedParams = {'last_gps' : last_gps, 'query_lock' : query_lock, 'last_query_result' : last_query_result, \ + 'last_query_pos' : last_query_pos, 'cache_valid' : cache_valid, 'traffic_status' : traffic_status, \ + 'traffic_confidence' : traffic_confidence, 'last_not_none_signal' : last_not_none_signal, \ + 'speedLimittraffic' : speedLimittraffic, 'speedLimittrafficvalid' : speedLimittrafficvalid, \ + 'speedLimittrafficAdvisory' : speedLimittrafficAdvisory, 'speedLimittrafficAdvisoryvalid' : speedLimittrafficAdvisoryvalid, 'osm_way_id' : osm_way_id} + + qt = QueryThread(1, "QueryThread", sharedParams=sharedParams) + mt = MapsdThread(2, "MapsdThread", sharedParams=sharedParams) + mggps = MessagedGPSThread(3, "MessagedGPSThread", sharedParams=sharedParams) + #mgarne = MessagedArneThread(4, "MessagedArneThread", sharedParams=sharedParams) + + qt.start() + mt.start() + mggps.start() + #mgarne.start() + +if __name__ == "__main__": + main() diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py new file mode 100755 index 00000000000000..ba5bafdcd2825f --- /dev/null +++ b/selfdrive/mapd/mapd_helpers.py @@ -0,0 +1,768 @@ +import math +import json +import numpy as np +from datetime import datetime +from common.basedir import BASEDIR +from common.op_params import opParams +from selfdrive.config import Conversions as CV +from common.transformations.coordinates import LocalCoord, geodetic2ecef + +LOOKAHEAD_TIME = 10. +MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME + +op_params = opParams() + +traffic_lights = op_params.get('traffic_lights') +traffic_lights_without_direction = op_params.get('traffic_lights_without_direction') +rolling_stop = op_params.get('rolling_stop') + +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 rate_curvature_points(p2,p3,curvature2,curvature3): + x2, y2, _ = p2 + x3, y3, _ = p3 + if abs(curvature3) > abs(curvature2): + return abs((curvature3-curvature2)/(np.sqrt((x3-x2)**2+(y3-y2)**2))) + else: + return 0 + +def distance(x0,y0,x1,y1,x2,y2): + return abs((x2-x1)*(y1-y0) - (x1-x0)*(y2-y1)) / np.sqrt(np.square(x2-x1) + np.square(y2-y1)) + +def circle_through_points(p1, p2, p3, force=False, direction=False): + """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) + try: + if abs((y3-y1)*x2-(x3-x1)*y2+x3*y1-y3*x1)/np.sqrt((y3-y1)**2+(x3-x1)**2) > 0.1 or force: + if direction: + if (x2-x1)*(y3-y1)-(y2-y1)*(x3-x1)>0: + return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + else: + return (-B / (2 * A), - C / (2 * A), -np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + else: + return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + else: + return (-B / (2 * A), - C / (2 * A), 10000) + except RuntimeWarning: + return x2, y2, 10000 + +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: + weekday = True + max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') + if cond.find('wet') > -0.5: + cond = cond.replace('wet','') + cond = cond.replace(' ','') + weekday = False + #TODO Check if road is wet waybe if wipers are on. + cond = cond[1:-1] + + now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays + if cond.find('Mo-Fr') > -0.5: + cond = cond.replace('Mo-Fr','') + cond = cond.replace(' ','') + if now.weekday() > 4: + weekday = False + if cond.find('Mo-Su') > -0.5: + cond = cond.replace('Mo-Su','') + cond = cond.replace(' ','') + if cond.find('; SH off') > -0.5: + cond = cond.replace('; SH off','') + cond = cond.replace(' ','') + if cond.find('Oct-Apr') > -0.5: + if 4 > now.month > 10: + weekday = False + else: + max_speed = max_speed_cond + else: + start, end = cond.split('-') + starthour, startminute = start.split(':') + endhour, endminute = end.split(':') + start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + midnight = datetime.strptime("00:00", "%H:%M").replace(year=now.year, month=now.month, day=now.day) + end1 = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + if int(endhour) + int(endminute)/60 < int(starthour) + int(startminute)/60: + end2 = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day+1) + if start <= now <= end2 or midnight <= now <= end1 and weekday: + max_speed = max_speed_cond + else: + if start <= now <= end1 and weekday: + 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): + if query_results is None: + return None + else: + # if prev_way is not None and len(prev_way.way.nodes) < 10: + # if prev_way.on_way(lat, lon, heading): + # return prev_way + # else: + # way = prev_way.next_way(heading) + # if way is not None and way.on_way(lat, lon, heading): + # return way + + results, tree, real_nodes, node_to_way, location_info = query_results + + cur_pos = geodetic2ecef((lat, lon, 0)) + nodes = tree.query_ball_point(cur_pos, 150) + + # If no nodes within 150m, 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) + # don't consider backward facing roads + if 'oneway' in way.way.tags and way.way.tags['oneway'] == 'yes': + angle=heading - math.atan2(way.way.nodes[0].lon-way.way.nodes[-1].lon,way.way.nodes[0].lat-way.way.nodes[-1].lat)*180/3.14159265358979 - 180 + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + backwards = abs(angle) > 90 + if backwards: + continue + + points = way.points_in_car_frame(lat, lon, heading, True) + + 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)) + if all(front): + angles[angles==0] = np.pi + 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) * (abs(b) + 1) * 3. + 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 + + if best_score is None: + return None + + # 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, heading): + """Extracts the (conditional) speed limit from a way""" + if not self.way: + return None + angle=heading - math.atan2(self.way.nodes[0].lon-self.way.nodes[-1].lon,self.way.nodes[0].lat-self.way.nodes[-1].lat)*180/3.14159265358979 - 180 + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + backwards = abs(angle) > 90 + if backwards: + if 'maxspeed:backward' in self.way.tags: + max_speed = self.way.tags['maxspeed:backward'] + max_speed = parse_speed_unit(max_speed) + return max_speed + else: + if 'maxspeed:forward' in self.way.tags: + max_speed = self.way.tags['maxspeed:forward'] + max_speed = parse_speed_unit(max_speed) + return max_speed + + 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, traffic_status, traffic_confidence, last_not_none_signal): + """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, True) + #print way_pts + # Check current lookahead distance + if way_pts[0,0] < 0 and way_pts[-1,0] < 0: + break + elif way_pts[0,0] < 0: + max_dist = np.linalg.norm(way_pts[-1, :]) + elif way_pts[-1,0] < 0: + max_dist = np.linalg.norm(way_pts[0, :]) + else: + max_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + + + if max_dist > 2 * lookahead: + #print "max_dist break" + break + try: + if way.way.tags['junction']=='roundabout' or way.way.tags['junction']=='circular': + latmin = 181 + lonmin = 181 + latmax = -181 + lonmax = -181 + for n in way.way.nodes: + lonmax = max(n.lon,lonmax) + lonmin = min(n.lon,lonmin) + latmax = max(n.lat,latmax) + latmin = min(n.lat,latmin) + if way.way.nodes[0].id == way.way.nodes[-1].id: + a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(lonmax-lonmin) + else: + if way.way.nodes[1].id == way.way.nodes[-1].id: + circle = [0,0,30] + else: + circle = circle_through_points([way.way.nodes[0].lat,way.way.nodes[0].lon,1], [way.way.nodes[1].lat,way.way.nodes[1].lon,1], [way.way.nodes[-1].lat,way.way.nodes[-1].lon,1],True) + a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(circle[2])*2 + speed_ahead = np.sqrt(2.0*a) + min_dist = 999.9 + for w in way_pts: + min_dist = min(min_dist, float(np.linalg.norm(w))) + speed_ahead_dist = min_dist + break + except KeyError: + pass + angle=heading - math.atan2(way.way.nodes[0].lon-way.way.nodes[-1].lon,way.way.nodes[0].lat-way.way.nodes[-1].lat)*180/3.14159265358979 - 180 + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + backwards = abs(angle) > 90 + if backwards: + if 'maxspeed:backward' in way.way.tags: + spd = way.way.tags['maxspeed:backward'] + spd = parse_speed_unit(spd) + if spd is not None and spd < current_speed_limit: + speed_ahead = spd + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + speed_ahead_dist = min_dist + break + else: + if 'maxspeed:forward' in way.way.tags: + spd = way.way.tags['maxspeed:forward'] + spd = parse_speed_unit(spd) + if spd is not None and spd < current_speed_limit: + speed_ahead = spd + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + speed_ahead_dist = min_dist + break + if 'maxspeed' in way.way.tags: + spd = parse_speed_tags(way.way.tags) + #print "spd found" + #print spd + if not spd: + location_info = self.query_results[4] + spd = geocode_maxspeed(way.way.tags, location_info) + #print "spd is actually" + #print spd + if spd is not None and spd < current_speed_limit: + speed_ahead = spd + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + speed_ahead_dist = min_dist + #print "slower speed found" + #print min_dist + + break + way_pts = way.points_in_car_frame(lat, lon, heading, False) + #print(way_pts) + + try: + count = 0 + loop_must_break = False + for n in way.way.nodes: + if 'highway' in n.tags and (n.tags['highway']=='stop' or n.tags['highway']=='give_way' or n.tags['highway']=='mini_roundabout' or (n.tags['highway']=='traffic_signals' and traffic_lights)) and way_pts[count,0] > 0: + if traffic_status == 'DEAD': + pass + elif traffic_confidence >= 50 and n.tags['highway']=='traffic_signals' and (traffic_status == 'GREEN' or (traffic_status == 'NONE' and not last_not_none_signal == 'SLOW')): + break + #elif traffic_confidence >= 75 and traffic_status == 'SLOW' and n.tags['highway'] != 'motorway': + # speed_ahead = 0 + # speed_ahead_dist = 250 + # loop_must_break = True + # break + if 'direction' in n.tags: + if backwards and (n.tags['direction']=='backward' or n.tags['direction']=='both'): + #print("backward") + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 3.0) + #print(speed_ahead_dist) + speed_ahead = 7/3.6 + if n.tags['highway']=='stop': + if rolling_stop: + speed_ahead = 2.5 + else: + speed_ahead = 0 + loop_must_break = True + break + elif not backwards and (n.tags['direction']=='forward' or n.tags['direction']=='both'): + #print("forward") + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 3.0) + #print(speed_ahead_dist) + speed_ahead = 7/3.6 + if n.tags['highway']=='stop': + if rolling_stop: + speed_ahead = 2.5 + else: + speed_ahead = 0 + loop_must_break = True + break + try: + if int(n.tags['direction']) > -0.1 and int(n.tags['direction']) < 360.1: + #print(int(n.tags['direction'])) + direction = int(n.tags['direction']) - heading + if direction < -180: + direction = direction + 360 + if direction > 180: + direction = direction - 360 + if abs(direction) > 135: + speed_ahead_dist = max(0. , way_pts[count, 0] - 3.0) + #print(speed_ahead_dist) + speed_ahead = 7/3.6 + if n.tags['highway']=='stop': + if rolling_stop: + speed_ahead = 2.5 + else: + speed_ahead = 0 + loop_must_break = True + break + except (KeyError, ValueError): + pass + elif 'traffic_signals:direction' in n.tags: + if backwards and (n.tags['traffic_signals:direction']=='backward' or n.tags['traffic_signals:direction']=='both'): + #print("backward") + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 6.0) + #print(speed_ahead_dist) + speed_ahead = 5/3.6 + if n.tags['highway']=='traffic_signals': + speed_ahead = 0 + loop_must_break = True + break + elif not backwards and (n.tags['traffic_signals:direction']=='forward' or n.tags['traffic_signals:direction']=='both'): + #print("forward") + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 6.0) + #print(speed_ahead_dist) + speed_ahead = 5/3.6 + if n.tags['highway']=='traffic_signals': + speed_ahead = 0 + loop_must_break = True + break + try: + if int(n.tags['traffic_signals:direction']) > -0.1 and int(n.tags['traffic_signals:direction']) < 360.1: + #print(int(n.tags['traffic_signals:direction'])) + direction = int(n.tags['traffic_signals:direction']) - heading + if direction < -180: + direction = direction + 360 + if direction > 180: + direction = direction - 360 + if abs(direction) > 135: + speed_ahead_dist = max(0. , way_pts[count, 0] - 6.0) + #print(speed_ahead_dist) + speed_ahead = 5/3.6 + if n.tags['highway']=='traffic_signals': + speed_ahead = 0 + loop_must_break = True + break + except (KeyError, ValueError): + pass + else: + if n.tags['highway']=='mini_roundabout': + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 5.0) + #print(speed_ahead_dist) + speed_ahead = 15/3.6 + loop_must_break = True + break + if way_pts[count, 0] > 0 and traffic_lights_without_direction: + #print("no direction") + speed_ahead_dist = max(0. , way_pts[count, 0] - 10.0) + #print(speed_ahead_dist) + speed_ahead = 5/3.6 + if n.tags['highway']=='stop': + if rolling_stop: + speed_ahead = 2.5 + else: + speed_ahead = 0 + loop_must_break = True + break + if 'railway' in n.tags and n.tags['railway']=='level_crossing': + if way_pts[count, 0] > 0 and traffic_confidence >= 50 and traffic_status == 'SLOW': + speed_ahead = 0 + speed_ahead_dist = max(0. , way_pts[count, 0] - 10.0) + loop_must_break = True + break + if 'traffic_calming' in n.tags: + if way_pts[count, 0] > 0: + if n.tags['traffic_calming']=='bump' or n.tags['traffic_calming']=='hump': + speed_ahead = 2.24 + speed_ahead_dist = way_pts[count, 0] + loop_must_break = True + break + elif n.tags['traffic_calming']=='chicane' or n.tags['traffic_calming']=='choker': + speed_ahead = 20/3.6 + speed_ahead_dist = way_pts[count, 0] + loop_must_break = True + break + elif n.tags['traffic_calming']=='yes': + speed_ahead = 40/3.6 + speed_ahead_dist = way_pts[count, 0] + loop_must_break = True + break + count += 1 + if loop_must_break: break + except (KeyError, IndexError, ValueError): + pass + # Find next way + way = way.next_way(heading) + if not way: + #print "no way break" + 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 len(self.way.nodes) < 10: + # maybe = False + # factor = max(111132.954*math.cos(float(lat)/180*3.141592), 111132.954 - 559.822 * math.cos( 2 * float(lat)/180*3.141592) + 1.175 * math.cos( 4 * float(lat)/180*3.141592)) + # for n in range(len(self.way.nodes)-1): + # if factor * distance(lat,lon,float(self.way.nodes[n].lat),float(self.way.nodes[n].lon),float(self.way.nodes[n+1].lat),float(self.way.nodes[n+1].lon)) < 10.0: + # maybe = True + # if not maybe: + # return False + if points is None: + points = self.points_in_car_frame(lat, lon, heading, True) + 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, True) + 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, True) + return np.min(np.linalg.norm(points, axis=1)) + + def points_in_car_frame(self, lat, lon, heading, flip): + 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 + + if points_carframe[-1,0] < points_carframe[0,0] and flip: + points_carframe = np.flipud(points_carframe) + + return points_carframe + + def next_way(self, heading): + results, tree, real_nodes, node_to_way, location_info = self.query_results + #print "way.id" + #print self.id + #print "node0.id" + #print self.way.nodes[0].id + #print "node-1.id" + #print self.way.nodes[-1].id + #print "heading" + #print heading + angle=heading - math.atan2(self.way.nodes[0].lon-self.way.nodes[-1].lon,self.way.nodes[0].lat-self.way.nodes[-1].lat)*180/3.14159265358979 - 180 + #print "angle before" + #print angle + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + #print "angle" + #print angle + backwards = abs(angle) > 90 + #print "backwards" + #print backwards + 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] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + if len(ways) == 2: + try: + if ways[0].tags['junction']=='roundabout' or ways[0].tags['junction']=='circular': + #print ("roundabout found") + way = Way(ways[0], self.query_results) + return way + except (KeyError, IndexError): + pass + try: + if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'): + if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id): + way = Way(ways[0], self.query_results) + return way + elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id): + way = Way(ways[1], self.query_results) + return way + except (KeyError, IndexError): + pass + ways = [w for w in ways if (w.nodes[0] == node or w.nodes[-1] == node)] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + # 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] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + if len(ways) == 2: + try: + if ways[0].tags['junction']=='roundabout' or ways[0].tags['junction']=='circular': + #print ("roundabout found") + way = Way(ways[0], self.query_results) + return way + except (KeyError, IndexError): + pass + try: + if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'): + if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id): + way = Way(ways[0], self.query_results) + return way + elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id): + way = Way(ways[1], self.query_results) + return way + except (KeyError, IndexError): + pass + # 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, True) + + try: + if way.way.tags['junction']=='roundabout' or way.way.tags['junction']=='circular': + break + except KeyError: + pass + if pnts is None: + pnts = new_pnts + valid = True + else: + new_pnts = np.delete(new_pnts,[0,0,0], axis=0) + pnts = np.vstack([pnts, new_pnts]) + + # Check current lookahead distance + max_dist = np.linalg.norm(pnts[-1, :]) + + if max_dist > 2 * lookahead: + break + + # Find next way + startid = way.way.nodes[0].id + endid = way.way.nodes[-1].id + way = way.next_way(heading) + if not way: + break + if not (way.way.nodes[0].id == startid or way.way.nodes[0].id == endid or way.way.nodes[-1].id == startid or way.way.nodes[-1].id == endid): + break + return pnts, valid diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 632cc2875fd85b..c0ee6f989f75cc 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -1,3 +1,8 @@ +#include +#include +#include +#include +#include #include "ui.hpp" #include #include @@ -19,6 +24,8 @@ extern "C"{ #include "sidebar.hpp" #include "paint_dp.hpp" +int border_shifter = 20; + // TODO: this is also hardcoded in common/transformations/camera.py // TODO: choose based on frame input size @@ -295,36 +302,105 @@ static void ui_draw_vision_maxspeed(UIState *s) { char maxspeed_str[32]; float maxspeed = s->scene.controls_state.getVCruise(); int maxspeed_calc = maxspeed * 0.6225 + 0.5; + float speedlimit = s->scene.speedlimit; + int speedlim_calc = speedlimit * 2.2369363 + 0.5; if (s->is_metric) { maxspeed_calc = maxspeed + 0.5; + speedlim_calc = speedlimit * 3.6 + 0.5; } - + int speed_lim_off = speedlim_calc * (1 + s->speed_lim_off / 100.0); bool is_cruise_set = (maxspeed != 0 && maxspeed != SET_SPEED_NA); - + bool is_speedlim_valid = s->scene.speedlimit_valid; + bool is_set_over_limit = is_speedlim_valid && s->scene.controls_state.getEnabled() && + is_cruise_set && maxspeed_calc > (speedlim_calc + speed_lim_off); int viz_maxspeed_w = 184; int viz_maxspeed_h = 202; - int viz_maxspeed_x = s->scene.viz_rect.x + (bdr_s*2); - int viz_maxspeed_y = s->scene.viz_rect.y + (bdr_s*1.5); + int viz_maxspeed_x = (s->video_rect.x + (bdr_is*2)); + int viz_maxspeed_y = (s->video_rect.y + (bdr_is*1.5)); int viz_maxspeed_xo = 180; - - viz_maxspeed_xo = 0; + viz_maxspeed_w += viz_maxspeed_xo; + viz_maxspeed_x += viz_maxspeed_w - (viz_maxspeed_xo * 2); + //viz_maxspeed_xo = 0; // Draw Background - ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, COLOR_BLACK_ALPHA(100), 30); + ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, + is_set_over_limit ? nvgRGBA(218, 111, 37, 180) : COLOR_BLACK_ALPHA(100), 30); // Draw Border NVGcolor color = COLOR_WHITE_ALPHA(100); + if (is_set_over_limit) { + color = COLOR_OCHRE; + } else if (is_speedlim_valid) { + color = s->is_ego_over_limit ? COLOR_WHITE_ALPHA(20) : COLOR_WHITE; + } ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, color, 20, 10); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); const int text_x = viz_maxspeed_x + (viz_maxspeed_xo / 2) + (viz_maxspeed_w / 2); - ui_draw_text(s->vg, text_x, 148, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), s->font_sans_regular); + ui_draw_text(s->vg, text_x, 148-border_shifter, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), s->font_sans_regular); if (is_cruise_set) { snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", maxspeed_calc); - ui_draw_text(s->vg, text_x, 242, maxspeed_str, 48 * 2.5, COLOR_WHITE, s->font_sans_bold); + ui_draw_text(s->vg, text_x, 242-border_shifter, maxspeed_str, 48 * 2.5, COLOR_WHITE, s->font_sans_bold); } else { - ui_draw_text(s->vg, text_x, 242, "N/A", 42 * 2.5, COLOR_WHITE_ALPHA(100), s->font_sans_semibold); + ui_draw_text(s->vg, text_x, 242-border_shifter, "-", 42 * 2.5, COLOR_WHITE_ALPHA(100), s->font_sans_semibold); + } +} + +static void ui_draw_vision_speedlimit(UIState *s) { + char speedlim_str[32]; + float speedlimit = s->scene.speedlimit; + int speedlim_calc = speedlimit * 2.2369363 + 0.5; + if (s->is_metric) { + speedlim_calc = speedlimit * 3.6 + 0.5; + } + + bool is_speedlim_valid = s->scene.speedlimit_valid; + float hysteresis_offset = 0.5; + if (s->is_ego_over_limit) { + hysteresis_offset = 0.0; + } + s->is_ego_over_limit = is_speedlim_valid && s->scene.controls_state.getVEgo() > (speedlimit + hysteresis_offset); + + int viz_speedlim_w = 180; + int viz_speedlim_h = 202; + int viz_speedlim_x = (s->video_rect.x + (bdr_is*2)); + int viz_speedlim_y = (s->video_rect.y + (bdr_is*1.5)); + if (!is_speedlim_valid) { + viz_speedlim_w -= 5; + viz_speedlim_h -= 10; + viz_speedlim_x += 9; + viz_speedlim_y += 5; + } + // Draw Background + NVGcolor color = COLOR_WHITE_ALPHA(100); + if (is_speedlim_valid && s->is_ego_over_limit) { + color = nvgRGBA(218, 111, 37, 180); + } else if (is_speedlim_valid) { + color = COLOR_WHITE; + } + ui_draw_rect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, color, is_speedlim_valid ? 30 : 15); + + // Draw Border + if (is_speedlim_valid) { + ui_draw_rect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, + s->is_ego_over_limit ? COLOR_OCHRE : COLOR_WHITE, 20, 10); + } + const float text_x = viz_speedlim_x + viz_speedlim_w / 2; + const float text_y = viz_speedlim_y + (is_speedlim_valid ? 50 : 45); + // Draw "Speed Limit" Text + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + color = is_speedlim_valid && s->is_ego_over_limit ? COLOR_WHITE : COLOR_BLACK; + ui_draw_text(s->vg, text_x + (is_speedlim_valid ? 6 : 0), text_y, "SMART", 50, color, s->font_sans_semibold); + ui_draw_text(s->vg, text_x + (is_speedlim_valid ? 6 : 0), text_y + 40, "SPEED", 50, color, s->font_sans_semibold); + + // Draw Speed Text + color = s->is_ego_over_limit ? COLOR_WHITE : COLOR_BLACK; + if (is_speedlim_valid) { + snprintf(speedlim_str, sizeof(speedlim_str), "%d", speedlim_calc); + ui_draw_text(s->vg, text_x, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), speedlim_str, 48*2.5, color, s->font_sans_bold); + } else { + ui_draw_text(s->vg, text_x, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), "N/A", 42*2.5, color, s->font_sans_semibold); } } @@ -377,12 +453,23 @@ static void ui_draw_vision_speed(UIState *s) { static void ui_draw_vision_event(UIState *s) { const int viz_event_w = 220; - const int viz_event_x = s->scene.viz_rect.right() - (viz_event_w + bdr_s*2); - const int viz_event_y = s->scene.viz_rect.y + (bdr_s*1.5); - if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { + const int viz_event_x = s->scene.viz_rect.right() - (viz_event_w + bdr_is*2); + const int viz_event_y = s->scene.viz_rect.y + (bdr_is*1.5); + if (s->scene.speedlimitahead_valid && s->scene.speedlimitaheaddistance < 300 && s->scene.controls_state.getEnabled() && s->limit_set_speed) { + const int img_turn_size = 160; + const int img_turn_x = viz_event_x-(img_turn_size/4)+80; + const int img_turn_y = viz_event_y+bdr_s-25; + float img_turn_alpha = 1.0f; + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, img_turn_x, img_turn_y, + img_turn_size, img_turn_size, 0, s->img_speed, img_turn_alpha); + nvgRect(s->vg, img_turn_x, img_turn_y, img_turn_size, img_turn_size); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); + } else if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { // draw winding road sign - const int img_turn_size = 160*1.5; - ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_s - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); + const int img_turn_size = 160*1.5*0.82; + ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_is - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); } else if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; @@ -394,9 +481,16 @@ static void ui_draw_vision_event(UIState *s) { } } +static void ui_draw_vision_map(UIState *s) { + const int map_size = 96; + const int map_x = (s->video_rect.x + (map_size * 3) + (bdr_s * 3)); + const int map_y = (s->scene.viz_rect.bottom() + ((footer_h - map_size) / 2)); + ui_draw_circle_image(s->vg, map_x, map_y, map_size, s->img_map, s->scene.map_valid); +} + static void ui_draw_vision_face(UIState *s) { const int face_size = 96; - const int face_x = (s->scene.viz_rect.x + face_size + (bdr_s * 2)); + const int face_x = (s->scene.viz_rect.x + face_size + (bdr_is * 2)); const int face_y = (s->scene.viz_rect.bottom() - footer_h + ((footer_h - face_size) / 2)); ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.dmonitoring_state.getFaceDetected()); } @@ -465,6 +559,7 @@ static void ui_draw_vision_header(UIState *s) { } if (s->scene.dpUiMaxSpeed) { ui_draw_vision_maxspeed(s); + ui_draw_vision_speedlimit(s); } if (s->scene.dpUiSpeed) { ui_draw_vision_speed(s); @@ -477,6 +572,7 @@ static void ui_draw_vision_header(UIState *s) { static void ui_draw_vision_footer(UIState *s) { if (s->scene.dpUiFace) { ui_draw_vision_face(s); + ui_draw_vision_map(s); } if ((int)s->scene.dpDynamicFollow > 0) { ui_draw_df_button(s); @@ -510,9 +606,9 @@ void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, color.a *= s->alert_blinking_alpha; int alr_s = alert_size_map[va_size]; - const int alr_x = scene->viz_rect.x - bdr_s + 100; - const int alr_w = scene->viz_rect.w + (bdr_s*2) - 200; - const int alr_h = alr_s+(va_size==cereal::ControlsState::AlertSize::NONE?0:bdr_s) - 100; + const int alr_x = scene->viz_rect.x - bdr_is + 100; + const int alr_w = scene->viz_rect.w + (bdr_is*2) - 200; + const int alr_h = alr_s+(va_size==cereal::ControlsState::AlertSize::NONE?0:bdr_is) - 100; const int alr_y = s->fb_h-alr_h - 100; ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, color, 20); @@ -702,6 +798,10 @@ void ui_nvg_init(UIState *s) { s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1); assert(s->img_wheel != 0); + s->img_map = nvgCreateImage(s->vg, "../assets/img_map.png", 1); + assert(s->img_map != 0); + s->img_speed = nvgCreateImage(s->vg, "../assets/img_trafficSign_speedahead.png", 1); + assert(s->img_speed != 0); s->img_turn = nvgCreateImage(s->vg, "../assets/img_trafficSign_turn.png", 1); assert(s->img_turn != 0); s->img_face = nvgCreateImage(s->vg, "../assets/img_driver_face.png", 1); diff --git a/selfdrive/ui/paint_dp.cc b/selfdrive/ui/paint_dp.cc index 1a0515699a617e..4fbf7c413ab687 100644 --- a/selfdrive/ui/paint_dp.cc +++ b/selfdrive/ui/paint_dp.cc @@ -354,12 +354,12 @@ void bb_ui_draw_measures_right(UIState *s, int bb_x, int bb_y, int bb_w ) { void ui_draw_bbui(UIState *s) { const int bb_dml_w = 184; - const int bb_dml_x = s->scene.viz_rect.x + (bdr_s*2); - const int bb_dml_y = s->scene.viz_rect.y + (bdr_s*1.5) + 220; + const int bb_dml_x = s->scene.viz_rect.x + (bdr_is*2); + const int bb_dml_y = s->scene.viz_rect.y + (bdr_is*1.5) + 220; const int bb_dmr_w = 184; - const int bb_dmr_x =s->scene.viz_rect.x + s->scene.viz_rect.w - bb_dmr_w - (bdr_s * 2); - const int bb_dmr_y = s->scene.viz_rect.y + (bdr_s*1.5) + 220; + const int bb_dmr_x =s->scene.viz_rect.x + s->scene.viz_rect.w - bb_dmr_w - (bdr_is * 2); + const int bb_dmr_y = s->scene.viz_rect.y + (bdr_is*1.5) + 220; bb_ui_draw_measures_right(s, bb_dml_x, bb_dml_y, bb_dml_w); bb_ui_draw_measures_left(s, bb_dmr_x, bb_dmr_y, bb_dmr_w); -} \ No newline at end of file +} diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2fb22f64f11980..0b2a110bacd8ad 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -25,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param } void ui_init(UIState *s) { - s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", + s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "liveMapData", "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents", "dragonConf", "carState"}); @@ -204,6 +204,13 @@ void update_sockets(UIState *s) { if (sm.updated("thermal")) { scene.thermal = sm["thermal"].getThermal(); } + if (sm.updated("liveMapData")) { + scene.map_valid = sm["liveMapData"].getLiveMapData().getMapValid(); + scene.speedlimit = sm["liveMapData"].getLiveMapData().getSpeedLimit(); + scene.speedlimit_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitValid(); + scene.speedlimitahead_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadValid(); + scene.speedlimitaheaddistance = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadDistance(); + } if (sm.updated("ubloxGnss")) { auto data = sm["ubloxGnss"].getUbloxGnss(); if (data.which() == cereal::UbloxGnss::MEASUREMENT_REPORT) { @@ -332,6 +339,10 @@ void ui_update(UIState *s) { // Read params if ((s->sm)->frame % (5*UI_FREQ) == 0) { read_param(&s->is_metric, "IsMetric"); + } else if ((s->sm)->frame % (7*UI_FREQ) == 0) { + read_param(&s->speed_lim_off, "SpeedLimitOffset"); + } else if ((s->sm)->frame % (11*UI_FREQ) == 0) { + read_param(&s->limit_set_speed, "LimitSetSpeed"); } else if ((s->sm)->frame % (6*UI_FREQ) == 0) { int param_read = read_param(&s->last_athena_ping, "LastAthenaPingTime"); if (param_read != 0) { // Failed to read param diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index afeb64468f7268..18a743559861ac 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -34,6 +34,8 @@ #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) #define COLOR_RED nvgRGBA(201, 34, 49, 255) #define COLOR_RED_ALPHA(x) nvgRGBA(201, 34, 49, x) +#define COLOR_OCHRE nvgRGBA(218, 111, 37, 255) +#define COLOR_OCHRE_ALPHA(x) nvgRGBA(218, 111, 37, x) #define UI_BUF_COUNT 4 @@ -48,7 +50,8 @@ typedef struct Rect { } Rect; const int sbr_w = 300; -const int bdr_s = 30; +const int bdr_s = 10; +const int bdr_is = 30; const int header_h = 420; const int footer_h = 280; const Rect settings_btn = {50, 35, 200, 117}; @@ -107,6 +110,12 @@ typedef struct UIScene { mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. bool world_objects_visible; + float speedlimit; + bool speedlimit_valid; + float speedlimitaheaddistance; + bool speedlimitahead_valid; + bool map_valid; + bool is_rhd; bool frontview; bool uilayout_sidebarcollapsed; @@ -219,6 +228,8 @@ typedef struct UIState { int img_battery; int img_battery_charging; int img_network[6]; + int img_map; + int img_speed; SubMaster *sm; @@ -250,6 +261,9 @@ typedef struct UIState { bool ignition; bool is_metric; bool longitudinal_control; + bool limit_set_speed; + bool is_ego_over_limit; + float speed_lim_off; uint64_t last_athena_ping; uint64_t started_frame;