Skip to content

other utils

commonroad_control.util.state_conversion

convert_state_db2kb(db_state)

Converts dynamic bicycle state to kinematic bicycle state.

Parameters:

Name Type Description Default
db_state DBState

DB state

 required

Returns:

Type Description
KBState

KB State
Source code in commonroad_control/util/state_conversion.py 
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
def convert_state_db2kb(db_state: "DBState") -> KBState:
    """
    Converts dynamic bicycle state to kinematic bicycle state.
    :param db_state: DB state
    :return: KB State
    """
    v = compute_velocity_from_components(v_long=db_state.velocity_long, v_lat=db_state.velocity_lat)

    return KBState(
        position_x=db_state.position_x,
        position_y=db_state.position_y,
        velocity=v,
        heading=db_state.heading,
        steering_angle=db_state.steering_angle,
    )

convert_state_kb2db(kb_state, vehicle_params)

Converts kinematic bicycle state to dynamic bicycle state.

Parameters:

Name Type Description Default
kb_state KBState

KB state

 required
vehicle_params VehicleParameters

Vehicle parameters

 required

Returns:

Type Description
DBState

KB state
Source code in commonroad_control/util/state_conversion.py 
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
def convert_state_kb2db(kb_state: "KBState", vehicle_params: VehicleParameters) -> DBState:
    """
    Converts kinematic bicycle state to dynamic bicycle state.
    :param kb_state: KB state
    :param vehicle_params: Vehicle parameters
    :return: KB state
    """

    slip_angle = compute_slip_angle_from_steering_angle_in_cog(
        steering_angle=kb_state.steering_angle,
        velocity=kb_state.velocity,
        l_wb=vehicle_params.l_wb,
        l_r=vehicle_params.l_r,
    )

    return DBState(
        position_x=kb_state.position_x,
        position_y=kb_state.position_y,
        velocity_long=kb_state.velocity * cos(slip_angle),
        velocity_lat=kb_state.velocity * sin(slip_angle),
        heading=kb_state.heading,
        yaw_rate=kb_state.velocity * sin(slip_angle) / vehicle_params.l_r,
        steering_angle=kb_state.steering_angle,
    )

commonroad_control.util.geometry

signed_distance_point_to_linestring(point, linestring)

Signed distance between a shapely point and shapely linestring. Positive means left of line vector

Parameters:

Name Type Description Default
point Point

shapely point

 required
linestring LineString

shapely linestring

 required

Returns:

Type Description
float

signed distance, positive meaning left of linestring segment vector
Source code in commonroad_control/util/geometry.py 
 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
def signed_distance_point_to_linestring(point: Point, linestring: LineString) -> float:
    """
    Signed distance between a shapely point and shapely linestring. Positive means left of line vector
    :param point: shapely point
    :param linestring: shapely linestring
    :return: signed distance, positive meaning left of linestring segment vector
    """

    distance: float = linestring.distance(point)
    coords = list(linestring.coords)
    arclength = linestring.project(point)
    point_on_line = linestring.interpolate(arclength)
    point_before_line = None
    for i in range(len(coords) - 1):
        seg_start = Point(coords[i])
        seg_end = Point(coords[i + 1])
        if linestring.project(seg_start) <= arclength and linestring.project(seg_end) >= arclength:
            point_before_line = seg_start
            break

    if point_before_line is None:
        logger.error("point before line is none")
        raise ValueError("point before line is none")

    vector_line: np.ndarray = np.asarray([point_on_line.x - point_before_line.x, point_on_line.y - point_before_line.y])
    vector_point: np.ndarray = np.asarray([point.x - point_on_line.x, point.y - point_on_line.y])
    cross_product = np.cross(vector_line, vector_point)

    return distance if cross_product >= 0.0 else -distance

commonroad_control.util.cr_logging_utils

configure_toolbox_logging(level=logging.INFO)

Configures toolbox level logging

Parameters:

Name Type Description Default
level

logging level

 INFO
