From 639db0b6544dd07ef2c2062c0d289db5ed7a4d84 Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Wed, 24 May 2023 13:08:51 -0400
Subject: [PATCH] Fix mission waypoints (#2035)

* Fix mission waypoints

* Update variable name and description

* Update spelling wordlist
---
 CHANGELOG.md               |  1 +
 docs/spelling_wordlist.txt |  2 ++
 smarts/core/lanepoints.py  | 30 ++++++++++++++++++++++--------
 3 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index bd0dded488..d1866569c0 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -38,6 +38,7 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - The routes of `SumoTrafficSimulation` traffic vehicles are now preserved to be passed over to other traffic simulators when the `SumoTrafficSimulation` disconnects.
 - `SumoTrafficSimulation` no longer reports that it manages vehicles when it is disconnected.
 - Fixed waypoints so that they will always be the length of the `lookahead` parameter, even in junctions.
+- Fixed an issue where a single waypoint would appear in off-route lanes for missions with a goal.
 ### Removed
 - Removed the following dependencies from smarts: `pandas`, `rich`, `twisted`, `sh`.
 - Moved `baselines/marl_benchmark` from this repository to `smarts-project/smarts-project.rl` repository.
diff --git a/docs/spelling_wordlist.txt b/docs/spelling_wordlist.txt
index 42e03306b3..156f0355e4 100644
--- a/docs/spelling_wordlist.txt
+++ b/docs/spelling_wordlist.txt
@@ -64,9 +64,11 @@ serializes
 serialized
 serializable
 str
+terminateds
 timestep
 Todo
 travelled
+truncateds
 unassociated
 unformatted
 unmanaged
diff --git a/smarts/core/lanepoints.py b/smarts/core/lanepoints.py
index bccea1d4bf..688acfcbd5 100644
--- a/smarts/core/lanepoints.py
+++ b/smarts/core/lanepoints.py
@@ -908,19 +908,33 @@ def closest_linked_lanepoint_on_road(
 
     @lru_cache(maxsize=32)
     def paths_starting_at_lanepoint(
-        self, lanepoint: LinkedLanePoint, lookahead: int, filter_edge_ids: tuple
+        self, lanepoint: LinkedLanePoint, lookahead: int, route_edge_ids: tuple
     ) -> List[List[LinkedLanePoint]]:
         """Returns all full branches from the given lane-point up to the length of the look-ahead.
+        Branches will be filtered at the lane level if they or their outgoing lanes do not belong
+        to a road in the route edge list.
         Args:
             lanepoint (LinkedLanePoint):
                 The starting lane-point.
             lookahead (int):
                 The maximum lane-points in a branch.
-            filter_edge_ids (Tuple[str]):
-                White-listed edge ids.
+            route_edge_ids (Tuple[str]):
+                White-listed edge ids for a route.
         Returns:
-            All branches(as lists) stemming from the input lane-point.
+            All branches (as lists) stemming from the input lane-point.
         """
+        # Early exit if there are no valid paths ahead in the route for this lane
+        lp_lane = lanepoint.lp.lane
+        if (
+            route_edge_ids
+            and lp_lane.road.road_id != route_edge_ids[-1]
+            and all(
+                out_lane.road.road_id not in route_edge_ids
+                for out_lane in lp_lane.outgoing_lanes
+            )
+        ):
+            return []
+
         lanepoint_paths = [[lanepoint]]
         for _ in range(lookahead):
             next_lanepoint_paths = []
@@ -931,13 +945,13 @@ def paths_starting_at_lanepoint(
                     # Filter only the edges we're interested in
                     next_lane = next_lp.lp.lane
                     edge_id = next_lane.road.road_id
-                    if filter_edge_ids and edge_id not in filter_edge_ids:
+                    if route_edge_ids and edge_id not in route_edge_ids:
                         continue
                     if (
-                        filter_edge_ids
-                        and edge_id != filter_edge_ids[-1]
+                        route_edge_ids
+                        and edge_id != route_edge_ids[-1]
                         and all(
-                            out_lane.road.road_id not in filter_edge_ids
+                            out_lane.road.road_id not in route_edge_ids
                             for out_lane in next_lane.outgoing_lanes
                         )
                     ):