Skip to content

Visualization

draw_route_state(renderer, reference_point, velocity, v_min, v_max, point_radius=0.1)

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.1
Source code in commonroad_velocity_planner/utils/visualization/visualize_velocity_planner.py 
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
def draw_route_state(
    renderer: MPRenderer,
    reference_point: np.ndarray,
    velocity: float,
    v_min: float,
    v_max: float,
    point_radius: float = 0.1,
) -> 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_velocity_planner/utils/visualization/visualize_velocity_planner.py 
138
139
140
141
142
143
144
145
146
147
148
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_velocity_planner/utils/visualization/visualize_velocity_planner.py 
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
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

visualize_global_trajectory(scenario, velocity_planning_problem, global_trajectory, save_path, size_x=10.0, save_img=False, saving_format='png', test=False)

Visualizes global trajectory in scenario

Parameters:

Name Type Description Default
scenario Scenario

cr scenario

 required
velocity_planning_problem VelocityPlanningProblem

velocity planning problem object

 required
global_trajectory GlobalTrajectory

global trajectory object

 required
save_path str

path to save image to

 required
size_x float

fig size

 10.0
save_img bool

if true, saves image, otherwise displays it

 False
saving_format str

saving format

 'png'
test bool

if true, neither displays nor saves iamge

 False
