From fbd4771469497908571ba0b9e24ce306cb9625e0 Mon Sep 17 00:00:00 2001 From: Jacob Pfeifer Date: Thu, 10 Aug 2023 22:43:39 -0400 Subject: [PATCH 1/2] mapd: utilize advisory speed limit in curves (#142) Co-authored-by: Jason Wen --- selfdrive/mapd/lib/NodesData.py | 56 ++++++++++++++++++++++++++----- selfdrive/mapd/lib/WayRelation.py | 16 +++++++++ 2 files changed, 63 insertions(+), 9 deletions(-) diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py index 9129923b92..a4cbf06b5c 100644 --- a/selfdrive/mapd/lib/NodesData.py +++ b/selfdrive/mapd/lib/NodesData.py @@ -17,10 +17,11 @@ _DIVERTION_SEARCH_RANGE = [-200., 50.] # mt. Range of distance to current locat def nodes_raw_data_array_for_wr(wr, drop_last=False): - """Provides an array of raw node data (id, lat, lon, speed_limit) for all nodes in way relation + """Provides an array of raw node data (id, lat, lon, speed_limit, advisory_speed_limit) for all nodes in way relation """ sl = wr.speed_limit - data = np.array([(n.id, n.lat, n.lon, sl) for n in wr.way.nodes], dtype=float) + asl = wr.advisory_speed_limit + data = np.array([(n.id, n.lat, n.lon, sl, asl) for n in wr.way.nodes], dtype=float) # reverse the order if way direction is backwards if wr.direction == DIRECTION.BACKWARD: @@ -243,12 +244,13 @@ class NodeDataIdx(Enum): lat = 1 lon = 2 speed_limit = 3 - x = 4 # x value of cartesian vector representing the section between last node and this node. - y = 5 # y value of cartesian vector representing the section between last node and this node. - dist_prev = 6 # distance to previous node. - dist_next = 7 # distance to next node - dist_route = 8 # cumulative distance on route - bearing = 9 # bearing of the vector departing from this node. + advisory_speed_limit = 4 + x = 5 # x value of cartesian vector representing the section between last node and this node. + y = 6 # y value of cartesian vector representing the section between last node and this node. + dist_prev = 7 # distance to previous node. + dist_next = 8 # distance to next node + dist_route = 9 # cumulative distance on route + bearing = 10 # bearing of the vector departing from this node. class NodesData: @@ -282,7 +284,7 @@ class NodesData: vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points) # append calculations to nodes_data - # nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, dist_route, bearing] + # nodes_data structure: [id, lat, lon, speed_limit, advisory_speed_limit, x, y, dist_prev, dist_next, dist_route, bearing] self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing)) # Build route diversion options data from the wr_index. @@ -335,6 +337,35 @@ class NodesData: return limits_ahead + + def advisory_speed_limits_ahead(self, ahead_idx, distance_to_node_ahead): + """Returns and array of SpeedLimitSection objects for the actual route ahead of current location + """ + if len(self._nodes_data) == 0 or ahead_idx is None: + return [] + + # Find the cumulative distances where speed limit changes. Build Speed limit sections for those. + dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])) + dist = np.cumsum(dist, axis=0) + sl = self.get(NodeDataIdx.advisory_speed_limit)[ahead_idx - 1:] + sl_next = np.concatenate((sl[1:], [0.])) + + # Create a boolean mask where speed limit changes and filter values + sl_change = sl != sl_next + distances = dist[sl_change] + speed_limits = sl[sl_change] + + # Create speed limits sections combining all continuous nodes that have same speed limit value. + start = 0. + limits_ahead = [] + for idx, end in enumerate(distances): + if speed_limits[idx] != None and speed_limits[idx] > 0: + limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx])) + start = end + + return limits_ahead + + def distance_to_end(self, ahead_idx, distance_to_node_ahead): if len(self._nodes_data) == 0 or ahead_idx is None: return None @@ -361,6 +392,13 @@ class NodesData: # Create speed limits sections limits_ahead = [TurnSpeedLimitSection(max(0., d[0]), d[1], d[2], d[3]) for d in data] + advisory_speed_limits_ahead = self.advisory_speed_limits_ahead(ahead_idx, distance_to_node_ahead) + for advisory_limit in advisory_speed_limits_ahead: + for limit in limits_ahead: + if limit.start >= advisory_limit.start and limit.end <= advisory_limit.end: + limit.value = advisory_limit.value + + return limits_ahead def possible_divertions(self, ahead_idx, distance_to_node_ahead): diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index d3c5313a0a..0ba49b1013 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/selfdrive/mapd/lib/WayRelation.py @@ -151,6 +151,7 @@ class WayRelation(): self.reset_location_variables() self.direction = DIRECTION.NONE self._speed_limit = None + self._advisory_speed_limit = None self._one_way = way.tags.get("oneway") self.name = way.tags.get('name') self.ref = way.tags.get('ref') @@ -293,9 +294,11 @@ class WayRelation(): self.location_rad = location_rad self.bearing_rad = bearing_rad self._speed_limit = None + self._advisory_speed_limit = None def update_direction_from_starting_node(self, start_node_id): self._speed_limit = None + self._advisory_speed_limit = None if self.edge_nodes_ids[0] == start_node_id: self.direction = DIRECTION.FORWARD elif self.edge_nodes_ids[-1] == start_node_id: @@ -341,6 +344,19 @@ class WayRelation(): self._speed_limit = limit return self._speed_limit + + @property + def advisory_speed_limit(self): + if self._advisory_speed_limit is not None: + return self._advisory_speed_limit + + limit_string = self.way.tags.get("maxspeed:advisory") + limit = speed_limit_for_osm_tag_limit_string(limit_string) + + self._advisory_speed_limit = limit + return self._advisory_speed_limit + + @property def active_bearing_delta(self): """Returns the sine of the delta between the current location bearing and the exact From e57d077fd3c8b324011b5248ea3eb68235c7b0cb Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 10 Aug 2023 22:45:00 -0400 Subject: [PATCH 2/2] Update CHANGELOGS.md --- CHANGELOGS.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index a3b8d3ea6e..1e403f8bc6 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -11,6 +11,7 @@ sunnypilot - 0.9.4.1 (2023-08-xx) * REMOVED: Speed Limit Style override * Honda Accord 2016-17 support thanks to mlocoteta! * Serial Steering hardware required. For more information, see https://github.com/mlocoteta/serialSteeringHardware +* mapd: utilize advisory speed limit in curves (#142) thanks to pfeiferj! sunnypilot - 0.9.3.1 (2023-07-09) ========================