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 _agent_spec_callback(self, ros_agent_spec: AgentSpec):
assert (len(ros_agent_spec.tasks) == 1
), "more than 1 task per agent is not yet supported"
task = ros_agent_spec.tasks[0]
task_params = json.loads(task.params_json) if task.params_json else {}
task_version = task.task_ver or "latest"
agent_locator = f"{self._zoo_module}:{task.task_ref}-{task_version}"
agent_spec = None
try:
agent_spec = registry.make(agent_locator, **task_params)
except ImportError as ie:
rospy.logerr(
f"Unable to locate agent with locator={agent_locator}: {ie}")
if not agent_spec:
rospy.logwarn(
f"got unknown task_ref '{task.task_ref}' in AgentSpec message with params='{task.param_json}'. ignoring."
)
return
if (ros_agent_spec.end_pose.position.x != 0.0
or ros_agent_spec.end_pose.position.y != 0.0):
goal = PositionalGoal(
(
ros_agent_spec.end_pose.position.x,
ros_agent_spec.end_pose.position.y,
),
ros_agent_spec.veh_length,
)
else:
goal = EndlessGoal()
mission = Mission(
start=Start.from_pose(
ROSDriver._pose_from_ros(ros_agent_spec.start_pose)),
goal=goal,
# TODO: how to prevent them from spawning on top of another existing vehicle? (see how it's done in SUMO traffic)
entry_tactic=default_entry_tactic(ros_agent_spec.start_speed),
vehicle_spec=VehicleSpec(
veh_id=f"veh_for_agent_{ros_agent_spec.agent_id}",
veh_config_type=ROSDriver._decode_vehicle_type(
ros_agent_spec.veh_type),
dimensions=Dimensions(
ros_agent_spec.veh_length,
ros_agent_spec.veh_width,
ros_agent_spec.veh_height,
),
),
)
with self._reset_lock:
if (ros_agent_spec.agent_id in self._agents
or ros_agent_spec.agent_id in self._agents_to_add):
rospy.logwarn(
f"trying to add new agent with existing agent_id '{ros_agent_spec.agent_id}'. ignoring."
)
return
self._agents_to_add[ros_agent_spec.agent_id] = (agent_spec,
mission)
def scenario():
mission = Mission(
start=Start(np.array([71.65, 63.78]), Heading(math.pi * 0.91)),
goal=EndlessGoal(),
)
scenario = Scenario(
scenario_root="scenarios/loop",
route="basic.rou.xml",
missions={AGENT_ID: mission},
)
return scenario
def get_vehicle_start_at_time(self, vehicle_id: str,
start_time: float) -> Tuple[Start, float]:
"""Returns a Start object that can be used to create a Mission for
a vehicle from a traffic history dataset starting at its location
at start_time. Also returns its speed at that time."""
pphs = self._traffic_history.vehicle_pose_at_time(
vehicle_id, start_time)
assert pphs
pos_x, pos_y, heading, speed = pphs
# missions start from front bumper, but pos is center of vehicle
veh_dims = self._traffic_history.vehicle_dims(vehicle_id)
hhx, hhy = radians_to_vec(heading) * (0.5 * veh_dims.length)
return (
Start(
(pos_x + hhx, pos_y + hhy),
Heading(heading),
),
speed,
)
def _prepare_sensors_for_agent_control(self, sim, vehicle_id, agent_id,
agent_interface, bubble):
plan = Plan(sim.road_map, None, find_route=False)
vehicle = sim.vehicle_index.start_agent_observation(
sim,
vehicle_id,
agent_id,
agent_interface,
plan,
boid=bubble.is_boid,
)
# Setup mission (also used for observations)
# XXX: this is not quite right. route may not be what the agent wants to take.
route = sim.traffic_sim.vehicle_route(vehicle_id=vehicle.id)
if len(route) > 0:
goal = PositionalGoal.from_road(route[-1], sim.scenario.road_map)
else:
goal = EndlessGoal()
mission = Mission(start=Start(vehicle.position[:2], vehicle.heading),
goal=goal)
plan.create_route(mission)
def _extract_mission(mission, road_map):
"""Takes a sstudio.types.(Mission, EndlessMission, etc.) and converts it to
the corresponding SMARTS mission types.
"""
def resolve_offset(offset, lane_length):
# epsilon to ensure we are within this edge and not the subsequent one
epsilon = 1e-6
lane_length -= epsilon
if offset == "base":
return epsilon
elif offset == "max":
return lane_length
elif offset == "random":
return random.uniform(epsilon, lane_length)
else:
return float(offset)
def to_position_and_heading(road_id, lane_index, offset, road_map):
road = road_map.road_by_id(road_id)
lane = road.lane_at_index(lane_index)
offset = resolve_offset(offset, lane.length)
position = lane.from_lane_coord(RefLinePoint(s=offset))
lane_vector = lane.vector_at_offset(offset)
heading = vec_to_radians(lane_vector[:2])
return position, Heading(heading)
def to_scenario_via(vias: Tuple[SSVia, ...],
road_map: RoadMap) -> Tuple[Via, ...]:
s_vias = []
for via in vias:
road = road_map.road_by_id(via.road_id)
lane = road.lane_at_index(via.lane_index)
lane_width = lane.width_at_offset(via.lane_offset)
hit_distance = (via.hit_distance
if via.hit_distance > 0 else lane_width / 2)
via_position = lane.from_lane_coord(
RefLinePoint(via.lane_offset))
s_vias.append(
Via(
lane_id=lane.lane_id,
lane_index=via.lane_index,
road_id=via.road_id,
position=tuple(via_position[:2]),
hit_distance=hit_distance,
required_speed=via.required_speed,
))
return tuple(s_vias)
# For now we discard the route and just take the start and end to form our
# missions.
if isinstance(mission, sstudio_types.Mission):
position, heading = to_position_and_heading(
*mission.route.begin,
road_map,
)
start = Start(position, heading)
position, _ = to_position_and_heading(
*mission.route.end,
road_map,
)
goal = PositionalGoal(position, radius=2)
return Mission(
start=start,
route_vias=mission.route.via,
goal=goal,
start_time=mission.start_time,
entry_tactic=mission.entry_tactic,
via=to_scenario_via(mission.via, road_map),
)
elif isinstance(mission, sstudio_types.EndlessMission):
position, heading = to_position_and_heading(
*mission.begin,
road_map,
)
start = Start(position, heading)
return Mission(
start=start,
goal=EndlessGoal(),
start_time=mission.start_time,
entry_tactic=mission.entry_tactic,
via=to_scenario_via(mission.via, road_map),
)
elif isinstance(mission, sstudio_types.LapMission):
start_road_id, start_lane, start_road_offset = mission.route.begin
end_road_id, end_lane, end_road_offset = mission.route.end
travel_road = road_map.road_by_id(start_road_id)
if start_road_id == end_road_id:
travel_road = travel_road.outgoing_roads[0]
end_road = road_map.road_by_id(end_road_id)
via_roads = [road_map.road_by_id(r) for r in mission.route.via]
route = road_map.generate_routes(travel_road, end_road, via_roads,
1)[0]
start_position, start_heading = to_position_and_heading(
*mission.route.begin,
road_map,
)
end_position, _ = to_position_and_heading(
*mission.route.end,
road_map,
)
return LapMission(
start=Start(start_position, start_heading),
goal=PositionalGoal(end_position, radius=2),
route_vias=mission.route.via,
num_laps=mission.num_laps,
route_length=route.road_length,
start_time=mission.start_time,
entry_tactic=mission.entry_tactic,
via_points=to_scenario_via(mission.via, road_map),
)
raise RuntimeError(
f"sstudio mission={mission} is an invalid type={type(mission)}")
def step(self, sim):
"""Run hijacking and update agent and actor states."""
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 = [
v_id for v_id in sim.vehicle_index.social_vehicle_ids()
if not sim.vehicle_index.vehicle_is_shadowed(v_id)
]
vehicles = {
v_id: sim.vehicle_index.vehicle_by_id(v_id)
for v_id in social_vehicle_ids
}
def largest_vehicle_plane_dimension(vehicle):
return max(*vehicle.chassis.dimensions.as_lwh[:2])
vehicle_comp = [(v.position[:2], largest_vehicle_plane_dimension(v), v)
for v in vehicles.values()]
for agent_id in sim.agent_manager.pending_agent_ids:
trap = self._traps[agent_id]
if trap is None:
continue
trap.step_trigger(sim.last_dt)
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[:2]),
)
for v_id in sorted_vehicle_ids:
# Skip the capturing process if history traffic is used
if sim.scenario.traffic_history is not None:
break
if not trap.includes(v_id):
continue
vehicle = vehicles[v_id]
point = vehicle.pose.point.as_shapely
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 = sim.switch_control_to_agent(vehicle_id,
agent_id,
mission,
recreate=True,
is_hijacked=False)
elif trap.patience_expired:
# Make sure there is not a vehicle in the same location
mission = trap.mission
nv_dims = Vehicle.agent_vehicle_dims(mission)
new_veh_maxd = max(nv_dims.as_lwh[:2])
overlapping = False
for pos, largest_dimension, _ in vehicle_comp:
if (squared_dist(pos, mission.start.position[:2]) <=
(0.5 * (largest_dimension + new_veh_maxd))**2):
overlapping = True
break
if overlapping:
continue
vehicle = TrapManager._make_vehicle(sim, agent_id, mission,
trap.default_entry_speed)
else:
continue
if vehicle == None:
continue
sim.create_vehicle_in_providers(vehicle, agent_id)
agents_given_vehicle.add(agent_id)
used_traps.append((agent_id, trap))
if len(agents_given_vehicle) > 0:
self.remove_traps(used_traps)
sim.agent_manager.remove_pending_agent_ids(agents_given_vehicle)