diff --git a/common/params.cc b/common/params.cc index a54aee61bf..9c1816b32d 100644 --- a/common/params.cc +++ b/common/params.cc @@ -210,6 +210,7 @@ std::unordered_map keys = { {"BelowSpeedPause", PERSISTENT}, {"BrakeLights", PERSISTENT}, {"BrightnessControl", PERSISTENT}, + {"ButtonAutoHide", PERSISTENT}, {"CameraControl", PERSISTENT}, {"CameraControlToggle", PERSISTENT}, {"CameraOffset", PERSISTENT}, diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py index d67caa8288..72962eacaf 100644 --- a/selfdrive/mapd/lib/NodesData.py +++ b/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. diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index 8c765b47d9..db5b8c38ae 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/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 diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py index 8f6232225f..51947481ae 100644 --- a/selfdrive/mapd/lib/geo.py +++ b/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`.