CI test for performance metrics

examples/single_agent.py

@@ -12,8 +12,6 @@
 from smarts.env.wrappers.single_agent import SingleAgent
 from smarts.zoo.agent_spec import AgentSpec
 
-logging.basicConfig(level=logging.INFO)
-
 
 class ChaseViaPointsAgent(Agent):
     def act(self, obs: Observation):

requirements.txt

@@ -108,7 +108,7 @@ rsa==4.9
 Rtree==1.0.1
 scipy==1.7.3
 sh==1.14.3
-shapely==2.0.0
+Shapely==1.8.5.post1
 six==1.16.0
 soupsieve==2.3.2.post1
 tableprint==0.9.1

smarts/core/lanepoints.py

@@ -21,13 +21,11 @@
 # to allow for typing to refer to class being defined (LinkedLanePoint)
 from __future__ import annotations
 
-import math
 import queue
-import warnings
 from collections import defaultdict
 from dataclasses import dataclass
 from functools import lru_cache
-from typing import List, NamedTuple, Optional, Sequence, Tuple
+from typing import List, NamedTuple, Sequence, Tuple
 
 import numpy as np
 from scipy.spatial import KDTree
@@ -36,9 +34,7 @@
 from smarts.core.road_map import RoadMap
 from smarts.core.utils.math import (
     fast_quaternion_from_angle,
-    lerp,
     squared_dist,
-    vec_2d,
     vec_to_radians,
 )
 

smarts/core/plan.py

@@ -320,11 +320,15 @@ def road_map(self) -> RoadMap:
         """The road map this plan is relative to."""
         return self._road_map
 
-    def create_route(self, mission: Mission):
+    def create_route(self, mission: Mission, 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]):
+                Radius (meter) to find the nearest starting lane for the given
+                mission. Defaults to `_default_lane_width` of the underlying
+                road_map.
         """
         assert not self._route, "already called create_route()"
         self._mission = mission or Mission.random_endless_mission(self._road_map)
@@ -335,45 +339,42 @@ def create_route(self, mission: Mission):
 
         assert isinstance(self._mission.goal, PositionalGoal)
 
-        start_lane = self._road_map.nearest_lane(
+        start_lanes = self._road_map.nearest_lanes(
             self._mission.start.point,
-            include_junctions=False,
+            include_junctions=True,
+            radius=radius,
         )
-
-        if not start_lane:
-            # it's possible that the Mission's start point wasn't explicitly
-            # specified by a user, but rather determined during the scenario run
-            # from the current position of a vehicle, in which case it may be
-            # in a junction.  But we only allow this if the previous query fails.
-            start_lane = self._road_map.nearest_lane(
-                self._mission.start.point,
-                include_junctions=True,
-            )
-        if start_lane is None:
+        if not start_lanes:
             self._mission = Mission.endless_mission(Pose.origin())
-            raise PlanningError("Cannot find start lane. Route must start in a lane.")
-        start_road = start_lane.road
+            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(
             self._mission.goal.position,
             include_junctions=False,
         )
         assert end_lane is not None, "route must end in a lane"
-        end_road = end_lane.road
-
-        via_roads = [self._road_map.road_by_id(via) for via in self._mission.route_vias]
 
-        self._route = self._road_map.generate_routes(
-            start_road, end_road, via_roads, 1
-        )[0]
+        # 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.road, end_lane.road, via_roads, 1
+            )[0]
+            if self._route.road_length > 0:
+                break
 
         if len(self._route.roads) == 0:
             self._mission = Mission.endless_mission(Pose.origin())
+            start_road_ids = [start_lane.road.road_id for start_lane, _ in start_lanes]
             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 is "
-                "after a junction.".format(start_road.road_id, end_road.road_id)
+                "after a junction.".format(start_road_ids, end_lane.road.road_id)
             )
 
         return

smarts/core/scenario.py

@@ -37,6 +37,7 @@
     Optional,
     Sequence,
     Tuple,
+    Union,
 )
 
 import cloudpickle
@@ -570,7 +571,7 @@ def get_vehicle_start_at_time(
             Start(
                 np.array([pos_x + hhx, pos_y + hhy]),
                 Heading(heading),
-                from_front_bumper=False,
+                from_front_bumper=True,
             ),
             speed,
         )
@@ -717,7 +718,7 @@ def _extract_mission(mission, road_map):
         the corresponding SMARTS mission types.
         """
 
-        def resolve_offset(offset, lane_length):
+        def resolve_offset(offset: Union[str, float], lane_length: float):
             # epsilon to ensure we are within this edge and not the subsequent one
             epsilon = 1e-6
             lane_length -= epsilon
@@ -730,7 +731,9 @@ def resolve_offset(offset, lane_length):
             else:
                 return float(offset)
 
-        def to_position_and_heading(road_id, lane_index, offset, road_map):
+        def to_position_and_heading(
+            road_id: str, lane_index: int, offset: Union[str, float], road_map: RoadMap
+        ):
             road = road_map.road_by_id(road_id)
             lane = road.lane_at_index(lane_index)
             offset = resolve_offset(offset, lane.length)

smarts/core/tests/test_trajectory_controller.py

@@ -21,10 +21,7 @@
 # THE SOFTWARE.
 import importlib.resources as pkg_resources
 import math
-import os
-import random
 from pathlib import Path
-from unittest import mock
 
 import numpy as np
 import pytest

smarts/env/hiway_env.py

@@ -195,6 +195,15 @@ def scenario_log(self) -> Dict[str, Union[float, str]]:
             "mission_hash": str(hash(frozenset(scenario.missions.items()))),
         }
 
+    @property
+    def scenario(self) -> Scenario:
+        """Returns underlying scenario.
+
+        Returns:
+            Scenario: Current simulated scenario.
+        """
+        return self._smarts.scenario
+
     def seed(self, seed: int) -> int:
         """Sets random number generator seed number.
 
@@ -292,4 +301,3 @@ def close(self):
         """Closes the environment and releases all resources."""
         if self._smarts is not None:
             self._smarts.destroy()
-            self._smarts = None

smarts/env/tests/test_frame_stack.py

@@ -57,8 +57,6 @@ def env(agent_specs):
         scenarios=["scenarios/sumo/figure_eight"],
         agent_specs=agent_specs,
         headless=True,
-        visdom=False,
-        fixed_timestep_sec=0.01,
     )
 
     yield env