Source code in commonroad_control/util/cr_logging_utils.py 
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
def configure_toolbox_logging(level=logging.INFO) -> logging.Logger:
    """
    Configures toolbox level logging
    :param level: logging level
    """
    logger = logging.getLogger("commonroad_control")
    logger.handlers.clear()  # prevents duplicates
    handler = logging.StreamHandler()
    handler.setLevel(level)
    formatter = logging.Formatter("%(name)s: %(message)s")
    handler.setFormatter(formatter)
    logger.addHandler(handler)
    logger.setLevel(level)
    return logger

commonroad_control.util.conversion_util

compute_position_of_cog_from_ra_cc(l_r, position_ra_x, position_ra_y, heading)

Given the position of the center of the rear-axle, this function returns the position of the center of gravity; each represented in Cartesian coordinates.

Parameters:

Name Type Description Default
l_r float

distance from center of rear-axle to the center of gravity

 required
position_ra_x float

longitudinal component of the position of the rear-axle (Cartesian coordinates)

 required
position_ra_y float

lateral component of the position of the rear-axle (Cartesian coordinates)

 required
heading float

orientation of the vehicle

 required

Returns:

Type Description
Tuple[float, float]

position of the center of gravity (Cartesian coordinates)
Source code in commonroad_control/util/conversion_util.py 
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
def compute_position_of_cog_from_ra_cc(
    l_r: float, position_ra_x: float, position_ra_y: float, heading: float
) -> Tuple[float, float]:
    """
    Given the position of the center of the rear-axle, this function returns the position of the center of gravity; each
    represented in Cartesian coordinates.
    :param l_r: distance from center of rear-axle to the center of gravity
    :param position_ra_x: longitudinal component of the position of the rear-axle (Cartesian coordinates)
    :param position_ra_y: lateral component of the position of the rear-axle (Cartesian coordinates)
    :param heading: orientation of the vehicle
    :return: position of the center of gravity (Cartesian coordinates)
    """

    position_cog_x = position_ra_x + l_r * math.cos(heading)
    position_cog_y = position_ra_y + l_r * math.sin(heading)

    return position_cog_x, position_cog_y

compute_position_of_ra_from_cog_cartesian(l_r, position_cog_x, position_cog_y, heading)

Given the position of the center of the center of gravity (COG), this function returns the position of the rear axle; each represented in Cartesian coordinates.

Parameters:

Name Type Description Default
l_r float

distance from center of rear-axle to the center of gravity

 required
position_cog_x float

longitudinal component of the position of the rear-axle (Cartesian coordinates)

 required
position_cog_y float

lateral component of the position of the rear-axle (Cartesian coordinates)

 required
heading float

orientation of the vehicle

 required

Returns:

Type Description
Tuple[float, float]

position of the rear axle(Cartesian coordinates)
Source code in commonroad_control/util/conversion_util.py 
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
def compute_position_of_ra_from_cog_cartesian(
    l_r: float, position_cog_x: float, position_cog_y: float, heading: float
) -> Tuple[float, float]:
    """
    Given the position of the center of the center of gravity (COG), this function returns the position of the rear axle; each
    represented in Cartesian coordinates.
    :param l_r: distance from center of rear-axle to the center of gravity
    :param position_cog_x: longitudinal component of the position of the rear-axle (Cartesian coordinates)
    :param position_cog_y: lateral component of the position of the rear-axle (Cartesian coordinates)
    :param heading: orientation of the vehicle
    :return: position of the rear axle(Cartesian coordinates)
    """
    position_ra_x = position_cog_x - l_r * math.cos(heading)
    position_ra_y = position_cog_y - l_r * math.sin(heading)

    return position_ra_x, position_ra_y

compute_slip_angle_from_steering_angle_in_cog(steering_angle, velocity, l_wb, l_r)

Compute slip angle in center of gravity from steering angle and wheelbase.

Parameters:

Name Type Description Default
steering_angle float

steering angle

 required
velocity float

longitudinal velocity - float

 required
l_wb float

wheelbase

 required
l_r float

distance from center of rear-axle to the center of gravity

 required

Returns:

Type Description
float

