Fix SMARTS ignores social agent start time.

CHANGELOG.md

@@ -10,9 +10,11 @@ Copy and pasting the git commit messages is __NOT__ enough.
 
 ## [Unreleased]
 ### Added
+- Agent manager now has `add_and_emit_social_agent` to generate a new social agent that is immediately in control of a vehicle.
 ### Changed
 ### Deprecated
 ### Fixed
+- Fixed an issue with SMARTS where the social vehicles started instantly regardless of what mission start time they were given.
 - Missing waypoint paths `'lane_id'`  is now added to the `hiway-v1` formatted observations.
 ### Removed
 ### Security
@@ -482,7 +484,7 @@ the missions for all agents.
 ### Changed
 – Note any changes to the software’s existing functionality.
 ### Deprecated
-– Note any features that were once stable but are no longer and have thus been removed.
+– Note any features that were once stable but are no longer and have thus been scheduled for removal.
 ### Fixed
 – List any bugs or errors that have been fixed in a change.
 ### Removed

scenarios/sumo/zoo_intersection/scenario.py

@@ -86,19 +86,20 @@
         social_agent_missions={
             f"s-agent-{social_agent2.name}": (
                 [social_agent2],
-                [Mission(RandomRoute())],
-            ),
-            f"s-agent-{social_agent1.name}": (
-                [social_agent1],
                 [
-                    EndlessMission(begin=("edge-south-SN", 0, 30)),
                     Mission(
                         Route(
-                            begin=("edge-west-WE", 0, 10), end=("edge-east-WE", 0, 10)
-                        )
+                            begin=("edge-south-SN", 0, 30), end=("edge-east-WE", 0, 10)
+                        ),
                     ),
                 ],
             ),
+            f"s-agent-{social_agent1.name}": (
+                [social_agent1],
+                [
+                    EndlessMission(begin=("edge-south-SN", 0, 10), start_time=0.7),
+                ],
+            ),
         },
     ),
     output_dir=Path(__file__).parent,

smarts/core/agent_manager.py

@@ -61,6 +61,7 @@ def __init__(self, sim, interfaces, zoo_addrs=None):
         # would not be included
         self._initial_interfaces = interfaces
         self._pending_agent_ids = set()
+        self._pending_social_agent_ids = set()
 
         # Agent interfaces are interfaces for _all_ active agents
         self._agent_interfaces = {}
@@ -118,6 +119,11 @@ def pending_agent_ids(self) -> Set[str]:
         """The IDs of agents that are waiting to enter the simulation"""
         return self._pending_agent_ids
 
+    @property
+    def pending_social_agent_ids(self) -> Set[str]:
+        """The IDs of social agents that are waiting to enter the simulation"""
+        return self._pending_social_agent_ids
+
     @property
     def active_agents(self) -> Set[str]:
         """A list of all active agents in the simulation (agents that have a vehicle.)"""
@@ -460,28 +466,7 @@ def _setup_social_agents(self):
         sim = self._sim()
         assert sim
         social_agents = sim.scenario.social_agents
-        if social_agents:
-            self._setup_agent_buffer()
-        else:
-            return
-
-        self._remote_social_agents = {
-            agent_id: self._agent_buffer.acquire_agent() for agent_id in social_agents
-        }
-
-        for agent_id, (social_agent, social_agent_model) in social_agents.items():
-            self._add_agent(
-                agent_id,
-                social_agent.interface,
-                social_agent_model,
-                trainable=False,
-                # XXX: Currently boids can only be run from bubbles
-                boid=False,
-            )
-            self._social_agent_ids.add(agent_id)
-
-        for social_agent_id, remote_social_agent in self._remote_social_agents.items():
-            remote_social_agent.start(social_agents[social_agent_id][0])
+        self._pending_social_agent_ids.update(social_agents.keys())
 
     def _start_keep_alive_boid_agents(self):
         """Configures and adds boid agents to the sim."""
@@ -510,6 +495,41 @@ def _start_keep_alive_boid_agents(self):
             )
             self.start_social_agent(agent_id, social_agent, social_agent_data_model)
 
