diff --git a/docs/spelling_wordlist.txt b/docs/spelling_wordlist.txt index 156f0355e4..ffeaf749cb 100644 --- a/docs/spelling_wordlist.txt +++ b/docs/spelling_wordlist.txt @@ -13,6 +13,7 @@ args bezier boid boids +broadphase centric coord coords diff --git a/smarts/core/bubble_manager.py b/smarts/core/bubble_manager.py index 1da83428ac..eab3c14201 100644 --- a/smarts/core/bubble_manager.py +++ b/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,6 +42,7 @@ 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 @@ -48,6 +50,7 @@ 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,17 +579,30 @@ 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]) @@ -560,6 +610,25 @@ def _sync_cursors(self, last_vehicle_index, vehicle_index): 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, @@ -567,6 +636,8 @@ def _sync_cursors(self, last_vehicle_index, vehicle_index): 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 diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index e429e2e195..eeb010320e 100644 --- a/smarts/core/smarts.py +++ b/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 diff --git a/smarts/core/tests/test_bubble_hijacking.py b/smarts/core/tests/test_bubble_hijacking.py index dfd9a9fce7..31500d6030 100644 --- a/smarts/core/tests/test_bubble_hijacking.py +++ b/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() diff --git a/smarts/core/tests/test_parallel_sensors.py b/smarts/core/tests/test_parallel_sensors.py index 049fb2e0dc..2843b3a96b 100644 --- a/smarts/core/tests/test_parallel_sensors.py +++ b/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)] diff --git a/smarts/core/vehicle_index.py b/smarts/core/vehicle_index.py index 0ca5057b08..dba9da18f9 100644 --- a/smarts/core/vehicle_index.py +++ b/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: diff --git a/smarts/ray/sensors/tests/test_ray_sensor_resolver.py b/smarts/ray/sensors/tests/test_ray_sensor_resolver.py index 5a7373c67c..ab9c4b2bad 100644 --- a/smarts/ray/sensors/tests/test_ray_sensor_resolver.py +++ b/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, diff --git a/smarts/sstudio/types/bubble.py b/smarts/sstudio/types/bubble.py index 87ccc8c3eb..2e0edb7265 100644 --- a/smarts/sstudio/types/bubble.py +++ b/smarts/sstudio/types/bubble.py @@ -25,30 +25,41 @@ from typing import Optional, Tuple from smarts.core import gen_id +from smarts.core.condition_state import ConditionState from smarts.core.utils.id import SocialAgentId from smarts.sstudio.types.actor.social_agent_actor import ( BoidAgentActor, SocialAgentActor, ) from smarts.sstudio.types.bubble_limits import BubbleLimits +from smarts.sstudio.types.condition import ( + Condition, + ConditionRequires, + LiteralCondition, +) from smarts.sstudio.types.zone import MapZone, Zone @dataclass(frozen=True) class Bubble: - """A descriptor that defines a capture bubble for social agents.""" + """A descriptor that defines a capture bubble for social agents. + + Bubbles consist of an airlock and hijack zone. The airlock is always the same size + or larger than the hijack zone. A vehicle must first pass into the airlock and + pass the conditions of the airlock to be considered by the hijack zone. + """ zone: Zone """The zone which to capture vehicles.""" actor: SocialAgentActor """The actor specification that this bubble works for.""" - margin: float = 2 # Used for "airlocking"; must be > 0 - """The exterior buffer area for air-locking. Must be > 0.""" - # If limit != None it will only allow that specified number of vehicles to be - # hijacked. N.B. when actor = BoidAgentActor the lesser of the actor capacity - # and bubble limit will be used. + margin: float = 2 + """The exterior buffer area that extends the air-locking zone area. Must be >= 0.""" limit: Optional[BubbleLimits] = None - """The maximum number of actors that could be captured.""" + """The maximum number of actors that could be captured. If limit != None it will + only allow that specified number of vehicles to be hijacked. + N.B. when actor = BoidAgentActor the lesser of the actor capacity and bubble limit will be used. + """ exclusion_prefixes: Tuple[str, ...] = field(default_factory=tuple) """Used to exclude social actors from capture.""" id: str = field(default_factory=lambda: f"bubble-{gen_id()}") @@ -71,6 +82,11 @@ class Bubble: which means it moves to follow the `follow_vehicle_id`'s vehicle. Offset is from the vehicle's center position to the bubble's center position. """ + active_condition: Condition = LiteralCondition(ConditionState.TRUE) + """Conditions that determine if the bubble is enabled.""" + airlock_condition: Condition = LiteralCondition(ConditionState.TRUE) + """This condition is used to determine if an actor is allowed into the bubble airlock. + """ def __post_init__(self): if self.margin < 0: @@ -107,6 +123,12 @@ def __post_init__(self): if follow_id else f"The zone polygon of {type(self.zone).__name__} of fixed position {self.id} is not a valid closed loop" ) + if ( + ConditionRequires.any_current_actor_state & self.active_condition.requires + ) != ConditionRequires.none: + raise ValueError( + "Actor state conditions not allowed in broadphase inclusion." + ) @staticmethod def to_actor_id(actor, mission_group):