slip angle in radian
Source code in commonroad_control/util/conversion_util.py 
 7
 8
 9
10
11
12
13
14
15
16
17
18
def compute_slip_angle_from_steering_angle_in_cog(
    steering_angle: float, velocity: float, l_wb: float, l_r: float
) -> float:
    """
    Compute slip angle in center of gravity from steering angle and wheelbase.
    :param steering_angle: steering angle
    :param velocity: longitudinal velocity - float
    :param l_wb: wheelbase
    :param l_r: distance from center of rear-axle to the center of gravity
    :return: slip angle in radian
    """
    return np.sign(velocity) * math.atan(math.tan(steering_angle) * l_r / l_wb)

compute_slip_angle_from_velocity_components(v_lon, v_lat)

Computes the slip angle from the long. and lat. velocity (body frame) at the center of gravity.

Parameters:

Name Type Description Default
v_lon float

longitudinal velocity at the center of gravity - float

 required
v_lat float

lateral velocity at the center of gravity - float

 required

Returns:

Type Description
float
Source code in commonroad_control/util/conversion_util.py 
21
22
23
24
25
26
27
28
def compute_slip_angle_from_velocity_components(v_lon: float, v_lat: float) -> float:
    """
    Computes the slip angle from the long. and lat. velocity (body frame) at the center of gravity.
    :param v_lon: longitudinal velocity at the center of gravity - float
    :param v_lat: lateral velocity at the center of gravity - float
    :return:
    """
    return math.atan2(v_lat, v_lon)

compute_velocity_components_from_slip_angle_and_velocity_in_cog(slip_angle, velocity)

Compute velocity components in long. and lat. direction (body frame) at the center of gravity from slip angle.

Parameters:

Name Type Description Default
slip_angle float

slip angle

 required
velocity float

total velocity

 required

Returns:

Type Description
Tuple[float, float]

v_lon, v_lat
Source code in commonroad_control/util/conversion_util.py 
31
32
33
34
35
36
37
38
39
40
def compute_velocity_components_from_slip_angle_and_velocity_in_cog(
    slip_angle: float, velocity: float
) -> Tuple[float, float]:
    """
    Compute velocity components in long. and lat. direction (body frame) at the center of gravity from slip angle.
    :param slip_angle: slip angle
    :param velocity: total velocity
    :return: v_lon, v_lat
    """
    return math.cos(slip_angle) * velocity, math.sin(slip_angle) * velocity

compute_velocity_components_from_steering_angle_in_cog(steering_angle, velocity_cog, l_wb, l_r)

Computes velocity components at center of gravity. To this end, the slip angle is derived from the steering angle using kinematic relations.

Parameters:

Name Type Description Default
steering_angle float

steering angle

 required
velocity_cog float

velocity at center of gravity

 required
l_wb float

wheelbase

 required
l_r float

distance from center of rear-axle to the center of gravity

 required

Returns:

Type Description
Tuple[float, float]

v_lon, v_lat
Source code in commonroad_control/util/conversion_util.py 
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
def compute_velocity_components_from_steering_angle_in_cog(
    steering_angle: float, velocity_cog: float, l_wb: float, l_r: float
) -> Tuple[float, float]:
    """
    Computes velocity components at center of gravity. To this end, the slip angle is derived from the steering angle
    using kinematic relations.
    :param steering_angle: steering angle
    :param velocity_cog: velocity at center of gravity
    :param l_wb: wheelbase
    :param l_r: distance from center of rear-axle to the center of gravity
    :return: v_lon, v_lat
    """
    slip_angle: float = compute_slip_angle_from_steering_angle_in_cog(
        steering_angle=steering_angle, velocity=velocity_cog, l_wb=l_wb, l_r=l_r
    )

    return compute_velocity_components_from_slip_angle_and_velocity_in_cog(slip_angle=slip_angle, velocity=velocity_cog)

compute_velocity_from_components(v_long, v_lat)

Computes the total velocity from its components.

Parameters:

Name Type Description Default
v_long float

longitudinal velocity

 required
v_lat float

