Skip to content

Race Line

RaceLine dataclass

A cr raceline object containing information on the generated raceline.

Parameters:

Name Type Description Default
points ndarray

(n,2) x,y points

 required
length_per_point ndarray

(n,) arclength (accumulated) per point

 required
velocity_long_per_point ndarray

(n,) velocity per point

 required
acceleration_long_per_point ndarray

(n,) acceleration per point

 required
curvature_per_point ndarray

(n,) curvature per point

 required
num_points Union[int, None]

number of points

 None
closed bool

true, if racetrack is closed

 False
sanity Union[bool, None]

true, if dimensions make sense

 None
Source code in commonroad_raceline_planner/raceline.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
 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
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
@dataclass
class RaceLine:
    """
    A cr raceline object containing information on the generated raceline.

    :param points: (n,2) x,y points
    :param length_per_point: (n,) arclength (accumulated) per point
    :param velocity_long_per_point: (n,) velocity per point
    :param acceleration_long_per_point: (n,) acceleration per point
    :param curvature_per_point: (n,) curvature per point
    :param num_points: number of points
    :param closed: true, if racetrack is closed
    :param sanity: true, if dimensions make sense
    """

    points: np.ndarray
    length_per_point: np.ndarray
    velocity_long_per_point: np.ndarray
    acceleration_long_per_point: np.ndarray
    curvature_per_point: np.ndarray
    heading_per_point: np.ndarray
    num_points: Union[int, None] = None
    sanity: Union[bool, None] = None
    closed: bool = False

    def __post_init__(self):
        self.num_points = self.points.shape[0]
        self.sanity = self.sanity_check()

    def to_7d_np_array(self) -> np.ndarray:
        """
        Convert raceline to np.ndarray
        :return: (num_point, 7) ndarray.
        np.ndarray(
        length_per_point, points, heading_per_point, curvature_per_point, velocity_long_per_point, acceleration_long_per_point
        )
        """
        return np.column_stack(
            (
                self.length_per_point,
                self.points,
                self.heading_per_point,
                self.curvature_per_point,
                self.velocity_long_per_point,
                self.acceleration_long_per_point,
            )
        )

    def get_closest_idx(self, point: np.ndarray) -> int:
        """
        Get idx of closest point on raceline
        :param point: (2,) numpy array
        :return: index
        """
        kd_tree: KDTree = KDTree(self.points)
        _, idx = kd_tree.query(point)
        return idx

    def get_closest_point(self, point: np.ndarray) -> Tuple[int, np.ndarray]:
        """
        Get idx and coords of closest point on raceline
        :param point: closest point on raceline
        :return: Tuple (idx, coordinates)
        """
        idx: int = self.get_closest_idx(point)
        return (idx, self.points[idx])

    def get_velocity_at_position_with_lookahead(
        self, position: np.ndarray, lookahead_s: float = 2.0
    ) -> float:
        """
        Get velocity at position. Uses closest point of raceline
        :param position: (2,) position array
        :return: velocity
        """
        idx_0: int = self.get_closest_idx(position)
        v_0: float = self.velocity_long_per_point[idx_0]
        s_0: float = self.length_per_point[idx_0]

        for idx in range(self.velocity_long_per_point[idx_0:].shape[0]):
            idx_1 = idx_0 + idx
            v_1: float = self.velocity_long_per_point[idx_1]
            s_1: np.ndarray = self.length_per_point[idx_1]

            delta_s = s_1 - s_0
            delta_v = v_0 + (v_1 - v_0) / 2

            if delta_v == 0:
                delta_t: float = delta_s / v_0
            else:
                delta_t: float = (s_1 - s_0) / (v_0 + (v_1 - v_0) / 2)

            if delta_t >= lookahead_s:
                break

        return self.velocity_long_per_point[idx_1]

    def sanity_check(self) -> bool:
        """
        Sanity check fo racline
        :return: returns false if certain parameters are wrong
        """
        sanity: bool = True
        if (
            not self.points.shape[0]
            == self.length_per_point.shape[0]
            == self.velocity_long_per_point.shape[0]
            == self.acceleration_long_per_point.shape[0]
            == self.curvature_per_point.shape[0]
            == self.heading_per_point.shape[0]
            == self.num_points
        ):
            warnings.warn(
                f"raceline has mismatching length of data: \n "
                f"points={self.points.shape[0]}  \n"
                f"num_length_per_point={self.length_per_point.shape[0]}  \n"
                f"num_acceleration_long_per_point={self.acceleration_long_per_point.shape[0]}  \n"
                f"num_curvature_per_point={self.curvature_per_point.shape[0]}  \n"
                f"num_heading_per_point={self.heading_per_point.shape[0]}  \n"
                f"num_points={self.num_points}  \n"
            )
            sanity = False

        return sanity

    def close_raceline(self) -> None:
        """
        Closes raceline by adding the first point to the end
        """
        self.points = np.hstack((self.points, self.points[0]))
        self.length_per_point = np.hstack(
            (self.length_per_point, self.length_per_point[0])
        )
        self.velocity_long_per_point = np.hstack(
            (self.velocity_long_per_point, self.velocity_long_per_point[0])
        )
        self.acceleration_long_per_point = np.hstack(
            (self.acceleration_long_per_point, self.acceleration_long_per_point[0])
        )
        self.curvature_per_point = np.hstack(
            (self.curvature_per_point, self.curvature_per_point[0])
        )
        self.heading_per_point = np.hstack(
            (self.curvature_per_point, self.curvature_per_point[0])
        )
        self.num_points = self.points.shape[0]
        self.sanity = self.sanity_check()

    def export_trajectory_to_csv_file(
        self, export_path: Union[Path, str], ggv_file_path: Union[Path, str]
    ) -> None:
        """
        Export trajectory to csv file.
        :param export_path: path to which the trajectory should be safed as csv
        :param ggv_file_path: ggv file path
        """
        export_traj_race(
            traj_race_export=export_path,
            ggv_file=ggv_file_path,
            traj_race=self.to_7d_np_array(),
        )
        print(f"Exported trajectory to {export_path}")

