From 4d8e0520f9dd9c65dd1b8749c81afec1adcbe376 Mon Sep 17 00:00:00 2001 From: Saul Field Date: Mon, 17 Apr 2023 10:27:50 -0400 Subject: [PATCH] Argoverse waypoint improvements in junctions (#1944) * Resolve waypoints in junctions for Argoverse maps * Filter out waypoints behind vehicle * Improve waypoints in junctions * Change closest_lanepoints() to only take a single pose --- CHANGELOG.md | 1 + smarts/core/argoverse_map.py | 50 +++++++++++++++++++++++++-- smarts/core/lanepoints.py | 12 +++---- smarts/core/opendrive_road_network.py | 2 +- smarts/core/sumo_road_network.py | 4 +-- smarts/core/waymo_map.py | 2 +- 6 files changed, 59 insertions(+), 12 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e290e3463f..6f15468185 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ Copy and pasting the git commit messages is __NOT__ enough. - Sstudio generated scenario vehicle traffic ids are now shortened. ### Deprecated ### Fixed +- Fixed issues related to waypoints in junctions on Argoverse maps. Waypoints will now be generated for all paths leading through the lane(s) the vehicle is on. - Fixed an issue where Argoverse scenarios with a `Mission` would not run properly. - `Trip.actor` field is now effective. Previously `actor` had no effect. - Fixed an issue where building sumo scenarios would sometimes stall. diff --git a/smarts/core/argoverse_map.py b/smarts/core/argoverse_map.py index 98ec48b81e..791b8f9e43 100644 --- a/smarts/core/argoverse_map.py +++ b/smarts/core/argoverse_map.py @@ -936,6 +936,21 @@ def _lanepoints(self): assert self._map_spec.lanepoint_spacing > 0 return LanePoints.from_argoverse(self, spacing=self._map_spec.lanepoint_spacing) + def _resolve_in_junction(self, junction_lane: RoadMap.Lane) -> List[List[str]]: + # There are no paths we can trace back through the junction, so return + if len(junction_lane.road.incoming_roads) == 0: + return [] + + # Trace back to the road that leads into the junction + inc_road: RoadMap.Road = junction_lane.road.incoming_roads[0] + paths = [] + for out_road in inc_road.outgoing_roads: + road_ids = [out_road.road_id] + [ + road.road_id for road in out_road.outgoing_roads + ] + paths.append(road_ids) + return paths + def waypoint_paths( self, pose: Pose, @@ -948,11 +963,42 @@ def waypoint_paths( road_ids = [road.road_id for road in route.roads] if road_ids: return self._waypoint_paths_along_route(pose.point, lookahead, road_ids) + closest_lps = self._lanepoints.closest_lanepoints( - [pose], within_radius=within_radius + pose, within_radius=within_radius ) - closest_lane = closest_lps[0].lane + closest_lane: RoadMap.Lane = closest_lps[0].lane + waypoint_paths = [] + if closest_lane.in_junction: + junction_lanes: Set[RoadMap.Lane] = set([closest_lane]) + for lp in closest_lps: + rel_heading = lp.pose.heading.relative_to(pose.heading) + if ( + lp.lane.in_junction + and abs(rel_heading) < np.pi / 2 + and lp.lane.contains_point(pose.point) + ): + junction_lanes.add(lp.lane) + + paths = set() + for junction_lane in junction_lanes: + paths_for_lane = self._resolve_in_junction(junction_lane) + for path in paths_for_lane: + paths.add(tuple(path)) + + for road_ids in paths: + new_paths = self._waypoint_paths_along_route( + pose.point, lookahead, road_ids + ) + for path in new_paths: + if ( + len(path) > 0 + and np.linalg.norm(path[0].pos - pose.position[:2]) < 5 + ): + waypoint_paths.append(path) + return waypoint_paths + for lane in closest_lane.road.lanes: waypoint_paths += lane._waypoint_paths_at(pose.point, lookahead) return sorted(waypoint_paths, key=lambda p: p[0].lane_index) diff --git a/smarts/core/lanepoints.py b/smarts/core/lanepoints.py index 0527b6894d..55bf0ce99a 100644 --- a/smarts/core/lanepoints.py +++ b/smarts/core/lanepoints.py @@ -858,14 +858,14 @@ def _closest_linked_lp_in_kd_tree_with_pose_batched( def closest_lanepoints( self, - poses: Sequence[Pose], + pose: Pose, within_radius: float = 10, maximum_count: int = 10, ) -> List[LanePoint]: - """Get the lanepoints closest to the given poses. + """Get the lanepoints closest to the given pose. Args: - poses: - The poses to look around for lanepoints. + pose: + The pose to look around for lanepoints. within_radius: The radius which lanepoints can be found from the given poses. maximum_count: @@ -874,14 +874,14 @@ def closest_lanepoints( lanepoints = self._linked_lanepoints kd_tree = self._lanepoints_kd_tree linked_lanepoints = LanePoints._closest_linked_lp_in_kd_tree_with_pose_batched( - poses, + [pose], lanepoints, kd_tree, within_radius=within_radius, k=maximum_count, filter_composites=True, ) - return [l_lps[0].lp for l_lps in linked_lanepoints] + return [l_lps.lp for l_lps in linked_lanepoints[0]] def closest_lanepoint_on_lane_to_point(self, point, lane_id: str) -> LanePoint: """Returns the closest lanepoint on the given lane to the given world coordinate.""" diff --git a/smarts/core/opendrive_road_network.py b/smarts/core/opendrive_road_network.py index 31eb40d54b..e43f32bd9d 100644 --- a/smarts/core/opendrive_road_network.py +++ b/smarts/core/opendrive_road_network.py @@ -1656,7 +1656,7 @@ def waypoint_paths( if road_ids: return self._waypoint_paths_along_route(pose.point, lookahead, road_ids) closest_lps = self._lanepoints.closest_lanepoints( - [pose], within_radius=within_radius + pose, within_radius=within_radius ) closest_lane = closest_lps[0].lane waypoint_paths = [] diff --git a/smarts/core/sumo_road_network.py b/smarts/core/sumo_road_network.py index e52862a917..3a50269c80 100644 --- a/smarts/core/sumo_road_network.py +++ b/smarts/core/sumo_road_network.py @@ -960,7 +960,7 @@ def waypoint_paths( ) else: closest_lps = self._lanepoints.closest_lanepoints( - [pose], within_radius=within_radius + pose, within_radius=within_radius ) closest_lane = closest_lps[0].lane # TAI: the above lines could be replaced by: @@ -975,7 +975,7 @@ def _resolve_in_junction(self, pose: Pose) -> List[RoadMap.Lane]: # We take the 10 closest lanepoints then filter down to that which has # the closest heading. This way we get the lanepoint on our lane instead of # a potentially closer lane that is on a different junction connection. - closest_lps = self._lanepoints.closest_lanepoints([pose], within_radius=None) + closest_lps = self._lanepoints.closest_lanepoints(pose, within_radius=None) closest_lps.sort(key=lambda lp: abs(pose.heading - lp.pose.heading)) lane = closest_lps[0].lane if not lane.in_junction: diff --git a/smarts/core/waymo_map.py b/smarts/core/waymo_map.py index 6f01b8e69c..aec00293b4 100644 --- a/smarts/core/waymo_map.py +++ b/smarts/core/waymo_map.py @@ -1833,7 +1833,7 @@ def waypoint_paths( if road_ids: return self._waypoint_paths_along_route(pose.point, lookahead, road_ids) closest_lps = self._lanepoints.closest_lanepoints( - [pose], within_radius=within_radius + pose, within_radius=within_radius ) closest_lane = closest_lps[0].lane waypoint_paths = []