From a43d9c9e0f0fcb8c297242782b2e03d2def5fb76 Mon Sep 17 00:00:00 2001 From: Tucker Date: Tue, 21 Feb 2023 22:12:48 -0500 Subject: [PATCH 1/8] Add a way to take over social vehicles. --- CHANGELOG.md | 2 + smarts/core/smarts.py | 98 ++++++++++++++++- .../tests/test_smarts_add_social_agents.py | 102 ++++++++++++++++++ 3 files changed, 201 insertions(+), 1 deletion(-) create mode 100644 smarts/core/tests/test_smarts_add_social_agents.py diff --git a/CHANGELOG.md b/CHANGELOG.md index d0b594cee8..add234b0ae 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index f1ea6b697a..4e6f5336fd 100644 --- a/smarts/core/smarts.py +++ b/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,101 @@ 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 = [] + for vehicle_id in 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 self.vehicle_index.agent_vehicle_ids() + or vehicle_id not in self.vehicle_index.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 + ) + try: + agent_interface = self.agent_manager.agent_interface_for_agent_id( + agent_id + ) + except KeyError: + return + 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): diff --git a/smarts/core/tests/test_smarts_add_social_agents.py b/smarts/core/tests/test_smarts_add_social_agents.py new file mode 100644 index 0000000000..b001a112fa --- /dev/null +++ b/smarts/core/tests/test_smarts_add_social_agents.py @@ -0,0 +1,102 @@ +# 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 itertools import cycle +from typing import Iterator + +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 scenarios(): + 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 cycle([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=False, auto_start=False)], + envision=None, + ) + + yield smarts + smarts.destroy() + + +def test_smarts_control_actors_with_social_agents( + smarts: SMARTS, scenarios: Iterator[Scenario] +): + scenario = next(scenarios) + smarts.reset(scenario) + + for _ in range(10): + smarts.step({}) + + provider: Provider = smarts.get_provider_by_type(SumoTrafficSimulation) + 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({}) From f75e7f9599f027fab54217b1f9fbbceaa00f990f Mon Sep 17 00:00:00 2001 From: Tucker Date: Tue, 21 Feb 2023 22:15:50 -0500 Subject: [PATCH 2/8] Simplify test. --- smarts/core/tests/test_smarts_add_social_agents.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/smarts/core/tests/test_smarts_add_social_agents.py b/smarts/core/tests/test_smarts_add_social_agents.py index b001a112fa..b31829b828 100644 --- a/smarts/core/tests/test_smarts_add_social_agents.py +++ b/smarts/core/tests/test_smarts_add_social_agents.py @@ -42,7 +42,7 @@ @pytest.fixture -def scenarios(): +def scenario(): mission = Mission( start=Start(np.array((71.65, 63.78)), Heading(math.pi * 0.91)), goal=EndlessGoal(), @@ -52,7 +52,7 @@ def scenarios(): traffic_specs=["scenarios/sumo/loop/build/traffic/basic.rou.xml"], missions={"Agent-007": mission}, ) - return cycle([scenario]) + return scenario @pytest.fixture @@ -75,9 +75,8 @@ def smarts(): def test_smarts_control_actors_with_social_agents( - smarts: SMARTS, scenarios: Iterator[Scenario] + smarts: SMARTS, scenario: Scenario ): - scenario = next(scenarios) smarts.reset(scenario) for _ in range(10): From 1613d494a7116c6971b2414c02dd479a223efa33 Mon Sep 17 00:00:00 2001 From: Tucker Date: Wed, 22 Feb 2023 10:14:42 -0500 Subject: [PATCH 3/8] Fix typing test --- smarts/core/smarts.py | 9 ++------- smarts/core/tests/test_smarts_add_social_agents.py | 4 +--- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index 4e6f5336fd..a1ca47194d 100644 --- a/smarts/core/smarts.py +++ b/smarts/core/smarts.py @@ -651,18 +651,13 @@ def control_actors_with_social_agents( is_boid=False, is_boid_keep_alive=False, agent_locator=locator, - policy_kwargs=(), + policy_kwargs={}, # initial_speed=10, ) self._agent_manager.start_social_agent( agent_id, social_agent, social_agent_data_model ) - try: - agent_interface = self.agent_manager.agent_interface_for_agent_id( - agent_id - ) - except KeyError: - return + agent_interface = self.agent_manager.agent_interface_for_agent_id(agent_id) vehicle = self.vehicle_index.switch_control_to_agent( self, vehicle_id, diff --git a/smarts/core/tests/test_smarts_add_social_agents.py b/smarts/core/tests/test_smarts_add_social_agents.py index b31829b828..4fb4d9c9c1 100644 --- a/smarts/core/tests/test_smarts_add_social_agents.py +++ b/smarts/core/tests/test_smarts_add_social_agents.py @@ -74,9 +74,7 @@ def smarts(): smarts.destroy() -def test_smarts_control_actors_with_social_agents( - smarts: SMARTS, scenario: Scenario -): +def test_smarts_control_actors_with_social_agents(smarts: SMARTS, scenario: Scenario): smarts.reset(scenario) for _ in range(10): From 8cd57de4e5c13c0871946c2c9d9d9fed00a276bc Mon Sep 17 00:00:00 2001 From: Tucker Date: Wed, 22 Feb 2023 10:48:40 -0500 Subject: [PATCH 4/8] Fix typing test. --- smarts/core/tests/test_smarts_add_social_agents.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/smarts/core/tests/test_smarts_add_social_agents.py b/smarts/core/tests/test_smarts_add_social_agents.py index 4fb4d9c9c1..ad2e4993d8 100644 --- a/smarts/core/tests/test_smarts_add_social_agents.py +++ b/smarts/core/tests/test_smarts_add_social_agents.py @@ -20,8 +20,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. import math -from itertools import cycle -from typing import Iterator +from typing import Optional import numpy as np import pytest @@ -80,7 +79,7 @@ def test_smarts_control_actors_with_social_agents(smarts: SMARTS, scenario: Scen for _ in range(10): smarts.step({}) - provider: Provider = smarts.get_provider_by_type(SumoTrafficSimulation) + provider: Optional[Provider] = smarts.get_provider_by_type(SumoTrafficSimulation) 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() From 27cf491c7a51f52391add834c4e46afe09ad5446 Mon Sep 17 00:00:00 2001 From: Tucker Date: Wed, 22 Feb 2023 12:55:32 -0500 Subject: [PATCH 5/8] Fix test --- smarts/core/tests/test_smarts_add_social_agents.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smarts/core/tests/test_smarts_add_social_agents.py b/smarts/core/tests/test_smarts_add_social_agents.py index ad2e4993d8..677fdd34fc 100644 --- a/smarts/core/tests/test_smarts_add_social_agents.py +++ b/smarts/core/tests/test_smarts_add_social_agents.py @@ -65,7 +65,7 @@ def smarts(): agents = {"Agent-007": buddha} smarts = SMARTS( agents, - traffic_sims=[SumoTrafficSimulation(headless=False, auto_start=False)], + traffic_sims=[SumoTrafficSimulation(headless=True, auto_start=False)], envision=None, ) From e9d6ec4df3b92c0ccbec27cfb68b8ea1ff58723b Mon Sep 17 00:00:00 2001 From: Tucker Date: Wed, 22 Feb 2023 12:58:25 -0500 Subject: [PATCH 6/8] Fix type test. --- smarts/core/tests/test_smarts_add_social_agents.py | 1 + 1 file changed, 1 insertion(+) diff --git a/smarts/core/tests/test_smarts_add_social_agents.py b/smarts/core/tests/test_smarts_add_social_agents.py index 677fdd34fc..e173cc2f81 100644 --- a/smarts/core/tests/test_smarts_add_social_agents.py +++ b/smarts/core/tests/test_smarts_add_social_agents.py @@ -80,6 +80,7 @@ def test_smarts_control_actors_with_social_agents(smarts: SMARTS, scenario: Scen 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() From 71cbbdde49ceeb01e8808548048aa67ba07cf097 Mon Sep 17 00:00:00 2001 From: Montgomery Alban Date: Fri, 24 Feb 2023 15:32:17 +0000 Subject: [PATCH 7/8] Optimize agent control function. --- smarts/core/smarts.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index a1ca47194d..a2ade44353 100644 --- a/smarts/core/smarts.py +++ b/smarts/core/smarts.py @@ -604,12 +604,14 @@ def control_actors_with_social_agents( rejected_vehicle_ids = [] agent_ids = [] - for vehicle_id in vehicle_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 self.vehicle_index.agent_vehicle_ids() - or vehicle_id not in self.vehicle_index.vehicle_ids() + or vehicle_id in current_agent_vehicle_ids + or vehicle_id not in current_vehicle_ids ): rejected_vehicle_ids.append(vehicle_id) continue From 1b95dcb75dd4c670e0f41a54c79bfaf28e110018 Mon Sep 17 00:00:00 2001 From: Montgomery Alban Date: Fri, 24 Feb 2023 15:33:51 +0000 Subject: [PATCH 8/8] Add additional tests to fill --- .../core/tests/test_smarts_add_social_agents.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/smarts/core/tests/test_smarts_add_social_agents.py b/smarts/core/tests/test_smarts_add_social_agents.py index e173cc2f81..c40ab472ad 100644 --- a/smarts/core/tests/test_smarts_add_social_agents.py +++ b/smarts/core/tests/test_smarts_add_social_agents.py @@ -97,3 +97,19 @@ def test_smarts_control_actors_with_social_agents(smarts: SMARTS, scenario: Scen 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