Long Example

Long Example

This file shows the long example, so one level down from our raceline fast api

Resources

You can find the .ini and the ggv file here.

Example 1: Long Example

from typing import Union, Optional
from pathlib import Path

# commonroad
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.planning.planning_problem import PlanningProblem

# own package
from commonroad_raceline_planner.configuration.ftm_config.ftm_config import FTMConfig, FTMConfigFactory
from commonroad_raceline_planner.configuration.ftm_config.optimization_config import OptimizationType
from commonroad_raceline_planner.racetrack import RaceTrackFactory
from commonroad_raceline_planner.planner.ftm_planner.ftm_mc_planner import MinimumCurvaturePlanner
from commonroad_raceline_planner.planner.ftm_planner.ftm_sp_planner import ShortestPathPlanner
from commonroad_raceline_planner.raceline import RaceLine
from commonroad_raceline_planner.utils.visualization.visualize_on_racetrack import plot_trajectory_with_all_quantities
from commonroad_raceline_planner.utils.visualization.visualize_over_arclength import plot_trajectory_over_arclength


def main(
        cr_path: Union[str, Path],
        ini_path: Union[str, Path],
        ggv_file: Union[str, Path],
        ax_max_machines_file: Union[str, Path],
        traj_race_export: Optional[Union[str, Path]] = None,
        velocity_profile_export: Optional[Union[str, Path]] = None,
        opt_type: OptimizationType = OptimizationType.SHORTEST_PATH,
        min_track_width: Optional[float] = None
) -> None:
    # Commonroad imports
    scenario, planning_problem_set = CommonRoadFileReader(cr_path).open()
    planning_problem: PlanningProblem = list(planning_problem_set.planning_problem_dict.values())[0]

    # generate configs
    ftm_config: FTMConfig = FTMConfigFactory().generate_from_files(
        path_to_ini=ini_path,
        ggv_file=ggv_file,
        ax_max_machines_file=ax_max_machines_file,
        optimization_type=OptimizationType.MINIMUM_CURVATURE,
        min_track_width=min_track_width
    )

    # import race track
    race_track = RaceTrackFactory().generate_racetrack_from_cr_scenario(
        lanelet_network=scenario.lanelet_network,
        planning_problem=planning_problem,
        vehicle_width=ftm_config.computation_config.general_config.vehicle_config.width
    )

    # plan
    if opt_type == OptimizationType.MINIMUM_CURVATURE:
        mcp = MinimumCurvaturePlanner(
            config=ftm_config, race_track=race_track
        )
        raceline: RaceLine = mcp.plan()

    elif opt_type == OptimizationType.SHORTEST_PATH:
        spp = ShortestPathPlanner(
            config=ftm_config, race_track=race_track
        )
        raceline: RaceLine = spp.plan()
    else:
        raise NotImplementedError(f'Planner {opt_type} not implemented')

    # export data
    if traj_race_export is not None and ggv_file is not None:
        raceline.export_trajectory_to_csv_file(
            export_path=traj_race_export,
            ggv_file_path=velocity_profile_export
        )

    plot_trajectory_with_all_quantities(
        race_line=raceline,
        lanelet_network=scenario.lanelet_network,
        planning_problem=planning_problem
    )

    plot_trajectory_over_arclength(
        race_line=raceline
    )


if __name__ == "__main__":
    cr_path = "PATH/TO/CR/FILE"
    ini_path = "PATH/TO/.ini",
    ggv_file = "PATH/TO/ggv",
    ax_max_machines_file = "PATH/TO/ENGINECONSTRAINTS",
    ini_path = "PATH/TO/.ini"
    ggv_file = "PATH/TO/ggv"
    ax_max_machines_file = "PATH/TO/ENGINECONSTRAINTS"
    traj_race_export = "/PATH/TO/EXPORT/TRAJECTORY"
    velocity_profile_export = "/PATH/TO/EXPORT/VELOCITY/PROFILE"
    opt_type: OptimizationType = OptimizationType.SHORTEST_PATH
    main(
        cr_path=cr_path,
        ini_path=ini_path,
        ggv_file=ggv_file,
        ax_max_machines_file=ax_max_machines_file,
        traj_race_export=traj_race_export,
        velocity_profile_export=velocity_profile_export,
        opt_type=opt_type,
    )