From 47c189bd47158af014d6b636f92822a1c2e0fbf0 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sat, 22 Jun 2019 20:44:26 +0200 Subject: [PATCH] Fix slow down only working for one side --- selfdrive/mapd/mapd_helpers.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index fac20faf2d487e..779fb9b5a16fc1 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -225,7 +225,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): way_pts = way.points_in_car_frame(lat, lon, heading) # Check current lookahead distance - max_dist = np.linalg.norm(way_pts[-1, :]) + max_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[-1, :])) if max_dist > 2 * lookahead: #print "max_dist break" @@ -242,7 +242,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): #print spd if spd < current_speed_limit: speed_ahead = spd - min_dist = np.linalg.norm(way_pts[1, :]) + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[-1, :])) speed_ahead_dist = min_dist #print "slower speed found"