def init_traps(self, scenario):
self._traps.clear()
trap_configs: Sequence[TrapConfig] = []
for agent_id in scenario.missions:
mission = scenario.missions[agent_id]
mission_planner = MissionPlanner(scenario.waypoints,
scenario.road_network)
if mission is None:
mission = mission_planner.random_endless_mission()
if not mission.entry_tactic:
mission = replace(mission, entry_tactic=default_entry_tactic())
if (not isinstance(mission.entry_tactic, TrapEntryTactic)
and mission.entry_tactic):
continue
mission_planner.plan(mission)
trap_config = self._mission2trap(scenario.road_network, mission)
trap_configs.append((agent_id, trap_config))
for agent_id, tc in trap_configs:
trap = Trap(
geometry=tc.zone.to_geometry(scenario.road_network),
mission=tc.mission,
exclusion_prefixes=tc.exclusion_prefixes,
reactivation_time=tc.reactivation_time,
remaining_time_to_reactivation=tc.activation_delay,
patience=tc.patience,
)
self.add_trap_for_agent_id(agent_id, trap)
def test_waypoints_sensor_with_uturn_task(uturn_scenarios):
scenario = next(uturn_scenarios)
sim = mock.Mock()
vehicle = mock.Mock()
sim.elapsed_sim_time = 1
sim.timestep_sec = 0.1
nei_vehicle = mock.Mock()
nei_vehicle.pose = Pose(
position=np.array([93, -59, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
nei_vehicle.speed = 13.8
sim.neighborhood_vehicles_around_vehicle = mock.MagicMock(
return_value=[nei_vehicle])
vehicle.pose = Pose(
position=np.array([25, -61, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network,
AgentBehavior(aggressiveness=3))
mission = scenario.missions[AGENT_ID]
mission_planner.plan(mission)
mission_planner._task_is_triggered = True
sensor = WaypointsSensor(sim, vehicle, mission_planner)
waypoints = sensor()
assert len(waypoints) == 1
def test_waypoints_sensor_with_cut_in_task(cut_in_scenarios):
scenario = next(cut_in_scenarios)
sim = mock.Mock()
nei_vehicle = mock.Mock()
nei_vehicle.speed = 10
sim.elapsed_sim_time = 4
nei_vehicle.pose = Pose(
position=np.array([25, -68, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
sim.neighborhood_vehicles_around_vehicle = mock.MagicMock(
return_value=[nei_vehicle])
vehicle = mock.Mock()
vehicle.pose = Pose(
position=np.array([35, -65, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network,
AgentBehavior(aggressiveness=3))
mission = scenario.missions[AGENT_ID]
mission_planner.plan(mission)
sensor = WaypointsSensor(sim, vehicle, mission_planner)
waypoints = sensor()
assert len(waypoints) == 1
def _airlock_social_vehicle_with_social_agent(
self,
vehicle_id: str,
social_agent_actor: SocialAgentActor,
) -> str:
"""When airlocked. The social agent will receive observations and execute
its policy, however it won't actually operate the vehicle's controller.
"""
self._log.debug(
f"Airlocked vehicle={vehicle_id} with actor={social_agent_actor}")
sim = self._sim
agent_id = BubbleManager._make_social_agent_id(vehicle_id,
social_agent_actor)
if agent_id in sim.agent_manager.social_agent_ids:
# E.g. if agent is a boid and was being re-used
interface = sim.agent_manager.agent_interface_for_agent_id(
agent_id)
else:
social_agent = make_social_agent(
locator=social_agent_actor.agent_locator,
**social_agent_actor.policy_kwargs,
)
interface = social_agent.interface
mission_planner = MissionPlanner(sim.scenario.waypoints,
sim.scenario.road_network)
is_boid = isinstance(social_agent_actor, BoidAgentActor)
vehicle = sim.vehicle_index.prepare_for_agent_control(sim,
vehicle_id,
agent_id,
interface,
mission_planner,
boid=is_boid)
# Setup mission (also used for observations)
route = sim.traffic_sim.vehicle_route(vehicle_id=vehicle.id)
mission = Mission(
start=Start(vehicle.position[:2], vehicle.heading),
goal=PositionalGoal.fromedge(route[-1], sim.scenario.road_network),
)
mission_planner.plan(mission=mission)
if agent_id not in sim.agent_manager.social_agent_ids:
social_agent_data_model = SocialAgent(
id=SocialAgentId.new(social_agent_actor.name),
name=social_agent_actor.name,
mission=mission,
agent_locator=social_agent_actor.agent_locator,
policy_kwargs=social_agent_actor.policy_kwargs,
initial_speed=social_agent_actor.initial_speed,
)
sim.agent_manager.start_social_agent(agent_id, social_agent,
social_agent_data_model)
return agent_id
def _hijack_vehicle(sim, vehicle_id, agent_id, mission):
agent_interface = sim.agent_manager.agent_interface_for_agent_id(agent_id)
planner = MissionPlanner(sim.scenario.waypoints, sim.scenario.road_network)
planner.plan(mission=mission)
# Apply agent vehicle association.
sim.vehicle_index.start_agent_observation(
sim, vehicle_id, agent_id, agent_interface, planner
)
vehicle = sim.vehicle_index.switch_control_to_agent(
sim, vehicle_id, agent_id, recreate=True, hijacking=False
)
return vehicle
def attach_sensors_to_vehicles(self, sim, agent_interface, vehicle_ids):
for sv_id in vehicle_ids:
if sv_id in self._vehicle_with_sensors:
continue
mission_planner = MissionPlanner(sim.scenario.waypoints,
sim.scenario.road_network)
mission_planner.plan(mission=None)
agent_id = f"Agent-{sv_id}"
self._vehicle_with_sensors[sv_id] = agent_id
self._agent_interfaces[agent_id] = agent_interface
sim.vehicle_index.attach_sensors_to_vehicle(
sim, sv_id, agent_interface, mission_planner)
def test_waypoints_sensor(scenarios):
scenario = next(scenarios)
sim = mock.Mock()
vehicle = mock.Mock()
vehicle.pose = Pose(
position=np.array([33, -65, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network)
mission = scenario.missions[AGENT_ID]
mission_planner.plan(mission)
sensor = WaypointsSensor(sim, vehicle, mission_planner)
waypoints = sensor()
assert len(waypoints) == 3
def _make_vehicle(sim, agent_id, mission):
agent_interface = sim.agent_manager.agent_interface_for_agent_id(
agent_id)
planner = MissionPlanner(sim.scenario.waypoints,
sim.scenario.road_network)
planner.plan(mission=mission)
# 3. Apply agent vehicle association.
vehicle = sim.vehicle_index.build_agent_vehicle(
sim,
agent_id,
agent_interface,
planner,
sim.scenario.vehicle_filepath,
sim.scenario.tire_parameters_filepath,
True,
sim.scenario.surface_patches,
sim.scenario.controller_parameters_filepath,
)
return vehicle
def init_traps(self, road_network, waypoints, missions):
self._traps.clear()
for agent_id, mission in missions.items():
mission_planner = MissionPlanner(waypoints, road_network)
if mission is None:
mission = mission_planner.random_endless_mission()
if not mission.entry_tactic:
mission = replace(mission, entry_tactic=default_entry_tactic())
if (not isinstance(mission.entry_tactic, TrapEntryTactic)
and mission.entry_tactic):
continue
mission = mission_planner.plan(mission)
trap = self._mission2trap(road_network, mission)
self.add_trap_for_agent_id(agent_id, trap)
def _prepare_sensors_for_agent_control(self, sim, vehicle_id, agent_id,
agent_interface, bubble):
mission_planner = MissionPlanner(sim.scenario.road_network)
vehicle = sim.vehicle_index.start_agent_observation(
sim,
vehicle_id,
agent_id,
agent_interface,
mission_planner,
boid=bubble.is_boid,
)
# Setup mission (also used for observations)
route = sim.traffic_sim.vehicle_route(vehicle_id=vehicle.id)
mission = Mission(
start=Start(vehicle.position[:2], vehicle.heading),
goal=PositionalGoal.fromedge(route[-1], sim.scenario.road_network),
)
mission_planner.plan(mission=mission)
def _make_vehicle(sim, agent_id, mission, initial_speed):
agent_interface = sim.agent_manager.agent_interface_for_agent_id(
agent_id)
planner = MissionPlanner(
sim.scenario.road_network,
agent_interface.agent_behavior,
)
planner.plan(mission=mission)
# 3. Apply agent vehicle association.
vehicle = sim.vehicle_index.build_agent_vehicle(
sim,
agent_id,
agent_interface,
planner,
sim.scenario.vehicle_filepath,
sim.scenario.tire_parameters_filepath,
True,
sim.scenario.surface_patches,
sim.scenario.controller_parameters_filepath,
initial_speed=initial_speed,
boid=False,
)
return vehicle
def _add_agent(self,
agent_id,
agent_interface,
agent_model,
sim,
boid=False,
trainable=True):
# TODO: Disentangle what is entangled below into:
# 1. AgentState initialization,
# 2. Agent vehicle initialization, which should live elsewhere, and
# 3. Provider related state initialization, which does not belong here.
#
# A couple of things forced the current unhappy state --
#
# 1. SMARTS internal coordinate system should be 100% unified. Coordinate
# transformation should happen only at the interface between SMARTS and
# its providers. For example, mission start pose should just be vehicle
# initial pose.
# 2. AgentState should be allowed to fully initialized (setup) without vehicle
# information. We currently rely on the creation of the Vehicle instance to
# do the coordinate transformation. :-(
# 3. Providers should not be creating vehicles. They do need to get notified
# about new vehicles entering their territory through the VehicleState
# message. But that does not need to happen at Agent instantiation.
assert isinstance(agent_id, str) # SUMO expects strings identifiers
scenario = sim.scenario
mission = scenario.mission(agent_id)
planner = MissionPlanner(
scenario.waypoints,
scenario.road_network,
agent_behavior=agent_interface.agent_behavior,
)
planner.plan(mission)
vehicle = sim.vehicle_index.build_agent_vehicle(
sim,
agent_id,
agent_interface,
planner,
scenario.vehicle_filepath,
scenario.tire_parameters_filepath,
trainable,
scenario.surface_patches,
scenario.controller_parameters_filepath,
agent_model.initial_speed,
boid=boid,
)
matching_providers = [
provider for provider in sim.providers
if agent_interface.action_space in provider.action_spaces
]
if matching_providers:
assert (
len(matching_providers) == 1
), f"Found {matching_providers} for action space {agent_interface.action_space}"
provider = matching_providers[0]
provider.create_vehicle(
VehicleState(
vehicle_id=vehicle.id,
vehicle_type=vehicle.vehicle_type,
pose=vehicle.pose,
dimensions=vehicle.chassis.dimensions,
source="NEW-AGENT",
))
self._agent_interfaces[agent_id] = agent_interface
self._social_agent_data_models[agent_id] = agent_model