Merge branch 'master' into dev-priv/master

common/params.cc

@@ -210,6 +210,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"BelowSpeedPause", PERSISTENT},
     {"BrakeLights", PERSISTENT},
     {"BrightnessControl", PERSISTENT},
+    {"ButtonAutoHide", PERSISTENT},
     {"CameraControl", PERSISTENT},
     {"CameraControlToggle", PERSISTENT},
     {"CameraOffset", PERSISTENT},

selfdrive/mapd/lib/NodesData.py

@@ -6,7 +6,7 @@ from scipy.interpolate import splev, splprep
 
 
 _TURN_CURVATURE_THRESHOLD = 0.002  # 1/mts. A curvature over this value will generate a speed limit section.
-_MAX_LAT_ACC = 2.3  # Maximum lateral acceleration in turns.
+_MAX_LAT_ACC = 2.  # Maximum lateral acceleration in turns.
 _SPLINE_EVAL_STEP = 5  # mts for spline evaluation for curvature calculation
 _MIN_SPEED_SECTION_LENGTH = 100.  # mts. Sections below this value will not be split in smaller sections.
 _MAX_CURV_DEVIATION_FOR_SPLIT = 2.  # Split a speed section if the max curvature deviates from mean by this factor.

selfdrive/mapd/lib/WayRelation.py

@@ -1,4 +1,4 @@
-from selfdrive.mapd.lib.geo import DIRECTION, R, vectors, bearing_to_points, distance_to_points
+from selfdrive.mapd.lib.geo import DIRECTION, R, vectors, bearing_to_points, distance_to_points, point_on_line
 from selfdrive.mapd.lib.osm import create_way
 from common.conversions import Conversions as CV
 from selfdrive.mapd.config import LANE_WIDTH
@@ -273,7 +273,6 @@ class WayRelation():
 
     # - Get the distance and bearings from location to all nodes. (N)
     bearings = bearing_to_points(location_rad, self._nodes_np)
-    distances = distance_to_points(location_rad, self._nodes_np)
 
     # - Get absolute bearing delta to current driving bearing. (N)
     delta = np.abs(bearing_rad - bearings)
@@ -288,16 +287,8 @@ class WayRelation():
     if len(possible_idxs) == 0:
       return
 
-    # - Find then angle formed between the vectors from the current location to consecutive nodes. This is the
-    # value of the difference in the bearings of the vectors.
-    teta = np.diff(bearings)
-
-    # - When two consecutive nodes will be ahead and behind, they will form a triangle with the current location.
-    # We find the closest distance to the way by solving the area of the triangle and finding the height (h).
-    # We must use the absolute value of the sin of the angle in the formula, which is equivalent to ensure we
-    # are considering the smallest of the two angles formed between the two vectors.
-    # https://www.mathsisfun.com/algebra/trig-area-triangle-without-right-angle.html
-    h = distances[:-1] * distances[1:] * np.abs(np.sin(teta)) / self._way_distances
+    projections = point_on_line(self._nodes_np[:-1], self._nodes_np[1:], location_rad)
+    h = distance_to_points(location_rad, projections)
 
     # - Calculate the delta between driving bearing and way bearings. (N-1)
     bw_delta = self._way_bearings - bearing_rad
@@ -313,16 +304,22 @@ class WayRelation():
     # - Get the index where the distance to the way is minimum. That is the chosen location.
     min_h_possible_idx = np.argmin(h_possible)
     min_delta_idx = possible_idxs[min_h_possible_idx]
+    projection = projections[min_delta_idx]
 
-    # - If the distance to the way is over 4 standard deviations of the gps accuracy + half the maximum road width
+    # - If the distance to the way is over 4 standard deviations of the gps accuracy + the maximum road width
     # estimate, then we are way too far to stick to this way (i.e. we are not on this way anymore)
-    half_road_width_estimate = self.lanes * LANE_WIDTH / 2.
-    if h_possible[min_h_possible_idx] > 4. * location_stdev + half_road_width_estimate:
+    # In theory the osm path is centered on the road which means half the road width would cover the whole road.
+    # however, often times the osm path is not perfectly centered so we'll make the possible route more lenient by using
+    # the full road width.
+    road_width_estimate = self.lanes * LANE_WIDTH
+    half_road_width_estimate = road_width_estimate / 2.
+    if h_possible[min_h_possible_idx] > 4. * location_stdev + road_width_estimate:
       return
 
-    # - If the distance to the road is greater than 2 standard deviations of the gps accuracy + half the maximum road
-    # width estimate then we are most likely diverting from this route.
-    diverting = h_possible[min_h_possible_idx] > 2. * location_stdev + half_road_width_estimate
+    # If the distance to the road is greater than 2 standard deviations of the gps accuracy + half the maximum road
+    # width estimate + 1 lane width then we are most likely diverting from this route. Adding a lane width to give
+    # leniency to not perfectly centered osm paths
+    diverting = h_possible[min_h_possible_idx] > 2. * location_stdev + half_road_width_estimate + LANE_WIDTH
 
     # Populate location variables with result
     if is_ahead[min_delta_idx]:
@@ -336,10 +333,10 @@ class WayRelation():
 
     self._distance_to_way = h[min_delta_idx]
     self._active_bearing_delta = abs_sin_bw_delta_possible[min_h_possible_idx]
-    # TODO: The distance to node ahead currently represent the distance from the GPS fix location.
-    # It would be perhaps more accurate to use the distance on the projection over the direct line between
-    # the two nodes.
-    self.distance_to_node_ahead = distances[self.ahead_idx]
+
+    # find the distance to the next node by projecting our location onto the line and finding the delta between that
+    # point and the next point on the route
+    self.distance_to_node_ahead = distance_to_points(projection, np.array([self._nodes_np[self.ahead_idx]]))[0]
     self.active = True
     self.diverting = diverting
     self.location_rad = location_rad

selfdrive/mapd/lib/geo.py

@@ -47,6 +47,18 @@ def bearing_to_points(point, points):
   y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1]))
   return np.arctan2(x, y)
 
+def point_on_line(start_points, end_points, point, extend_line = False):
+  """project a single point onto each line for an np array of start points and end points
+  ref: https://stackoverflow.com/a/61342198
+  """
+  ap = np.subtract(point, start_points)
+  ab = np.subtract(end_points, start_points)
+  t = np.array([np.dot(ap[i], ab[i]) / np.dot(ab[i], ab[i]) for i in range(len(ap))])
+  # if you need the the closest point belonging to the segment
+  if not extend_line:
+    t = np.maximum(0, np.minimum(1, t))
+  result = np.add(start_points, np.array([t[i] * ab[i] for i in range(len(t))]))
+  return result
 
 def distance_to_points(point, points):
   """Calculate the distance of the vectors between `point` and each one of the entries in `points`.