Visualization

draw_trajectory_positions(renderer, reference_point, velocity, v_min, v_max, point_radius=0.5)

Draws reference_path state

Parameters:

Name	Type	Description	Default
renderer	MPRenderer	cr MPRenderer	required
reference_point	ndarray	point to draw as (2,) np.ndarry	required
velocity	float	velocity of point	required
v_min	float	v_min	required
v_max	float	v_max	required
point_radius	float	radius to display point	0.5

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def draw_trajectory_positions(
    renderer: MPRenderer,
    reference_point: np.ndarray,
    velocity: float,
    v_min: float,
    v_max: float,
    point_radius: float = 0.5,
) -> None:
    """
    Draws reference_path state
    :param renderer: cr MPRenderer
    :param reference_point: point to draw as (2,) np.ndarry
    :param velocity: velocity of point
    :param v_min: v_min
    :param v_max: v_max
    :param point_radius: radius to display point
    """
    normalized_velocity: float = (
        (velocity - v_min) / (v_max - v_min) if not np.isclose(v_max, v_min) else 0
    )
    rbg_color = cmap(normalized_velocity)
    hex_color = rgb2hex(rbg_color)
    draw_params = copy.copy(renderer.draw_params)
    draw_params.shape.facecolor = hex_color
    draw_params.shape.edgecolor = hex_color

    occ_pos = Circle(radius=point_radius, center=reference_point)
    occ_pos.draw(renderer, draw_params=draw_params)

get_velocity_min_max_from_trajectory(velocity_profile)

Gets min and max velocity from global trajectory for color coding.

Parameters:

Name	Type	Description	Default
velocity_profile	ndarray	velocity profile as (n,2) np.ndarray	required

Returns:

Type	Description
Tuple[float, float]	tuple[v_min, v_max]

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def get_velocity_min_max_from_trajectory(
    velocity_profile: np.ndarray,
) -> Tuple[float, float]:
    """
    Gets min and max velocity from global trajectory for color coding.
    :param velocity_profile: velocity profile as (n,2) np.ndarray
    :return: tuple[v_min, v_max]
    """
    min_velocity: float = np.min(velocity_profile)
    max_velocity: float = np.max(velocity_profile)
    return (min_velocity, max_velocity)

obtain_plot_limits_from_reference_path(reference_path, margin=10.0)

Obtrains plot limits from reference path

Parameters:

Name	Type	Description	Default
reference_path	ndarray	reference path (2,) np.ndarray	required

Returns:

Type	Description
List[int]	list [xmin, xmax, ymin, xmax] of plot limits

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def obtain_plot_limits_from_reference_path(
    reference_path: np.ndarray, margin: float = 10.0
) -> List[int]:
    """
    Obtrains plot limits from reference path
    :param reference_path: reference path (2,) np.ndarray
    :return: list [xmin, xmax, ymin, xmax] of plot limits
    """
    x_min = min(reference_path[:, 0])
    x_max = max(reference_path[:, 0])
    y_min = min(reference_path[:, 1])
    y_max = max(reference_path[:, 1])

    plot_limits = [x_min - margin, x_max + margin, y_min - margin, y_max + margin]

    return plot_limits

plot_param(lanelet_network, planning_problem, race_line, title, param, size_x=10)

Plots a parameter of the raceline

Parameters:

Name	Type	Description	Default
lanelet_network	LaneletNetwork	cr lanelet network	required
planning_problem	PlanningProblem	cr planning problem	required
race_line	RaceLine	cr raceline	required
title	str	title string of plot	required
param	ndarray	param as (n,) np.ndarray	required
size_x	float	size of plot	10

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def plot_param(
    lanelet_network: LaneletNetwork,
    planning_problem: PlanningProblem,
    race_line: RaceLine,
    title: str,
    param: np.ndarray,
    size_x: float = 10,
) -> None:
    """
    Plots a parameter of the raceline
    :param lanelet_network: cr lanelet network
    :param planning_problem: cr planning problem
    :param race_line: cr raceline
    :param title: title string of plot
    :param param: param as (n,) np.ndarray
    :param size_x: size of plot
    """
    _ = plt.figure(figsize=(40, 20))

    # get plot limits from reference path
    plot_limits: List[float] = obtain_plot_limits_from_reference_path(
        race_line.points, margin=20
    )
    ratio_x_y = (plot_limits[1] - plot_limits[0]) / (plot_limits[3] - plot_limits[2])

    renderer = MPRenderer(plot_limits=plot_limits, figsize=(size_x, size_x / ratio_x_y))
    renderer.draw_params.dynamic_obstacle.draw_icon = True

    lanelet_network.draw(renderer)
    planning_problem.draw(renderer)

    param_min = np.min(param)
    param_max = np.max(param)

    for idx in range(race_line.points.shape[0]):
        draw_trajectory_positions(
            renderer,
            reference_point=race_line.points[idx],
            velocity=param[idx],
            v_min=param_min,
            v_max=param_max,
        )

    # draw scenario and renderer
    renderer.render()

    # plot legend before divider so they appear nice
    plt.xlabel("East [m]")
    plt.ylabel("North [m]")

    # colorbar work-around
    divider = make_axes_locatable(plt.gca())
    cax = divider.append_axes("right", size="5%", pad=0.05)
    norm = Normalize(vmin=param_min, vmax=param_max)
    sm = plt.cm.ScalarMappable(cmap=cmap, norm=norm)
    sm.set_array([])
    plt.colorbar(sm, cax=cax, cmap="plasma", orientation="vertical")
    plt.title(title)
    plt.show()

