Fix SumoTrafficSimulation route transfer.

CHANGELOG.md

@@ -13,9 +13,12 @@ Copy and pasting the git commit messages is __NOT__ enough.
 ### Changed
 - Changed waypoints in sumo maps to use more incoming lanes into junctions.
 - Increased the cutoff radius for filtering out waypoints that are too far away in junctions in sumo maps.
+- `SumoTrafficSimulator` now uses the last vehicle subscription update to back `route_for_vehicle()`. This means that the routes of vehicles can still be determined even if `SumoTrafficSimulation` disconnects.
 ### Deprecated
 ### Fixed
 - Fixed implementations of `RoadMap.waypoint_paths()` to ensure that the result is never empty.
+- The routes of `SumoTrafficSimulation` traffic vehicles are now preserved to be passed over to other traffic simulators when the `SumoTrafficSimulation` disconnects.
+- `SumoTrafficSimulation` no longer reports that it manages vehicles when it is disconnected.
 ### Removed
 ### Security
 

smarts/core/argoverse_map.py

@@ -887,8 +887,10 @@ def random_route(
     def empty_route(self) -> RoadMap.Route:
         return ArgoverseMap.Route(self)
 
-    def route_from_road_ids(self, road_ids: Sequence[str]) -> RoadMap.Route:
-        return ArgoverseMap.Route.from_road_ids(self, road_ids)
+    def route_from_road_ids(
+        self, road_ids: Sequence[str], resolve_intermediaries: bool = False
+    ) -> RoadMap.Route:
+        return ArgoverseMap.Route.from_road_ids(self, road_ids, resolve_intermediaries)
 
     class _WaypointsCache:
         def __init__(self):

smarts/core/opendrive_road_network.py

@@ -1590,8 +1590,12 @@ def random_route(
     def empty_route(self) -> RoadMap.Route:
         return OpenDriveRoadNetwork.Route(self)
 
-    def route_from_road_ids(self, road_ids: Sequence[str]) -> RoadMap.Route:
-        return OpenDriveRoadNetwork.Route.from_road_ids(self, road_ids)
+    def route_from_road_ids(
+        self, road_ids: Sequence[str], resolve_intermediaries: bool = False
+    ) -> RoadMap.Route:
+        return OpenDriveRoadNetwork.Route.from_road_ids(
+            self, road_ids, resolve_intermediaries
+        )
 
     class _WaypointsCache:
         def __init__(self):

smarts/core/provider.py

@@ -119,7 +119,7 @@ class ProviderManager:
     # other Providers that are willing to accept new actors could watch for this.
 
     def provider_relinquishing_actor(
-        self, provider: "Provider", state: ActorState
+        self, previous_provider: "Provider", state: ActorState
     ) -> Optional["Provider"]:
         """Find a new Provider for an actor from among the Providers managed
         by this ProviderManager.  Returns the new provider or None if a suitable
@@ -190,7 +190,7 @@ def add_actor(
     ):
         """Management of the actor with state is being assigned
         (or transferred if from_provider is not None) to this Provider.
-        Will only be called if can_accept_actor() has returned True."""
+        Should only be called if can_accept_actor() has returned True."""
         raise NotImplementedError
 
     def reset(self):

smarts/core/road_map.py

@@ -182,8 +182,19 @@ def empty_route(self) -> RoadMap.Route:
         """Generate an empty route."""
         raise NotImplementedError()
 
-    def route_from_road_ids(self, road_ids: Sequence[str]) -> RoadMap.Route:
-        """Generate a route containing the specified roads."""
+    def route_from_road_ids(
+        self, road_ids: Sequence[str], resolve_intermediaries: bool = False
+    ) -> RoadMap.Route:
+        """Generate a route containing the specified roads.
+        Args:
+            road_ids (Sequence[str]):
+                The road ids of the route.
+            resolve_intermediaries (bool):
+                If to fill in the gaps in the route. This may be needed to complete an incomplete route or fill in junctions roads.
+
+        Returns:
+            A route that satisfies the given road id restrictions.
+        """
         raise NotImplementedError()
 
     def waypoint_paths(

smarts/core/route_cache.py

@@ -69,9 +69,24 @@ def road_ids(self) -> List[str]:
 
     @classmethod
     def from_road_ids(
-        cls, road_map: RoadMap, road_ids: Sequence[str]
+        cls,
+        road_map: RoadMap,
+        road_ids: Sequence[str],
+        resolve_intermediaries: bool = False,
     ) -> "RouteWithCache":
         """Factory to generate a new RouteWithCache from a sequence of road ids."""
+
+        if len(road_ids) > 0 and resolve_intermediaries:
+            via_roads = [road_map.road_by_id(r) for r in road_ids[1:-1]]
+            routes = road_map.generate_routes(
+                start_road=road_map.road_by_id(road_ids[0]),
+                end_road=road_map.road_by_id(road_ids[-1]),
+                via=via_roads,
+                max_to_gen=1,
+            )
+            if len(routes) > 0:
+                return routes[0]
+
         route = cls(road_map)
         for road_id in road_ids:
             road = road_map.road_by_id(road_id)

smarts/core/smarts.py

@@ -655,9 +655,11 @@ def _provider_for_actor(self, actor_id: str) -> Optional[Provider]:
                 return provider
         return None
 
-    def _stop_managing_with_providers(self, actor_id: str):
+    def _stop_managing_with_providers(self, actor_id: str, exclusion=None):
         managing_providers = [p for p in self.providers if p.manages_actor(actor_id)]
         for provider in managing_providers:
+            if provider is exclusion:
+                continue
             provider.stop_managing(actor_id)
 
     def _remove_vehicle_from_providers(self, vehicle_id: str):
@@ -673,10 +675,10 @@ def create_vehicle_in_providers(
         """Notify providers of the existence of an agent-controlled vehicle,
         one of which should assume management of it."""
         self._check_valid()
+        prev_provider: Optional[Provider] = self._provider_for_actor(vehicle.id)
         self._stop_managing_with_providers(vehicle.id)
         role = ActorRole.EgoAgent if is_ego else ActorRole.SocialAgent
         interface = self.agent_manager.agent_interface_for_agent_id(agent_id)
-        prev_provider = self._provider_for_actor(vehicle.id)
         for provider in self.providers:
             if interface.action in provider.actions:
                 state = VehicleState(
@@ -756,30 +758,36 @@ def _agent_relinquishing_actor(
         return new_prov
 
     def provider_relinquishing_actor(
-        self, provider: Provider, state: ActorState
+        self, previous_provider: Provider, state: ActorState
     ) -> Optional[Provider]:
         """Find a new provider for an actor.  Returns the new provider
         or None if a suitable one could not be found."""
-        self._stop_managing_with_providers(state.actor_id)
-
         # now try to find one who will take it...
         if isinstance(state, VehicleState):
             state.role = ActorRole.Social  # XXX ASSUMPTION: might use Unknown instead?
-        for new_provider in self.providers:
-            if new_provider == provider:
+        new_provider = None
+        for provider in self.providers:
+            if provider is previous_provider:
                 continue
-            if new_provider.can_accept_actor(state):
+            if provider.can_accept_actor(state):
                 # Here we just use the first provider we find that accepts it.
                 # If we want to give preference to, say, Sumo over SMARTS traffic,
                 # then we should ensure that Sumo comes first in the traffic_sims
                 # list we pass to SMARTS __init__().
-                new_provider.add_actor(state, provider)
-                return new_provider
-        self._log.warning(
-            f"could not find a provider to assume control of vehicle {state.actor_id} with role={state.role.name} after being relinquished.  removing it."
-        )
-        self.provider_removing_actor(provider, state.actor_id)
-        return None
+                new_provider = provider
+                break
+        else:
+            self._log.warning(
+                "could not find a provider to assume control of vehicle %s with role=%s after being relinquished.  removing it.",
+                state.actor_id,
+                state.role.name,
+            )
+            self.provider_removing_actor(previous_provider, state.actor_id)
+
+        if new_provider is not None:
+            new_provider.add_actor(state, previous_provider)
+        self._stop_managing_with_providers(state.actor_id, exclusion=new_provider)
+        return new_provider
 
     def provider_removing_actor(self, provider: Provider, actor_id: str):
         # Note: for vehicles, pybullet_provider_sync() will also call teardown

smarts/core/sumo_road_network.py

@@ -941,8 +941,12 @@ def random_route(
     def empty_route(self) -> RoadMap.Route:
         return SumoRoadNetwork.Route(self)
 
-    def route_from_road_ids(self, road_ids: Sequence[str]) -> RoadMap.Route:
-        return SumoRoadNetwork.Route.from_road_ids(self, road_ids)
+    def route_from_road_ids(
+        self, road_ids: Sequence[str], resolve_intermediaries: bool = False
+    ) -> RoadMap.Route:
+        return SumoRoadNetwork.Route.from_road_ids(
+            self, road_ids, resolve_intermediaries
+        )
 
     @cached_property
     def _lanepoints(self):

smarts/core/sumo_traffic_simulation.py

@@ -41,6 +41,7 @@
     ProviderState,
 )
 from smarts.core.road_map import RoadMap
+from smarts.core.route_cache import RouteWithCache
 from smarts.core.signals import SignalLightState, SignalState
 from smarts.core.sumo_road_network import SumoRoadNetwork
 from smarts.core.traffic_provider import TrafficProvider
@@ -117,6 +118,7 @@ def __init__(
         self._traffic_lights = dict()
         self._tls_cache = dict()
         self._last_provider_state = ProviderState()
+        self._last_vehicle_subscriptions = dict()
         self._sim = None
         self._handling_error = False
 
@@ -323,13 +325,16 @@ def setup(self, scenario) -> ProviderState:
             except traci.exceptions.FatalTraCIError:
                 return ProviderState()
         elif self._allow_reload:
+            assert (
+                self._traci_conn is not None
+            ), "TraCI should be connected at this point."
             try:
                 self._traci_conn.load(self._base_sumo_load_params())
             except traci.exceptions.FatalTraCIError as err:
                 self._handle_traci_exception(err, actors_relinquishable=False)
                 return ProviderState()
 
-        assert self._traci_conn is not None, "No active traci connection"
+        assert self._traci_conn is not None, "No active TraCI connection"
 
         self._traci_conn.simulation.subscribe(
             [tc.VAR_DEPARTED_VEHICLES_IDS, tc.VAR_ARRIVED_VEHICLES_IDS]
@@ -434,6 +439,7 @@ def teardown(self):
         self._num_dynamic_ids_used = 0
         self._to_be_teleported = dict()
         self._reserved_areas = dict()
+        self._last_vehicle_subscriptions = dict()
 
     @property
     def connected(self):
@@ -504,6 +510,7 @@ def _sync(self, provider_state: ProviderState):
         # Represents current state
         traffic_vehicle_states = self._traci_conn.vehicle.getAllSubscriptionResults()
         traffic_vehicle_ids = set(traffic_vehicle_states)
+        self._last_vehicle_subscriptions = traffic_vehicle_states
 
         # State / ownership changes
         external_vehicles_that_have_joined = (
@@ -760,6 +767,8 @@ def _compute_provider_state(self) -> ProviderState:
         )
 
     def manages_actor(self, actor_id: str) -> bool:
+        if not self.connected:
+            return False
         return actor_id in self._sumo_vehicle_ids
 
     def _compute_traffic_vehicles(self) -> List[VehicleState]:
@@ -931,14 +940,10 @@ def _reroute_vehicles(self, vehicle_states):
             self._traci_conn.vehicle.setRoute(vehicle_id, new_route_edges)
 
     def _route_for_vehicle(self, vehicle_id: str) -> Optional[List[str]]:
-        if not self.connected:
+        state = self._last_vehicle_subscriptions.get(vehicle_id)
+        if state is None:
             return None
-        try:
-            route = self._traci_conn.vehicle.getRoute(vehicle_id)
-        except self._traci_exceptions as err:
-            self._handle_traci_exception(err)
-            return None
-        return route
+        return state[tc.VAR_EDGES]
 
     def vehicle_dest_road(self, vehicle_id: str) -> Optional[str]:
         route = self._route_for_vehicle(vehicle_id)
@@ -948,8 +953,12 @@ def route_for_vehicle(self, vehicle_id: str) -> Optional[RoadMap.Route]:
         sim = self._sim()
         if sim is None or sim.road_map is None:
             return None
-        route = self._route_for_vehicle(vehicle_id)
-        return sim.road_map.route_from_road_ids(route) if route else None
+        route_ids = self._route_for_vehicle(vehicle_id)
+        return (
+            sim.road_map.route_from_road_ids(route_ids, resolve_intermediaries=True)
+            if route_ids
+            else None
+        )
 
     def reserve_traffic_location_for_vehicle(
         self,
@@ -1020,12 +1029,21 @@ def add_actor(
         self, provider_actor: ActorState, from_provider: Optional[Provider] = None
     ):
         assert isinstance(provider_actor, VehicleState)
-        assert provider_actor.actor_id in self._hijacked
-        self._hijacked.remove(provider_actor.actor_id)
+        if provider_actor.actor_id not in self._hijacked:
+            route = None
+            if from_provider and isinstance(from_provider, TrafficProvider):
+                route = from_provider.route_for_vehicle(provider_actor.actor_id)
+                assert not route or isinstance(route, RouteWithCache)
+                self._traci_conn.vehicle.setRoute(
+                    provider_actor.actor_id, route.road_ids
+                )
+        # elif hijacked there is no need to get the route from from_provider because this vehicle
+        # is one that we used to manage, and Sumo/Traci should remember it.
+
+        self._non_sumo_vehicle_ids.discard(provider_actor.actor_id)
+        self._hijacked.discard(provider_actor.actor_id)
         provider_actor.source = self.source_str
         provider_actor.role = ActorRole.Social
-        # no need to get the route from from_provider because this vehicle
-        # is one that we used to manage, and Sumo/Traci should remember it.
         self._log.info(
             "traffic actor %s transferred to %s.",
             provider_actor.actor_id,

smarts/core/tests/test_bubble_manager.py

@@ -308,6 +308,7 @@ def test_bubble_manager_state_change(
         got_hijacked = index.vehicle_is_hijacked(vehicle_id)
         assert_msg = (
             f"position={next_position}\n"
+            f"\tvehicle_position={index.vehicle_position(vehicle_id)}\n"
             f"\t(expected: shadowed={shadowed}, hijacked={hijacked})\n"
             f"\t(received: shadowed={got_shadowed}, hijacked={got_hijacked})"
         )