Skip to content

Visualization

visualize_physics(idm_trajectory, idm_config, delta_t, threshold_plot_v_max=3.0, threshold_plot_acc_max=0.5, threshold_plot_decel_max=0.5, save_img=False, save_path=None)

Visualizes physical properties of planner, such as velocity, acceleration and jerk

Parameters:

Name Type Description Default
idm_trajectory IDMTrajectory

idm trajectory

 required
idm_config IDMConfig

idm config

 required
delta_t float

time-step size

 required
threshold_plot_v_max float

v_max threshold

 3.0
threshold_plot_acc_max float

accel threshold

 0.5
threshold_plot_decel_max float

decel threshold

 0.5
save_img bool

if True, image is not displayed but saved to save_path

 False
save_path Union[str, Path]

idm_path to save images to

 None
Source code in commonroad_idm_planner/util/visualization/visualize_physics.py 
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
def visualize_physics(
    idm_trajectory: IDMTrajectory,
    idm_config: IDMConfig,
    delta_t: float,
    threshold_plot_v_max: float = 3.0,
    threshold_plot_acc_max: float = 0.5,
    threshold_plot_decel_max: float = 0.5,
    save_img: bool = False,
    save_path: Union[str, Path] = None,
) -> None:
    """
    Visualizes physical properties of planner, such as velocity, acceleration and jerk
    :param idm_trajectory: idm trajectory
    :param idm_config: idm config
    :param delta_t: time-step size
    :param threshold_plot_v_max: v_max threshold
    :param threshold_plot_acc_max: accel threshold
    :param threshold_plot_decel_max: decel threshold
    :param save_img: if True, image is not displayed but saved to save_path
    :param save_path: idm_path to save images to
    """

    visualize_velocity_over_time(
        idm_trajectory=idm_trajectory,
        idm_config=idm_config,
        threshold_plot_v_max=threshold_plot_v_max,
        save_img=save_img,
        save_path=save_path,
    )

    visualize_acceleration_over_time(
        idm_trajectory=idm_trajectory,
        idm_config=idm_config,
        threshold_plot_accel_max=threshold_plot_acc_max,
        threshold_plot_decel_max=threshold_plot_decel_max,
        save_img=save_img,
        save_path=save_path,
    )

    visualize_jerk_over_time(
        idm_trajectory=idm_trajectory,
        save_img=save_img,
        save_path=save_path,
        delta_t=delta_t,
    )

obtain_plot_limits_from_reference_path(reference_path, margin=10.0)

Obtrains plot limits from reference idm_path

Parameters:

Name Type Description Default
reference_path ndarray

reference idm_path (2,) np.ndarray

 required

Returns:

Type Description
List[int]

