From 634fd0ff68354ea6251c62be675ecb3ac8b87197 Mon Sep 17 00:00:00 2001 From: adaickalavan Date: Mon, 10 Apr 2023 13:58:49 -0400 Subject: [PATCH 1/2] Improved chase via points agent. --- smarts/core/vehicle.py | 3 +- zoo/policies/__init__.py | 40 +++++++++++++- zoo/policies/chase_via_points_agent.py | 76 ++++++++++++++++++++++++++ 3 files changed, 117 insertions(+), 2 deletions(-) create mode 100644 zoo/policies/chase_via_points_agent.py diff --git a/smarts/core/vehicle.py b/smarts/core/vehicle.py index d2f7a333db..ee53a0b9b1 100644 --- a/smarts/core/vehicle.py +++ b/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, ) ) diff --git a/zoo/policies/__init__.py b/zoo/policies/__init__.py index 353483865c..22dc625241 100644 --- a/zoo/policies/__init__.py +++ b/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 diff --git a/zoo/policies/chase_via_points_agent.py b/zoo/policies/chase_via_points_agent.py new file mode 100644 index 0000000000..6a38fa6ad0 --- /dev/null +++ b/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 From cd8140f7ccb76b54b132cf7d1bcec53c9bb86cf6 Mon Sep 17 00:00:00 2001 From: adaickalavan Date: Mon, 10 Apr 2023 14:08:44 -0400 Subject: [PATCH 2/2] Add changelog. --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index fb8b43c801..4c9c588c53 100644 --- a/CHANGELOG.md +++ b/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.