Add chase-via-points-agent-v0 in agent zoo

CHANGELOG.md

@@ -12,8 +12,10 @@ Copy and pasting the git commit messages is __NOT__ enough.
 ### Added
 - Added an actor capture manager interface, `ActorCaptureManager`, which describes a manager that handles the change of control of actors. Operations in an actor manager step should not cause conflict in the simulation.
 - Added a new entry tactic, `IdEntryTactic`, which provides the scenario the ability to select a specific actor for an agent to take over.
+- Registered a new `chase-via-points-agent-v0` agent in agent zoo, which can effectively chase via points across different road sections by using the waypoints.
 ### Changed
 - The trap manager, `TrapManager`, is now a subclass of `ActorCaptureManager`.
+- Considering lane-change time ranges between 3s and 6s, assuming a speed of 13.89m/s, the via sensor lane acquisition range was increased from 40m to 80m, for better driving ability.
 ### Deprecated
 ### Fixed
 - Fixed an issue where Argoverse scenarios with a `Mission` would not run properly.

smarts/core/vehicle.py

@@ -558,7 +558,8 @@ def attach_sensors_to_vehicle(sim, vehicle, agent_interface, plan):
             ViaSensor(
                 vehicle=vehicle,
                 plan=plan,
-                lane_acquisition_range=40,
+                # At lane change time of 6s and speed of 13.89m/s, acquistion range = 6s x 13.89m/s = 83.34m.
+                lane_acquisition_range=80,
                 speed_accuracy=1.5,
             )
         )

zoo/policies/__init__.py

@@ -1,11 +1,18 @@
 import importlib
 from pathlib import Path
 
-from smarts.core.agent_interface import AgentInterface, AgentType
+from smarts.core.agent_interface import (
+    RGB,
+    AgentInterface,
+    AgentType,
+    DoneCriteria,
+    Waypoints,
+)
 from smarts.core.controllers import ActionSpaceType
 from smarts.zoo.agent_spec import AgentSpec
 from smarts.zoo.registry import make, register
 
+from .chase_via_points_agent import ChaseViaPointsAgent
 from .keep_lane_agent import KeepLaneAgent
 from .non_interactive_agent import NonInteractiveAgent
 from .waypoint_tracking_agent import WaypointTrackingAgent
@@ -37,6 +44,37 @@
     ),
 )
 
+register(
+    locator="chase-via-points-agent-v0",
+    entry_point=lambda **kwargs: AgentSpec(
+        interface=AgentInterface(
+            action=ActionSpaceType.LaneWithContinuousSpeed,
+            done_criteria=DoneCriteria(
+                collision=True,
+                off_road=True,
+                off_route=False,
+                on_shoulder=False,
+                wrong_way=False,
+                not_moving=False,
+                agents_alive=None,
+                actors_alive=None,
+            ),
+            accelerometer=False,
+            drivable_area_grid_map=False,
+            lane_positions=False,
+            lidar_point_cloud=False,
+            max_episode_steps=None,
+            neighborhood_vehicle_states=True,
+            occupancy_grid_map=False,
+            top_down_rgb=False,
+            road_waypoints=False,
+            waypoint_paths=Waypoints(lookahead=80),
+            signals=False,
+        ),
+        agent_builder=ChaseViaPointsAgent,
+    ),
+)
+
 
 def klws_entrypoint(speed):
     from .keep_left_with_speed_agent import KeepLeftWithSpeedAgent

zoo/policies/chase_via_points_agent.py

@@ -0,0 +1,76 @@
+import numpy as np
+
+from smarts.core.agent import Agent
+from smarts.core.observations import Observation
+from smarts.core.sensors import LANE_ID_CONSTANT
+
+
+class ChaseViaPointsAgent(Agent):
+    def act(self, obs: Observation):
+        assert obs.waypoint_paths, (
+            f"Waypoint paths = {obs.waypoint_paths}; "
+            "cannot be empty or None. Enable waypoint paths in agent interface."
+        )
+
+        lane_change_dist = 80
+
+        # Truncate all paths to be of the same length
+        min_len = min(lane_change_dist, min(map(len, obs.waypoint_paths)))
+        trunc_waypoints = list(map(lambda x: x[:min_len], obs.waypoint_paths))
+        waypoints = [list(map(lambda x: x.pos, path)) for path in trunc_waypoints]
+        waypoints = np.array(waypoints, dtype=np.float64)
+
+        # Ego status
+        ego_lane_id = obs.ego_vehicle_state.lane_id
+        assert ego_lane_id is not LANE_ID_CONSTANT, f"Ego lane cannot be {ego_lane_id}."
+        ego_pos = obs.ego_vehicle_state.position[:2]
+        dist = np.linalg.norm(waypoints[:, 0, :] - ego_pos, axis=-1)
+        ego_wp_inds = np.where(dist == dist.min())[0]
+
+        # Get target via point.
+        via_points = np.array(
+            [via_point.position for via_point in obs.via_data.near_via_points]
+        )
+        via_point_wp_ind, via_point_ind = _nearest_waypoint(waypoints, via_points)
+
+        # No nearby via points. Hence, remain in same lane.
+        if via_point_ind is None:
+            return (obs.waypoint_paths[ego_wp_inds[0]][0].speed_limit, 0)
+
+        # Target via point is in the same path. Hence, remain in same lane.
+        if via_point_wp_ind[0] in ego_wp_inds:
+            return (obs.via_data.near_via_points[via_point_ind].required_speed, 0)
+
+        # Change to left lane since target via point is on the left lane.
+        if ego_wp_inds[0] < via_point_wp_ind[0]:
+            return (obs.via_data.near_via_points[via_point_ind].required_speed, 1)
+
+        # Change to right lane since target via point is on the right lane.
+        if ego_wp_inds[0] > via_point_wp_ind[0]:
+            return (obs.via_data.near_via_points[via_point_ind].required_speed, -1)
+
+        raise Exception("ChaseViaPointsAgent did not catch any preprogrammed actions.")
+
+
+def _nearest_waypoint(matrix: np.ndarray, points: np.ndarray, radius: float = 2):
+    cur_point_index = ((np.intp(1e10), np.intp(1e10)), None)
+
+    if points.shape == (0,):
+        return cur_point_index
+
+    assert len(matrix.shape) == 3
+    assert matrix.shape[2] == 2
+    assert len(points.shape) == 2
+    assert points.shape[1] == 2
+
+    points_expanded = np.expand_dims(points, (1, 2))
+    diff = matrix - points_expanded
+    dist = np.linalg.norm(diff, axis=-1)
+    for ii in range(points.shape[0]):
+        index = np.argmin(dist[ii])
+        index_unravel = np.unravel_index(index, dist[ii].shape)
+        min_dist = dist[ii][index_unravel]
+        if min_dist <= radius and index_unravel[1] < cur_point_index[0][1]:
+            cur_point_index = (index_unravel, ii)
+
+    return cur_point_index