Add interface to take over social vehicles.

CHANGELOG.md

@@ -13,6 +13,8 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - Added a math utility for generating combination groups out of two sequences with unique index use per group. This is intended for use to generate the combinations needed to give a unique agent-mission set per reset.
 - Added basic tests for `hiway-v1` resetting and unformatted observations and actions.
 - Added `"steps_completed"` to observation formatter.
+- Added a utility for taking over social vehicles with social agents in `SMARTS.control_actors_with_social_agents`.
+- Added test for `control_actors_with_social_agents`.
 ### Changed
 ### Deprecated
 ### Fixed

smarts/core/smarts.py

@@ -29,6 +29,7 @@
 from envision import types as envision_types
 from envision.client import Client as EnvisionClient
 from smarts import VERSION
+from smarts.core.data_model import SocialAgent
 from smarts.core.plan import Plan
 from smarts.core.utils.logging import timeit
 
@@ -58,7 +59,7 @@
 from .traffic_provider import TrafficProvider
 from .trap_manager import TrapManager
 from .utils import pybullet
-from .utils.id import Id
+from .utils.id import Id, SocialAgentId
 from .utils.math import rounder_for_dt
 from .utils.pybullet import bullet_client as bc
 from .utils.visdom_client import VisdomClient
@@ -581,6 +582,98 @@ def switch_control_to_agent(
 
         return vehicle
 
+    def control_actors_with_social_agents(
+        self, locator, vehicle_ids
+    ) -> Tuple[Set[str], Set[str]]:
+        """Take over control of the specified vehicles with a social agent.
+
+        Args:
+            locator (str): The agent locator string that points to a valid social agent.
+            vehicle_ids (Iterable[str]): The vehicles for the social agent to take over
+
+        Returns:
+            Tuple[Set[str], Set[str]]: The new agent ids and the rejected vehicle ids.
+        """
+        from smarts.zoo.registry import make
+        from smarts.core.plan import (
+            EndlessGoal,
+            PlanningError,
+            PositionalGoal,
+            Start,
+        )
+
+        rejected_vehicle_ids = []
+        agent_ids = []
+        current_agent_vehicle_ids = self.vehicle_index.agent_vehicle_ids()
+        current_vehicle_ids = self.vehicle_index.vehicle_ids()
+        for vehicle_id in set(vehicle_ids):
+            # TODO MTA: ensure that vehicle_id cache is not worthless due to inserts
+            if (
+                self.vehicle_index.vehicle_is_hijacked(vehicle_id)
+                or vehicle_id in current_agent_vehicle_ids
+                or vehicle_id not in current_vehicle_ids
+            ):
+                rejected_vehicle_ids.append(vehicle_id)
+                continue
+
+            agent_id = BubbleManager._make_social_agent_id(vehicle_id)
+            social_agent = make(
+                locator=locator,
+            )
+            interface = social_agent.interface
+            plan = Plan(self.road_map, None, find_route=False)
+            vehicle = self.vehicle_index.start_agent_observation(
+                self,
+                vehicle_id,
+                agent_id,
+                interface,
+                plan,
+                boid=False,
+            )
+            dest_road_id = None
+            for traffic_sim in self.traffic_sims:
+                if traffic_sim.manages_actor(vehicle.id):
+                    dest_road_id = traffic_sim.vehicle_dest_road(vehicle.id)
+                    if dest_road_id is not None:
+                        break
+            if dest_road_id:
+                goal = PositionalGoal.from_road(dest_road_id, self.scenario.road_map)
+            else:
+                goal = EndlessGoal()
+            mission = Mission(
+                start=Start(vehicle.position[:2], vehicle.heading), goal=goal
+            )
+            try:
+                plan.create_route(mission)
+            except PlanningError:
+                plan.route = self.road_map.empty_route()
+            social_agent_data_model = SocialAgent(
+                id=SocialAgentId.new(agent_id),
+                name=agent_id,
+                is_boid=False,
+                is_boid_keep_alive=False,
+                agent_locator=locator,
+                policy_kwargs={},
+                # initial_speed=10,
+            )
+            self._agent_manager.start_social_agent(
+                agent_id, social_agent, social_agent_data_model
+            )
+            agent_interface = self.agent_manager.agent_interface_for_agent_id(agent_id)
+            vehicle = self.vehicle_index.switch_control_to_agent(
+                self,
+                vehicle_id,
+                agent_id,
+                boid=False,
+                hijacking=True,
+                recreate=False,
+                agent_interface=agent_interface,
+            )
+            self.create_vehicle_in_providers(vehicle, agent_id)
+            agent_ids.append(agent_id)
+
+        return set(agent_ids), set(rejected_vehicle_ids)
+
     def _provider_for_actor(self, actor_id: str) -> Optional[Provider]:
         for provider in self.providers:
             if provider.manages_actor(actor_id):

smarts/core/tests/test_smarts_add_social_agents.py

@@ -0,0 +1,115 @@
+# MIT License
+#
+# Copyright (C) 2021. Huawei Technologies Co., Ltd. All rights reserved.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+import math
+from typing import Optional
+
+import numpy as np
+import pytest
+
+from smarts.core.agent_interface import (
+    ActionSpaceType,
+    AgentInterface,
+    DoneCriteria,
+    NeighborhoodVehicles,
+)
+from smarts.core.coordinates import Heading
+from smarts.core.plan import EndlessGoal, Mission, Start
+from smarts.core.scenario import Scenario
+from smarts.core.smarts import SMARTS
+from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation
+from smarts.core.utils.custom_exceptions import RendererException
+from smarts.core.provider import Provider
+
+
+@pytest.fixture
+def scenario():
+    mission = Mission(
+        start=Start(np.array((71.65, 63.78)), Heading(math.pi * 0.91)),
+        goal=EndlessGoal(),
+    )
+    scenario = Scenario(
+        scenario_root="scenarios/sumo/loop",
+        traffic_specs=["scenarios/sumo/loop/build/traffic/basic.rou.xml"],
+        missions={"Agent-007": mission},
+    )
+    return scenario
+
+
+@pytest.fixture
+def smarts():
+    buddha = AgentInterface(
+        max_episode_steps=1000,
+        neighborhood_vehicle_states=NeighborhoodVehicles(radius=20),
+        action=ActionSpaceType.Lane,
+        done_criteria=DoneCriteria(collision=False, off_road=False),
+    )
+    agents = {"Agent-007": buddha}
+    smarts = SMARTS(
+        agents,
+        traffic_sims=[SumoTrafficSimulation(headless=True, auto_start=False)],
+        envision=None,
+    )
+
+    yield smarts
+    smarts.destroy()
+
+
+def test_smarts_control_actors_with_social_agents(smarts: SMARTS, scenario: Scenario):
+    smarts.reset(scenario)
+
+    for _ in range(10):
+        smarts.step({})
+
+    provider: Optional[Provider] = smarts.get_provider_by_type(SumoTrafficSimulation)
+    assert provider
+    assert provider.actor_ids
+    vehicle_ids = set(list(provider.actor_ids)[:5] + ["Agent-007"])
+    original_social_agent_vehicle_ids = smarts.vehicle_index.agent_vehicle_ids()
+    agent_ids, rejected_vehicle_ids = smarts.control_actors_with_social_agents(
+        "scenarios.sumo.zoo_intersection.agent_prefabs:zoo-agent2-v0", vehicle_ids
+    )
+
+    new_social_agent_vehicle_ids = smarts.vehicle_index.agent_vehicle_ids()
+    assert rejected_vehicle_ids
+    assert smarts.agent_manager.social_agent_ids.issuperset(agent_ids)
+    assert agent_ids
+    assert new_social_agent_vehicle_ids.isdisjoint(rejected_vehicle_ids)
+    assert len(original_social_agent_vehicle_ids) < len(new_social_agent_vehicle_ids)
+
+    for _ in range(10):
+        smarts.step({})
+
+
+def test_smarts_shadow_with_social_agent(smarts: SMARTS, scenario: Scenario):
+    pass
+
+
+def test_smarts_promote_social_agent_to_actor_owner(smarts: SMARTS, scenario: Scenario):
+    pass
+
+
+def test_smarts_remove_social_agent(smarts: SMARTS, scenario: Scenario):
+    pass
+
+
+def test_smarts_add_social_boid_agent(smarts: SMARTS, scenario: Scenario):
+    pass