mapd (dragonpilot-community#81)

* mapd * do not clean the cache * exe * try this * smart speed. * Revert "smart speed." This reverts commit 19c7fb3. * planener crash * miss no_curvature_speed * 10mph * uidebug * Revert "uidebug" This reverts commit fbaf01f. * mapd ui (dragonpilot-community#82) * bordershifter * smartspeed variables, sm logic, and img * speedlimit ui elements * add back smartspeed * more speedlimit * fixes * map_size * missing assets * Smartspeed (dragonpilot-community#83) * bordershifter * smartspeed variables, sm logic, and img * speedlimit ui elements * add back smartspeed * more speedlimit * fixes * map_size * missing assets * missing param Co-authored-by: Comma Device <device@comma.ai>
loveks520 · Dec 12, 2020 · 2b69ba4 · 2b69ba4
1 parent 55396eb
commit 2b69ba4
Show file tree

Hide file tree

Showing 17 changed files with 2,113 additions and 36 deletions.
diff --git 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;
-}
+}
diff --git 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
@@ -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
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -0,0 +1 @@
+