0.5.7 hotfixes

README.md

@@ -94,13 +94,13 @@ Supported Cars
 | Toyota               | Rav4 2017-18             | All                  | Yes     | Yes<sup>2</sup>| 20mph            | 0mph           | Toyota            |
 | Toyota               | Rav4 Hybrid 2017-18      | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 
-<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
-<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota)
-<sup>3</sup>[GM installation guide](https://zoneos.com/volt/).
-<sup>4</sup>It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/).
-<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-<sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed ofor the 2019 Sante Fe; pinout may differ for other Hyundais. <br />
-<sup>7</sup>Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/) <br />
+<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***  
+<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota)  
+<sup>3</sup>[GM installation guide](https://zoneos.com/volt/).  
+<sup>4</sup>It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/).  
+<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.  
+<sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed ofor the 2019 Sante Fe; pinout may differ for other Hyundais.  
+<sup>7</sup>Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/).  
 
 Community Maintained Cars
 ------

selfdrive/mapd/mapd.py

@@ -15,17 +15,22 @@
   args.extend(sys.argv)
   os.execv(sys.executable, args)
 
+import os
+import sys
 import time
 import zmq
 import threading
 import numpy as np
 import overpy
 from collections import defaultdict
 
+from common.params import Params
 from common.transformations.coordinates import geodetic2ecef
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
 from mapd_helpers import LOOKAHEAD_TIME, MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points
+import selfdrive.crash as crash
+from selfdrive.version import version, dirty
 
 
 OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter"
@@ -39,13 +44,37 @@
 last_query_pos = None
 
 
+def setup_thread_excepthook():
+  """
+  Workaround for `sys.excepthook` thread bug from:
+  http://bugs.python.org/issue1230540
+  Call once from the main thread before creating any threads.
+  Source: https://stackoverflow.com/a/31622038
+  """
+  init_original = threading.Thread.__init__
+
+  def init(self, *args, **kwargs):
+    init_original(self, *args, **kwargs)
+    run_original = self.run
+
+    def run_with_except_hook(*args2, **kwargs2):
+      try:
+        run_original(*args2, **kwargs2)
+      except Exception:
+        sys.excepthook(*sys.exc_info())
+
+    self.run = run_with_except_hook
+
+  threading.Thread.__init__ = init
+
+
 def build_way_query(lat, lon, radius=50):
   """Builds a query to find all highways within a given radius around a point"""
   pos = "  (around:%f,%f,%f)" % (radius, lat, lon)
   q = """(
   way
   """ + pos + """
-  [highway];
+  [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"];
   >;);out;
   """
   return q
@@ -56,6 +85,7 @@ def query_thread():
   api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.)
 
   while True:
+    time.sleep(1)
     if last_gps is not None:
       fix_ok = last_gps.flags & 1
       if not fix_ok:
@@ -99,7 +129,6 @@ def query_thread():
         query_lock.acquire()
         last_query_result = None
         query_lock.release()
-    time.sleep(1)
 
 
 def mapsd_thread():
@@ -145,7 +174,7 @@ def mapsd_thread():
       speed = gps.speed
 
       query_lock.acquire()
-      cur_way = Way.closest(last_query_result, lat, lon, heading)
+      cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
       if cur_way is not None:
         pnts, curvature_valid = cur_way.get_lookahead(last_query_result, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
 
@@ -157,7 +186,7 @@ def mapsd_thread():
           curvature_valid = False
 
         # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
-        if curvature_valid:
+        if curvature_valid and pnts.shape[0] > 3:
           # Compute the curvature for each point
           with np.errstate(divide='ignore'):
             circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
@@ -224,6 +253,13 @@ def mapsd_thread():
 
 
 def main(gctx=None):
+  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_thread_excepthook()
   main_thread = threading.Thread(target=mapsd_thread)
   main_thread.daemon = True
   main_thread.start()

selfdrive/mapd/mapd_helpers.py

@@ -49,7 +49,7 @@ def __init__(self, way):
     self.points = np.asarray(points)
 
   @classmethod
-  def closest(cls, query_results, lat, lon, heading):
+  def closest(cls, query_results, lat, lon, heading, prev_way=None):
     results, tree, real_nodes, node_to_way = query_results
 
     cur_pos = geodetic2ecef((lat, lon, 0))
@@ -104,6 +104,12 @@ def closest(cls, query_results, lat, lon, heading):
       # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error
       # (A 20 degree heading offset results in an a of about 1/3)
       score = abs(a) * 60. + abs(b)
+
+      # Prefer same type of road
+      if prev_way is not None:
+        if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''):
+          score *= 0.5
+
       if closest_way is None or score < best_score:
         closest_way = way
         best_score = score