Skip to content

Velocity Planner Interface

IVelocityPlanner

Velocity planner interface.

Source code in commonroad_velocity_planner/velocity_planner_interface.py 
 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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
class IVelocityPlanner:
    """
    Velocity planner interface.
    """

    def __init__(self):
        """
        Interface for all velocity planners.
        """
        self._logger = logging.Logger(name="IVelocityPlanner", level=logging.WARNING)

        # TODO: Currently leave them as members so people have an easier time debugging
        self._reference_path: ReferencePath = None
        self._velocity_planner_config: VelocityPlannerConfig = None
        self._vpp: VelocityPlanningProblem = None
        self._planner: Union[LinearProgramPlanner, BangBangSTPlanner] = None
        self._spline_profile: SplineProfile = None
        self._global_trajectory: GlobalTrajectory = None
        self._velocity_planner: ImplementedPlanners = None

    @property
    def reference_path(self) -> ReferencePath:
        """
        :return: cr reference_path object
        """
        return self._reference_path

    @property
    def velocity_planning_problem(self) -> VelocityPlanningProblem:
        """
        :return: current velocity planning problem
        """
        return self._vpp

    @property
    def config(self) -> VelocityPlannerConfig:
        """
        :return: velocity planner config
        """
        return self._velocity_planner_config

    @property
    def planner(self) -> Union[LinearProgramPlanner]:
        """
        :return: solver interface
        """
        return self._planner

    def plan_velocity(
        self,
        reference_path: ReferencePath,
        velocity_planning_problem: VelocityPlanningProblem,
        planner_config: VelocityPlannerConfig,
        velocity_planner: ImplementedPlanners = ImplementedPlanners.LinearProgramPlanner,
        return_spline_profile: bool = False,
    ) -> Union[GlobalTrajectory, Tuple[GlobalTrajectory, SplineProfile]]:
        """
        Plans velocity profile and returns a global trajectory object, or a Tuple [Global Trajectory, Spline Profile].
        :param reference_path: cr reference path
        :param planning_problem: cr planning problem
        :param planner_config: velocity planner config
        :param velocity_planner: selected velocity planner
        :param resampling_distance: resampling distance to speed up profile
        :param default_goal_velocity: default goal velocity is none is given
        :param return_spline_profile: if true, returns a Tuple with global_trajectory and spline_velocity_profile
        :return: returns either only the global trajectory or a Tuple with global_trajectory and spline_velocity_profile
        """

        # TODO: For now make them members as this is usually easier to debug
        self._velocity_planner: ImplementedPlanners = velocity_planner
        self._velocity_planner_config: VelocityPlannerConfig = planner_config
        self._reference_path: ReferencePath = reference_path
        self._vpp: VelocityPlanningProblem = velocity_planning_problem

        # init planner
        self._init_planner()

        # plan and create spline profile
        self._spline_profile: SplineProfile = self._planner.plan_velocity(
            problem=self._vpp, config=planner_config
        )

        # create global trajectory
        self._global_trajectory: GlobalTrajectory = (
            factory_from_reference_path_and_velocity_profile(
                reference_path=reference_path,
                velocity_profile=self._spline_profile,
                velocity_planning_problem=self._vpp,
            )
        )

        if return_spline_profile:
            retval = (self._global_trajectory, self._spline_profile)
        else:
            retval = self._global_trajectory

        return retval

    def _init_planner(self) -> None:
        """
        Init planner given api choice
        """
        if self._velocity_planner == ImplementedPlanners.LinearProgramPlanner:
            self._planner = LinearProgramPlanner(config=self._velocity_planner_config)
            self._logger.info(f"Initialized planner: {self._velocity_planner}")
        elif self._velocity_planner == ImplementedPlanners.BangBangSTPlanner:
            self._planner = BangBangSTPlanner(config=self._velocity_planner_config)
            self._logger.info(f"Initialized planner:  {self._velocity_planner}")
        else:
            self._logger.error(f"{self._velocity_planner} is not implemented")
            NotImplementedError(f"{self._velocity_planner} is not implemented")

config: VelocityPlannerConfig property

Returns:

Type Description
VelocityPlannerConfig

velocity planner config

