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, )