Feature bubble conditions

docs/spelling_wordlist.txt

@@ -13,6 +13,7 @@ args
 bezier
 boid
 boids
+broadphase
 centric
 coord
 coords

smarts/core/bubble_manager.py

@@ -31,6 +31,7 @@
 from shapely.geometry import CAP_STYLE, JOIN_STYLE, Point, Polygon
 
 from smarts.core.actor_capture_manager import ActorCaptureManager
+from smarts.core.condition_state import ConditionState
 from smarts.core.data_model import SocialAgent
 from smarts.core.plan import (
     EndlessGoal,
@@ -41,13 +42,15 @@
     Start,
 )
 from smarts.core.road_map import RoadMap
+from smarts.core.utils.cache import cache, clear_cache
 from smarts.core.utils.id import SocialAgentId
 from smarts.core.utils.string import truncate
 from smarts.core.vehicle import Vehicle
 from smarts.core.vehicle_index import VehicleIndex
 from smarts.sstudio.types import BoidAgentActor
 from smarts.sstudio.types import Bubble as SSBubble
 from smarts.sstudio.types import BubbleLimits, SocialAgentActor
+from smarts.sstudio.types.condition import Condition
 from smarts.zoo.registry import make as make_social_agent
 
 
@@ -111,6 +114,8 @@ def safe_min(a, b):
         self._limit = bubble_limit
         self._cached_inner_geometry = geometry
         self._exclusion_prefixes = bubble.exclusion_prefixes