lateral velocity

 required

Returns:

Type Description
float

v
Source code in commonroad_control/util/conversion_util.py 
62
63
64
65
66
67
68
69
def compute_velocity_from_components(v_long: float, v_lat: float) -> float:
    """
    Computes the total velocity from its components.
    :param v_long: longitudinal velocity
    :param v_lat: lateral velocity
    :return: v
    """
    return np.sign(v_long) * math.sqrt(v_long**2 + v_lat**2)

map_velocity_from_cog_to_ra(l_wb, l_r, velocity_cog, steering_angle)

Given the velocity at the center of gravity, this function computes the velocity at the vehicle's rear axle.

Parameters:

Name Type Description Default
l_wb float

wheelbase

 required
l_r float

distance from center of rear-axle to the center of gravity

 required
velocity_cog float

velocity at the center of gravity

 required
steering_angle float

steering angle

 required

Returns:

Type Description
float

velocity at the center of gravity
Source code in commonroad_control/util/conversion_util.py 
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
def map_velocity_from_cog_to_ra(l_wb: float, l_r: float, velocity_cog: float, steering_angle: float) -> float:
    """
    Given the velocity at the center of gravity, this function computes the velocity at the vehicle's rear axle.
    :param l_wb: wheelbase
    :param l_r: distance from center of rear-axle to the center of gravity
    :param velocity_cog: velocity at the center of gravity
    :param steering_angle: steering angle
    :return: velocity at the center of gravity
    """
    if abs(steering_angle) > 1e-6:
        len_ray_ra = abs(l_wb / math.tan(steering_angle))
        len_ray_cog = math.sqrt(len_ray_ra**2 + l_r**2)
        velocity_ra = velocity_cog * len_ray_ra / len_ray_cog
    else:
        # for steering_angle = 0, the velocities are identical
        velocity_ra = velocity_cog

    return velocity_ra

map_velocity_from_ra_to_cog(l_wb, l_r, velocity_ra, steering_angle)

Given the velocity at the center of the rear axle, this function computes the velocity at the vehicle's center of gravity using kinematic relations.

Parameters:

Name Type Description Default
l_wb float

wheelbase

 required
l_r float

distance from center of rear-axle to the center of gravity

 required
velocity_ra float

velocity at the center of the rear axle

 required
steering_angle float

steering angle

 required

Returns:

Type Description
float

velocity at the center of gravity
Source code in commonroad_control/util/conversion_util.py 
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
def map_velocity_from_ra_to_cog(l_wb: float, l_r: float, velocity_ra: float, steering_angle: float) -> float:
    """
    Given the velocity at the center of the rear axle, this function computes the velocity at the vehicle's center of
    gravity using kinematic relations.
    :param l_wb: wheelbase
    :param l_r: distance from center of rear-axle to the center of gravity
    :param velocity_ra: velocity at the center of the rear axle
    :param steering_angle: steering angle
    :return: velocity at the center of gravity
    """
    if abs(steering_angle) > 1e-6:
        v_ra = velocity_ra
        len_ray_ra = abs(l_wb / math.tan(steering_angle))
        len_ray_cog = math.sqrt(len_ray_ra**2 + l_r**2)
        v_cog = v_ra * len_ray_cog / len_ray_ra
    else:
        # for steering_angle = 0, the velocities are identical
        v_cog = velocity_ra

    return v_cog

unwrap_angle(alpha_prev, alpha_next)

This function returns an alpha_next adjusted to be "continuous" with alpha_prev. To this end, alpha_next is mapped to ]-pi, pi]. As an example, if the desired heading of the vehicle is 10° and the current heading is 340°, wrapping the angle makes the vehicle turn 30° clockwise, not 330° counter-clockwise.

Parameters:

Name Type Description Default
alpha_prev float

previous angle - float

 required
alpha_next float

next angle before wrapping - float

 required

Returns:

Type Description
float