+    def add_and_emit_social_agent(
+        self, agent_id: str, agent_spec, agent_model: SocialAgent
+    ):
+        """Generates an entirely new social agent and emits a vehicle for it immediately.
+
+        Args:
+            agent_id (str): The agent id for the new agent.
+            agent_spec (AgentSpec): The agent spec of the new agent
+            agent_model (SocialAgent): The agent configuration of the new vehicle.
+        Returns:
+            bool:
+                If the agent is added. False if the agent id is already reserved
+                by a pending ego agent or current social/ego agent.
+        """
+        if agent_id in self.agent_ids or agent_id in self.pending_agent_ids:
+            return False
+
+        self._setup_agent_buffer()
+        remote_agent = self._agent_buffer.acquire_agent()
+        self._add_agent(
+            agent_id=agent_id,
+            agent_interface=agent_spec.interface,
+            agent_model=agent_model,
+            trainable=False,
+            boid=False,
+        )
+        if agent_id in self._pending_social_agent_ids:
+            self._pending_social_agent_ids.remove(agent_id)
+        remote_agent.start(agent_spec=agent_spec)
+        self._remote_social_agents[agent_id] = remote_agent
+        self._agent_interfaces[agent_id] = agent_spec.interface
+        self._social_agent_ids.add(agent_id)
+        self._social_agent_data_models[agent_id] = agent_model
+        return True
+
     def _add_agent(
         self, agent_id, agent_interface, agent_model, boid=False, trainable=True
     ):

smarts/core/scenario.py

@@ -1025,7 +1025,7 @@ def missions(self) -> Dict[str, Mission]:
         return self._missions
 
     @property
-    def social_agents(self) -> Dict[str, SocialAgent]:
+    def social_agents(self) -> Dict[str, Tuple[Any, SocialAgent]]:
         """Managed social agents within this scenario."""
         return self._social_agents
 

smarts/core/smarts.py

@@ -560,9 +560,10 @@ def switch_control_to_agent(
         assert not self.vehicle_index.vehicle_is_hijacked(
             vehicle_id
         ), f"Vehicle has already been hijacked: {vehicle_id}"
-        assert (
-            not vehicle_id in self.vehicle_index.agent_vehicle_ids()
-        ), f"Can't hijack vehicle that is already controlled by an agent: {vehicle_id}"
+        assert not vehicle_id in self.vehicle_index.agent_vehicle_ids(), (
+            f"`{agent_id}` can't hijack vehicle that is already controlled by an agent"
+            f" `{self.vehicle_index.actor_id_from_vehicle_id(vehicle_id)}`: {vehicle_id}"
+        )
 
         # Switch control to agent
         plan = Plan(self.road_map, mission)

smarts/core/trap_manager.py

@@ -22,7 +22,7 @@
 import random as rand
 from collections import defaultdict
 from dataclasses import dataclass
-from typing import Dict, List, Sequence, Set
+from typing import Dict, List, Optional, Sequence, Set, Tuple
 
 from shapely.geometry import Polygon
 
@@ -161,10 +161,15 @@ def step(self, sim):
         from smarts.core.smarts import SMARTS
 
         assert isinstance(sim, SMARTS)
-        captures_by_agent_id = defaultdict(list)
+        captures_by_agent_id: Dict[str, List[Tuple[str, Trap, Mission]]] = defaultdict(
+            list
+        )
 
         # Do an optimization to only check if there are pending agents.
-        if not sim.agent_manager.pending_agent_ids:
+        if (
+            not sim.agent_manager.pending_agent_ids
+            | sim.agent_manager.pending_social_agent_ids
+        ):
             return
 
         social_vehicle_ids: List[str] = [
@@ -184,7 +189,10 @@ def largest_vehicle_plane_dimension(vehicle: Vehicle):
             for v in vehicles.values()
         ]
 
-        for agent_id in sim.agent_manager.pending_agent_ids:
+        for agent_id in (
+            sim.agent_manager.pending_agent_ids
+            | sim.agent_manager.pending_social_agent_ids
+        ):
             trap = self._traps.get(agent_id)
 
             if trap is None:
@@ -224,29 +232,33 @@ def largest_vehicle_plane_dimension(vehicle: Vehicle):
                         ),
                     )
                 )
-                # TODO: Resolve overlap using a tree instead of just removing.
                 social_vehicle_ids.remove(v_id)
                 break
 
-        # Use fed in trapped vehicles.
-        agents_given_vehicle = set()
         used_traps = []
-        for agent_id in sim._agent_manager.pending_agent_ids:
-            if agent_id not in self._traps:
-                continue
-
-            trap = self._traps[agent_id]
+        for agent_id in (
+            sim.agent_manager.pending_agent_ids
+            | sim.agent_manager.pending_social_agent_ids
+        ):
+            trap = self._traps.get(agent_id)
 