+        self._airlock_condition = bubble.airlock_condition
+        self._active_condition = bubble.active_condition
 
         self._cached_airlock_geometry = self._cached_inner_geometry.buffer(
             bubble.margin,
@@ -161,6 +166,16 @@ def keep_alive(self):
         """
         return self._bubble.keep_alive
 
+    @property
+    def airlock_condition(self) -> Condition:
+        """Conditions under which this bubble will accept an agent."""
+        return self._airlock_condition
+
+    @property
+    def active_condition(self) -> Condition:
+        """Fast inclusions for the bubble."""
+        return self._active_condition
+
     # XXX: In the case of traveling bubbles, the geometry and zone are moving
     #      according to the follow vehicle.
     @property
@@ -173,6 +188,15 @@ def airlock_geometry(self) -> Polygon:
         """The airlock geometry of the managed bubble."""
         return self._cached_airlock_geometry
 
+    def condition_passes(
+        self,
+        active_condition_requirements,
+    ):
+        """If the broadphase condition allows for this"""
+        return ConditionState.TRUE in self.active_condition.evaluate(
+            **active_condition_requirements
+        )
+
     def admissibility(
         self,
         vehicle_id: str,
@@ -345,6 +369,8 @@ def from_pos(
         index: VehicleIndex,
         vehicle_ids_per_bubble: Dict[Bubble, Set[str]],
         running_cursors: Set["Cursor"],
+        is_hijack_admissible,
+        is_airlock_admissible,
     ) -> "Cursor":
         """Generate a cursor.
         Args:
@@ -364,9 +390,6 @@ def from_pos(
         in_bubble_zone, in_airlock_zone = bubble.in_bubble_or_airlock(position)
         is_social = vehicle_id in index.social_vehicle_ids()
         is_hijacked, is_shadowed = index.vehicle_is_hijacked_or_shadowed(vehicle_id)
-        is_hijack_admissible, is_airlock_admissible = bubble.admissibility(
-            vehicle_id, index, vehicle_ids_per_bubble, running_cursors
-        )
         was_in_this_bubble = vehicle_id in vehicle_ids_per_bubble[bubble]
 
         # XXX: When a traveling bubble disappears and an agent is airlocked or
@@ -425,35 +448,42 @@ def __init__(self, bubbles: Sequence[SSBubble], road_map: RoadMap):
         self._cursors: Set[Cursor] = set()
         self._last_vehicle_index = VehicleIndex.identity()
         self._bubbles = [Bubble(b, road_map) for b in bubbles]
+        self._active_bubbles: Sequence[Bubble] = []
 
     @property
-    def bubbles(self) -> Sequence[Bubble]:
+    def active_bubbles(self) -> Sequence[Bubble]:
         """A sequence of currently active bubbles."""
-        active_bubbles, _ = self._bubble_groups()
-        return active_bubbles
+        return self._active_bubbles
 
-    def _bubble_groups(self) -> Tuple[List[Bubble], List[Bubble]]:
+    @cache
+    def _bubble_groups(self, sim) -> Tuple[List[Bubble], List[Bubble]]:
         # Filter out traveling bubbles that are missing their follow vehicle
-        def is_active(bubble):
+        def is_active(bubble: Bubble) -> bool:
+            active_condition_requirements = {
+                **self._gen_simulation_condition_kwargs(
+                    sim, bubble.active_condition.requires
+                ),
+                **self._gen_mission_condition_kwargs(
+                    bubble.actor.name, None, bubble.active_condition.requires
+                ),
+            }
+            if not bubble.condition_passes(active_condition_requirements):
+                return False
+
             if not bubble.is_traveling:
                 return True
 
-            vehicles = []
+            vehicle = None
             if bubble.follow_actor_id is not None:
-                vehicles += self._last_vehicle_index.vehicles_by_owner_id(
+                vehicles = self._last_vehicle_index.vehicles_by_owner_id(
                     bubble.follow_actor_id
                 )
+                vehicle = vehicles[0] if len(vehicles) else None
             if bubble.follow_vehicle_id is not None:
                 vehicle = self._last_vehicle_index.vehicle_by_id(
                     bubble.follow_vehicle_id, None
                 )
-                if vehicle is not None:
-                    vehicles += [vehicle]
-            if len(vehicles) > 1:
-                logging.error(
-                    f"bubble `{bubble.id} follows multiple vehicles: {[v.id for v in vehicles]}"
-                )
-            return len(vehicles) == 1
+            return vehicle is not None
 
         active_bubbles = []
         inactive_bubbles = []
@@ -498,14 +528,18 @@ def agent_ids_for_bubble(self, bubble: Bubble, sim) -> Set[str]:
                 agent_ids.add(agent_id)
         return agent_ids
 
+    @clear_cache
     def step(self, sim):
         """Update the associations between bubbles, actors, and agents"""
+        self._active_bubbles, _ = self._bubble_groups(sim)
         self._move_traveling_bubbles(sim)
-        self._cursors = self._sync_cursors(self._last_vehicle_index, sim.vehicle_index)
+        self._cursors = self._sync_cursors(
+            self._last_vehicle_index, sim.vehicle_index, sim
+        )
         self._handle_transitions(sim, self._cursors)
         self._last_vehicle_index = deepcopy(sim.vehicle_index)
 
-    def _sync_cursors(self, last_vehicle_index, vehicle_index):
+    def _sync_cursors(self, last_vehicle_index, vehicle_index, sim):
         # TODO: Not handling newly added vehicles means we require an additional step
         #       before we trigger hijacking.
         # Newly added vehicles
@@ -521,7 +555,10 @@ def _sync_cursors(self, last_vehicle_index, vehicle_index):
         # Calculate latest cursors
         vehicle_ids_per_bubble = self.vehicle_ids_per_bubble()
         cursors = set()
-        active_bubbles, inactive_bubbles = self._bubble_groups()
+        active_bubbles, inactive_bubbles = self._bubble_groups(sim)
+        active_bubbles: Sequence[Bubble]
+        inactive_bubbles: Sequence[Bubble]
+
         inactive_bubbles_to_run = [
             b for b in inactive_bubbles if len(vehicle_ids_per_bubble[b])
         ]
@@ -542,31 +579,65 @@ def _sync_cursors(self, last_vehicle_index, vehicle_index):
         if not active_bubbles:
             return cursors
 
-        for _, vehicle in persisted_vehicle_index.vehicleitems():
-            # XXX: Turns out Shapely Point(...) creation is very expensive (~0.02ms) which
-            #      when inside of a loop x large number of vehicles makes a big
-            #      performance hit.
-            point = vehicle.pose.point
-            v_radius = math.sqrt(
-                vehicle.width * vehicle.width + vehicle.length * vehicle.length
+        # Cut down on duplicate generation of values
+        vehicle_data = [
+            (
+                vehicle,
+                vehicle.pose.point,
+                math.sqrt(
+                    vehicle.width * vehicle.width + vehicle.length * vehicle.length
+                ),
+            )
+            for _, vehicle in persisted_vehicle_index.vehicleitems()
+        ]
+
+        for bubble in active_bubbles:
+            sim_condition_kwargs = self._gen_simulation_condition_kwargs(
+                sim, condition_requires=bubble.airlock_condition.requires
+            )
+            mission_condition_kwargs = self._gen_mission_condition_kwargs(
+                bubble.actor.name,
+                None,
+                condition_requires=bubble.airlock_condition.requires,
             )
+            was_in_this_bubble = vehicle_ids_per_bubble[bubble]
 
-            for bubble in active_bubbles:
-                was_in_this_bubble = vehicle_ids_per_bubble[bubble]
+            for vehicle, point, v_radius in vehicle_data:
                 sq_distance = (point.x - bubble.centroid[0]) * (
                     point.x - bubble.centroid[0]
                 ) + (point.y - bubble.centroid[1]) * (point.y - bubble.centroid[1])
 
                 if vehicle.id in was_in_this_bubble or sq_distance <= pow(
                     v_radius + bubble.radius + bubble._bubble.margin, 2
                 ):
+                    actor_condition_kwargs = self._gen_actor_state_condition_args(
+                        sim.road_map,
+                        vehicle.state,
+                        bubble.airlock_condition.requires,
+                    )
+                    is_hijack_admissible, is_airlock_admissible = bubble.admissibility(
+                        vehicle.id,
+                        persisted_vehicle_index,
+                        vehicle_ids_per_bubble,
+                        cursors,
+                    )
+                    is_airlock_admissible = is_airlock_admissible and (
+                        ConditionState.TRUE
+                        in bubble.airlock_condition.evaluate(
+                            **sim_condition_kwargs,
+                            **mission_condition_kwargs,
+                            **actor_condition_kwargs,
+                        )
+                    )
                     cursor = Cursor.from_pos(
                         position=point.as_shapely,
                         vehicle_id=vehicle.id,
                         bubble=bubble,
                         index=persisted_vehicle_index,
                         vehicle_ids_per_bubble=vehicle_ids_per_bubble,
                         running_cursors=cursors,
+                        is_hijack_admissible=is_hijack_admissible,
+                        is_airlock_admissible=is_airlock_admissible,
                     )
                     cursors.add(cursor)
 
@@ -595,7 +666,7 @@ def _handle_transitions(self, sim, cursors: Set[Cursor]):
                 sim.vehicle_exited_bubble(cursor.vehicle_id, agent_id, teardown)
 
     def _move_traveling_bubbles(self, sim):
-        active_bubbles, inactive_bubbles = self._bubble_groups()
+        active_bubbles, inactive_bubbles = self._bubble_groups(sim)
         for bubble in [*active_bubbles, *inactive_bubbles]:
             if not bubble.is_traveling:
                 continue

smarts/core/smarts.py

@@ -1652,7 +1652,7 @@ def _try_emit_envision_state(self, provider_state: ProviderState, obs, scores):
         if filter.simulation_data_filter["bubble_geometry"].enabled:
             bubble_geometry = [
                 list(bubble.geometry.exterior.coords)
-                for bubble in self._bubble_manager.bubbles
+                for bubble in self._bubble_manager.active_bubbles
             ]
 
         scenario_folder_path = self.scenario._root

smarts/core/tests/test_bubble_hijacking.py

@@ -44,7 +44,7 @@ def num_vehicles():
 
 
 @pytest.fixture
-def bubbles():
+def active_bubbles():
     actor = t.SocialAgentActor(
         name="keep-lane-agent",
         agent_locator="zoo.policies:keep-lane-agent-v0",
@@ -70,7 +70,7 @@ def traffic_sim(request):
 
 
 @pytest.fixture
-def scenarios(bubbles, num_vehicles, traffic_sim):
+def scenarios(active_bubbles, num_vehicles, traffic_sim):
     with temp_scenario(name="6lane", map="maps/6lane.net.xml") as scenario_root:
         traffic = t.Traffic(
             engine=traffic_sim,
@@ -88,7 +88,7 @@ def scenarios(bubbles, num_vehicles, traffic_sim):
         )
 
         gen_scenario(
-            t.Scenario(traffic={"all": traffic}, bubbles=bubbles),
+            t.Scenario(traffic={"all": traffic}, bubbles=active_bubbles),
             output_dir=scenario_root,
         )
 
@@ -117,7 +117,7 @@ class ZoneSteps:
 
 # TODO: Consider a higher-level DSL syntax to fulfill these tests
 @pytest.mark.parametrize("traffic_sim", ["SUMO", "SMARTS"], indirect=True)
-def test_bubble_hijacking(smarts, scenarios, bubbles, num_vehicles, traffic_sim):
+def test_bubble_hijacking(smarts, scenarios, active_bubbles, num_vehicles, traffic_sim):
     """Ensures bubble airlocking, hijacking, and relinquishing are functional.
     Additionally, we test with multiple bubbles and vehicles to ensure operation is
     correct in these conditions as well.
@@ -126,16 +126,16 @@ def test_bubble_hijacking(smarts, scenarios, bubbles, num_vehicles, traffic_sim)
     smarts.reset(scenario)
 
     index = smarts.vehicle_index
-    geometries = [bubble_geometry(b, smarts.road_map) for b in bubbles]
+    geometries = [bubble_geometry(b, smarts.road_map) for b in active_bubbles]
 
     # bubble: vehicle: steps per zone
-    steps_driven_in_zones = {b.id: defaultdict(ZoneSteps) for b in bubbles}
-    vehicles_made_to_through_bubble = {b.id: [] for b in bubbles}
+    steps_driven_in_zones = {b.id: defaultdict(ZoneSteps) for b in active_bubbles}
+    vehicles_made_to_through_bubble = {b.id: [] for b in active_bubbles}
     for _ in range(300):
         smarts.step({})
         for vehicle in index.vehicles:
             position = Point(vehicle.position)
-            for bubble, geometry in zip(bubbles, geometries):
+            for bubble, geometry in zip(active_bubbles, geometries):
                 in_bubble = position.within(geometry.bubble)
                 is_shadowing = index.shadower_id_from_vehicle_id(vehicle.id) is not None
                 is_agent_controlled = vehicle.id in index.agent_vehicle_ids()

smarts/core/tests/test_parallel_sensors.py

@@ -33,7 +33,6 @@
 from smarts.core.simulation_local_constants import SimulationLocalConstants
 from smarts.core.smarts import SMARTS
 from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation
-from smarts.core.utils.file import unpack
 from smarts.core.utils.logging import diff_unpackable
 
 AGENT_IDS = [f"agent-00{i}" for i in range(3)]

smarts/core/vehicle_index.py

@@ -309,7 +309,7 @@ def vehicle_by_id(self, vehicle_id, default=...):
     @clear_cache
     def teardown_vehicles_by_vehicle_ids(self, vehicle_ids, renderer: Optional[object]):
         """Terminate and remove a vehicle from the index using its id."""
-        self._log.debug(f"Tearing down vehicle ids: {vehicle_ids}")
+        self._log.debug("Tearing down vehicle ids: %s", vehicle_ids)
 
         vehicle_ids = [_2id(id_) for id_ in vehicle_ids]
         if len(vehicle_ids) == 0:

smarts/ray/sensors/tests/test_ray_sensor_resolver.py

@@ -99,7 +99,7 @@ def test_sensor_parallelization(
 
     parallel_resolver.get_ray_worker_actors(1)
 
-    assert len(simulation_frame.agent_ids) > 1
+    assert len(simulation_frame.agent_ids) > 0
     p_observations, p_dones, p_updated_sensors = parallel_resolver.observe(
         sim_frame=simulation_frame,
         sim_local_constants=simulation_local_constants,