next angle after wrapping - float
Source code in commonroad_control/util/conversion_util.py 
151
152
153
154
155
156
157
158
159
160
161
162
163
def unwrap_angle(alpha_prev: float, alpha_next: float) -> float:
    """
    This function returns an alpha_next adjusted to be "continuous" with alpha_prev. To this end, alpha_next is mapped to ]-pi, pi].
    As an example, if the desired heading of the vehicle is 10° and the current heading is 340°, wrapping the angle makes the vehicle turn 30° clockwise, not 330° counter-clockwise.
    :param alpha_prev: previous angle - float
    :param alpha_next: next angle before wrapping - float
    :return: next angle after wrapping - float
    """

    # wrap to ]pi, pi]
    diff = (alpha_next - alpha_prev + np.pi) % (2 * np.pi) - np.pi
    # unwrap
    return alpha_prev + diff

commonroad_control.util.clcs_control_util

extend_kb_reference_trajectory_lane_following(x_ref, u_ref, lanelet_network, vehicle_params, delta_t, horizon)

Extends kinematic bicycle trajectory using the CommonRoad reference path planner to account for extended MPC horizons, or controllers with look-ahead etc.

Parameters:

Name Type Description Default
x_ref TrajectoryInterface

State trajectory

 required
u_ref TrajectoryInterface

Input trajectory

 required
lanelet_network LaneletNetwork

CommonRoad lanelet network

 required
vehicle_params VehicleParameters

Vehicle parameters object

 required
delta_t float

sampling time in seconds - float

 required
horizon int

time horizon for extension (number of time steps) - int

 required

Returns:

Type Description
Tuple[CurvilinearCoordinateSystem, TrajectoryInterface, TrajectoryInterface]

Curvilinear coordinate system, extended state trajectory, extended input trajectory
Source code in commonroad_control/util/clcs_control_util.py 
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
def extend_kb_reference_trajectory_lane_following(
    x_ref: TrajectoryInterface,
    u_ref: TrajectoryInterface,
    lanelet_network: LaneletNetwork,
    vehicle_params: VehicleParameters,
    delta_t: float,
    horizon: int,
) -> Tuple[CurvilinearCoordinateSystem, TrajectoryInterface, TrajectoryInterface]:
    """
    Extends kinematic bicycle trajectory using the CommonRoad reference path planner to account for
    extended MPC horizons, or controllers with look-ahead etc.
    :param x_ref: State trajectory
    :param u_ref: Input trajectory
    :param lanelet_network: CommonRoad lanelet network
    :param vehicle_params: Vehicle parameters object
    :param delta_t: sampling time in seconds - float
    :param horizon: time horizon for extension (number of time steps) - int
    :return: Curvilinear coordinate system, extended state trajectory, extended input trajectory
    """
    # positional trajectory
    positional_trajectory = np.asarray([[state.position_x, state.position_y] for state in x_ref.points.values()])

    # extend trajectory
    (
        clcs_traj_ext,
        position_xy,
        velocity,
        acceleration,
        heading,
        yaw_rate,
        steering_angle,
        steering_angle_velocity,
    ) = extend_reference_trajectory_lane_following(
        positional_trajectory=positional_trajectory,
        lanelet_network=lanelet_network,
        final_state=x_ref.final_point,
        horizon=horizon,
        delta_t=delta_t,
        l_wb=vehicle_params.l_wb,
    )

    # append states
    for kk in range(horizon):
        x_ref.append_point(
            KBState(
                position_x=position_xy[kk][0],
                position_y=position_xy[kk][1],
                velocity=velocity[kk],
                heading=heading[kk],
                steering_angle=steering_angle[kk],
            )
        )

    # append control inputs
    for kk in range(horizon):
        u_ref.append_point(
            KBInput(
                acceleration=acceleration[kk],
                steering_angle_velocity=steering_angle_velocity[kk],
            )
        )

    return clcs_traj_ext, x_ref, u_ref

extend_ref_path_with_route_planner(positional_trajectory, lanelet_network, final_state_time_step=0, planning_problem_id=30000)

Extends the positional information of the trajectory using the CommonRoad route planner.

Parameters:

Name Type Description Default
positional_trajectory ndarray

