diff --git a/README.md b/README.md index f105957ed3..88f8eda8af 100644 --- a/README.md +++ b/README.md @@ -94,13 +94,13 @@ Supported Cars | Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota | | Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | -1[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)*** -2When 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) -3[GM installation guide](https://zoneos.com/volt/). -4It 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/). -528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. -6Open 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.
-7Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/)
+1[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)*** +2When 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) +3[GM installation guide](https://zoneos.com/volt/). +4It 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/). +528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. +6Open 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. +7Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/). Community Maintained Cars ------ diff --git a/installer/updater/updater b/installer/updater/updater index 37f149f8bf..898644280f 100755 Binary files a/installer/updater/updater and b/installer/updater/updater differ diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index e121885909..e1a9595e8c 100755 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -15,6 +15,8 @@ except ImportError as e: args.extend(sys.argv) os.execv(sys.executable, args) +import os +import sys import time import zmq import threading @@ -22,10 +24,13 @@ 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_result = None 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() diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index f3c9c3445a..6d84fbd7d5 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -49,7 +49,7 @@ class 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 @@ class Way: # 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