def get_marker_array_from_map(m):
known_tile_types, known_sign_types = get_list_of_supported_types()
marker_array = MarkerArray()
marker_id = 0
# Add all tiles
records = list(iterate_by_class(m, dw.Tile))
for record in records:
tile = record.object
matrix = record.transform_sequence.asmatrix2d().m
translation, angle, scale = geo.translation_angle_scale_from_E2(matrix)
if tile.kind not in known_tile_types:
rospy.logerr("Unknown tile type: %s", tile.kind)
marker = get_tile_marker(translation[0], translation[1], angle,
tile.kind, marker_id, scale)
rospy.loginfo("Created marker for %s", tile.kind)
marker_array.markers.append(marker)
marker_id += 1
marker_id = 0
# Add all signs
records = list(iterate_by_class(m, dw.Sign))
for record in records:
sign = record.object
matrix = record.transform_sequence.asmatrix2d().m
translation, angle, scale = geo.translation_angle_scale_from_E2(matrix)
if sign.get_name_texture() not in known_sign_types:
rospy.logerr("Unknown sign type: %s", sign.get_name_texture())
marker = get_sign_marker(translation[0], translation[1], angle,
sign.get_name_texture(), marker_id, scale)
rospy.loginfo("Created marker for %s", sign.get_name_texture())
marker_array.markers.append(marker)
marker_id += 1
marker_id = 0
# Add all apriltags
records = list(iterate_by_class(m, dw.tags_db.FloorTag))
for record in records:
matrix = record.transform_sequence.asmatrix2d().m
translation, angle, scale = geo.translation_angle_scale_from_E2(matrix)
marker = get_apriltag_marker(translation[0], translation[1], angle,
"apriltag", marker_id, scale)
rospy.loginfo("Created marker for Apriltag-%s", record.fqn[0])
marker_array.markers.append(marker)
marker_id += 1
return marker_array
def lane_pose_from_SE2_generic(self, q, tol=0.001):
p, _, _ = geo.translation_angle_scale_from_E2(q)
beta, q0 = self.find_along_lane_closest_point(p, tol=tol)
along_lane = self.along_lane_from_beta(beta)
rel = relative_pose(q0, q)
r, relative_heading, _ = geo.translation_angle_scale_from_E2(rel)
lateral = r[1]
# extra = r[0]
# along_lane -= extra
# logger.info('ms: %s' % r[0])
return self.lane_pose(relative_heading=relative_heading,
lateral=lateral,
along_lane=along_lane)
def get_marker_array_from_map(m):
known_types = get_list_of_supported_types()
marker_array = MarkerArray()
marker_id = 0
# Add all tiles
records = list(iterate_by_class(m, dw.Tile))
for record in records: # only first 15
tile = record.object
matrix = record.transform_sequence.asmatrix2d().m
translation, angle, scale = geo.translation_angle_scale_from_E2(matrix)
if tile.kind not in known_types:
rospy.logerr("Unknown tile type: %s", tile.kind)
marker = get_tile_marker(translation[0], translation[1], angle,
tile.kind, marker_id, scale)
rospy.loginfo("Created marker for %s", tile.kind)
marker_array.markers.append(marker)
marker_id += 1
return marker_array
def lane_pose_from_SE2_straight(self, q):
cp1 = self.control_points[0]
rel = relative_pose(cp1.as_SE2(), q)
tas = geo.translation_angle_scale_from_E2(rel)
lateral = tas.translation[1]
along_lane = tas.translation[0]
relative_heading = tas.angle
return self.lane_pose(relative_heading=relative_heading,
lateral=lateral,
along_lane=along_lane)
def sample_good_starting_pose(m: PlacedObject, only_straight: bool, along_lane: float) -> geo.SE2value:
""" Samples a good starting pose on a straight lane """
choices = list(iterate_by_class(m, LaneSegment))
if only_straight:
choices = [_ for _ in choices if is_straight(_)]
choice = random.choice(choices)
ls: LaneSegment = choice.object
lp: LanePose = ls.lane_pose(along_lane, 0.0, 0.0)
rel: SE2Transform = ls.SE2Transform_from_lane_pose(lp)
m1 = choice.transform_sequence.asmatrix2d().m
m2 = rel.asmatrix2d().m
g = np.dot(m1, m2)
t, a, s = geo.translation_angle_scale_from_E2(g)
g = geo.SE2_from_translation_angle(t, a)
return g
def evaluate(self, context: RuleEvaluationContext,
result: RuleEvaluationResult):
interval = context.get_interval()
lane_pose_seq = context.get_lane_pose_seq()
ego_pose_sequence = context.get_ego_pose_global()
timestamps = []
driven_any = []
driven_lanedir = []
tile_fqn2lane_fqn = {}
for idt in iterate_with_dt(interval):
t0, t1 = idt.v0, idt.v1 # not v
try:
name2lpr = lane_pose_seq.at(t0)
p0 = ego_pose_sequence.at(t0).as_SE2()
p1 = ego_pose_sequence.at(t1).as_SE2()
except UndefinedAtTime:
dr_any = dr_lanedir = 0.0
else:
prel = relative_pose(p0, p1)
translation, _ = geo.translation_angle_from_SE2(prel)
dr_any = np.linalg.norm(translation)
if name2lpr:
ds = []
for k, lpr in name2lpr.items():
if lpr.tile_fqn in tile_fqn2lane_fqn:
if lpr.lane_segment_fqn != tile_fqn2lane_fqn[
lpr.tile_fqn]:
# msg = 'Backwards detected'
# print(msg)
continue
tile_fqn2lane_fqn[lpr.tile_fqn] = lpr.lane_segment_fqn
assert isinstance(lpr, GetLanePoseResult)
c0 = lpr.center_point
ctas = geo.translation_angle_scale_from_E2(
c0.asmatrix2d().m)
c0_ = geo.SE2_from_translation_angle(
ctas.translation, ctas.angle)
prelc0 = relative_pose(c0_, p1)
tas = geo.translation_angle_scale_from_E2(prelc0)
# otherwise this lane should not be reported
# assert tas.translation[0] >= 0, tas
ds.append(tas.translation[0])
dr_lanedir = max(ds) if ds else 0.0
else:
# no lp
dr_lanedir = 0.0
driven_any.append(dr_any)
driven_lanedir.append(dr_lanedir)
timestamps.append(t0)
title = "Consecutive lane distance"
driven_lanedir_incremental = SampledSequence[float](timestamps,
driven_lanedir)
driven_lanedir_cumulative = accumulate(driven_lanedir_incremental)
if len(driven_lanedir_incremental) <= 1:
total = 0
else:
total = driven_lanedir_cumulative.values[-1]
description = textwrap.dedent("""\
This metric computes how far the robot drove **in the direction of the correct lane**,
discounting whenever it was driven in the wrong direction with respect to the start.
""")
result.set_metric(
name=("driven_lanedir_consec", ),
total=total,
incremental=driven_lanedir_incremental,
title=title,
description=description,
cumulative=driven_lanedir_cumulative,
)
def evaluate(self, context: RuleEvaluationContext,
result: RuleEvaluationResult):
interval = context.get_interval()
lane_pose_seq = context.get_lane_pose_seq()
ego_pose_sequence = context.get_ego_pose_global()
driven_any_builder = SampledSequenceBuilder[float]()
driven_lanedir_builder = SampledSequenceBuilder[float]()
for idt in iterate_with_dt(interval):
t0, t1 = idt.v0, idt.v1 # not v
try:
name2lpr = lane_pose_seq.at(t0)
p0 = ego_pose_sequence.at(t0).as_SE2()
p1 = ego_pose_sequence.at(t1).as_SE2()
except UndefinedAtTime:
dr_any = dr_lanedir = 0.0
else:
prel = relative_pose(p0, p1)
translation, _ = geo.translation_angle_from_SE2(prel)
dr_any = np.linalg.norm(translation)
if name2lpr:
ds = []
for k, lpr in name2lpr.items():
assert isinstance(lpr, GetLanePoseResult)
c0 = lpr.center_point
ctas = geo.translation_angle_scale_from_E2(
c0.asmatrix2d().m)
c0_ = geo.SE2_from_translation_angle(
ctas.translation, ctas.angle)
prelc0 = relative_pose(c0_, p1)
tas = geo.translation_angle_scale_from_E2(prelc0)
# otherwise this lane should not be reported
# assert tas.translation[0] >= 0, tas
ds.append(tas.translation[0])
dr_lanedir = max(ds)
else:
# no lp
dr_lanedir = 0.0
driven_any_builder.add(t0, dr_any)
driven_lanedir_builder.add(t0, dr_lanedir)
driven_any_incremental = driven_any_builder.as_sequence()
driven_any_cumulative = accumulate(driven_any_incremental)
if len(driven_any_incremental) <= 1:
total = 0
else:
total = driven_any_cumulative.values[-1]
title = "Distance"
description = textwrap.dedent("""\
This metric computes how far the robot drove.
""")
result.set_metric(
name=("driven_any", ),
total=total,
incremental=driven_any_incremental,
title=title,
description=description,
cumulative=driven_any_cumulative,
)
title = "Lane distance"
driven_lanedir_incremental = driven_lanedir_builder.as_sequence()
driven_lanedir_cumulative = accumulate(driven_lanedir_incremental)
if len(driven_lanedir_incremental) <= 1:
total = 0
else:
total = driven_lanedir_cumulative.values[-1]
description = textwrap.dedent("""\
This metric computes how far the robot drove
**in the direction of the lane**.
""")
result.set_metric(
name=("driven_lanedir", ),
total=total,
incremental=driven_lanedir_incremental,
title=title,
description=description,
cumulative=driven_lanedir_cumulative,
)
