Skip to content

visualization

commonroad_control.util.visualization.visualize_trajectories

make_gif(path_to_img_dir, scenario_name, num_imgs, duration=0.1, abort_img_threshold=100)

Generates a .gif from a folder of .png images. Assumes images sorted by name.

Parameters:

Name Type Description Default
path_to_img_dir Union[Path, str]

Path to the image folder

 required
scenario_name str

Name of the scenario for the .gif

 required
num_imgs int

how many images should the folder contain

 required
duration float

time duration between two consecutive images

 0.1
abort_img_threshold int

Safety threshold of number of images in folder, above which execution is aborted

 100
Source code in commonroad_control/util/visualization/visualize_trajectories.py 
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
def make_gif(
    path_to_img_dir: Union[Path, str],
    scenario_name: str,
    num_imgs: int,
    duration: float = 0.1,
    abort_img_threshold: int = 100,
) -> None:
    """
    Generates a .gif from a folder of .png images. Assumes images sorted by name.
    :param path_to_img_dir: Path to the image folder
    :param scenario_name: Name of the scenario for the .gif
    :param num_imgs: how many images should the folder contain
    :param duration: time duration between two consecutive images
    :param abort_img_threshold: Safety threshold of number of images in folder, above which execution is aborted
    """

    if not os.path.exists(path_to_img_dir) or not os.path.isdir(path_to_img_dir) or not os.path.isabs(path_to_img_dir):
        logger.error(f"image dir {path_to_img_dir} must exist, be a directory and be absolute")
        raise FileNotFoundError(f"image dir {path_to_img_dir} must exist, be a directory and be absolute")

    # get all files in dir
    imgs = sorted(
        [el for el in os.listdir(path_to_img_dir) if ".png" in el],
        key=lambda x: int(x.split(".")[0].split("_")[-1]),
    )

    logger.info("creating gif")

    # poll until all imgs ware saved
    cnt = 0
    while len(imgs) != num_imgs and cnt < 50:
        imgs = sorted(
            [el for el in os.listdir(path_to_img_dir) if ".png" in el],
            key=lambda x: int(x.split(".")[0].split("_")[-1]),
        )
        time.sleep(0.2)
        cnt += 1

    if cnt == abort_img_threshold:
        logger.error("Could not find all expected imgs")
        raise ValueError("Could not find all expected imgs")

    imgs_pil = [Image.open(os.path.join(path_to_img_dir, img)) for img in imgs]
    output_path = os.path.join(path_to_img_dir, scenario_name + ".gif")

    imgs_pil[0].save(
        output_path,
        save_all=True,
        append_images=imgs_pil[1:],
        duration=duration,
        loop=0,
    )

obtain_plot_limits_from_reference_path(trajectory, margin=10.0)

Obtains plot limits from a given trajectory

Parameters:

Name Type Description Default
trajectory TrajectoryInterface

trajectory for extracting plot limits

 required

Returns:

Type Description
List[int]

list [xmin, xmax, ymin, xmax] of plot limits
Source code in commonroad_control/util/visualization/visualize_trajectories.py 
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
def obtain_plot_limits_from_reference_path(trajectory: TrajectoryInterface, margin: float = 10.0) -> List[int]:
    """
    Obtains plot limits from a given trajectory
    :param trajectory: trajectory for extracting plot limits
    :return: list [xmin, xmax, ymin, xmax] of plot limits
    """
    arr = np.asarray([[point.position_x, point.position_y] for point in trajectory.points.values()])

    x_min = min(arr[:, 0])
    x_max = max(arr[:, 0])
    y_min = min(arr[:, 1])
    y_max = max(arr[:, 1])

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

visualize_trajectories(scenario, planning_problem, planner_trajectory, controller_trajectory, vehicle_width=1.8, vehicle_length=4.5, size_x=10.0, save_img=False, save_path=None, opacity_planning=0.3, opacity_control=1.0, use_icon_controlled_traj=True, use_icon_planned_traj=False)

Visualize scenario with planned and driven trajectory. Per default, the driven trajectory is depicted as an orange car and the planned trajectory as a black rectangle.

Parameters:

Name Type Description Default
scenario Scenario

CommonRoad scenario.

 required
planning_problem PlanningProblem

Commonroad planning problem

 required
planner_trajectory TrajectoryInterface

Planned trajectory

 required
controller_trajectory TrajectoryInterface

Actual trajectory (from controller or simulation)

 required
