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

@@ -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.

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)

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."""

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 = []

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:

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 = []