Performance metrics

CHANGELOG.md

@@ -17,6 +17,7 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - Added `ConfigurableZone` for `Zone` object to types which enable users to build bubble by providing coordinates of the polygon.
 - Added "SMARTS Performance Benchmark" development tool for evaluating the simulation performance.
 - Added a "All Simulation" button on the header of Envision and made small-windowed simulation(s) in the "All Simulations" page clickable to maximize.
+- An env wrapper `Metrics` is introduced to compute agents' performance metrics.
 ### Deprecated
 ### Changed
 ### Removed

smarts/core/coordinates.py

@@ -291,6 +291,7 @@ class Pose:
     """A pair of position and orientation values."""
 
     position: np.ndarray  # [x, y, z]
+    """Center of vehicle."""
     orientation: np.ndarray  # [a, b, c, d] -> a + bi + cj + dk = 0
     heading_: Optional[Heading] = None  # cached heading to avoid recomputing
 
@@ -335,8 +336,8 @@ def point(self) -> Point:
         return Point(*self.position)
 
     @classmethod
-    def from_front_bumper(cls, front_bumper_position, heading, length):
-        """Convert from front bumper location
+    def from_front_bumper(cls, front_bumper_position, heading, length) -> "Pose":
+        """Convert from front bumper location to a Pose with center of vehicle.
 
         Args:
             front_bumper_position: The (x, y) position of the centre front of the front bumper

smarts/core/plan.py

@@ -21,20 +21,18 @@
 # to allow for typing to refer to class being defined (Mission)...
 from __future__ import annotations
 
-import logging
 import math
 import random
 from dataclasses import dataclass, field
-from typing import Optional, Tuple, Union
+from typing import Optional, Tuple
 
 import numpy as np
 
+from smarts.core.coordinates import Dimensions, Heading, Point, Pose, RefLinePoint
+from smarts.core.road_map import RoadMap
+from smarts.core.utils.math import min_angles_difference_signed, vec_to_radians
 from smarts.sstudio.types import EntryTactic, TrapEntryTactic
 
-from .coordinates import Dimensions, Heading, Point, Pose, RefLinePoint
-from .road_map import RoadMap
-from .utils.math import min_angles_difference_signed, vec_to_radians
-
 
 class PlanningError(Exception):
     """Raised in cases when map related planning fails."""
@@ -130,7 +128,7 @@ class TraverseGoal(Goal):
     successfully finishes traversing a non-closed (acyclic) map
     It's a way for the vehicle to exit the simulation successfully,
     for example, driving across from one side to the other on a
-    straight road and then continuing off the map.  This goal is
+    straight road and then continuing off the map. This goal is
     non-specific about *where* the map is exited, save for that
     the vehicle must be going the correct direction in its lane
     just prior to doing so."""
@@ -322,7 +320,7 @@ def road_map(self) -> RoadMap:
         """The road map this plan is relative to."""
         return self._road_map
 
-    def create_route(self, mission: Mission) -> Mission:
+    def create_route(self, mission: Mission):
         """Generates a route that conforms to a mission.
         Args:
             mission (Mission):
@@ -333,7 +331,7 @@ def create_route(self, mission: Mission) -> Mission:
 
         if not self._mission.requires_route:
             self._route = self._road_map.empty_route()
-            return self._mission
+            return
 
         assert isinstance(self._mission.goal, PositionalGoal)
 
@@ -374,8 +372,8 @@ def create_route(self, mission: Mission) -> Mission:
             raise PlanningError(
                 "Unable to find a route between start={} and end={}. If either of "
                 "these are junctions (not well supported today) please switch to "
-                "roads and ensure there is a > 0 offset into the road if it's "
+                "roads and ensure there is a > 0 offset into the road if it is "
                 "after a junction.".format(start_road.road_id, end_road.road_id)
             )
 
-        return self._mission
+        return

smarts/core/road_map.py

@@ -34,8 +34,8 @@
 from shapely.geometry import Point as SPoint
 from shapely.geometry import Polygon
 