(n,2) positional trajectory

 required
lanelet_network LaneletNetwork

CommonRoad lanelet network object.

 required
planning_problem_id int

Id of current planning problem

 30000

Returns:

Type Description
ndarray

(n,2) positional trajectory of extended reference path
Source code in commonroad_control/util/clcs_control_util.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
def extend_ref_path_with_route_planner(
    positional_trajectory: np.ndarray,
    lanelet_network: LaneletNetwork,
    final_state_time_step: int = 0,
    planning_problem_id: int = 30000,
) -> np.ndarray:
    """
    Extends the positional information of the trajectory using the CommonRoad route planner.
    :param positional_trajectory: (n,2) positional trajectory
    :param lanelet_network: CommonRoad lanelet network object.
    :param planning_problem_id: Id of current planning problem
    :return: (n,2) positional trajectory of extended reference path
    """

    initial_state: InitialState = InitialState(
        position=np.asarray(positional_trajectory[-1]),
        orientation=0.0,
        velocity=0.0,
        yaw_rate=0.0,
        acceleration=0.0,
        slip_angle=0.0,
        time_step=final_state_time_step,
    )

    goal_region: GoalRegion = GoalRegion(state_list=list())

    planning_problem: PlanningProblem = PlanningProblem(
        planning_problem_id=planning_problem_id,
        initial_state=initial_state,
        goal_region=goal_region,
    )

    reference_path: ReferencePath = generate_reference_path_from_lanelet_network_and_planning_problem(
        planning_problem=planning_problem, lanelet_network=lanelet_network
    )

    kd_tree: KDTree = KDTree(reference_path.reference_path)
    _, idx = kd_tree.query(positional_trajectory[-1])

    clcs_line: np.ndarray = np.concatenate(
        (
            positional_trajectory,
            reference_path.reference_path[min(reference_path.reference_path.shape[0] - 1, idx + 1) :],
        )
    )

    return clcs_line

extend_reference_trajectory_lane_following(positional_trajectory, lanelet_network, final_state, horizon, delta_t, l_wb)

Extends planned trajectory using the CommonRoad reference path planner to account for extended MPC horizons, controller overshoot etc.

Parameters:

Name Type Description Default
positional_trajectory ndarray

(n,2) positional planner trajectory

 required
lanelet_network LaneletNetwork

CommonRoad lanelet network

 required
final_state Union[StateInterface, KBState, DBState, Any]

Final planner trajectory state

 required
horizon int

time horizon in steps

 required
delta_t float

time step size in seconds

 required
l_wb float

wheelbase length

 required

Returns:

Type Description
Tuple[CurvilinearCoordinateSystem, List[ndarray], List[ndarray], List[float], List[float], List[float], List[float]]

Curvilinear coordinate system object, (n,2) positions in cartesian coordinates, acceleration, heading, yaw_rate, steering angle, steering angle velocity,
Source code in commonroad_control/util/clcs_control_util.py 
 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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