list [xmin, xmax, ymin, xmax] of plot limits
Source code in commonroad_idm_planner/util/visualization/visualize_scenario.py 
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
def obtain_plot_limits_from_reference_path(
    reference_path: np.ndarray, margin: float = 10.0
) -> List[int]:
    """
    Obtrains plot limits from reference idm_path
    :param reference_path: reference idm_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_idm_trajectory(scenario, planning_problem, idm_path, idm_trajectory, idm_config, size_x=10.0, save_img=False, save_path=None)

Visualizes IDM Trajectory

Parameters:

Name Type Description Default
scenario Scenario

cr scenario

 required
planning_problem PlanningProblem

cr planning problem

 required
idm_path IDMPath

cr idm path

 required
idm_trajectory IDMTrajectory

idm trajectory

 required
idm_config IDMConfig

idm config

 required
size_x float

size of fig

 10.0
save_img bool

if True, image is not displayed but saved

 False
save_path Union[str, Path]

idm_path image is saved to if save_img

 None
Source code in commonroad_idm_planner/util/visualization/visualize_scenario.py 
 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
105
106
107
108
109
110
def visualize_idm_trajectory(
    scenario: Scenario,
    planning_problem: PlanningProblem,
    idm_path: IDMPath,
    idm_trajectory: IDMTrajectory,
    idm_config: IDMConfig,
    size_x: float = 10.0,
    save_img: bool = False,
    save_path: Union[str, Path] = None,
) -> None:
    """
    Visualizes IDM Trajectory
    :param scenario: cr scenario
    :param planning_problem: cr planning problem
    :param idm_path: cr idm path
    :param idm_trajectory: idm trajectory
    :param idm_config: idm config
    :param size_x: size of fig
    :param save_img: if True, image is not displayed but saved
    :param save_path: idm_path image is saved to if save_img
    """
    for state in idm_trajectory.state_list:
        plt.cla()

        # get plot limits from reference idm_path
        plot_limits: List[float] = obtain_plot_limits_from_reference_path(
            idm_path.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
        renderer.draw_params.dynamic_obstacle.show_label = True
        renderer.draw_params.time_begin = state.time_step

        scenario.draw(renderer)

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

        for idx in range(idm_path.reference_path.shape[0]):
            draw_route_state(
                renderer,
                reference_point=idm_path.reference_path[idx],
            )

        ego_vehicle = idm_trajectory.to_cr_dynamic_obstacle(
            vehicle_width=idm_config.vehicle_width,
            vehicle_length=idm_config.vehicle_length,
            vehicle_id=30000,
        )

        draw_params = copy.copy(renderer.draw_params)
        draw_params.dynamic_obstacle.draw_icon = True
        draw_params.dynamic_obstacle.trajectory.draw_trajectory = False
        draw_params.dynamic_obstacle.show_label = False
        draw_params.planning_problem.initial_state.state.draw_arrow = False
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.facecolor = "#E37222"
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.edgecolor = "#9C4100"
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.zorder = 50
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.opacity = 1
        draw_params.time_begin = state.time_step

        ego_vehicle.draw(renderer, draw_params=draw_params)

        # draw scenario and renderer
        renderer.render()
        plt.title(f"Time step = {state.time_step}")

        if save_img:
            save_file: str = os.path.join(
                save_path,
                str(scenario.scenario_id) + "_" + str(state.time_step) + ".png",
            )
            os.makedirs(save_path, exist_ok=True)  # Ensure the directory exists
            plt.savefig(save_file, format="png")
        else:
            plt.show()

visualize_acceleration_input_over_time(input_list, acc_max=0.5, decel_max=0.5, save_img=False, save_path=None)

Visualizes acceleration input of planner.

Parameters:

Name Type Description Default
input_list List[IDMInput]

list of idm inputs

 required
acc_max float

accel threshold

 0.5
decel_max float

decel threshold

 0.5
save_img bool

image is not displayed but saved to save_path

 False
save_path Union[str, Path]

idm_path to save images to

 None
Source code in commonroad_idm_planner/util/visualization/visualize_inputs.py 
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
def visualize_acceleration_input_over_time(
    input_list: List[IDMInput],
    acc_max: float = 0.5,
    decel_max: float = 0.5,
    save_img: bool = False,
    save_path: Union[str, Path] = None,
) -> None:
    """
    Visualizes acceleration input of planner.
    :param input_list: list of idm inputs
    :param acc_max: accel threshold
    :param decel_max: decel threshold
    :param save_img: image is not displayed but saved to save_path
    :param save_path: idm_path to save images to
    """

    time_steps: List[int] = [idm_input.time_step for idm_input in input_list]
    accelerations: List[float] = [idm_input.acceleration for idm_input in input_list]

    acc_max: np.ndarray = np.ones_like(time_steps) * acc_max
    dec_max: np.ndarray = -np.ones_like(time_steps) * decel_max

    plt.cla()
    plt.plot(time_steps, accelerations, "blue", label="Accel.")
    plt.plot(time_steps, acc_max, "red", label="Accel. max")
    plt.plot(time_steps, dec_max, "red", label="Decel. max")
    plt.xlabel("Time steps")

    plt.ylabel("Input acceleration [m/s^2]")
    plt.title("Reconstructed input acceleration")
    plt.legend()

    if save_img:
        save_dir: str = os.path.join(save_path)
        os.makedirs(save_dir, exist_ok=True)
        save_filename: str = os.path.join(save_dir, "input_accel.png")
        plt.savefig(save_filename, format="png")
    else:
        plt.show()

visualize_inputs(input_list, max_steering_angle_vel=0.5, acc_max=0.5, decel_max=0.5, save_img=False, save_path=None)

Visualizes inputs of planner, such as acceleration and steering angle velocity

Parameters:

Name Type Description Default
input_list Optional[List[IDMInput]]

list of idm inputs

 required
max_steering_angle_vel float

max steering angle velocity

 0.5
acc_max float

accel threshold

 0.5
decel_max float

decel threshold

 0.5
save_img bool

if True, image is not displayed but saved to save_path

 False
save_path Union[str, Path]

idm_path to save images to

 None
Source code in commonroad_idm_planner/util/visualization/visualize_inputs.py 
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
def visualize_inputs(
    input_list: Optional[List[IDMInput]],
    max_steering_angle_vel: float = 0.5,
    acc_max: float = 0.5,
    decel_max: float = 0.5,
    save_img: bool = False,
    save_path: Union[str, Path] = None,
) -> None:
    """
    Visualizes inputs of planner, such as acceleration and steering angle velocity
    :param input_list: list of idm inputs
    :param max_steering_angle_vel: max steering angle velocity
    :param acc_max: accel threshold
    :param decel_max: decel threshold
    :param save_img: if True, image is not displayed but saved to save_path
    :param save_path: idm_path to save images to
    """
    visualize_acceleration_input_over_time(
        input_list=input_list,
        acc_max=acc_max,
        decel_max=decel_max,
        save_img=save_img,
        save_path=save_path,
    )
    visualize_steering_angle_velocity_input_over_time(
        input_list=input_list,
        max_steering_angle_vel=max_steering_angle_vel,
        save_img=save_img,
        save_path=save_path,
    )

visualize_steering_angle_velocity_input_over_time(input_list, max_steering_angle_vel=0.5, save_img=False, save_path=None)

Visualizes steering angle velocity input of planner.

Parameters:

Name Type Description Default
input_list List[IDMInput]

list of idm inputs

 required
max_steering_angle_vel float

max steering angle velocity

 0.5
save_img bool

image is not displayed but saved to save_path

 False
save_path Union[str, Path]

idm_path to save images to

 None
Source code in commonroad_idm_planner/util/visualization/visualize_inputs.py 
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
def visualize_steering_angle_velocity_input_over_time(
    input_list: List[IDMInput],
    max_steering_angle_vel: float = 0.5,
    save_img: bool = False,
    save_path: Union[str, Path] = None,
) -> None:
    """
    Visualizes steering angle velocity input of planner.
    :param input_list: list of idm inputs
    :param max_steering_angle_vel: max steering angle velocity
    :param save_img: image is not displayed but saved to save_path
    :param save_path: idm_path to save images to
    """
    time_steps: List[int] = [idm_input.time_step for idm_input in input_list]
    steering_angle_velocities: List[float] = [
        idm_input.steering_angle_velocity for idm_input in input_list
    ]

    acc_max: np.ndarray = np.ones_like(time_steps) * max_steering_angle_vel
    dec_max: np.ndarray = -np.ones_like(time_steps) * max_steering_angle_vel

    plt.cla()
    plt.plot(time_steps, steering_angle_velocities, "blue", label="delta dot")
    plt.plot(time_steps, acc_max, "red", label="delta dot max")
    plt.plot(time_steps, dec_max, "red", label="delta dot  min")
    plt.xlabel("Time steps")

    plt.ylabel("delta dot [rad/s]")
    plt.title("Reconstructed input steering angle velocity")
    plt.legend()

    if save_img:
        save_dir: str = os.path.join(save_path)
        os.makedirs(save_dir, exist_ok=True)
        save_filename: str = os.path.join(save_dir, "steering_angle_vel.png")
        plt.savefig(save_filename, format="png")
    else:
        plt.show()