close_raceline()

Closes raceline by adding the first point to the end

Source code in commonroad_raceline_planner/raceline.py 
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
def close_raceline(self) -> None:
    """
    Closes raceline by adding the first point to the end
    """
    self.points = np.hstack((self.points, self.points[0]))
    self.length_per_point = np.hstack(
        (self.length_per_point, self.length_per_point[0])
    )
    self.velocity_long_per_point = np.hstack(
        (self.velocity_long_per_point, self.velocity_long_per_point[0])
    )
    self.acceleration_long_per_point = np.hstack(
        (self.acceleration_long_per_point, self.acceleration_long_per_point[0])
    )
    self.curvature_per_point = np.hstack(
        (self.curvature_per_point, self.curvature_per_point[0])
    )
    self.heading_per_point = np.hstack(
        (self.curvature_per_point, self.curvature_per_point[0])
    )
    self.num_points = self.points.shape[0]
    self.sanity = self.sanity_check()

export_trajectory_to_csv_file(export_path, ggv_file_path)

Export trajectory to csv file.

Parameters:

Name Type Description Default
export_path Union[Path, str]

path to which the trajectory should be safed as csv

 required
ggv_file_path Union[Path, str]

ggv file path

 required
Source code in commonroad_raceline_planner/raceline.py 
163
164
165
166
167
168
169
170
171
172
173
174
175
176
def export_trajectory_to_csv_file(
    self, export_path: Union[Path, str], ggv_file_path: Union[Path, str]
) -> None:
    """
    Export trajectory to csv file.
    :param export_path: path to which the trajectory should be safed as csv
    :param ggv_file_path: ggv file path
    """
    export_traj_race(
        traj_race_export=export_path,
        ggv_file=ggv_file_path,
        traj_race=self.to_7d_np_array(),
    )
    print(f"Exported trajectory to {export_path}")

get_closest_idx(point)

Get idx of closest point on raceline

Parameters:

Name Type Description Default
point ndarray

(2,) numpy array

 required

Returns:

Type Description
int

index
Source code in commonroad_raceline_planner/raceline.py 
63
64
65
66
67
68
69
70
71
def get_closest_idx(self, point: np.ndarray) -> int:
    """
    Get idx of closest point on raceline
    :param point: (2,) numpy array
    :return: index
    """
    kd_tree: KDTree = KDTree(self.points)
    _, idx = kd_tree.query(point)
    return idx

get_closest_point(point)

Get idx and coords of closest point on raceline

Parameters:

Name Type Description Default
point ndarray

closest point on raceline

 required

Returns:

Type Description
Tuple[int, ndarray]

Tuple (idx, coordinates)
Source code in commonroad_raceline_planner/raceline.py 
73
74
75
76
77
78
79
80
def get_closest_point(self, point: np.ndarray) -> Tuple[int, np.ndarray]:
    """
    Get idx and coords of closest point on raceline
    :param point: closest point on raceline
    :return: Tuple (idx, coordinates)
    """
    idx: int = self.get_closest_idx(point)
    return (idx, self.points[idx])

get_velocity_at_position_with_lookahead(position, lookahead_s=2.0)

Get velocity at position. Uses closest point of raceline

Parameters:

Name Type Description Default
position ndarray

(2,) position array

 required

Returns:

Type Description
float

velocity
Source code in commonroad_raceline_planner/raceline.py 
 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 get_velocity_at_position_with_lookahead(
    self, position: np.ndarray, lookahead_s: float = 2.0
) -> float:
    """
    Get velocity at position. Uses closest point of raceline
    :param position: (2,) position array
    :return: velocity
    """
    idx_0: int = self.get_closest_idx(position)
    v_0: float = self.velocity_long_per_point[idx_0]
    s_0: float = self.length_per_point[idx_0]

    for idx in range(self.velocity_long_per_point[idx_0:].shape[0]):
        idx_1 = idx_0 + idx
        v_1: float = self.velocity_long_per_point[idx_1]
        s_1: np.ndarray = self.length_per_point[idx_1]

        delta_s = s_1 - s_0
        delta_v = v_0 + (v_1 - v_0) / 2

        if delta_v == 0:
            delta_t: float = delta_s / v_0
        else:
            delta_t: float = (s_1 - s_0) / (v_0 + (v_1 - v_0) / 2)

        if delta_t >= lookahead_s:
            break

    return self.velocity_long_per_point[idx_1]