-            captures = captures_by_agent_id[agent_id]
+            if trap is None:
+                continue
 
             if not trap.ready(sim.elapsed_sim_time):
                 continue
 
-            vehicle = None
+            captures = captures_by_agent_id[agent_id]
+
+            vehicle: Optional[Vehicle] = None
             if len(captures) > 0:
                 vehicle_id, trap, mission = rand.choice(captures)
-                vehicle = sim.switch_control_to_agent(
-                    vehicle_id, agent_id, mission, recreate=True, is_hijacked=False
+                vehicle = self._take_existing_vehicle(
+                    sim,
+                    vehicle_id,
+                    agent_id,
+                    mission,
+                    social=agent_id in sim.agent_manager.pending_social_agent_ids,
                 )
             elif trap.patience_expired(sim.elapsed_sim_time):
                 # Make sure there is not a vehicle in the same location
@@ -265,28 +277,54 @@ def largest_vehicle_plane_dimension(vehicle: Vehicle):
                     if overlapping:
                         continue
 
-                vehicle = TrapManager._make_vehicle(
-                    sim, agent_id, mission, trap.default_entry_speed
+                vehicle = TrapManager._make_new_vehicle(
+                    sim,
+                    agent_id,
+                    mission,
+                    trap.default_entry_speed,
+                    social=agent_id in sim.agent_manager.pending_social_agent_ids,
                 )
             else:
                 continue
-            if vehicle == None:
+            if vehicle is None:
                 continue
-            sim.create_vehicle_in_providers(vehicle, agent_id, True)
-            agents_given_vehicle.add(agent_id)
             used_traps.append((agent_id, trap))
 
-        if len(agents_given_vehicle) > 0:
+        if len(used_traps) > 0:
             self.remove_traps(used_traps)
-            sim.agent_manager.remove_pending_agent_ids(agents_given_vehicle)
 
     @property
     def traps(self) -> Dict[str, Trap]:
         """The traps in this manager."""
         return self._traps
 
     @staticmethod
-    def _make_vehicle(sim, agent_id, mission, initial_speed):
+    def _take_existing_vehicle(
+        sim, vehicle_id, agent_id, mission, social=False
+    ) -> Optional[Vehicle]:
+        from smarts.core.smarts import SMARTS
+
+        assert isinstance(sim, SMARTS)
+        if social:
+            # Not supported
+            return None
+        vehicle = sim.switch_control_to_agent(
+            vehicle_id, agent_id, mission, recreate=True, is_hijacked=False
+        )
+        if vehicle is not None:
+            sim.agent_manager.remove_pending_agent_ids({agent_id})
+            sim.create_vehicle_in_providers(vehicle, agent_id, True)
+        return vehicle
+
+    @staticmethod
+    def _make_new_vehicle(
+        sim, agent_id, mission, initial_speed, social=False
+    ) -> Optional[Vehicle]:
+        from smarts.core.smarts import SMARTS
+
+        assert isinstance(sim, SMARTS)
+        if social:
+            return TrapManager._make_new_social_vehicle(sim, agent_id, initial_speed)
         agent_interface = sim.agent_manager.agent_interface_for_agent_id(agent_id)
         plan = Plan(sim.road_map, mission)
         # 3. Apply agent vehicle association.
@@ -302,11 +340,31 @@ def _make_vehicle(sim, agent_id, mission, initial_speed):
             initial_speed=initial_speed,
             boid=False,
         )
+        if vehicle is not None:
+            sim.agent_manager.remove_pending_agent_ids({agent_id})
+            sim.create_vehicle_in_providers(vehicle, agent_id, True)
         return vehicle
 
+    @staticmethod
+    def _make_new_social_vehicle(sim, agent_id, initial_speed):
+        from smarts.core.smarts import SMARTS
+
+        sim: SMARTS = sim
+        social_agent_spec, social_agent_model = sim.scenario.social_agents[agent_id]
+
+        social_agent_model = replace(social_agent_model, initial_speed=initial_speed)
+        sim.agent_manager.add_and_emit_social_agent(
+            agent_id,
+            social_agent_spec,
+            social_agent_model,
+        )
+        vehicles = sim.vehicle_index.vehicles_by_actor_id(agent_id)
+
+        return vehicles[0] if len(vehicles) else None
+
     def reset(self):
         """Resets to a pre-initialized state."""
-        self.captures_by_agent_id = defaultdict(list)
+        pass
 
     def teardown(self):
         """Clear internal state"""