From 724c3f99f3ab2e5674b812482b44a6e5991ae997 Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Wed, 14 Jun 2023 13:48:50 -0400
Subject: [PATCH 1/8] Remove TruncatedDistribution from sstudio

---
 CHANGELOG.md                         |  1 +
 smarts/sstudio/types/distribution.py | 21 ---------------------
 2 files changed, 1 insertion(+), 21 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 44be7ff454..b6b9789971 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -70,6 +70,7 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - Removed `SMARTS.traffic_sim` property.
 - Removed remote agent modules.
 - Removed `protobuf` as a core package dependency.
+- Removed `TruncatedDistribution` from scenario studio.
 ### Security
 
 ## [1.1.0] # 2023-04-28
diff --git a/smarts/sstudio/types/distribution.py b/smarts/sstudio/types/distribution.py
index 7493967157..0b7e7d9f57 100644
--- a/smarts/sstudio/types/distribution.py
+++ b/smarts/sstudio/types/distribution.py
@@ -55,24 +55,3 @@ def __post_init__(self):
     def sample(self):
         """Get the next sample."""
         return random.uniform(self.a, self.b)
-
-
-@dataclass
-class TruncatedDistribution:
-    """A truncated normal distribution, by default, location=0, scale=1"""
-
-    a: float
-    b: float
-    loc: float = 0
-    scale: float = 1
-
-    def __post_init__(self):
-        assert self.a != self.b
-        if self.b < self.a:
-            self.a, self.b = self.b, self.a
-
-    def sample(self):
-        """Get the next sample"""
-        from scipy.stats import truncnorm
-
-        return truncnorm.rvs(self.a, self.b, loc=self.loc, scale=self.scale)

From 6e4129e5bfcafec2fa1d712a8cf0214a1ed3d502 Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Wed, 14 Jun 2023 15:47:16 -0400
Subject: [PATCH 2/8] Replace scipy cdist call with numpy equivalent

---
 smarts/core/vehicle_state.py | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/smarts/core/vehicle_state.py b/smarts/core/vehicle_state.py
index f1c34fc4ef..12f2154aac 100644
--- a/smarts/core/vehicle_state.py
+++ b/smarts/core/vehicle_state.py
@@ -23,7 +23,6 @@
 from typing import NamedTuple, Optional, Tuple
 
 import numpy as np
-from scipy.spatial.distance import cdist
 from shapely.affinity import rotate as shapely_rotate
 from shapely.geometry import Polygon
 from shapely.geometry import box as shapely_box
@@ -184,9 +183,7 @@ def neighborhood_vehicles_around_vehicle(
         return []
 
     # calculate euclidean distances
-    distances = cdist(
-        other_positions, [vehicle_state.pose.position], metric="euclidean"
-    ).reshape(-1)
+    distances = np.linalg.norm(other_positions - vehicle_state.pose.position, axis=1)
 
     indices = np.argwhere(distances <= radius).flatten()
     return [other_states[i] for i in indices]

From 850864816fb96ba737ccf5a81bc4ddb312d49cb9 Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Wed, 14 Jun 2023 17:17:42 -0400
Subject: [PATCH 3/8] Replace scipy.signal.place_poles with numpy
 implementation of Ackermann's formula

---
 .../controllers/lane_following_controller.py  | 43 ++++++++++++++-----
 1 file changed, 32 insertions(+), 11 deletions(-)

diff --git a/smarts/core/controllers/lane_following_controller.py b/smarts/core/controllers/lane_following_controller.py
index f92d3c3791..cf4f9479e4 100644
--- a/smarts/core/controllers/lane_following_controller.py
+++ b/smarts/core/controllers/lane_following_controller.py
@@ -20,7 +20,6 @@
 import math
 
 import numpy as np
-from scipy import signal
 
 from smarts.core.chassis import AckermannChassis
 from smarts.core.controllers.trajectory_tracking_controller import (
@@ -376,6 +375,31 @@ def find_current_lane(wp_paths, vehicle_position):
         ]
         return np.argmin(relative_distant_lane)
 
+    @staticmethod
+    def place_poles(A: np.ndarray, B: np.ndarray, poles: np.ndarray) -> np.ndarray:
+        """Given a linear system described by ẋ = Ax + Bu, compute the gain matrix K
+        such that the closed loop eigenvalues of A - BK are in the desired pole locations.
+
+        References:
+         - https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.place_poles.html
+         - https://en.wikipedia.org/wiki/Ackermann%27s_formula
+        """
+
+        # Controllability matrix
+        C = np.hstack(
+            [B] + [np.linalg.matrix_power(A, i) @ B for i in range(1, A.shape[0])]
+        )
+
+        # Desired characteristic polynomial of A - BK
+        poly = np.real(np.poly(poles))
+
+        # Solve using Ackermann's formula
+        n = np.size(poly)
+        p = poly[n - 1] * np.linalg.matrix_power(A, 0)
+        for i in np.arange(1, n):
+            p = p + poly[n - i - 1] * np.linalg.matrix_power(A, i)
+        return np.linalg.solve(C, p)[-1][:]
+
     @staticmethod
     def calculate_lateral_gains(sim, state, vehicle, desired_poles, target_speed):
         """Update the state lateral gains"""
@@ -420,18 +444,15 @@ def calculate_lateral_gains(sim, state, vehicle, desired_poles, target_speed):
                     [road_stiffness / (vehicle_mass * target_speed)],
                 ]
             )
