コード例 #1
0
ファイル: debug_topo.py プロジェクト: zk20/apollo
def plot_central_curve_with_s_range(central_curve, start_s, end_s, color):
    """plot topology graph node with given start and end s, return middle point"""
    node_x = []
    node_y = []
    for curve in central_curve.segment:
        px, py = proto_utils.flatten(curve.line_segment.point, ['x', 'y'])
        node_x.extend(px)
        node_y.extend(py)
    start_plot_index = 0
    end_plot_index = len(node_x)
    node_s = calculate_s(node_x, node_y)
    for i in range(len(node_s)):
        if node_s[i] >= start_s:
            start_plot_index = i
            break
    for i in range(len(node_s) - 1, -1, -1):
        if node_s[i] <= end_s:
            end_plot_index = i + 1
            break
    plt.gca().plot(node_x[start_plot_index:end_plot_index],
                   node_y[start_plot_index:end_plot_index],
                   color=color,
                   lw=3,
                   alpha=0.8)
    mid_index = (start_plot_index + end_plot_index) // 2
    return [node_x[mid_index], node_y[mid_index]]
コード例 #2
0
def draw_boundary(line_segment):
    """
    :param line_segment:
    :return:
    """
    px, py = proto_utils.flatten(line_segment.point, ['x', 'y'])
    px, py = downsample_array(px), downsample_array(py)
    plt.gca().plot(px, py, 'k')
コード例 #3
0
def draw_line(line_segment, color):
    """
    :param line_segment:
    :return: none
    """
    px, py = proto_utils.flatten(line_segment.point, ['x', 'y'])
    px, py = downsample_array(px), downsample_array(py)
    plt.gca().plot(px, py, lw=10, alpha=0.8, color=color)
    return px[len(px) // 2], py[len(py) // 2]
コード例 #4
0
    def callback_planning(self, data):
        """New Planning Trajectory"""
        if self.stgraph:
            st_s, st_t, polygons_s, polygons_t = proto_utils.flatten(
                data.debug.planning_data.st_graph,
                ['speed_profile.s',
                 'speed_profile.t',
                 'boundary.point.s',
                 'boundary.point.t'])

            with self.lock:
                for i in range(len(st_s)):
                    self.ax[i].new_planning(st_t[i], st_s[i],
                                            polygons_t[i], polygons_s[i])

        else:
            if len(data.trajectory_point) == 0:
                print(data)
                return

            x, y, speed, theta, kappa, acc, relative_time = np.array(
                proto_utils.flatten(data.trajectory_point,
                                    ['path_point.x',
                                     'path_point.y',
                                     'v',
                                     'path_point.theta',
                                     'path_point.kappa',
                                     'a',
                                     'relative_time']))
            relative_time += data.header.timestamp_sec

            with self.lock:
                self.ax[0].new_planning(relative_time, x, y)
                self.ax[1].new_planning(relative_time, speed)

                if self.ax[2].title == "Curvature":
                    self.ax[2].new_planning(relative_time, kappa)

                if self.ax[3].title == "Heading":
                    self.ax[3].new_planning(relative_time, theta)
                else:
                    self.ax[3].new_planning(relative_time, acc)
コード例 #5
0
def draw_map(drivemap):
    """ draw map from mapfile"""
    print('Map info:')
    print('\tVersion:\t', end=' ')
    print(drivemap.header.version)
    print('\tDate:\t', end=' ')
    print(drivemap.header.date)
    print('\tDistrict:\t', end=' ')
    print(drivemap.header.district)

    road_lane_set = []
    for road in drivemap.road:
        lanes = []
        for sec in road.section:
            lanes.extend(proto_utils.flatten(sec.lane_id, 'id'))
        road_lane_set.append(lanes)

    for lane in drivemap.lane:
        for curve in lane.central_curve.segment:
            if curve.HasField('line_segment'):
                road_idx = get_road_index_of_lane(lane.id.id, road_lane_set)
                if road_idx == -1:
                    print('Failed to get road index of lane')
                    sys.exit(-1)
                center_x, center_y = draw_line(curve.line_segment,
                                               g_color[road_idx % len(g_color)])
                draw_id(center_x, center_y, str(road_idx))
                # break
            # if curve.HasField('arc'):
            #    draw_arc(curve.arc)

        for curve in lane.left_boundary.curve.segment:
            if curve.HasField('line_segment'):
                draw_boundary(curve.line_segment)

        for curve in lane.right_boundary.curve.segment:
            if curve.HasField('line_segment'):
                draw_boundary(curve.line_segment)
                # break

    return drivemap
コード例 #6
0
ファイル: debug_topo.py プロジェクト: zk20/apollo
def draw_line(line, color):
    """draw line, return x array and y array"""
    px, py = proto_utils.flatten(line.point, ['x', 'y'])
    px, py = util.downsample_array(px), util.downsample_array(py)
    plt.gca().plot(px, py, color=color, lw=3, alpha=0.8)
    return px, py