def extend_reference_trajectory_lane_following(
    positional_trajectory: np.ndarray,
    lanelet_network: LaneletNetwork,
    final_state: Union[StateInterface, KBState, DBState, Any],
    horizon: int,
    delta_t: float,
    l_wb: float,
) -> Tuple[
    CurvilinearCoordinateSystem,
    List[np.ndarray],
    List[np.ndarray],
    List[float],
    List[float],
    List[float],
    List[float],
]:
    """
    Extends planned trajectory using the CommonRoad reference path planner to account for
    extended MPC horizons, controller overshoot etc.
    :param positional_trajectory: (n,2) positional planner trajectory
    :param lanelet_network: CommonRoad lanelet network
    :param final_state: Final planner trajectory state
    :param horizon: time horizon in steps
    :param delta_t: time step size in seconds
    :param l_wb: wheelbase length
    :return: Curvilinear coordinate system object, (n,2) positions in cartesian coordinates, acceleration, heading, yaw_rate, steering angle, steering angle velocity,
    """
    # extend path with route planner
    clcs_line = extend_ref_path_with_route_planner(positional_trajectory, lanelet_network)

    # convert to curvilinear coordinate system
    clcs_traj_ext: CurvilinearCoordinateSystem = CurvilinearCoordinateSystem(
        reference_path=clcs_line, params=CLCSParams()
    )

    # sample states along path using velocity of final state
    v_ref = final_state.velocity
    position_s_0, _ = clcs_traj_ext.convert_to_curvilinear_coords(x=final_state.position_x, y=final_state.position_y)
    position_xy = []
    velocity = [v_ref for _ in range(horizon)]
    heading = []
    yaw_rate = []
    steering_angle = []
    acceleration = [0.0 for _ in range(horizon)]
    steering_angle_velocity = []
    if hasattr(final_state, "heading"):
        heading_0 = final_state.heading
    elif hasattr(final_state, "velocity_y") and hasattr(final_state, "velocity_x"):
        heading_0 = atan2(final_state.velocity_y, final_state.velocity_x)
    else:
        logger.error("Could not compute heading at the final state of the reference trajectory!")
        raise Exception("Could not compute heading at the final state of the reference trajectory!")

    if hasattr(final_state, "steering_angle"):
        steering_angle_0 = final_state.steering_angle
    else:
        steering_angle_0: float = 0.0

    for kk in range(horizon):
        position_s = position_s_0 + (kk + 1) * delta_t * v_ref

        # convert position to Cartesian coordinates
        position_xy.append(clcs_traj_ext.convert_to_cartesian_coords(s=position_s, l=0.0))

        # orientation of reference trajectory at
        tangent = clcs_traj_ext.tangent(position_s)
        # ... unwrap heading to ensure continuity (avoid discontinuity)
        tmp_heading = atan2(tangent[1], tangent[0])
        heading.append(unwrap_angle(alpha_prev=heading_0, alpha_next=tmp_heading))

        # compute yaw rate
        yaw_rate.append(float((heading[kk] - heading_0) / delta_t))
        heading_0: float = heading[kk]

        # compute steering angle
        steering_angle.append(float(atan2(yaw_rate[kk] * l_wb, v_ref)))

        # compute steering angle velocity
        steering_angle_velocity.append(float((steering_angle[kk] - steering_angle_0) / delta_t))
        steering_angle_0 = steering_angle[kk]

    return (
        clcs_traj_ext,
        position_xy,
        velocity,
        acceleration,
        heading,
        yaw_rate,
        steering_angle,
        steering_angle_velocity,
    )

commonroad_control.util.planner_execution_util.reactive_planner_exec_util

run_reactive_planner(scenario, scenario_xml_file_name, planning_problem, planning_problem_set, reactive_planner_config_path, logging_level='ERROR', show_planner_debug_plots=False, maximum_iterations=200, evaluate_planner=False)

Util wrapper to easily run the reactive planner

Parameters:

Name Type Description Default
scenario Scenario

CommonRoad scenario object

 required
scenario_xml_file_name str

e.g. ZAM-1_1.xml

 required
planning_problem PlanningProblem

CommonRoad planning problem object

 required
planning_problem_set PlanningProblemSet

planning problem set orbject

 required
reactive_planner_config_path Union[str, Path]

path to reactive planner config

 required
logging_level str

logging level as string (see reactive planner documentation)

 'ERROR'
show_planner_debug_plots bool

if true shows debug plots

 False
maximum_iterations int

maximum planner iterations to solve a problem before raising an exception

 200
evaluate_planner bool

if true planner output is evaluated and plotted

 False

Returns:

Type Description
Tuple[List[ReactivePlannerState], List[InputState]]