plot_trajectory_with_acceleration(lanelet_network, planning_problem, race_line, size_x=10)

Plots trajectory with acceleration

Parameters:

Name	Type	Description	Default
lanelet_network	LaneletNetwork	cr lanelet network	required
planning_problem	PlanningProblem	cr planning problem	required
race_line	RaceLine	cr raceline	required
size_x	float	size of plot	10

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def plot_trajectory_with_acceleration(
    lanelet_network: LaneletNetwork,
    planning_problem: PlanningProblem,
    race_line: RaceLine,
    size_x: float = 10,
) -> None:
    """
    Plots trajectory with acceleration
    :param lanelet_network: cr lanelet network
    :param planning_problem: cr planning problem
    :param race_line: cr raceline
    :param size_x: size of plot
    """
    plot_param(
        lanelet_network=lanelet_network,
        planning_problem=planning_problem,
        race_line=race_line,
        param=race_line.acceleration_long_per_point,
        size_x=size_x,
        title="Acceleration [m/s^2]",
    )

plot_trajectory_with_all_quantities(lanelet_network, planning_problem, race_line, size_x=10)

Plot all quantities Plots a parameter of the raceline

Parameters:

Name	Type	Description	Default
lanelet_network	LaneletNetwork	cr lanelet network	required
planning_problem	PlanningProblem	cr planning problem	required
race_line	RaceLine	cr raceline	required
size_x	float	size of plot	10

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def plot_trajectory_with_all_quantities(
    lanelet_network: LaneletNetwork,
    planning_problem: PlanningProblem,
    race_line: RaceLine,
    size_x: float = 10,
) -> None:
    """
    Plot all quantities
    Plots a parameter of the raceline
    :param lanelet_network: cr lanelet network
    :param planning_problem: cr planning problem
    :param race_line: cr raceline
    :param size_x: size of plot
    """
    plot_trajectory_with_velocity(
        lanelet_network=lanelet_network,
        planning_problem=planning_problem,
        race_line=race_line,
        size_x=size_x,
    )
    plot_trajectory_with_acceleration(
        lanelet_network=lanelet_network,
        planning_problem=planning_problem,
        race_line=race_line,
        size_x=size_x,
    )
    plot_trajectory_with_curvature(
        lanelet_network=lanelet_network,
        planning_problem=planning_problem,
        race_line=race_line,
        size_x=size_x,
    )
    plot_trajectory_with_heading(
        lanelet_network=lanelet_network,
        planning_problem=planning_problem,
        race_line=race_line,
        size_x=size_x,
    )

plot_trajectory_with_curvature(lanelet_network, planning_problem, race_line, size_x=10)

Plots trajectory with curvature

Parameters:

Name	Type	Description	Default
lanelet_network	LaneletNetwork	cr lanelet network	required
planning_problem	PlanningProblem	cr planning problem	required
race_line	RaceLine	cr raceline	required
size_x	float	size of plot	10

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def plot_trajectory_with_curvature(
    lanelet_network: LaneletNetwork,
    planning_problem: PlanningProblem,
    race_line: RaceLine,
    size_x: float = 10,
) -> None:
    """
    Plots trajectory with curvature
    :param lanelet_network: cr lanelet network
    :param planning_problem: cr planning problem
    :param race_line: cr raceline
    :param size_x: size of plot
    """
    plot_param(
        lanelet_network=lanelet_network,
        planning_problem=planning_problem,
        race_line=race_line,
        param=race_line.curvature_per_point,
        size_x=size_x,
        title="Curvature [1/m]",
    )

