def to_scenario_via(
vias: Tuple[SSVia, ...], sumo_road_network: SumoRoadNetwork
) -> Tuple[Via, ...]:
s_vias = []
for via in vias:
lane = sumo_road_network.lane_by_index_on_edge(
via.edge_id, via.lane_index
)
hit_distance = (
via.hit_distance if via.hit_distance > 0 else lane.getWidth() / 2
)
via_position = sumo_road_network.world_coord_from_offset(
lane,
via.lane_offset,
)
s_vias.append(
Via(
lane_id=lane.getID(),
lane_index=via.lane_index,
edge_id=via.edge_id,
position=tuple(via_position),
hit_distance=hit_distance,
required_speed=via.required_speed,
)
)
return tuple(s_vias)
def _build_single_scenario(clean, allow_offset_map, scenario):
import importlib.resources as pkg_resources
from smarts.sstudio.sumo2mesh import generate_glb_from_sumo_network
click.echo(f"build-scenario {scenario}")
if clean:
_clean(scenario)
scenario_root = Path(scenario)
map_net = str(scenario_root / "map.net.xml")
if not allow_offset_map:
SumoRoadNetwork.from_file(map_net, shift_to_origin=True)
elif os.path.isfile(SumoRoadNetwork.shifted_net_file_path(map_net)):
click.echo(
"WARNING: {} already exists. Remove it if you want to use unshifted/offset map.net.xml instead."
.format(SumoRoadNetwork.shifted_net_file_name))
map_glb = scenario_root / "map.glb"
generate_glb_from_sumo_network(map_net, str(map_glb))
requirements_txt = scenario_root / "requirements.txt"
if requirements_txt.exists():
import zoo.policies
with pkg_resources.path(zoo.policies, "") as path:
# Serve policies through the static file server, then kill after
# we've installed scenario requirements
pip_index_proc = subprocess.Popen(
["twistd", "-n", "web", "--path", path],
# Hide output to keep display simple
stdout=subprocess.DEVNULL,
stderr=subprocess.STDOUT,
)
pip_install_cmd = [
sys.executable,
"-m",
"pip",
"install",
"-r",
str(requirements_txt),
]
click.echo(
f"Installing scenario dependencies via '{' '.join(pip_install_cmd)}'"
)
try:
subprocess.check_call(pip_install_cmd,
stdout=subprocess.DEVNULL)
finally:
pip_index_proc.terminate()
pip_index_proc.wait()
scenario_py = scenario_root / "scenario.py"
if scenario_py.exists():
subprocess.check_call([sys.executable, scenario_py])
def to_geometry(self, road_network: SumoRoadNetwork) -> Polygon:
def resolve_offset(offset, geometry_length, lane_length,
buffer_from_ends):
if offset == "base" or offset == 0:
return buffer_from_ends
# push off of end of lane
elif offset == "max":
return lane_length - geometry_length - buffer_from_ends
elif offset == "random":
return random.uniform(
0, lane_length - geometry_length - buffer_from_ends)
else:
return float(offset)
lane_shapes = []
edge_id, lane_idx, offset = self.start
edge = road_network.edge_by_id(edge_id)
for lane_idx in range(lane_idx, lane_idx + self.n_lanes):
lane = edge.getLanes()[lane_idx]
lane_length = lane.getLength()
geom_length = max(self.length - 1e-6, 1e-6)
assert lane_length > geom_length # Geom is too long for lane
assert geom_length > 0 # Geom length is negative
lane_shape = SumoRoadNetwork.buffered_lane_or_edge(
lane, width=lane.getWidth() + 0.3)
min_cut = resolve_offset(offset, geom_length, lane_length, 1e-6)
# Second cut takes into account shortening of geometry by `min_cut`.
max_cut = min(min_cut + geom_length, lane_length - min_cut - 1e-6)
lane_shape = road_network.split_lane_shape_at_offset(
Polygon(lane_shape), lane, min_cut)
if isinstance(lane_shape, GeometryCollection):
if len(lane_shape) < 2:
break
lane_shape = lane_shape[1]
lane_shape = road_network.split_lane_shape_at_offset(
lane_shape,
lane,
max_cut,
)[0]
lane_shapes.append(lane_shape)
geom = unary_union(MultiPolygon(lane_shapes))
return geom
def to_edge(self, sumo_road_network: SumoRoadNetwork):
return sumo_road_network.get_edge_in_junction(
self.start_edge_id,
self.start_lane_index,
self.end_edge_id,
self.end_lane_index,
)
def __init__(
self,
scenario_root: str,
route: str = None,
missions: Dict[str, Mission] = None,
social_agents: Dict[str, SocialAgent] = None,
log_dir: str = None,
surface_patches: list = None,
traffic_history: str = None,
):
self._logger = logging.getLogger(self.__class__.__name__)
self._root = scenario_root
self._route = route
self._missions = missions or {}
self._bubbles = Scenario._discover_bubbles(scenario_root)
self._social_agents = social_agents or {}
self._surface_patches = surface_patches
self._log_dir = self._resolve_log_dir(log_dir)
self._validate_assets_exist()
if traffic_history:
self._traffic_history = TrafficHistory(traffic_history)
default_lane_width = self.traffic_history.lane_width
else:
self._traffic_history = None
default_lane_width = None
net_file = os.path.join(self._root, "map.net.xml")
self._road_network = SumoRoadNetwork.from_file(
net_file, default_lane_width=default_lane_width, lanepoint_spacing=1.0
)
self._net_file_hash = file_md5_hash(self._road_network.net_file)
self._scenario_hash = path2hash(str(Path(self.root_filepath).resolve()))
def _compute_geometry(self):
return [
SumoRoadNetwork.buffered_lane_or_edge(
edge, width=sum([lane.getWidth() for lane in edge.getLanes()])
)
for edge in self.edges
]
def __init__(
self,
scenario_root: str,
route: str = None,
missions: Dict[str, Mission] = None,
social_agents: Dict[str, SocialAgent] = None,
log_dir: str = None,
surface_patches: list = None,
traffic_history: str = None,
):
self._logger = logging.getLogger(self.__class__.__name__)
self._root = scenario_root
self._route = route
self._missions = missions or {}
self._bubbles = Scenario._discover_bubbles(scenario_root)
self._social_agents = social_agents or {}
self._surface_patches = surface_patches
self._log_dir = self._resolve_log_dir(log_dir)
self._validate_assets_exist()
self._road_network = SumoRoadNetwork.from_file(self.net_filepath)
self._net_file_hash = file_md5_hash(self.net_filepath)
self._waypoints = Waypoints(self._road_network, spacing=1.0)
self._scenario_hash = path2hash(str(
Path(self.root_filepath).resolve()))
self._traffic_history_service = Traffic_history_service(
traffic_history)
def from_file(cls, net_file: str):
"""Constructs a route generator from the given file
Args:
net_file: The path to a '\\*.net.xml' file (generally 'map.net.xml')
"""
# XXX: Spacing is crudely "large enough" so we less likely overlap vehicles
road_network = SumoRoadNetwork.from_file(net_file, lanepoint_spacing=2.0)
return cls(road_network)
def map_bounding_box(self):
# This function returns the following tuple:
# (bbox length, bbox width, bbox center)
net_file = os.path.join(self._root, "map.net.xml")
road_network = SumoRoadNetwork.from_file(net_file)
# 2D bbox in format (xmin, ymin, xmax, ymax)
bounding_box = road_network.graph.getBoundary()
bounding_box_length = bounding_box[2] - bounding_box[0]
bounding_box_width = bounding_box[3] - bounding_box[1]
bounding_box_center = [
(bounding_box[0] + bounding_box[2]) / 2,
(bounding_box[1] + bounding_box[3]) / 2,
0,
]
return (bounding_box_length, bounding_box_width, bounding_box_center)
def discover_agent_missions(scenario_root, agents_to_be_briefed):
"""Returns a sequence of {agent_id: mission} mappings.
If no missions are discovered we generate random ones. If there is only one
agent to be briefed we return a list of `{agent_id: mission}` cycling through
each mission. If there are multiple agents to be briefed we assume that each
one is intended to get its own mission and that `len(agents_to_be_briefed) ==
len(missions)`. In this case a list of one dictionary is returned.
"""
net_file = os.path.join(scenario_root, "map.net.xml")
road_network = SumoRoadNetwork.from_file(net_file)
missions = []
missions_file = os.path.join(scenario_root, "missions.pkl")
if os.path.exists(missions_file):
with open(missions_file, "rb") as f:
missions = pickle.load(f)
missions = [
Scenario._extract_mission(actor_and_mission.mission,
road_network)
for actor_and_mission in missions
]
if not missions:
missions = [None for _ in range(len(agents_to_be_briefed))]
if len(agents_to_be_briefed) == 1:
# single-agent, so we cycle through all missions individually.
return missions
elif len(agents_to_be_briefed) > 1:
# multi-agent, so we assume missions "drive" the agents (i.e. one
# mission per agent) and we will not be cycling through missions.
assert not missions or len(missions) == len(agents_to_be_briefed), (
"You must either provide an equal number of missions ({}) to "
"agents ({}) or provide no missions at all so they can be "
"randomly generated.".format(len(missions),
len(agents_to_be_briefed)))
return missions
def is_valid_scenario(scenario_root):
"""Checks if the scenario_root directory matches our expected scenario structure
>>> Scenario.is_valid_scenario("scenarios/loop")
True
>>> Scenario.is_valid_scenario("scenarios/non_existant")
False
"""
paths = [
os.path.join(scenario_root, "map.net.xml"),
]
for f in paths:
if not os.path.exists(f):
return False
# make sure we can load the sumo network
net_file = os.path.join(scenario_root, "map.net.xml")
net = SumoRoadNetwork.from_file(net_file)
if net is None:
return False
return True
def to_geometry(self, road_network: SumoRoadNetwork) -> Polygon:
def resolve_offset(offset, geometry_length, lane_length):
if offset == "base":
return 0
# push off of end of lane
elif offset == "max":
return lane_length - geometry_length
elif offset == "random":
return random.uniform(0, lane_length - geometry_length)
else:
return float(offset)
def pick_remaining_shape_after_split(geometry_collection, length, lane):
lane_shape = geometry_collection
if not isinstance(lane_shape, GeometryCollection):
return lane_shape
# For simplicty, we only deal w/ the == 1 or 2 case
if len(lane_shape) not in {1, 2}:
return None
if len(lane_shape) == 1:
return lane_shape[0]
expected_bbox_area = lane.getWidth() * length
keep_index = 0
if (lane_shape[0].minimum_rotated_rectangle.area - expected_bbox_area) < (
lane_shape[1].minimum_rotated_rectangle.area - expected_bbox_area
):
# 0 is the discard piece, keep the other
keep_index = 1
lane_shape = lane_shape[keep_index]
return lane_shape
lane_shapes = []
edge_id, lane_idx, offset = self.start
edge = road_network.edge_by_id(edge_id)
buffer_from_ends = 1e-6
for lane_idx in range(lane_idx, lane_idx + self.n_lanes):
lane = edge.getLanes()[lane_idx]
lane_length = lane.getLength()
geom_length = self.length
if geom_length > lane_length:
logging.debug(
f"Geometry is too long={geom_length} with offset={offset} for "
f"lane={lane.getID()}, using length={lane_length} instead"
)
geom_length = lane_length
assert geom_length > 0 # Geom length is negative
lane_shape = SumoRoadNetwork._buffered_lane_or_edge(
lane, width=lane.getWidth() + 0.3
)
lane_offset = resolve_offset(offset, geom_length, lane_length)
lane_offset += buffer_from_ends
geom_length = max(geom_length - buffer_from_ends, buffer_from_ends)
lane_length = max(lane_length - buffer_from_ends, buffer_from_ends)
min_cut = min(lane_offset, lane_length)
# Second cut takes into account shortening of geometry by `min_cut`.
max_cut = min(min_cut + geom_length, lane_length - min_cut)
lane_shape = road_network.split_lane_shape_at_offset(
lane_shape, lane, min_cut
)
lane_shape = pick_remaining_shape_after_split(lane_shape, min_cut, lane)
if lane_shape is None:
continue
lane_shape = road_network.split_lane_shape_at_offset(
lane_shape, lane, max_cut,
)
lane_shape = pick_remaining_shape_after_split(lane_shape, max_cut, lane)
if lane_shape is None:
continue
lane_shapes.append(lane_shape)
geom = unary_union(MultiPolygon(lane_shapes))
return geom
def road_network():
return SumoRoadNetwork.from_file(
"scenarios/intersections/4lane_t/map.net.xml")
def _cache_road_network(self):
if not self._road_network:
self._road_network = SumoRoadNetwork.from_file(self._road_network_path)
def generate_glb_from_sumo_file(sumo_net_file: str, out_glb_file: str):
"""Creates a geometry file from a sumo map file."""
map_spec = MapSpec(sumo_net_file)
road_network = SumoRoadNetwork.from_spec(map_spec)
road_network.to_glb(out_glb_file)
def _discover_social_agents_info(
scenario,
) -> Sequence[Dict[str, SocialAgent]]:
"""Loops through the social agent mission pickles, instantiating corresponding
implementations for the given types. The output is a list of
{agent_id: (mission, locator)}, where each dictionary corresponds to the
social agents to run for a given concrete Scenario (which translates to
"per episode" when swapping).
"""
scenario_root = (
scenario.root_filepath if isinstance(scenario, Scenario) else scenario
)
net_file = os.path.join(scenario_root, "map.net.xml")
road_network = SumoRoadNetwork.from_file(net_file)
social_agents_path = os.path.join(scenario_root, "social_agents")
if not os.path.exists(social_agents_path):
return []
# [ ( missions_file, agent_actor, Mission ) ]
agent_bucketer = []
# like dict.setdefault
def setdefault(l: Sequence[Any], index: int, default):
while len(l) < index + 1:
l.append([])
return l[index]
file_match = os.path.join(social_agents_path, "*.pkl")
for missions_file_path in glob.glob(file_match):
with open(missions_file_path, "rb") as missions_file:
count = 0
missions = pickle.load(missions_file)
for mission_and_actor in missions:
# Each pickle file will contain a list of actor/mission pairs. The pairs
# will most likely be generated in an M:N fashion
# (i.e. A1: M1, A1: M2, A2: M1, A2: M2). The desired behavior is to have
# a single pair per concrete Scenario (which would translate to
# "per episode" when swapping)
assert isinstance(
mission_and_actor.actor, sstudio_types.SocialAgentActor
)
actor = mission_and_actor.actor
extracted_mission = Scenario._extract_mission(
mission_and_actor.mission, road_network
)
namespace = os.path.basename(missions_file_path)
namespace = os.path.splitext(namespace)[0]
setdefault(agent_bucketer, count, []).append(
(
SocialAgent(
id=SocialAgentId.new(actor.name, group=namespace),
name=actor.name,
is_boid=False,
is_boid_keep_alive=False,
agent_locator=actor.agent_locator,
policy_kwargs=actor.policy_kwargs,
initial_speed=actor.initial_speed,
),
extracted_mission,
)
)
count += 1
social_agents_info = []
for l in agent_bucketer:
social_agents_info.append(
{agent.id: (agent, mission) for agent, mission in l}
)
return social_agents_info
def resolve_edge_length(self, edge_id, lane_id):
if not self._road_network:
self._road_network = SumoRoadNetwork.from_file(
self._road_network_path)
lane = self._road_network.edge_by_id(edge_id).getLanes()[lane_id]
return lane.getLength()
def generate_glb_from_sumo_network(sumo_net_file, out_glb_file):
road_network = SumoRoadNetwork.from_file(net_file=sumo_net_file)
glb = road_network.build_glb(scale=1000)
glb.write_glb(out_glb_file)
def _cache_road_network(self):
if not self._road_network:
from smarts.core.sumo_road_network import SumoRoadNetwork
map_spec = types.MapSpec(self._road_network_path)
self._road_network = SumoRoadNetwork.from_spec(map_spec)