-from .coordinates import BoundingBox, Heading, Point, Pose, RefLinePoint
-from .utils.math import (
+from smarts.core.coordinates import BoundingBox, Heading, Point, Pose, RefLinePoint
+from smarts.core.utils.math import (
     fast_quaternion_from_angle,
     min_angles_difference_signed,
     signed_dist_to_line,

smarts/core/route_cache.py

@@ -159,7 +159,7 @@ def _find_along(
                     return RoadMap.Route.RouteLane(cand_lane, rind)
             except ValueError:
                 pass
-        self._logger.warning(f"unable to find road on route near point {rpt}")
+        self._logger.warning(f"Unable to find road on route near point {rpt}")
         return None
 
     @lru_cache(maxsize=8)

smarts/core/sensors.py

@@ -32,7 +32,7 @@
 from smarts.core.plan import Plan
 from smarts.core.road_map import RoadMap, Waypoint
 from smarts.core.signals import SignalLightState, SignalState
-from smarts.core.utils.math import squared_dist, vec_2d, yaw_from_quaternion
+from smarts.core.utils.math import squared_dist
 
 from .coordinates import Dimensions, Heading, Point, Pose, RefLinePoint
 from .events import Events
@@ -88,7 +88,7 @@ class EgoVehicleObservation(NamedTuple):
     steering: float
     """Angle of front wheels in radians between [-pi, pi]."""
     yaw_rate: float
-    """Rotational speed in radians per second"""
+    """Speed of vehicle-heading rotation about the z-axis. Equivalent scalar representation of angular_velocity. Units: rad/s."""
     road_id: str
     """The identifier for the road nearest to this vehicle."""
     lane_id: str
@@ -98,17 +98,17 @@ class EgoVehicleObservation(NamedTuple):
     mission: Mission
     """A field describing the vehicle plotted route"""
     linear_velocity: np.ndarray
-    """Vehicle velocity along body coordinate axes. A numpy array of shape=(3,) and dtype=np.float64."""
+    """Velocity of vehicle along the global coordinate axes. Units: m/s. A numpy array of shape=(3,) and dtype=np.float64."""
     angular_velocity: np.ndarray
-    """Angular velocity vector. A numpy array of shape=(3,) and dtype=np.float64."""
+    """Velocity of vehicle-heading rotation about the z-axis. Equivalent vector representation of yaw_rate. Units: rad/s. A numpy array of shape=(3,) and dtype=np.float64."""
     linear_acceleration: Optional[np.ndarray]
-    """Linear acceleration vector. A numpy array of shape=(3,). dtype=np.float64. Requires accelerometer sensor."""
+    """Acceleration of vehicle along the global coordinate axes. Units: m/s^2. A numpy array of shape=(3,). dtype=np.float64. Requires accelerometer sensor."""
     angular_acceleration: Optional[np.ndarray]
-    """Angular acceleration vector. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor."""
+    """Acceleration of vehicle-heading rotation about the z-axis. Units: rad/s^2. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor."""
     linear_jerk: Optional[np.ndarray]
-    """Linear jerk vector. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor."""
+    """Jerk of vehicle along the global coordinate axes. Units: m/s^3. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor."""
     angular_jerk: Optional[np.ndarray]
-    """Angular jerk vector. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor."""
+    """Jerk of vehicle-heading rotation about the z-axis. Units: rad/s^3. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor."""
     lane_position: Optional[RefLinePoint] = None
     """(s,t,h) coordinates within the lane, where s is the longitudinal offset along the lane, t is the lateral displacement from the lane center, and h (not yet supported) is the vertical displacement from the lane surface.
     See the Reference Line coordinate system in OpenDRIVE here: https://www.asam.net/index.php?eID=dumpFile&t=f&f=4089&token=deea5d707e2d0edeeb4fccd544a973de4bc46a09#_coordinate_systems """
@@ -204,7 +204,7 @@ class SignalObservation:
     """If known, the simulation time this signal last changed its state."""
 
 
-@dataclass
+@dataclass(frozen=True)
 class Observation:
     """The simulation observation."""
 

smarts/core/smarts.py

@@ -1255,7 +1255,7 @@ def resetting(self) -> bool:
         return self._resetting
 
     @property
-    def scenario(self):
+    def scenario(self) -> Scenario:
         """The current simulation scenario."""
         return self._scenario
 

smarts/core/utils/math.py

@@ -20,7 +20,7 @@
 import math
 from dataclasses import dataclass
 from math import factorial
-from typing import Callable, List, Optional, Sequence, Tuple, Union
+from typing import Callable, List, Sequence, Tuple, Union
 
 
 @dataclass(frozen=True)
@@ -526,11 +526,13 @@ def welford() -> Tuple[
     Callable[[float], None], Callable[[], float], Callable[[], float], Callable[[], int]
 ]:
     """Welford's online mean and std computation
-    Reference: https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#On-line_algorithm
-    Reference: https://www.adamsmith.haus/python/answers/how-to-find-a-running-standard-deviation-in-python
+    Reference:
+        https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#On-line_algorithm
+        https://www.adamsmith.haus/python/answers/how-to-find-a-running-standard-deviation-in-python
 
-    :return: Callable functions to update, get mean, get std, get steps
-    :rtype: Tuple[ Callable[[float], None], Callable[[], float], Callable[[], float], Callable[[], int] ]
+    Returns:
+        Tuple[ Callable[[float], None], Callable[[], float], Callable[[], float], Callable[[], int] ]: Callable
+            functions to update, get mean, get std, and get steps.
     """
 
     import math
@@ -548,6 +550,7 @@ def update(val: float):
         S = newS
 
     def mean() -> float:
+        nonlocal M
         return M
 
     def std() -> float:
@@ -559,6 +562,25 @@ def std() -> float:
         return std
 
     def steps() -> int:
+        nonlocal n
         return n
 
     return update, mean, std, steps
+
+
+def running_mean(prev_mean: float, prev_step: int, new_val: float) -> Tuple[float, int]:
+    """
+    Returns a new running mean value, when given previous mean, previous step
+    count, and new value,
+
+    Args:
+        prev_mean (float): Previous mean value.
+        prev_step (int): Previous step count.
+        new_val (float): New value to be averaged.
+
+    Returns:
+        Tuple[float, int]: Updated mean and step count.
+    """
+    new_step = prev_step + 1
+    new_mean = prev_mean + (new_val - prev_mean) / new_step
+    return new_mean, new_step

smarts/core/vehicle.py

@@ -22,7 +22,7 @@
 import os
 from dataclasses import dataclass
 from functools import lru_cache
-from typing import Any, Dict, List, Optional, Sequence, Tuple, Union
+from typing import Any, Dict, List, Optional, Tuple, Union
 
 import numpy as np
 from shapely.affinity import rotate as shapely_rotate
@@ -35,7 +35,7 @@
 from . import models
 from .actor import ActorRole, ActorState
 from .chassis import AckermannChassis, BoxChassis, Chassis
-from .colors import Colors, SceneColors
+from .colors import SceneColors
 from .coordinates import Dimensions, Heading, Pose
 from .sensors import (
     AccelerometerSensor,
@@ -152,9 +152,7 @@ class VehicleConfig:
 class Vehicle:
     """Represents a single vehicle."""
 
-    _HAS_DYNAMIC_ATTRIBUTES = True
-
-    _HAS_DYNAMIC_ATTRIBUTES = True  # pytype dynamic
+    _HAS_DYNAMIC_ATTRIBUTES = True  # pytype: dynamic attribute
 
     def __init__(
         self,

smarts/env/multi_scenario_env.py

@@ -66,7 +66,8 @@ def multi_scenario_v0_env(
     .. note::
 
         A ``smarts.core.controllers.ActionSpaceType.TargetPose``, which is a
-        sequence of ``[x-coordinate, y-coordinate, heading, and time-delta]``.
+        sequence of [x-coordinate, y-coordinate, heading, and time-delta].
+        Use time-delta = 0.1 .
 
         Type:
 
@@ -91,7 +92,7 @@ def multi_scenario_v0_env(
             * - Ego's next heading with respect to the map's axes
               - [-π,π]
             * - Time delta to reach the given pose
-              - [-1e10,1e10]
+              - [0,1e10]
 
     Reward:
 

smarts/env/wrappers/metric/__init__.py

@@ -0,0 +1,19 @@
+# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.