def assert_raises_s(E, contained, f, *args):
try:
f(*args)
except E as e:
s = traceback.format_exc()
if contained not in s:
msg = "Expected a substring in the error"
raise ZException(msg, expected_substring=contained, s=s) from e
except BaseException as e:
msg = "Expected to get %s but found %s: %s" % (E, type(e), e)
raise ZException(msg)
else:
msg = "Expected to find exception %s but none happened." % str(E)
raise ZException(msg)
def sample_many_good_starting_poses(
po: dw.PlacedObject,
nrobots: int,
only_straight: bool,
min_dist: float,
delta_theta_rad: float,
delta_y_m: float,
timeout: float = 10,
) -> List[np.ndarray]:
poses = []
def far_enough(pose_):
for p in poses:
if distance_poses(p, pose_) < min_dist:
return False
return True
t0 = time.time()
while len(poses) < nrobots:
pose = sample_good_starting_pose(po,
only_straight=only_straight,
along_lane=0.2)
if far_enough(pose):
theta = np.random.uniform(-delta_theta_rad, +delta_theta_rad)
y = np.random.uniform(-delta_y_m, +delta_y_m)
t = [0, y]
q = g.SE2_from_translation_angle(t, theta)
pose = g.SE2.multiply(pose, q)
poses.append(pose)
dt = time.time() - t0
if dt > timeout:
msg = "Cannot sample the poses"
raise ZException(msg)
return poses
def sample_duckies(
nduckies: int,
robot_positions: List[SE2value],
min_dist_from_robot: float,
min_dist_from_other_duckie: float,
bounds: dw.RectangularArea,
timeout: float = 10,
) -> List[SE2value]:
poses: List[SE2value] = []
def far_enough(pose_: SE2value) -> bool:
for p in poses:
if distance_poses(p, pose_) < min_dist_from_other_duckie:
return False
for p in robot_positions:
if distance_poses(p, pose_) < min_dist_from_robot:
return False
return True
t0 = time.time()
while len(poses) < nduckies:
theta = np.random.uniform(-np.pi, +np.pi)
t = sample_in_rect(bounds)
pose = g.SE2_from_translation_angle(t, theta)
if far_enough(pose):
poses.append(pose)
dt = time.time() - t0
if dt > timeout:
msg = "Cannot sample in time."
raise ZException(msg)
return poses
def __init__(
self, node: Node, nid: NID, mime: Optional[MimeType], caption: Optional[str]
):
self.node = node
self.nid = nid
self.mime = mime
self.caption = caption
if node.has_child(nid):
msg = "Node %s (id = %r) already has child %r" % (node, node.nid, nid)
raise ValueError(msg)
if self.mime is not None:
if self.mime in mime_to_ext:
suffix = "." + mime_to_ext[self.mime]
else:
suffix = mimetypes.guess_extension(self.mime)
if not suffix:
msg = "Cannot guess extension for MIME %r." % mime
raise ZException(msg)
if suffix == ".svgz":
suffix = ".svg"
else:
suffix = ".bin"
self.temp_file = tempfile.NamedTemporaryFile(suffix=suffix)
def get_data_file(bn: str) -> str:
import act4e_interfaces
p = os.path.dirname(act4e_interfaces.__file__)
data = os.path.join(p, 'data')
f = os.path.join(data, bn)
if not os.path.exists(f):
raise ZException('does not exist', f=f)
return read_ustring_from_utf8_file(f)
def sample_duckies_poses(
po: dw.PlacedObject,
nduckies: int,
robot_positions: List[SE2value],
min_dist_from_robot: float,
min_dist_from_other_duckie: float,
from_side_bounds: Tuple[float, float],
delta_theta_rad: float,
timeout: float = 10,
) -> List[np.ndarray]:
poses: List[SE2value] = []
def far_enough(pose_: SE2value) -> bool:
for p in poses:
if distance_poses(p, pose_) < min_dist_from_other_duckie:
return False
for p in robot_positions:
if distance_poses(p, pose_) < min_dist_from_robot:
return False
return True
t0 = time.time()
while len(poses) < nduckies:
along_lane = np.random.uniform(0, 1)
pose = sample_good_starting_pose(po,
only_straight=False,
along_lane=along_lane)
if not far_enough(pose):
continue
theta = np.random.uniform(-delta_theta_rad, +delta_theta_rad)
y = np.random.uniform(from_side_bounds[0], from_side_bounds[1])
t = [0, y]
q = g.SE2_from_translation_angle(t, theta)
pose = g.SE2.multiply(pose, q)
poses.append(pose)
dt = time.time() - t0
if dt > timeout:
msg = "Cannot sample in time."
raise ZException(msg)
return poses
def get_skeleton_graph(po: DuckietownMap) -> SkeletonGraphResult:
""" Returns a graph with the lane segments of the map """
root = PlacedObject()
meeting_points: Dict[PointLabel, MeetingPoint] = defaultdict(MeetingPoint)
for i, it in enumerate(iterate_by_class(po, LaneSegment)):
lane_segment = cast(LaneSegment, it.object)
assert isinstance(lane_segment, LaneSegment), lane_segment
absolute_pose = it.transform_sequence.asmatrix2d()
lane_segment_transformed = transform_lane_segment(
lane_segment, absolute_pose)
identity = SE2Transform.identity()
name = f"ls{i:03d}"
root.set_object(name, lane_segment_transformed, ground_truth=identity)
p0 = discretize(lane_segment_transformed.control_points[0])
p1 = discretize(lane_segment_transformed.control_points[-1])
if not p0 in meeting_points:
meeting_points[p0] = MeetingPoint(
set(),
set(),
set(),
lane_segment_transformed.control_points[0],
None,
None,
)
if not p1 in meeting_points:
meeting_points[p1] = MeetingPoint(
set(),
set(),
set(),
lane_segment_transformed.control_points[-1],
None,
None,
)
# meeting_points[p0].point = lane_segment_transformed.control_points[0]
meeting_points[p0].outcoming.add(name)
# meeting_points[p1].point = lane_segment_transformed.control_points[-1]
meeting_points[p1].incoming.add(name)
meeting_points[p0].connects_to.add(p1)
tile_coords = [
_ for _ in it.transform_sequence.transforms
if isinstance(_, TileCoords)
]
if not tile_coords:
raise ZException(p0=p0,
p1=p1,
transforms=it.transform_sequence.transforms)
tile_coord = tile_coords[0]
ij = tile_coord.i, tile_coord.j
meeting_points[p0].into_tile = ij
meeting_points[p1].from_tile = ij
for k, mp in meeting_points.items():
if (len(mp.incoming) == 0) or (len(mp.outcoming) == 0):
msg = "Completeness assumption violated at point %s: %s" % (k, mp)
raise Exception(msg)
G0 = graph_for_meeting_points(meeting_points)
# compress the lanes which are contiguous
aliases = {}
created = {}
def resolve_alias(x):
return x if x not in aliases else resolve_alias(aliases[x])
for k, mp in list(meeting_points.items()):
# continue
if not (len(mp.incoming) == 1 and len(mp.outcoming) == 1):
continue
# not necessary anymore
meeting_points.pop(k)
lin_name = list(mp.incoming)[0]
lout_name = list(mp.outcoming)[0]
lin_name = resolve_alias(lin_name)
lout_name = resolve_alias(lout_name)
# print(' -> %s and %s meet at %s' % (lin_name, lout_name, mp))
# print('%s and %s meet at %s' % (lin_name, lout_name, k))
def get(it):
if it in root.children:
return root.children[it]
else:
return created[it]
lin = get(lin_name)
lout = get(lout_name)
# name = 'alias%s' % (len(aliases))
# name = '%s-%s' % (lin_name, lout_name)
name = "L%d" % (len(created))
width = lin.width
control_points = lin.control_points + lout.control_points[1:]
ls = LaneSegment(width=width, control_points=control_points)
created[name] = ls
aliases[lin_name] = name
aliases[lout_name] = name
# print('new alias %s' % name)
#
# print('created: %s' % list(created))
# print('aliases: %s' % aliases)
root2 = PlacedObject()
for k, v in created.items():
if not k in aliases:
root2.set_object(k, v, ground_truth=SE2Transform.identity())
for k, v in root.children.items():
if not k in aliases:
root2.set_object(k, v, ground_truth=SE2Transform.identity())
G = nx.MultiDiGraph()
k2name = {}
for i, (k, mp) in enumerate(meeting_points.items()):
node_name = "P%d" % i
k2name[k] = node_name
G.add_node(node_name, point=mp.point)
ls2start = {}
ls2end = {}
for i, (k, mp) in enumerate(meeting_points.items()):
node_name = k2name[k]
for l in mp.incoming:
ls2end[resolve_alias(l)] = node_name
for l in mp.outcoming:
ls2start[resolve_alias(l)] = node_name
# print(ls2start)
# print(ls2end)
for l in ls2start:
n1 = ls2start[l]
n2 = ls2end[l]
G.add_edge(n1, n2, lane=l)
return SkeletonGraphResult(root=root, root2=root2, G=G, G0=G0)