planner: Union[LinearProgramPlanner] property

Returns:

Type Description
Union[LinearProgramPlanner]

solver interface

reference_path: ReferencePath property

Returns:

Type Description
ReferencePath

cr reference_path object

velocity_planning_problem: VelocityPlanningProblem property

Returns:

Type Description
VelocityPlanningProblem

current velocity planning problem

__init__()

Interface for all velocity planners.

Source code in commonroad_velocity_planner/velocity_planner_interface.py 
40
41
42
43
44
45
46
47
48
49
50
51
52
53
def __init__(self):
    """
    Interface for all velocity planners.
    """
    self._logger = logging.Logger(name="IVelocityPlanner", level=logging.WARNING)

    # TODO: Currently leave them as members so people have an easier time debugging
    self._reference_path: ReferencePath = None
    self._velocity_planner_config: VelocityPlannerConfig = None
    self._vpp: VelocityPlanningProblem = None
    self._planner: Union[LinearProgramPlanner, BangBangSTPlanner] = None
    self._spline_profile: SplineProfile = None
    self._global_trajectory: GlobalTrajectory = None
    self._velocity_planner: ImplementedPlanners = None

plan_velocity(reference_path, velocity_planning_problem, planner_config, velocity_planner=ImplementedPlanners.LinearProgramPlanner, return_spline_profile=False)

Plans velocity profile and returns a global trajectory object, or a Tuple [Global Trajectory, Spline Profile].

Parameters:

Name Type Description Default
reference_path ReferencePath

cr reference path

 required
planning_problem

cr planning problem

 required
planner_config VelocityPlannerConfig

velocity planner config

 required
velocity_planner ImplementedPlanners

selected velocity planner

 LinearProgramPlanner
resampling_distance

resampling distance to speed up profile

 required
default_goal_velocity

default goal velocity is none is given

 required
return_spline_profile bool

if true, returns a Tuple with global_trajectory and spline_velocity_profile

 False

Returns:

Type Description
Union[GlobalTrajectory, Tuple[GlobalTrajectory, SplineProfile]]

returns either only the global trajectory or a Tuple with global_trajectory and spline_velocity_profile
Source code in commonroad_velocity_planner/velocity_planner_interface.py 
 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
122
123
124
125
126
127
128
129
130
131
def plan_velocity(
    self,
    reference_path: ReferencePath,
    velocity_planning_problem: VelocityPlanningProblem,
    planner_config: VelocityPlannerConfig,
    velocity_planner: ImplementedPlanners = ImplementedPlanners.LinearProgramPlanner,
    return_spline_profile: bool = False,
) -> Union[GlobalTrajectory, Tuple[GlobalTrajectory, SplineProfile]]:
    """
    Plans velocity profile and returns a global trajectory object, or a Tuple [Global Trajectory, Spline Profile].
    :param reference_path: cr reference path
    :param planning_problem: cr planning problem
    :param planner_config: velocity planner config
    :param velocity_planner: selected velocity planner
    :param resampling_distance: resampling distance to speed up profile
    :param default_goal_velocity: default goal velocity is none is given
    :param return_spline_profile: if true, returns a Tuple with global_trajectory and spline_velocity_profile
    :return: returns either only the global trajectory or a Tuple with global_trajectory and spline_velocity_profile
    """

    # TODO: For now make them members as this is usually easier to debug
    self._velocity_planner: ImplementedPlanners = velocity_planner
    self._velocity_planner_config: VelocityPlannerConfig = planner_config
    self._reference_path: ReferencePath = reference_path
    self._vpp: VelocityPlanningProblem = velocity_planning_problem

    # init planner
    self._init_planner()

    # plan and create spline profile
    self._spline_profile: SplineProfile = self._planner.plan_velocity(
        problem=self._vpp, config=planner_config
    )

    # create global trajectory
    self._global_trajectory: GlobalTrajectory = (
        factory_from_reference_path_and_velocity_profile(
            reference_path=reference_path,
            velocity_profile=self._spline_profile,
            velocity_planning_problem=self._vpp,
        )
    )

    if return_spline_profile:
        retval = (self._global_trajectory, self._spline_profile)
    else:
        retval = self._global_trajectory

    return retval