plot_trajectory_with_heading(lanelet_network, planning_problem, race_line, size_x=10)

Plots trajectory with heading

Parameters:

Name	Type	Description	Default
lanelet_network	LaneletNetwork	cr lanelet network	required
planning_problem	PlanningProblem	cr planning problem	required
race_line	RaceLine	cr raceline	required
size_x	float	size of plot	10

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def plot_trajectory_with_heading(
    lanelet_network: LaneletNetwork,
    planning_problem: PlanningProblem,
    race_line: RaceLine,
    size_x: float = 10,
) -> None:
    """
    Plots trajectory with heading
    :param lanelet_network: cr lanelet network
    :param planning_problem: cr planning problem
    :param race_line: cr raceline
    :param size_x: size of plot
    """
    plot_param(
        lanelet_network=lanelet_network,
        planning_problem=planning_problem,
        race_line=race_line,
        param=race_line.heading_per_point,
        size_x=size_x,
        title="Heading [rad]",
    )

plot_trajectory_with_velocity(lanelet_network, planning_problem, race_line, size_x=10)

Plots trajectory with velocity

Parameters:

Name	Type	Description	Default
lanelet_network	LaneletNetwork	cr lanelet network	required
planning_problem	PlanningProblem	cr planning problem	required
race_line	RaceLine	cr raceline	required
size_x	float	size of plot	10

Source code in commonroad_raceline_planner/utils/visualization/visualize_on_racetrack.py

def plot_trajectory_with_velocity(
    lanelet_network: LaneletNetwork,
    planning_problem: PlanningProblem,
    race_line: RaceLine,
    size_x: float = 10,
) -> None:
    """
    Plots trajectory with velocity
    :param lanelet_network: cr lanelet network
    :param planning_problem: cr planning problem
    :param race_line: cr raceline
    :param size_x: size of plot
    """
    plot_param(
        lanelet_network=lanelet_network,
        planning_problem=planning_problem,
        race_line=race_line,
        param=race_line.velocity_long_per_point,
        size_x=size_x,
        title="Velocity [m/s]",
    )

plot_laptime(race_line, vel_min_threshold=0.2)

plot lap time

Parameters:

Name	Type	Description	Default
race_line	RaceLine	cr race line	required

Source code in commonroad_raceline_planner/utils/visualization/visualize_over_arclength.py

def plot_laptime(race_line: RaceLine, vel_min_threshold: float = 0.2) -> None:
    """
    plot lap time
    :param race_line: cr race line
    """
    lap_time_per_point: List[float] = []
    sum_t = 0
    for idx in range(race_line.length_per_point.shape[0] - 1):
        # t = s/v
        s = race_line.length_per_point[idx + 1] - race_line.length_per_point[idx]
        v = race_line.velocity_long_per_point[idx]
        t = s / v if v > vel_min_threshold else 0.1
        sum_t += t
        lap_time_per_point.append(sum_t + t)

    plt.title("laptime over arclength")
    plt.plot(race_line.length_per_point[:-1], lap_time_per_point)
    plt.xlabel("Arc Length [m]")
    plt.ylabel("Lap Time (accumulated) [s]")
    plt.show()

plot_trajectory_over_arclength(race_line)

plot raceline over arclength

Parameters:

Name	Type	Description	Default
race_line	RaceLine	cr raceline	required

Source code in commonroad_raceline_planner/utils/visualization/visualize_over_arclength.py

def plot_trajectory_over_arclength(race_line: RaceLine) -> None:
    """
    plot raceline over arclength
    :param race_line: cr raceline
    """
    plt.title("velocity over arclength")
    plt.plot(race_line.length_per_point, race_line.velocity_long_per_point)
    plt.xlabel("Arc Length [m]")
    plt.ylabel("Velocity [m/s]")
    plt.show()

    plt.title("acceleration over arclength")
    plt.plot(race_line.length_per_point, race_line.acceleration_long_per_point)
    plt.xlabel("Arc Length [m]")
    plt.ylabel("Acceleration [m/s^2]")
    plt.show()

    plt.title("heading over arclength")
    plt.plot(race_line.length_per_point, race_line.heading_per_point)
    plt.xlabel("Arc Length [m]")
    plt.ylabel("Heading [rad]")
    plt.show()

    plt.title("curvature over arclength")
    plt.plot(race_line.length_per_point, race_line.curvature_per_point)
    plt.xlabel("Arc Length [m]")
    plt.ylabel("Curvature [1/m]")
    plt.show()

    plot_laptime(race_line=race_line)