def scenarios():
mission = Mission(start=Start((71.65, 63.78), Heading(math.pi * 0.91)),
goal=EndlessGoal())
scenario = Scenario(
scenario_root="scenarios/loop",
route="basic.rou.xml",
missions={"Agent-007": mission},
)
return cycle([scenario])
def step(
self,
sim,
):
captures_by_agent_id = defaultdict(list)
# Do an optimization to only check if there are pending agents.
if not sim.agent_manager.pending_agent_ids:
return
social_vehicle_ids = sim.vehicle_index.social_vehicle_ids
vehicles = {
v_id: sim.vehicle_index.vehicle_by_id(v_id)
for v_id in social_vehicle_ids
}
existing_agent_vehicles = (
sim.vehicle_index.vehicle_by_id(v_id)
for v_id in sim.vehicle_index.agent_vehicle_ids)
def largest_vehicle_plane_dimension(vehicle):
return max(*vehicle.chassis.dimensions.as_lwh[:2])
agent_vehicle_comp = [(v.position[:2],
largest_vehicle_plane_dimension(v), v)
for v in existing_agent_vehicles]
for agent_id in sim.agent_manager.pending_agent_ids:
trap = self._traps[agent_id]
if trap is None:
continue
trap.step_trigger(sim.timestep_sec)
if not trap.ready:
continue
# Order vehicle ids by distance.
sorted_vehicle_ids = sorted(
list(social_vehicle_ids),
key=lambda v: squared_dist(vehicles[v].position[:2], trap.
mission.start.position),
)
for v_id in sorted_vehicle_ids:
vehicle = vehicles[v_id]
point = Point(vehicle.position)
if any(
v.startswith(prefix)
for prefix in trap.exclusion_prefixes):
continue
if not point.within(trap.geometry):
continue
captures_by_agent_id[agent_id].append((
v_id,
trap,
replace(
trap.mission,
start=Start(vehicle.position[:2],
vehicle.pose.heading),
),
))
# 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]
captures = captures_by_agent_id[agent_id]
if not trap.ready:
continue
vehicle = None
if len(captures) > 0:
vehicle_id, trap, mission = rand.choice(captures)
vehicle = TrapManager._hijack_vehicle(sim, vehicle_id,
agent_id, mission)
elif trap.patience_expired:
if len(agent_vehicle_comp) > 0:
agent_vehicle_comp.sort(key=lambda v: squared_dist(
v[0], trap.mission.start.position))
# Make sure there is not an agent vehicle in the same location
pos, largest_dimension, _ = agent_vehicle_comp[0]
if (squared_dist(pos, trap.mission.start.position) <
largest_dimension):
continue
vehicle = TrapManager._make_vehicle(sim, agent_id,
trap.mission)
else:
continue
if vehicle == None:
continue
agents_given_vehicle.add(agent_id)
used_traps.append((agent_id, trap))
for provider in sim.providers:
if (sim.agent_manager.agent_interface_for_agent_id(
agent_id).action_space in provider.action_spaces):
provider.create_vehicle(
VehicleState(
vehicle_id=vehicle.id,
vehicle_type="passenger",
pose=vehicle.pose,
dimensions=vehicle.chassis.dimensions,
speed=vehicle.speed,
source="EGO-HIJACK",
))
if len(agents_given_vehicle) > 0:
self.reset_traps(used_traps)
sim.agent_manager.remove_pending_agent_ids(agents_given_vehicle)
