Function plot_roadgraph

limsim/utils/plotting.py:21–97

(roadgraph: RoadGraph)

def plot_roadgraph(roadgraph: RoadGraph) -> None:
    edges = roadgraph.edges
    lanes = roadgraph.lanes
    fig, ax = plt.subplots()
    for edge in edges.values():
        for lane_index in range(edge.lane_num):
            lane_id = edge.id + '_' + str(lane_index)
            lane = lanes[lane_id]

            lane.center_line, lane.left_bound, lane.right_bound = [], [], []
            s = np.linspace(0, lane.course_spline.s[-1], num=50)
            for si in s:
                lane.center_line.append(lane.course_spline.calc_position(si))
                lane.left_bound.append(
                    lane.course_spline.frenet_to_cartesian1D(
                        si, lane.width / 2)
                )
                lane.right_bound.append(
                    lane.course_spline.frenet_to_cartesian1D(
                        si, -lane.width / 2)
                )
            ax.plot(*zip(*lane.center_line), "w:", linewidth=1.5)
            plt.arrow(
                lane.center_line[0][0],
                lane.center_line[0][1],
                lane.center_line[2][0] - lane.center_line[0][0],
                lane.center_line[2][1] - lane.center_line[0][1],
                shape='full',
                width=0.3,
                length_includes_head=False,
                zorder=2,
                color="w",
            )
            if lane_index == edge.lane_num - 1:
                ax.plot(*zip(*lane.left_bound), "k", linewidth=1.5)
            else:
                ax.plot(*zip(*lane.left_bound), "k--", linewidth=1)
            if lane_index == 0:
                ax.plot(*zip(*lane.right_bound), "k", linewidth=1.5)

            if edge.from_junction == None:
                ax.plot(
                    [lane.left_bound[0][0], lane.right_bound[0][0]],
                    [lane.left_bound[0][1], lane.right_bound[0][1]],
                    "r",
                    linewidth=2,
                )
            if edge.to_junction == None:
                ax.plot(
                    [lane.left_bound[-1][0], lane.right_bound[-1][0]],
                    [lane.left_bound[-1][1], lane.right_bound[-1][1]],
                    "r",
                    linewidth=2,
                )

    for lane in lanes.values():
        for junctionlane, dir in lane.next_lanes.values():
            s = np.linspace(0, junctionlane.course_spline.s[-1], num=50)