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..a2ade44353 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,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): 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..c40ab472ad --- /dev/null +++ b/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