vehicle_width float

vehicle width

 1.8
vehicle_length float

vehicle length

 4.5
size_x float

abscissa size of the figure

 10.0
save_img bool

if true and save_path is valid, saves figure. If false, figure is displayed.

 False
save_path Union[str, Path]

if valid and save_img is true, saves figure

 None
opacity_planning float

If planned trajectory does not use icon (default), opacity of black rectangle.

 0.3
opacity_control float

If actual trajectory does not use icon (not default default), opacity of orange rectangle.

 1.0
use_icon_controlled_traj bool

If true, uses car icon for actual trajectory, else rectangle.

 True
use_icon_planned_traj bool

If true, uses car icon for planned trajectory, else rectangle.

 False

Returns:

Type Description
None
Source code in commonroad_control/util/visualization/visualize_trajectories.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
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
def visualize_trajectories(
    scenario: Scenario,
    planning_problem: PlanningProblem,
    planner_trajectory: TrajectoryInterface,
    controller_trajectory: TrajectoryInterface,
    vehicle_width: float = 1.8,
    vehicle_length: float = 4.5,
    size_x: float = 10.0,
    save_img: bool = False,
    save_path: Union[str, Path] = None,
    opacity_planning: float = 0.3,
    opacity_control: float = 1.0,
    use_icon_controlled_traj: bool = True,
    use_icon_planned_traj: bool = False,
) -> None:
    """
    Visualize scenario with planned and driven trajectory.
    Per default, the driven trajectory is depicted as an orange car and the planned trajectory
    as a black rectangle.
    :param scenario: CommonRoad scenario.
    :param planning_problem: Commonroad planning problem
    :param planner_trajectory: Planned trajectory
    :param controller_trajectory: Actual trajectory (from controller or simulation)
    :param vehicle_width: vehicle width
    :param vehicle_length: vehicle length
    :param size_x: abscissa size of the figure
    :param save_img: if true and save_path is valid, saves figure. If false, figure is displayed.
    :param save_path: if valid and save_img is true, saves figure
    :param opacity_planning: If planned trajectory does not use icon (default), opacity of black rectangle.
    :param opacity_control: If actual trajectory does not use icon (not default default), opacity of orange rectangle.
    :param use_icon_controlled_traj: If true, uses car icon for actual trajectory, else rectangle.
    :param use_icon_planned_traj: If true, uses car icon for planned trajectory, else rectangle.
    :return:
    """
    for step in planner_trajectory.steps:
        plt.cla()

        # get plot limits from reference idm_path
        plot_limits: List[float] = obtain_plot_limits_from_reference_path(planner_trajectory, 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 = step

        scenario.draw(renderer)

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

        draw_params = copy.copy(renderer.draw_params)
        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.time_begin = step

        # planned
        planner_vehicle = planner_trajectory.to_cr_dynamic_obstacle(
            vehicle_width=vehicle_width,
            vehicle_length=vehicle_length,
            vehicle_id=30000,
        )
        draw_params.dynamic_obstacle.draw_icon = use_icon_planned_traj
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.opacity = opacity_planning
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.facecolor = "#000000"
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.edgecolor = "#808080"
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.zorder = 20
        planner_vehicle.draw(renderer, draw_params=draw_params)

        # controlled
        draw_params = copy.copy(renderer.draw_params)
        controller_vehicle = controller_trajectory.to_cr_dynamic_obstacle(
            vehicle_width=vehicle_width,
            vehicle_length=vehicle_length,
            vehicle_id=30001,
        )
        draw_params.dynamic_obstacle.draw_icon = use_icon_controlled_traj
        draw_params.dynamic_obstacle.vehicle_shape.occupancy.shape.opacity = opacity_control
        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
        controller_vehicle.draw(renderer, draw_params=draw_params)

        # draw scenario and renderer
        renderer.render()

        plt.title(f"Time step = {step}")

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

commonroad_control.util.visualization.visualize_control_state

visualize_reference_vs_actual_states(reference_trajectory, actual_trajectory, time_steps=None, state_names=None, save_img=False, save_path=None)

Plots selected components of the reference and actual (simulated or driven) trajectories as well as the respective tracking error.

Parameters:

Name Type Description Default
reference_trajectory Union[TrajectoryInterface]

reference trajectory

 required
actual_trajectory Union[TrajectoryInterface]

actual (simulated or driven) trajectory

 required
time_steps List[int]

simulation time steps

 None
state_names Optional[List[str]]

(optional) list of state components, which should be plotted - valid values are attribute names of the StateInterface objects

 None
save_img bool

boolean indicting whether the plot should be saved or not

 False
save_path Union[str, Path]

path for saving the plot

 None

Returns:

Type Description
None
Source code in commonroad_control/util/visualization/visualize_control_state.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
 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
def visualize_reference_vs_actual_states(
    reference_trajectory: Union[TrajectoryInterface],
    actual_trajectory: Union[TrajectoryInterface],
    time_steps: List[int] = None,
    state_names: Optional[List[str]] = None,
    save_img: bool = False,
    save_path: Union[str, Path] = None,
) -> None:
    """
    Plots selected components of the reference and actual (simulated or driven) trajectories as well as the respective tracking error.
    :param reference_trajectory: reference trajectory
    :param actual_trajectory: actual (simulated or driven) trajectory
    :param time_steps: simulation time steps
    :param state_names: (optional) list of state components, which should be plotted - valid values are attribute names of the StateInterface objects
    :param save_img: boolean indicting whether the plot should be saved or not
    :param save_path: path for saving the plot
    :return:
    """

    logger.debug("visualizing control")

    if state_names is None:
        state_names = vars(reference_trajectory.initial_point).keys()

    # check which items of state_names are defined for the reference trajectory and the actual trajectory
    present_ref_state_names = [a for a in state_names if hasattr(reference_trajectory.initial_point, a)]
    present_actual_state_names = [a for a in state_names if hasattr(actual_trajectory.initial_point, a)]
    plot_state_names = list(set(present_ref_state_names) & set(present_actual_state_names))
    if not plot_state_names:
        logger.warning("No matching state names for the reference and actual trajectory " "- nothing to be plotted.")
        return
    elif len(plot_state_names) < len(state_names):
        removed_names = list(set(state_names) - set(plot_state_names))
        logger.warning(
            f"The following state names are not defined for the reference or actual trajectory "
            f"{removed_names} and will not be plotted."
        )

    # plot reference and actual trajectories
    fig, axes = plt.subplots(nrows=len(plot_state_names), ncols=1, figsize=(16, 12))
    plt.title("Desired vs. Actual state")

    for ii in range(len(plot_state_names)):
        reference_state_val: List[float] = [
            getattr(reference_trajectory.get_point_at_time_step(kk), plot_state_names[ii]) for kk in time_steps
        ]
        actual_state_val: List[float] = [
            getattr(actual_trajectory.get_point_at_time_step(kk), plot_state_names[ii]) for kk in time_steps
        ]

        axes[ii].plot(time_steps, reference_state_val, label="reference", color="blue")
        axes[ii].plot(time_steps, actual_state_val, label="actual", color="orange")
        axes[ii].title.set_text(plot_state_names[ii])
        axes[ii].legend()

    plt.tight_layout()  # Avoid overlap

    if save_img and save_path is not None:
        save_dir = os.path.join(save_path, "control")
        save_file: str = os.path.join(save_path, "control", "states.png")
        os.makedirs(save_dir, exist_ok=True)  # Ensure the directory exists
        plt.savefig(save_file, format="png")
    else:
        plt.show()

    # plot deviation
    fig_err, axes_err = plt.subplots(nrows=len(plot_state_names), ncols=1, figsize=(16, 12))
    plt.title("Error")

    for ii in range(len(plot_state_names)):
        reference_state_val: List[float] = [
            getattr(reference_trajectory.get_point_at_time_step(kk), plot_state_names[ii]) for kk in time_steps
        ]
        actual_state_val: List[float] = [
            getattr(actual_trajectory.get_point_at_time_step(kk), plot_state_names[ii]) for kk in time_steps
        ]
        error: List[float] = [actual_state_val[kk] - reference_state_val[kk] for kk in range(len(reference_state_val))]

        axes_err[ii].plot(time_steps, error, label="error", color="red")
        axes_err[ii].title.set_text(plot_state_names[ii])
        axes_err[ii].legend()

    plt.tight_layout()  # Avoid overlap

    if save_img and save_path is not None:
        save_dir = os.path.join(save_path, "control")
        save_file: str = os.path.join(save_dir, "error.png")
        os.makedirs(save_dir, exist_ok=True)  # Ensure the directory exists
        plt.savefig(save_file, format="png")
    else:
        plt.show()

    return

commonroad_control.util.visualization.visualize_ff_and_fb_inputs