-            fsf1 = signal.place_poles(
-                state_matrix, input_matrix, desired_poles, method="KNV0"
+            K = LaneFollowingController.place_poles(
+                state_matrix, input_matrix, desired_poles
             )
-            # 0.01 and 0.015 denote the max and min gains for heading controller
-            # This is done to ensure that the linearization error will not affect
-            # the stability of the controller.
-            state.heading_error_gain = np.clip(fsf1.gain_matrix[0][1], 0.02, 0.04)
-            # 3.4 and 4.1 denote the max and min gains for lateral error controller
-            # As for heading, this is done to ensure that the linearization error
-            # will not affect the stability and performance of the controller.
-            state.lateral_error_gain = np.clip(fsf1.gain_matrix[0][0], 3.4, 4.1)
 
+            # These constants are the min/max gains for the lateral error controller
+            # and the heading controller, respectively. This is done to ensure that
+            # the linearization error will not affect the stability of the controller.
+            state.lateral_error_gain = np.clip(K[0], 3.4, 4.1)
+            state.heading_error_gain = np.clip(K[1], 0.02, 0.04)
         else:
             # 0.01 and 0.36 are initial values for heading and lateral gains
             # This is only done to ensure that the vehicle starts to move for

From dbc340703a7cc3b56f7baafbe5626ece0fcbf94c Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Wed, 14 Jun 2023 17:24:23 -0400
Subject: [PATCH 4/8] Update changelog and remove unused code

---
 CHANGELOG.md                                         | 1 +
 smarts/core/controllers/lane_following_controller.py | 7 -------
 2 files changed, 1 insertion(+), 7 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b6b9789971..bf169d081d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -47,6 +47,7 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - `DistToDestination` metric is now computed by summing the (i) off-route distance driven by the vehicle from its last on-route position, and (ii) the distance to goal from the vehicle's last on-route position. 
 - `Steps` metric is capped by scenario duration set in the scenario metadata.
 - Overall metric score is weighted by each agent's task difficulty.
+- `LaneFollowingController` now uses a different pole placement method to compute lateral/heading gains. Numerical behaviour is unchanged. Performance is slightly faster.
 ### Deprecated
 - `visdom` is set to be removed from the SMARTS object parameters.
 - Deprecated `start_time` on missions.
diff --git a/smarts/core/controllers/lane_following_controller.py b/smarts/core/controllers/lane_following_controller.py
index cf4f9479e4..550696000c 100644
--- a/smarts/core/controllers/lane_following_controller.py
+++ b/smarts/core/controllers/lane_following_controller.py
@@ -244,13 +244,6 @@ def perform_lane_following(
             vehicle_look_ahead_pt
         )
 
-        abs_heading_error = min(
-            abs((vehicle.heading % (2 * math.pi)) - reference_heading),
-            abs(
-                2 * math.pi - abs((vehicle.heading % (2 * math.pi)) - reference_heading)
-            ),
-        )
-
         curvature_radius = TrajectoryTrackingController.curvature_calculation(
             trajectory
         )

From 4957b8377181b023eb5f6387d9901b30319b6b32 Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Thu, 15 Jun 2023 13:09:23 -0400
Subject: [PATCH 5/8] Fix docs

---
 docs/spelling_wordlist.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/docs/spelling_wordlist.txt b/docs/spelling_wordlist.txt
index 291a9c6d4c..78851b61ba 100644
--- a/docs/spelling_wordlist.txt
+++ b/docs/spelling_wordlist.txt
@@ -4,6 +4,7 @@
 Δy
 Δheading
 ΔTime
+ẋ
 accelerometer
 Ackermann
 acyclic

From 2eb2f9171ddb2058a1f7ae9676430b4ba934105f Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Fri, 23 Jun 2023 11:52:26 -0400
Subject: [PATCH 6/8] Replace scipy KDTree in lanepoints

---
 setup.cfg                             |   1 -
 smarts/core/agent_interface.py        |   1 +
 smarts/core/argoverse_map.py          |   2 +-
 smarts/core/coordinates.py            |   1 +
 smarts/core/lanepoints.py             | 162 ++++++--------------------
 smarts/core/opendrive_road_network.py |   4 +-
 smarts/core/sumo_road_network.py      |   4 +-
 smarts/core/tests/test_map.py         |  20 ----
 smarts/core/waymo_map.py              |   4 +-
 9 files changed, 43 insertions(+), 156 deletions(-)

diff --git a/setup.cfg b/setup.cfg
index 13a02840d7..8c7eb45595 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -42,7 +42,6 @@ install_requires =
     # The following are planned for removal
     gym>=0.17.3,<=0.19.0
     cloudpickle>=1.3.0,<=2.1.0
-    scipy
 
 [options.packages.find]
 exclude =
diff --git a/smarts/core/agent_interface.py b/smarts/core/agent_interface.py
index 50cd6f1a89..ff1f23470f 100644
--- a/smarts/core/agent_interface.py
+++ b/smarts/core/agent_interface.py
@@ -415,6 +415,7 @@ def from_type(requested_type: AgentType, **kwargs):
         elif requested_type == AgentType.LanerWithSpeed:
             interface = AgentInterface(
                 waypoint_paths=True,
+                neighborhood_vehicle_states=NeighborhoodVehicles(5),
                 action=ActionSpaceType.LaneWithContinuousSpeed,
             )
         # The trajectory tracking agent which receives a series of reference trajectory
diff --git a/smarts/core/argoverse_map.py b/smarts/core/argoverse_map.py
index f444d16df9..d7ee8226f0 100644
--- a/smarts/core/argoverse_map.py
+++ b/smarts/core/argoverse_map.py
@@ -980,7 +980,7 @@ def waypoint_paths(
         # No route provided, so generate paths based on the closest lanepoints
         waypoint_paths = []
         closest_lps: List[LanePoint] = self._lanepoints.closest_lanepoints(
-            pose, within_radius=within_radius, maximum_count=15
+            pose, maximum_count=15
         )
         closest_lane: RoadMap.Lane = closest_lps[0].lane
 
diff --git a/smarts/core/coordinates.py b/smarts/core/coordinates.py
index a18bf84aa8..8d904c147a 100644
--- a/smarts/core/coordinates.py
+++ b/smarts/core/coordinates.py
@@ -311,6 +311,7 @@ def __post_init__(self):
         assert len(self.position) <= 3
         if len(self.position) < 3:
             self.position = np.resize(self.position, 3)
+            self.position[-1] = 0
         assert len(self.orientation) == 4
         if not isinstance(self.orientation, np.ndarray):
             self.orientation = np.array(self.orientation, dtype=np.float64)
diff --git a/smarts/core/lanepoints.py b/smarts/core/lanepoints.py
index 688acfcbd5..13b5707669 100644
--- a/smarts/core/lanepoints.py
+++ b/smarts/core/lanepoints.py
@@ -28,15 +28,10 @@
 from typing import List, NamedTuple, Sequence, Tuple
 
 import numpy as np
-from scipy.spatial import KDTree
 
 from smarts.core.coordinates import Heading, Point, Pose
 from smarts.core.road_map import RoadMap
-from smarts.core.utils.math import (
-    fast_quaternion_from_angle,
-    squared_dist,
-    vec_to_radians,
-)
+from smarts.core.utils.math import fast_quaternion_from_angle, vec_to_radians
 
 
 @dataclass(frozen=True)
@@ -76,7 +71,9 @@ def __init__(self, shape_lps: List[LinkedLanePoint], spacing: float):
         self._linked_lanepoints = LanePoints._interpolate_shape_lanepoints(
             shape_lps, spacing
         )
-        self._lanepoints_kd_tree = LanePoints._build_kd_tree(self._linked_lanepoints)
+        self._lp_points = np.array(
+            [l_lp.lp.pose.point.as_np_array[:2] for l_lp in self._linked_lanepoints]
+        )
 
         self._lanepoints_by_lane_id = defaultdict(list)
         self._lanepoints_by_edge_id = defaultdict(list)
@@ -85,13 +82,13 @@ def __init__(self, shape_lps: List[LinkedLanePoint], spacing: float):
             self._lanepoints_by_lane_id[linked_lp.lp.lane.lane_id].append(linked_lp)
             self._lanepoints_by_edge_id[lp_edge_id].append(linked_lp)
 
-        self._lanepoints_kd_tree_by_lane_id = {
-            lane_id: LanePoints._build_kd_tree(l_lps)
+        self._lp_points_by_lane_id = {
+            lane_id: np.array([l_lp.lp.pose.point.as_np_array[:2] for l_lp in l_lps])
             for lane_id, l_lps in self._lanepoints_by_lane_id.items()
         }
 
-        self._lanepoints_kd_tree_by_edge_id = {
-            edge_id: LanePoints._build_kd_tree(l_lps)
+        self._lp_points_by_edge_id = {
+            edge_id: np.array([l_lp.lp.pose.point.as_np_array[:2] for l_lp in l_lps])
             for edge_id, l_lps in self._lanepoints_by_edge_id.items()
         }
 
@@ -180,7 +177,7 @@ def _shape_lanepoints_along_lane(
                     lp=LanePoint(
                         lane=curr_lanepoint.lp.lane,
                         pose=Pose(
-                            position=lane_shape[-1],
+                            position=lane_shape[-1][:2],
                             orientation=curr_lanepoint.lp.pose.orientation,
                         ),
                         lane_width=lane_width,
@@ -610,13 +607,6 @@ def _shape_lanepoints_along_lane(
 
         return cls(shape_lps, spacing)
 
-    @staticmethod
-    def _build_kd_tree(linked_lps: Sequence[LinkedLanePoint]) -> KDTree:
-        return KDTree(
-            np.array([l_lp.lp.pose.point.as_np_array[:2] for l_lp in linked_lps]),
-            leafsize=50,
-        )
-
     @staticmethod
     def _interpolate_shape_lanepoints(
         shape_lanepoints: Sequence[LinkedLanePoint], spacing: float
@@ -760,151 +750,73 @@ def _process_interp_for_lane_lp(
         return curr_lanepoint
 
     @staticmethod
-    def _closest_linked_lp_in_kd_tree_batched(
-        points: Sequence[Point],
-        linked_lps,
-        tree: KDTree,
+    def _closest_linked_lp_to_point(
+        point: Point,
+        linked_lps: List[LinkedLanePoint],
+        points: np.ndarray,
         k: int = 1,
         filter_composites: bool = False,
-    ):
-        p2ds = np.array([p.as_np_array[:2] for p in points])
-        _, closest_indices = tree.query(p2ds, k=min(k, len(linked_lps)))
-        closest_indices = np.atleast_2d(closest_indices)
+    ) -> List[LinkedLanePoint]:
+        x = point.as_np_array[:2]
+        dists = np.sqrt(np.sum((points - x) ** 2, axis=1))
+        closest_indices = np.argsort(dists)[:k]
+
         if filter_composites:
             result = [
-                [
-                    linked_lps[idx]
-                    for idx in idxs
-                    if not linked_lps[idx].lp.lane.is_composite
-                ]
-                for idxs in closest_indices
+                linked_lps[idx]
+                for idx in closest_indices
+                if not linked_lps[idx].lp.lane.is_composite
             ]
             if result:
                 return result
-        # if filtering, only return lane-points in composite lanes if we didn't hit any in simple lanes...
-        return [[linked_lps[idx] for idx in idxs] for idxs in closest_indices]
-
-    @staticmethod
-    def _closest_linked_lp_in_kd_tree_with_pose_batched(
-        poses,
-        lanepoints,
-        tree,
-        within_radius: float,
-        k: int = 10,
-        filter_composites: bool = False,
-    ):
-        linked_lanepoints = LanePoints._closest_linked_lp_in_kd_tree_batched(
-            [pose.point for pose in poses],
-            lanepoints,
-            tree,
-            k=k,
-            filter_composites=filter_composites,
-        )
 
-        linked_lanepoints = [
-            sorted(
-                l_lps,
-                key=lambda _llp: squared_dist(
-                    poses[idx].as_position2d(), _llp.lp.pose.as_position2d()
-                ),
-            )
-            for idx, l_lps in enumerate(linked_lanepoints)
-        ]
-        # exclude those outside radius except closest
-        if within_radius is not None:
-            radius_sq = within_radius * within_radius
-            linked_lanepoints = [
-                [
-                    _llp
-                    for i, _llp in enumerate(_llps)
-                    if squared_dist(
-                        poses[idx].as_position2d(), _llp.lp.pose.as_position2d()
-                    )
-                    <= radius_sq
-                    or i == 0
-                ]
-                for idx, _llps in enumerate(linked_lanepoints)
-            ]
-        # Get the nearest point for the points where the radius check failed
-        unfound_lanepoints = [
-            (i, poses[i])
-            for i, group in enumerate(linked_lanepoints)
-            if len(group) == 0
-        ]
-        if len(unfound_lanepoints) > 0:
-            remaining_linked_lps = LanePoints._closest_linked_lp_in_kd_tree_batched(
-                [pose.point for _, pose in unfound_lanepoints],
-                lanepoints,
-                tree=tree,
-                k=k,
-                filter_composites=filter_composites,
-            )
-            # Replace the empty lanepoint locations
-            for (i, _), lps in [
-                g for g in zip(unfound_lanepoints, remaining_linked_lps)
-            ]:
-                linked_lanepoints[i] = [lps]
-
-        return [
-            sorted(
-                l_lps,
-                key=lambda _llp: squared_dist(
-                    poses[idx].as_position2d(), _llp.lp.pose.as_position2d()
-                )
-                + abs(poses[idx].heading.relative_to(_llp.lp.pose.heading)),
-            )
-            for idx, l_lps in enumerate(linked_lanepoints)
-        ]
+        # if filtering, only return lane-points in composite lanes if we didn't hit any in simple lanes...
+        return [linked_lps[idx] for idx in closest_indices]
 
     def closest_lanepoints(
         self,
         pose: Pose,
-        within_radius: float = 10,
         maximum_count: int = 10,
     ) -> List[LanePoint]:
         """Get the lane-points closest to the given pose.
         Args:
             pose:
                 The pose to look around for lane-points.
-            within_radius:
-                The radius which lane-points can be found from the given pose.
             maximum_count:
                 The maximum number of lane-points that should be found.
         """
         lanepoints = self._linked_lanepoints
         kd_tree = self._lanepoints_kd_tree
-        linked_lanepoints = LanePoints._closest_linked_lp_in_kd_tree_with_pose_batched(
-            [pose],
+        linked_lanepoints = LanePoints._closest_linked_lp_to_point(
+            pose.point,
             lanepoints,
             kd_tree,
-            within_radius=within_radius,
+            self._lp_points,
             k=maximum_count,
             filter_composites=True,
         )
-        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 lane-point on the given lane to the given world coordinate."""
-        return self.closest_linked_lanepoint_on_lane_to_point(point, lane_id).lp
+        return [llp.lp for llp in linked_lanepoints]
 
     def closest_linked_lanepoint_on_lane_to_point(
         self, point: Point, lane_id: str
     ) -> LinkedLanePoint:
         """Returns the closest linked lane-point on the given lane."""
-        lane_kd_tree = self._lanepoints_kd_tree_by_lane_id[lane_id]
-        return LanePoints._closest_linked_lp_in_kd_tree_batched(
-            [point], self._lanepoints_by_lane_id[lane_id], lane_kd_tree, k=1
-        )[0][0]
+        return LanePoints._closest_linked_lp_to_point(
+            point,
+            self._lanepoints_by_lane_id[lane_id],
+            self._lp_points_by_lane_id[lane_id],
+            k=1,
+        )[0]
 
     def closest_linked_lanepoint_on_road(
         self, point: Point, road_id: str
     ) -> LinkedLanePoint:
         """Returns the closest linked lane-point on the given road."""
-        return LanePoints._closest_linked_lp_in_kd_tree_batched(
-            [point],
+        return LanePoints._closest_linked_lp_to_point(
+            point,
             self._lanepoints_by_edge_id[road_id],
-            self._lanepoints_kd_tree_by_edge_id[road_id],
-        )[0][0]
+            self._lp_points_by_edge_id[road_id],
+        )[0]
 
     @lru_cache(maxsize=32)
     def paths_starting_at_lanepoint(
diff --git a/smarts/core/opendrive_road_network.py b/smarts/core/opendrive_road_network.py
index ae275bda7b..258401351f 100644
--- a/smarts/core/opendrive_road_network.py
+++ b/smarts/core/opendrive_road_network.py
@@ -1668,9 +1668,7 @@ 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
-        )
+        closest_lps = self._lanepoints.closest_lanepoints(pose)
         closest_lane = closest_lps[0].lane
         waypoint_paths = []
         for lane in closest_lane.road.lanes:
diff --git a/smarts/core/sumo_road_network.py b/smarts/core/sumo_road_network.py
index 0d2a5cff8b..540ab5245f 100644
--- a/smarts/core/sumo_road_network.py
+++ b/smarts/core/sumo_road_network.py
@@ -996,9 +996,7 @@ def waypoint_paths(
 
         # No route provided, so generate paths based on the closest lanepoints
         waypoint_paths = []
-        closest_lps: List[LanePoint] = self._lanepoints.closest_lanepoints(
-            pose, within_radius=within_radius
-        )
+        closest_lps: List[LanePoint] = self._lanepoints.closest_lanepoints(pose)
 
         # First, see if we are in a junction and need to do something special
         junction_paths = self._resolve_in_junction(pose, closest_lps)
diff --git a/smarts/core/tests/test_map.py b/smarts/core/tests/test_map.py
index b80a20b732..96bd89c02e 100644
--- a/smarts/core/tests/test_map.py
+++ b/smarts/core/tests/test_map.py
@@ -337,13 +337,6 @@ def test_opendrive_map_4lane(opendrive_scenario_4lane):
     # Lanepoints
     lanepoints = road_map._lanepoints
 
-    point = Point(148.0, -17.0, 0)
-    l1_lane_point = lanepoints.closest_lanepoint_on_lane_to_point(point, l1.lane_id)
-    assert (
-        round(l1_lane_point.pose.position[0], 2),
-        round(l1_lane_point.pose.position[1], 2),
-    ) == (148.4, -17.0)
-
     r5 = road_map.road_by_id("60_0_R")
     point = Point(148.00, -47.00)
     r5_linked_lane_point = lanepoints.closest_linked_lanepoint_on_road(
@@ -669,13 +662,6 @@ def test_opendrive_map_merge(opendrive_scenario_merge):
     # Lanepoints
     lanepoints = road_map._lanepoints
 
-    point = Point(48.39, 0.4, 0)
-    l1_lane_point = lanepoints.closest_lanepoint_on_lane_to_point(point, "1_1_R_-1")
-    assert (
-        round(l1_lane_point.pose.position[0], 2),
-        round(l1_lane_point.pose.position[1], 2),
-    ) == (48.5, -0.15)
-
     point = Point(20.0, 1.3, 0)
     r0_linked_lane_point = lanepoints.closest_linked_lanepoint_on_road(point, "1_0_L")
     assert r0_linked_lane_point.lp.lane.lane_id == "1_0_L_1"
@@ -932,12 +918,6 @@ def test_waymo_map():
 
     # Lanepoints
     lanepoints = road_map._lanepoints
-    point = Point(2715.0, -2763.5, 0)
-    l1_lane_point = lanepoints.closest_lanepoint_on_lane_to_point(point, l1.lane_id)
-    assert (
-        round(l1_lane_point.pose.position[0], 2),
-        round(l1_lane_point.pose.position[1], 2),
-    ) == (2713.84, -2762.52)
 
     r1 = road_map.road_by_id("waymo_road-100")
     r1_linked_lane_point = lanepoints.closest_linked_lanepoint_on_road(
diff --git a/smarts/core/waymo_map.py b/smarts/core/waymo_map.py
index 960c3dbe57..55fc6e132a 100644
--- a/smarts/core/waymo_map.py
+++ b/smarts/core/waymo_map.py
@@ -1853,9 +1853,7 @@ 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
-        )
+        closest_lps = self._lanepoints.closest_lanepoints(pose)
         closest_lane = closest_lps[0].lane
         waypoint_paths = []
         for lane in closest_lane.road.lanes:

From cb0dec995b3a25e8015d2464f5562e108c2a2783 Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Fri, 23 Jun 2023 11:59:31 -0400
Subject: [PATCH 7/8] Fixes and changelog updates

---
 CHANGELOG.md                   | 5 +++--
 smarts/core/agent_interface.py | 1 -
 smarts/core/lanepoints.py      | 5 +----
 3 files changed, 4 insertions(+), 7 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index ee6db12124..9e79868dca 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -18,11 +18,14 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - Updated `RLlibHiWayEnv` to use the `gymnasium` interface.
 - Renamed `rllib/rllib.py` to `rllib/pg_pbt_example.py`.
 - Loosened constraint of `gymnasium` from `==0.27.0` to `>=0.26.3`.
+- `LaneFollowingController` now uses a different pole placement method to compute lateral/heading gains. Numerical behaviour is unchanged. Performance is slightly faster.
 ### Deprecated
 ### Fixed
 - Missing neighborhood vehicle state `'lane_id'` is now added to the `hiway-v1` formatted observations.
 - Fixed a regression where `pybullet` build time messages returned.
 ### Removed
+- Removed `TruncatedDistribution` from scenario studio.
+- Removed `scipy` as a core package dependency.
 ### Security
 
 ## [1.2.0] # 2023-06-14
@@ -56,7 +59,6 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - `DistToDestination` metric is now computed by summing the (i) off-route distance driven by the vehicle from its last on-route position, and (ii) the distance to goal from the vehicle's last on-route position. 
 - `Steps` metric is capped by scenario duration set in the scenario metadata.
 - Overall metric score is weighted by each agent's task difficulty.
-- `LaneFollowingController` now uses a different pole placement method to compute lateral/heading gains. Numerical behaviour is unchanged. Performance is slightly faster.
 ### Deprecated
 - `visdom` is set to be removed from the SMARTS object parameters.
 - Deprecated `start_time` on missions.
@@ -80,7 +82,6 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - Removed `SMARTS.traffic_sim` property.
 - Removed remote agent modules.
 - Removed `protobuf` as a core package dependency.
-- Removed `TruncatedDistribution` from scenario studio.
 ### Security
 
 ## [1.1.0] # 2023-04-28
diff --git a/smarts/core/agent_interface.py b/smarts/core/agent_interface.py
index ff1f23470f..50cd6f1a89 100644
--- a/smarts/core/agent_interface.py
+++ b/smarts/core/agent_interface.py
@@ -415,7 +415,6 @@ def from_type(requested_type: AgentType, **kwargs):
         elif requested_type == AgentType.LanerWithSpeed:
             interface = AgentInterface(
                 waypoint_paths=True,
-                neighborhood_vehicle_states=NeighborhoodVehicles(5),
                 action=ActionSpaceType.LaneWithContinuousSpeed,
             )
         # The trajectory tracking agent which receives a series of reference trajectory
diff --git a/smarts/core/lanepoints.py b/smarts/core/lanepoints.py
index 13b5707669..f1086064a8 100644
--- a/smarts/core/lanepoints.py
+++ b/smarts/core/lanepoints.py
@@ -785,12 +785,9 @@ def closest_lanepoints(
             maximum_count:
                 The maximum number of lane-points that should be found.
         """
-        lanepoints = self._linked_lanepoints
-        kd_tree = self._lanepoints_kd_tree
         linked_lanepoints = LanePoints._closest_linked_lp_to_point(
             pose.point,
-            lanepoints,
-            kd_tree,
+            self._linked_lanepoints,
             self._lp_points,
             k=maximum_count,
             filter_composites=True,

From 255748fd2d18ea25ab243ec928e350c6f9d05e96 Mon Sep 17 00:00:00 2001
From: Saul Field <saul.field@gmail.com>
Date: Fri, 23 Jun 2023 12:42:12 -0400
Subject: [PATCH 8/8] Fix types

---
 smarts/core/tests/test_map.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/smarts/core/tests/test_map.py b/smarts/core/tests/test_map.py
index e4eb9e0fe4..01ba68c0dd 100644
--- a/smarts/core/tests/test_map.py
+++ b/smarts/core/tests/test_map.py
@@ -784,6 +784,7 @@ def test_waymo_map():
     # nearest lane for a point outside all lanes
     point = Point(2910.0, -2612.0, 0)
     l3 = road_map.nearest_lane(point)
+    assert l3
     assert l3.lane_id == "156"
     assert not l3.contains_point(point)