Tuple[list of reactive planner states, list of reactive planner inputs]
Source code in commonroad_control/util/planner_execution_util/reactive_planner_exec_util.py 
 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
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
def run_reactive_planner(
    scenario: Scenario,
    scenario_xml_file_name: str,
    planning_problem: PlanningProblem,
    planning_problem_set: PlanningProblemSet,
    reactive_planner_config_path: Union[str, Path],
    logging_level: str = "ERROR",
    show_planner_debug_plots: bool = False,
    maximum_iterations: int = 200,
    evaluate_planner: bool = False,
) -> Tuple[List[ReactivePlannerState], List[InputState]]:
    """
    Util wrapper to easily run the reactive planner
    :param scenario: CommonRoad scenario object
    :param scenario_xml_file_name: e.g. ZAM-1_1.xml
    :param planning_problem: CommonRoad planning problem object
    :param planning_problem_set: planning problem set orbject
    :param reactive_planner_config_path: path to reactive planner config
    :param logging_level: logging level as string (see reactive planner documentation)
    :param show_planner_debug_plots: if true shows debug plots
    :param maximum_iterations: maximum planner iterations to solve a problem before raising an exception
    :param evaluate_planner: if true planner output is evaluated and plotted
    :return: Tuple[list of reactive planner states, list of reactive planner inputs]
    """

    config = ReactivePlannerConfiguration.load(reactive_planner_config_path, scenario_xml_file_name)
    config.update(scenario=scenario, planning_problem=planning_problem)
    config.planning_problem_set = planning_problem_set
    config.debug.logging_level = logging_level
    config.debug.show_plots = show_planner_debug_plots

    # initialize and get logger
    rp_logger = initialize_logger(config)
    rp_logger.setLevel(level=logger.getEffectiveLevel())

    ref_path_orig = create_initial_ref_path(config.scenario.lanelet_network, config.planning_problem)
    rp_cosys: CoordinateSystem = create_coordinate_system(ref_path_orig)

    # initialize reactive planner
    planner = ReactivePlanner(config)
    planner.set_reference_path(coordinate_system=rp_cosys)

    # Run Planning
    planner.record_state_and_input(planner.x_0)

    SAMPLING_ITERATION_IN_PLANNER = True

    cnt: int = 0

    while not planner.goal_reached() and cnt < maximum_iterations:
        current_count = len(planner.record_state_list) - 1

        # check if planning cycle or not
        plan_new_trajectory = current_count % config.planning.replanning_frequency == 0
        if plan_new_trajectory:
            # new planning cycle -> plan a new optimal trajectory
            planner.set_desired_velocity(current_speed=planner.x_0.velocity)
            if SAMPLING_ITERATION_IN_PLANNER:
                optimal = planner.plan()
            else:
                optimal = None
                i = 1
                while optimal is None and i <= planner.sampling_level:
                    optimal = planner.plan(i)
                    i += 1

            if not optimal:
                break

            # record state and input
            planner.record_state_and_input(optimal[0].state_list[1])

            # reset planner state for re-planning
            planner.reset(
                initial_state_cart=planner.record_state_list[-1],
                initial_state_curv=(optimal[1][1], optimal[2][1]),
                collision_checker=planner.collision_checker,
                coordinate_system=planner.coordinate_system,
            )

        else:
            # continue on optimal trajectory
            temp = current_count % config.planning.replanning_frequency

            # record state and input
            planner.record_state_and_input(optimal[0].state_list[1 + temp])

            # reset planner state for re-planning
            planner.reset(
                initial_state_cart=planner.record_state_list[-1],
                initial_state_curv=(optimal[1][1 + temp], optimal[2][1 + temp]),
                collision_checker=planner.collision_checker,
                coordinate_system=planner.coordinate_system,
            )

    if cnt >= maximum_iterations - 1:
        logger.error(f"Reactive planner exceeded maximum number of iterations {maximum_iterations}")
        raise Exception(f"Reactive planner exceeded maximum number of iterations {maximum_iterations}")

    # Evaluate results
    if evaluate_planner:
        _, _ = run_evaluation(planner.config, planner.record_state_list, planner.record_input_list)

    # Move input up one time step so that the idx of the input correspond the state to which it is applied to to come
    # into the next state
    input_list: List[InputState] = planner.record_input_list[1:]
    for el in input_list:
        el.time_step = el.time_step - 1

    if planner.goal_reached():
        logger.debug("Reactive planner reached goal")

    return planner.record_state_list, input_list