sanity_check()

Sanity check fo racline

Returns:

Type Description
bool

returns false if certain parameters are wrong
Source code in commonroad_raceline_planner/raceline.py 
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 sanity_check(self) -> bool:
    """
    Sanity check fo racline
    :return: returns false if certain parameters are wrong
    """
    sanity: bool = True
    if (
        not self.points.shape[0]
        == self.length_per_point.shape[0]
        == self.velocity_long_per_point.shape[0]
        == self.acceleration_long_per_point.shape[0]
        == self.curvature_per_point.shape[0]
        == self.heading_per_point.shape[0]
        == self.num_points
    ):
        warnings.warn(
            f"raceline has mismatching length of data: \n "
            f"points={self.points.shape[0]}  \n"
            f"num_length_per_point={self.length_per_point.shape[0]}  \n"
            f"num_acceleration_long_per_point={self.acceleration_long_per_point.shape[0]}  \n"
            f"num_curvature_per_point={self.curvature_per_point.shape[0]}  \n"
            f"num_heading_per_point={self.heading_per_point.shape[0]}  \n"
            f"num_points={self.num_points}  \n"
        )
        sanity = False

    return sanity

to_7d_np_array()

Convert raceline to np.ndarray

Returns:

Type Description
ndarray

(num_point, 7) ndarray. np.ndarray( length_per_point, points, heading_per_point, curvature_per_point, velocity_long_per_point, acceleration_long_per_point )
Source code in commonroad_raceline_planner/raceline.py 
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
def to_7d_np_array(self) -> np.ndarray:
    """
    Convert raceline to np.ndarray
    :return: (num_point, 7) ndarray.
    np.ndarray(
    length_per_point, points, heading_per_point, curvature_per_point, velocity_long_per_point, acceleration_long_per_point
    )
    """
    return np.column_stack(
        (
            self.length_per_point,
            self.points,
            self.heading_per_point,
            self.curvature_per_point,
            self.velocity_long_per_point,
            self.acceleration_long_per_point,
        )
    )

RaceLineFactory

Generates raceline

Source code in commonroad_raceline_planner/raceline.py 
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
class RaceLineFactory:
    """
    Generates raceline
    """

    @staticmethod
    def generate_raceline(
        points: np.ndarray,
        length_per_point: np.ndarray,
        velocity_long_per_point: np.ndarray,
        acceleration_long_per_point: np.ndarray,
        curvature_per_point: np.ndarray,
        heading_per_point: np.ndarray,
        closed: bool,
    ) -> RaceLine:
        """
        Generates race line
        :param points: (n,2) x,y points
        :param length_per_point: arc length per point (accumulated)
        :param velocity_long_per_point: velocity per point
        :param acceleration_long_per_point: acceleration per point
        :param curvature_per_point: curvature per point
        :param heading_per_point: heading per point
        :return: cr raceline
        """
        return RaceLine(
            points=points,
            length_per_point=length_per_point,
            velocity_long_per_point=velocity_long_per_point,
            acceleration_long_per_point=acceleration_long_per_point,
            curvature_per_point=curvature_per_point,
            heading_per_point=heading_per_point,
            closed=closed,
        )

generate_raceline(points, length_per_point, velocity_long_per_point, acceleration_long_per_point, curvature_per_point, heading_per_point, closed) staticmethod

Generates race line

Parameters:

Name Type Description Default
points ndarray

(n,2) x,y points

 required
length_per_point ndarray

arc length per point (accumulated)

 required
velocity_long_per_point ndarray

velocity per point

 required
acceleration_long_per_point ndarray

acceleration per point

 required
curvature_per_point ndarray

curvature per point

 required
heading_per_point ndarray

heading per point

 required

Returns:

Type Description
RaceLine

cr raceline
Source code in commonroad_raceline_planner/raceline.py 
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
@staticmethod
def generate_raceline(
    points: np.ndarray,
    length_per_point: np.ndarray,
    velocity_long_per_point: np.ndarray,
    acceleration_long_per_point: np.ndarray,
    curvature_per_point: np.ndarray,
    heading_per_point: np.ndarray,
    closed: bool,
) -> RaceLine:
    """
    Generates race line
    :param points: (n,2) x,y points
    :param length_per_point: arc length per point (accumulated)
    :param velocity_long_per_point: velocity per point
    :param acceleration_long_per_point: acceleration per point
    :param curvature_per_point: curvature per point
    :param heading_per_point: heading per point
    :return: cr raceline
    """
    return RaceLine(
        points=points,
        length_per_point=length_per_point,
        velocity_long_per_point=velocity_long_per_point,
        acceleration_long_per_point=acceleration_long_per_point,
        curvature_per_point=curvature_per_point,
        heading_per_point=heading_per_point,
        closed=closed,
    )