Universal DistToDestination metric

CHANGELOG.md

@@ -31,7 +31,7 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - `info` returned by `hiway-v1` in `reset()` and `step()` methods are unified.
 - Changed instances of `hiway-v0` and `gym` to use `hiway-v1` and `gymnasium`, respectively.
 - `RoadMap.Route` now optionally stores the start and end lanes of the route.
-- `DistToDestination` metric now adds lane error penalty when agent terminates in different lane but same road as the goal position. 
+- `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. 
 ### Deprecated
 - `visdom` is set to be removed from the SMARTS object parameters.
 - Deprecated `start_time` on missions.

docs/examples/drive.rst

@@ -0,0 +1,6 @@
+.. _drive:
+
+Drive
+=====
+
+This example was developed in conjunction with the :ref:`Driving SMARTS 2023.1 & 2023.2 <driving_smarts_2023_1>` benchmark, hence refer to it for details.

docs/examples/rl_model.rst

@@ -6,4 +6,5 @@ RL Model
 .. toctree::
    :maxdepth: 1
 
+   drive.rst 
    platoon.rst

docs/spelling_wordlist.txt

@@ -1,3 +1,4 @@
+θ
 π
 Δx
 Δy
@@ -44,9 +45,10 @@ laner
 lidar
 lookahead
 natively
+ndarray
+nomask
 np
 numpy
-ndarray
 param
 pre
 quaternion

examples/rl/drive/train/reward.py

@@ -33,9 +33,6 @@ def step(self, action):
             if terminated[agent_id] == True:
                 if agent_obs["events"]["reached_goal"]:
                     print(f"{agent_id}: Hooray! Reached goal.")
-                    raise Exception(
-                        f"{agent_id}: Goal has been leaked to the ego agent!"
-                    )
                 elif agent_obs["events"]["reached_max_episode_steps"]:
                     print(f"{agent_id}: Reached max episode steps.")
                 elif (

smarts/benchmark/driving_smarts/v2023/metric_formula_drive.py

@@ -28,6 +28,7 @@
 from smarts.env.gymnasium.wrappers.metric.formula import FormulaBase, Score, avg_costs
 from smarts.env.gymnasium.wrappers.metric.params import (
     Comfort,
+    DistToDestination,
     DistToObstacles,
     JerkLinear,
     Params,
@@ -53,6 +54,9 @@ def params(self) -> Params:
             comfort=Comfort(
                 active=True,
             ),
+            dist_to_destination=DistToDestination(
+                active=True,
+            ),
             dist_to_obstacles=DistToObstacles(
                 active=False,
             ),

smarts/core/plan.py

@@ -338,16 +338,25 @@ def road_map(self) -> RoadMap:
         """The road map this plan is relative to."""
         return self._road_map
 
-    def create_route(self, mission: Mission, radius: Optional[float] = None):
+    def create_route(
+        self,
+        mission: Mission,
+        start_lane_radius: Optional[float] = None,
+        end_lane_radius: Optional[float] = None,
+    ):
         """Generates a route that conforms to a mission.
 
         Args:
             mission (Mission):
                 A mission the agent should follow. Defaults to endless if `None`.
-            radius (Optional[float]):
+            start_lane_radius (Optional[float]):
                 Radius (meter) to find the nearest starting lane for the given
-                mission. Defaults to `_default_lane_width` of the underlying
-                road_map.
+                mission. Defaults to a function of `_default_lane_width` of the
+                underlying road_map.
+            end_lane_radius (Optional[float]):
+                Radius (meter) to find the nearest ending lane for the given
+                mission. Defaults to a function of `_default_lane_width` of the
+                underlying road_map.
         """
         assert not self._route or not len(
             self._route.road_ids
@@ -363,28 +372,32 @@ def create_route(self, mission: Mission, radius: Optional[float] = None):
         start_lanes = self._road_map.nearest_lanes(
             self._mission.start.point,
             include_junctions=True,
-            radius=radius,
+            radius=start_lane_radius,
         )
         if not start_lanes:
             self._mission = Mission.endless_mission(Pose.origin())
             raise PlanningError("Starting lane not found. Route must start in a lane.")
 
         via_roads = [self._road_map.road_by_id(via) for via in self._mission.route_vias]
 
-        end_lane = self._road_map.nearest_lane(
+        end_lanes = self._road_map.nearest_lanes(
             self._mission.goal.position,
             include_junctions=False,
+            radius=end_lane_radius,
         )
-        assert end_lane is not None, "route must end in a lane"
+        assert end_lanes is not None, "No end lane found. Route must end in a lane."
 
         # When an agent is in an intersection, the `nearest_lanes` method might
         # not return the correct road as the first choice. Hence, nearest
         # starting lanes are tried in sequence until a route is found or until
         # all nearby starting lane options are exhausted.
-        for start_lane, _ in start_lanes:
-            self._route = self._road_map.generate_routes(
-                start_lane, end_lane, via_roads, 1
-            )[0]
+        for end_lane, _ in end_lanes:
+            for start_lane, _ in start_lanes:
+                self._route = self._road_map.generate_routes(
+                    start_lane, end_lane, via_roads, 1
+                )[0]
+                if self._route.road_length > 0:
+                    break
             if self._route.road_length > 0:
                 break
 

smarts/core/sensor.py

@@ -34,16 +34,12 @@
 from smarts.core.masks import RenderMasks
 from smarts.core.observations import (
     DrivableAreaGridMap,
-    EgoVehicleObservation,
     GridMapMetadata,
-    Observation,
     OccupancyGridMap,
     RoadWaypoints,
     SignalObservation,
     TopDownRGB,
-    VehicleObservation,
     ViaPoint,
-    Vias,
 )
 from smarts.core.plan import Plan
 from smarts.core.renderer_base import RendererBase

smarts/env/gymnasium/wrappers/metric/costs.py

@@ -22,11 +22,11 @@
 import warnings
 from collections import deque
 from dataclasses import dataclass
-from typing import Callable, Dict, List, NewType
+from typing import Callable, Dict, List, NewType, Optional, Tuple
 
 import numpy as np
 
-from smarts.core.coordinates import Heading, Point
+from smarts.core.coordinates import Heading, Point, RefLinePoint
 from smarts.core.observations import Observation
 from smarts.core.plan import Mission, Plan, PositionalGoal, Start
 from smarts.core.road_map import RoadMap
@@ -108,28 +108,93 @@ def func(
 
 
 def _dist_to_destination(
-    end_pos: Point, dist_tot: float
+    end_pos: Point,
+    dist_tot: float,
+    route: RoadMap.Route,
+    prev_route_lane: RoadMap.Lane,
+    prev_route_lane_point: Point,
+    prev_route_displacement: float,
 ) -> Callable[[RoadMap, VehicleIndex, Done, Observation], Costs]:
     mean = 0
     step = 0
     end_pos = end_pos
     dist_tot = dist_tot
+    route = route
+    prev_route_lane = prev_route_lane
+    prev_route_lane_point = prev_route_lane_point
+    prev_route_displacement = prev_route_displacement
+    prev_dist_travelled = 0
+    tot_dist_travelled = 0
 
     def func(
         road_map: RoadMap, vehicle_index: VehicleIndex, done: Done, obs: Observation
     ) -> Costs:
-        nonlocal mean, step, end_pos, dist_tot
+        nonlocal mean, step, end_pos, dist_tot, route, prev_route_lane, prev_route_lane_point, prev_route_displacement, prev_dist_travelled, tot_dist_travelled
+
+        tot_dist_travelled += obs.distance_travelled
 
         if not done:
+            cur_pos = Point(*obs.ego_vehicle_state.position)
+            cur_on_route, cur_route_lane, cur_route_lane_point, cur_route_displacement = on_route(
+                road_map=road_map, route=route, point=cur_pos
+            )
+
+            if cur_on_route:
+                prev_route_lane = cur_route_lane
+                prev_route_lane_point = cur_route_lane_point
+                prev_route_displacement = cur_route_displacement
+                prev_dist_travelled = tot_dist_travelled
+
             return Costs(dist_to_destination=-np.inf)
         elif obs.events.reached_goal:
             return Costs(dist_to_destination=0)
         else:
             cur_pos = Point(*obs.ego_vehicle_state.position)
-            dist_remainder = get_dist(
-                road_map=road_map, point_a=cur_pos, point_b=end_pos
+            cur_on_route, cur_route_lane, cur_route_lane_point, cur_route_displacement = on_route(
+                road_map=road_map, route=route, point=cur_pos
+            )
+
+            # Step 1: Compute the last off-route distance driven by the vehicle, if any.
+            if not cur_on_route:
+                off_route_dist = tot_dist_travelled - prev_dist_travelled
+                assert off_route_dist >= 0
+                off_route_dist += prev_route_displacement
+                last_route_lane = prev_route_lane
+                last_route_pos = prev_route_lane_point
+            else:
+                off_route_dist = cur_route_displacement
+                last_route_lane = cur_route_lane
+                last_route_pos = cur_route_lane_point
+
+            # Step 2: Compute the remaining route distance from the last recorded on-route position.
+            on_route_dist = route.distance_between(
+                start=RoadMap.Route.RoutePoint(pt=last_route_pos),
+                end=RoadMap.Route.RoutePoint(pt=end_pos),
             )
+
+            # Step 3: Compute absolute `on_route_dist` because it could be
+            # negative when an agent overshoots the end position while
+            # remaining outside the goal capture radius at all times.
+            on_route_dist = abs(on_route_dist)
+
+            # Step 4: Compute lane error penalty if vehicle is in the same road as goal, but in a different lane.
+            # TODO: Lane error penalty should be computed. It is not computed
+            # currently because the end lane of a SUMO traffic vehicle of
+            # interest is currently not accessible.
+            lane_error_dist = 0
+            # end_lane = route.end_lane
+            # if last_route_lane.road == end_lane.road:
+            #     lane_error = abs(last_route_lane.index - end_lane.index)
+            #     end_offset = end_lane.offset_along_lane(world_point=end_pos)
+            #     lane_width, _ = end_lane.width_at_offset(end_offset)
+            #     lane_error_dist = lane_error * lane_width
+
+            # Step 5: Total distance to destination.
+            dist_remainder = off_route_dist + on_route_dist + lane_error_dist
+
+            # Step 6: Cap distance to destination.
             dist_remainder_capped = min(dist_remainder, dist_tot)
+
             return Costs(dist_to_destination=dist_remainder_capped / dist_tot)
 
     return func
@@ -484,7 +549,7 @@ class CostFuncsBase:
     # fmt: off
     collisions: Callable[[], Callable[[RoadMap, VehicleIndex, Done, Observation], Costs]] = _collisions
     comfort: Callable[[], Callable[[RoadMap, VehicleIndex, Done, Observation], Costs]] = _comfort
-    dist_to_destination: Callable[[Point,float], Callable[[RoadMap, VehicleIndex, Done, Observation], Costs]] = _dist_to_destination
+    dist_to_destination: Callable[[Point,float,RoadMap.Route,RoadMap.Lane,Point,float], Callable[[RoadMap, VehicleIndex, Done, Observation], Costs]] = _dist_to_destination
     dist_to_obstacles: Callable[[List[str]], Callable[[RoadMap, VehicleIndex, Done, Observation], Costs]] = _dist_to_obstacles
     jerk_linear: Callable[[], Callable[[RoadMap, VehicleIndex, Done, Observation], Costs]] = _jerk_linear
     lane_center_offset: Callable[[], Callable[[RoadMap, VehicleIndex, Done, Observation], Costs]] = _lane_center_offset
@@ -531,7 +596,9 @@ class CostError(Exception):
     pass
 
 
-def get_dist(road_map: RoadMap, point_a: Point, point_b: Point) -> float:
+def get_dist(
+    road_map: RoadMap, point_a: Point, point_b: Point, tolerate: bool = False
+) -> Tuple[float, RoadMap.Route]:
     """
     Computes the shortest route distance from point_a to point_b in the road
     map. Both points should lie on a road in the road map. Key assumption about
@@ -542,9 +609,13 @@ def get_dist(road_map: RoadMap, point_a: Point, point_b: Point) -> float:
         road_map: Scenario road map.
         point_a: A point, in world-map coordinates, which lies on a road.
         point_b: A point, in world-map coordinates, which lies on a road.
+        tolerate: If False, raises an error when distance is negative due to
+            route being computed in reverse direction from point_b to point_a.
+            Defaults to False.
 
     Returns:
         float: Shortest road distance between two points in the road map.
+        RoadMap.Route: Planned route between point_a and point_b.
     """
 
     mission = Mission(
@@ -559,30 +630,55 @@ def get_dist(road_map: RoadMap, point_a: Point, point_b: Point) -> float:
         ),
     )
     plan = Plan(road_map=road_map, mission=mission, find_route=False)
-    plan.create_route(mission=mission, radius=20)
+    plan.create_route(mission=mission, start_lane_radius=3, end_lane_radius=0.5)
     assert isinstance(plan.route, RoadMap.Route)
     from_route_point = RoadMap.Route.RoutePoint(pt=point_a)
     to_route_point = RoadMap.Route.RoutePoint(pt=point_b)
 
     dist_tot = plan.route.distance_between(start=from_route_point, end=to_route_point)
     if dist_tot == None:
         raise CostError("Unable to find road on route near given points.")
-    elif dist_tot < 0:
-        # This happens when agent overshoots the goal position while
-        # remaining outside the goal capture radius at all times. Default
-        # positional goal radius is 2m.
-        dist_tot = abs(dist_tot)
-
-    # Account for agent ending in a different lane but in the same road as
-    # the goal position.
-    start_lane = plan.route.start_lane
-    end_lane = plan.route.end_lane
-    lane_error = abs(start_lane.index - end_lane.index)
-    if len(plan.route.roads) == 1 and lane_error > 0:
-        assert start_lane.road == end_lane.road
-        end_offset = end_lane.offset_along_lane(world_point=point_b)
-        lane_width, _ = end_lane.width_at_offset(end_offset)
-        lane_error_dist = lane_error * lane_width
-        dist_tot += lane_error_dist
-
-    return dist_tot
+    elif dist_tot < 0 and not tolerate:
+        raise CostError(
+            "Route computed in reverse direction from point_b to "
+            f"point_a resulting in negative distance: {dist_tot}."
+        )
+
+    return dist_tot, plan.route
+
+
+def on_route(
+    road_map: RoadMap, route: RoadMap.Route, point: Point, radius: float = 7
+) -> Tuple[bool, Optional[RoadMap.Lane], Optional[Point], Optional[float]]:
+    """
+    Computes whether `point` is within the search `radius` distance from any
+    lane in the `route`.
+
+    Args:
+        road_map (RoadMap): Road map.
+        route (RoadMap.Route): Route consisting of a set of roads.
+        point (smarts.core.coordinates.Point): A world-coordinate point.
+        radius (float): Search radius.
+
+    Returns:
+        Tuple[bool, Optional[RoadMap.Lane], Optional[smarts.core.coordinates.Point], Optional[float]]:
+            True if `point` is nearby any road in `route`, else False. If true,
+            additionally returns the (i) nearest lane in route, (ii) its
+            nearest lane center point, and (iii) displacement between `point`
+            and lane center point.
+    """
+    lanes = road_map.nearest_lanes(
+        point=point,
+        radius=radius,
+        include_junctions=True,
+    )
+
+    route_roads = route.roads
+    for lane, _ in lanes:
+        if lane.road in route_roads:
+            offset = lane.offset_along_lane(world_point=point)
+            lane_point = lane.from_lane_coord(RefLinePoint(s=offset))
+            displacement = np.linalg.norm(lane_point.as_np_array - point.as_np_array)
+            return True, lane, lane_point, displacement
+
+    return False, None, None, None