Source code in commonroad_velocity_planner/utils/visualization/visualize_velocity_planner.py 
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
def visualize_global_trajectory(
    scenario: Scenario,
    velocity_planning_problem: VelocityPlanningProblem,
    global_trajectory: GlobalTrajectory,
    save_path: str,
    size_x: float = 10.0,
    save_img: bool = False,
    saving_format: str = "png",
    test: bool = False,
) -> None:
    """
    Visualizes global trajectory in scenario
    :param scenario: cr scenario
    :param velocity_planning_problem: velocity planning problem object
    :param global_trajectory: global trajectory object
    :param save_path: path to save image to
    :param size_x: fig size
    :param save_img: if true, saves image, otherwise displays it
    :param saving_format: saving format
    :param test: if true, neither displays nor saves iamge
    """
    plt.cla()
    _ = plt.figure(figsize=(20, 10))

    # get plot limits from reference path
    plot_limits: List[float] = obtain_plot_limits_from_reference_path(
        global_trajectory.reference_path, 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

    scenario.draw(renderer)

    scenario.lanelet_network.draw(renderer)
    velocity_planning_problem.planning_problem.draw(renderer)

    v_min, v_max = get_velocity_min_max_from_trajectory(
        global_trajectory.velocity_profile
    )
    for idx in range(global_trajectory.reference_path.shape[0]):
        draw_route_state(
            renderer,
            reference_point=global_trajectory.reference_path[idx],
            velocity=global_trajectory.velocity_profile[idx],
            v_min=v_min,
            v_max=v_max,
        )

    # draw scenario and renderer
    renderer.render()
    # plt.axis('off')
    v_start = global_trajectory.velocity_profile[
        global_trajectory.planning_problem_start_idx
    ]
    v_goal = global_trajectory.velocity_profile[
        global_trajectory.planning_problem_goal_idx
    ]
    plt.title(
        f"v_min={round(v_min, 2)}  --  v_max={round(v_max, 2)} | v_start={round(v_start, 2)}  --  v_goal={round(v_goal, 2)}"
    )

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

    # save or show scenario
    if not test:
        if save_img:
            plt.savefig(save_path, format=saving_format, dpi=300, bbox_inches="tight")
            print(f"saving image to {save_path}")
        else:
            plt.show()

visualize_acceleration_over_arclength(path_length_per_point, acceleration_profile, a_min, a_max, save_path, save_img=False, saving_format='png', test=False)

Visualizes acceleration over arc length

Parameters:

Name Type Description Default
path_length_per_point ndarray

arc length per point

 required
acceleration_profile ndarray

acceleration profile

 required
a_min float

minimum acceleration threshold

 required
a_max float

maximum acceleration threshold

 required
save_path str

path to save image to

 required
save_img bool

if true, saves image, otherwise displays it

 False
saving_format str

saving format

 'png'
test bool

if true, neither saves nor displays figures

 False
Source code in commonroad_velocity_planner/utils/visualization/visualize_quantities.py 
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
def visualize_acceleration_over_arclength(
    path_length_per_point: np.ndarray,
    acceleration_profile: np.ndarray,
    a_min: float,
    a_max: float,
    save_path: str,
    save_img: bool = False,
    saving_format: str = "png",
    test: bool = False,
) -> None:
    """
    Visualizes acceleration over arc length
    :param path_length_per_point: arc length per point
    :param acceleration_profile: acceleration profile
    :param a_min: minimum acceleration threshold
    :param a_max: maximum acceleration threshold
    :param save_path: path to save image to
    :param save_img: if true, saves image, otherwise displays it
    :param saving_format: saving format
    :param test: if true, neither saves nor displays figures
    """
    plt.figure()

    plt.title("Acceleration over arc length")

    plt.plot(path_length_per_point, np.ones_like(acceleration_profile) * a_min, "black")
    plt.plot(path_length_per_point, np.ones_like(acceleration_profile) * a_max, "black")

    plt.plot(path_length_per_point, acceleration_profile)

    # save or show scenario
    if not test:
        if save_img:
            plt.savefig(save_path, format=saving_format, dpi=300, bbox_inches="tight")
        else:
            plt.show()

visualize_velocity_over_arclength(path_length_per_point, velocity_profile, v_min, v_max, save_path, save_img=False, saving_format='png', test=False)

Visualizes velocity over arc length

Parameters:

Name Type Description Default
path_length_per_point ndarray

arc length per point

 required
velocity_profile ndarray

velocity profile

 required
v_min float

minimum velocity threshold

 required
v_max float

maximum velocity threshold

 required
save_path str

path to same image to

 required
save_img bool

if true, saves image, otherwise displays it

 False
saving_format str

saving format

 'png'
test bool

if test, neither displays nor saves images

 False
Source code in commonroad_velocity_planner/utils/visualization/visualize_quantities.py 
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
def visualize_velocity_over_arclength(
    path_length_per_point: np.ndarray,
    velocity_profile: np.ndarray,
    v_min: float,
    v_max: float,
    save_path: str,
    save_img: bool = False,
    saving_format: str = "png",
    test: bool = False,
) -> None:
    """
    Visualizes velocity over arc length
    :param path_length_per_point: arc length per point
    :param velocity_profile: velocity profile
    :param v_min: minimum velocity threshold
    :param v_max: maximum velocity threshold
    :param save_path: path to same image to
    :param save_img: if true, saves image, otherwise displays it
    :param saving_format: saving format
    :param test: if test, neither displays nor saves images
    """
    plt.cla()
    plt.clf()

    plt.title("Velocity over arc length")

    plt.plot(path_length_per_point, np.ones_like(velocity_profile) * v_min, "black")
    plt.plot(path_length_per_point, np.ones_like(velocity_profile) * v_max, "black")
    plt.plot(path_length_per_point, velocity_profile)

    # save or show scenario
    if not test:
        if save_img:
            plt.savefig(save_path, format=saving_format, dpi=300, bbox_inches="tight")
        else:
            plt.show()

visualize_acceleration_over_arclength(path_length_per_point, velocity_profile, save_path, save_img=False, saving_format='png', test=False)

Visualization for the velocity

Parameters:

Name Type Description Default
path_length_per_point ndarray

arc length per point

 required
velocity_profile SplineProfile

spline velocity provile

 required
save_path str

save image to

 required
save_img bool

if true, saves image, otherwise displays it

 False
saving_format str

saving format

 'png'
test bool

if test, image is neither shown nor displayed

 False
Source code in commonroad_velocity_planner/utils/visualization/visualize_acceleration.py 
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
def visualize_acceleration_over_arclength(
    path_length_per_point: np.ndarray,
    velocity_profile: SplineProfile,
    save_path: str,
    save_img: bool = False,
    saving_format: str = "png",
    test: bool = False,
) -> None:
    """
    Visualization for the velocity
    :param path_length_per_point: arc length per point
    :param velocity_profile: spline velocity provile
    :param save_path: save image to
    :param save_img: if true, saves image, otherwise displays it
    :param saving_format: saving format
    :param test: if test, image is neither shown nor displayed
    """
    plt.figure()

    x: List[float] = []
    y: List[float] = []
    v_last = velocity_profile.interpolate_velocity_at_arc_lenth(
        np.asarray([path_length_per_point[0]])
    )
    s_last = path_length_per_point[0]
    for idx in range(1, path_length_per_point.shape[0]):

        v_current = velocity_profile.interpolate_velocity_at_arc_lenth(
            np.asarray([path_length_per_point[idx]])
        )
        s_current = path_length_per_point[idx]

        delta_t = abs((s_current - s_last) / ((v_current - v_last) / 2 + v_last))

        a = (v_current - v_last) / delta_t

        v_last = v_current
        s_last = s_current

        x.append(path_length_per_point[idx])
        y.append(a)

    plt.plot(x, y, "r")
    plt.title("Acceleration over arc length")

    # save or show scenario
    if not test:
        if save_img:
            plt.savefig(save_path, format=saving_format, dpi=300, bbox_inches="tight")
        else:
            plt.show()