def create_dynamic_traffic_history_mission(
self, veh_id: str, trigger_time: float,
positional_radius: int) -> Tuple[Mission, Mission]:
"""Builds a vehicle out of
Args:
veh_id:
The id of a vehicle in the traffic history dataset.
trigger_time:
The time that this mission should become active.
positional_radius:
The goal radius for the positional goal.
Returns:
(positional_mission, traverse_mission): A positional mission that follows the initial
original vehicle's travel as well as a traverse style mission which is done when the
vehicle leaves the map.
"""
start, speed = self.get_vehicle_start_at_time(veh_id, trigger_time)
veh_goal = self._get_vehicle_goal(veh_id)
entry_tactic = default_entry_tactic(speed)
# create a positional mission and a traverse mission
positional_mission = Mission(
start=start,
entry_tactic=entry_tactic,
start_time=0,
goal=PositionalGoal(veh_goal, radius=positional_radius),
)
traverse_mission = Mission(
start=start,
entry_tactic=entry_tactic,
start_time=0,
goal=TraverseGoal(self._road_map),
)
return positional_mission, traverse_mission
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 _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)}")