Skip to content

Commit fef2a45

Browse files
fangedwardfangedward
authoredMar 9, 2020
C++ Port of Frenet Optimal Trajectory (#1)
1 parent 90cfae5 commit fef2a45

25 files changed

+1168
-1
lines changed
 

‎.gitignore

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
__pycache__/
2+
build/
3+
venv/
4+
.idea/
5+
cmake-build-debug/

‎CMakeLists.txt

+47
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
cmake_minimum_required(VERSION 3.15)
2+
project(FrenetOptimalTrajectory)
3+
4+
set(CMAKE_CXX_STANDARD 17)
5+
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
6+
include_directories(Eigen3)
7+
include_directories("${CMAKE_SOURCE_DIR}/src"
8+
"${CMAKE_SOURCE_DIR}/src/CubicSpline"
9+
"${CMAKE_SOURCE_DIR}/src/Polynomials"
10+
"${CMAKE_SOURCE_DIR}/src/FrenetOptimalTrajectory")
11+
12+
add_library(FrenetOptimalTrajectory SHARED
13+
src/main.cpp
14+
src/utils.h
15+
src/constants.h
16+
src/Polynomials/QuarticPolynomial.cpp
17+
src/Polynomials/QuarticPolynomial.h
18+
src/Polynomials/QuinticPolynomial.cpp
19+
src/Polynomials/QuinticPolynomial.h
20+
src/CubicSpline/CubicSpline1D.cpp
21+
src/CubicSpline/CubicSpline1D.h
22+
src/CubicSpline/CubicSpline2D.cpp
23+
src/CubicSpline/CubicSpline2D.h
24+
src/FrenetOptimalTrajectory/FrenetOptimalTrajectory.cpp
25+
src/FrenetOptimalTrajectory/FrenetOptimalTrajectory.h
26+
src/FrenetOptimalTrajectory/FrenetPath.cpp
27+
src/FrenetOptimalTrajectory/FrenetPath.h
28+
src/FrenetOptimalTrajectory/fot_wrapper.cpp)
29+
add_executable(FrenetOptimalTrajectoryTest
30+
src/main.cpp
31+
src/utils.h
32+
src/constants.h
33+
src/Polynomials/QuarticPolynomial.cpp
34+
src/Polynomials/QuarticPolynomial.h
35+
src/Polynomials/QuinticPolynomial.cpp
36+
src/Polynomials/QuinticPolynomial.h
37+
src/CubicSpline/CubicSpline1D.cpp
38+
src/CubicSpline/CubicSpline1D.h
39+
src/CubicSpline/CubicSpline2D.cpp
40+
src/CubicSpline/CubicSpline2D.h
41+
src/FrenetOptimalTrajectory/FrenetOptimalTrajectory.cpp
42+
src/FrenetOptimalTrajectory/FrenetOptimalTrajectory.h
43+
src/FrenetOptimalTrajectory/FrenetPath.cpp
44+
src/FrenetOptimalTrajectory/FrenetPath.h
45+
src/FrenetOptimalTrajectory/fot_wrapper.cpp)
46+
target_link_libraries(FrenetOptimalTrajectory Eigen3::Eigen)
47+
target_link_libraries(FrenetOptimalTrajectoryTest Eigen3::Eigen)

‎FrenetOptimalTrajectory/__init__.py

Whitespace-only changes.

‎FrenetOptimalTrajectory/fot.py

+108
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
import fot_wrapper
2+
import time
3+
import numpy as np
4+
import matplotlib.pyplot as plt
5+
6+
7+
def main():
8+
"""A debug script for the frenet optimal trajectory planner.
9+
10+
This script will solve the frenet optimal trajectory problem in a
11+
standalone simulation and visualize the results or raise an error if a
12+
path is not found.
13+
14+
To run, replace `conds = {}` with the corresponding output from the pylot
15+
debug logs. The output will be from the `fot_planning_operator` and it will
16+
look like `initial_conditions = {...}`.
17+
"""
18+
print(__file__ + " start!!")
19+
sim_loop = 40
20+
area = 40.0 # animation area length [m]
21+
show_animation = True
22+
23+
conds = {'s0': 12.59999999999999,
24+
'c_speed': 7.10126796146418,
25+
'c_d': 0.10761995326139745,
26+
'c_d_d': 0.009671559894548877,
27+
'c_d_dd': 0.0,
28+
'wx': [132.67, 128.67, 124.67, 120.67, 116.67, 112.67, 108.67,
29+
104.67, 101.43, 97.77, 94.84, 92.89, 92.4 , 92.4 ,
30+
92.4 , 92.4 , 92.4 , 92.4 , 92.4 , 92.39, 92.39,
31+
92.39, 92.39, 92.39, 92.39],
32+
'wy': [195.14, 195.14, 195.14, 195.14, 195.14, 195.14, 195.14,
33+
195.14, 195.14, 195.03, 193.88, 191.75, 188.72, 185.32,
34+
181.32, 177.32, 173.32, 169.32, 165.32, 161.32, 157.32,
35+
153.32, 149.32, 145.32, 141.84],
36+
'obstacle_list': [[92.89, 191.75]],
37+
'x': 120.06613159179688,
38+
'y': 195.03477478027344,
39+
'vx': -7.101262092590332,
40+
'vy': 0.009129776619374752} # paste output from debug log
41+
42+
# way points
43+
wx = np.array(conds['wx'])
44+
wy = np.array(conds['wy'])
45+
46+
# initial conditions
47+
x = conds['x']
48+
y = conds['y']
49+
50+
# obstacle lists
51+
obs = np.array(conds['obstacle_list'])
52+
53+
# initial state
54+
c_speed = conds['c_speed'] # current speed [m/s]
55+
c_d = conds['c_d'] # current lateral position [m]
56+
c_d_d = conds['c_d_d'] # current lateral speed [m/s]
57+
c_d_dd = conds['c_d_dd'] # current latral acceleration [m/s]
58+
s0 = conds['s0'] # current course position
59+
total_time = 0
60+
for i in range(sim_loop):
61+
print("Iteration: {}".format(i))
62+
start_time = time.time()
63+
result_x, result_y, _, params, success = \
64+
fot_wrapper.get_fot_frenet_space(s0, c_speed, c_d, c_d_d, c_d_dd,
65+
wx, wy, obs, 10)
66+
end_time = time.time() - start_time
67+
print("Time taken: {}".format(end_time))
68+
total_time += end_time
69+
s0 = params['s0']
70+
c_d = params['c_d']
71+
c_d_d = params['c_d_d']
72+
c_d_dd = params['c_d_dd']
73+
c_speed = params['c_speed']
74+
if np.hypot(result_x[1] - wx[-1], result_y[1] - wy[-1]) <= 1.0:
75+
print("Goal")
76+
break
77+
78+
if show_animation: # pragma: no cover
79+
plt.cla()
80+
# for stopping simulation with the esc key.
81+
plt.gcf().canvas.mpl_connect(
82+
'key_release_event',
83+
lambda event: [exit(0) if event.key == 'escape' else None]
84+
)
85+
plt.plot(wx, wy)
86+
if obs.shape[0] == 0:
87+
obs = np.empty((0, 2))
88+
plt.scatter(obs[:, 0], obs[:, 1], marker='o', s=(3*6)**2)
89+
plt.plot(result_x[1:], result_y[1:], "-or")
90+
plt.plot(result_x[1], result_y[1], "vc")
91+
plt.xlim(result_x[1] - area, result_x[1] + area)
92+
plt.ylim(result_y[1] - area, result_y[1] + area)
93+
plt.xlabel("X axis")
94+
plt.ylabel("Y axis")
95+
plt.title("v[m/s]:" + str(c_speed)[0:4])
96+
plt.grid(True)
97+
plt.pause(0.1)
98+
99+
print("Finish")
100+
print("Average time per iteration: {}".format(total_time / i))
101+
if show_animation: # pragma: no cover
102+
plt.grid(True)
103+
plt.pause(0.1)
104+
plt.show()
105+
106+
107+
if __name__ == '__main__':
108+
main()

‎FrenetOptimalTrajectory/fot_wrapper.py

+110
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
import ctypes
2+
import numpy as np
3+
from ctypes import c_double, c_int
4+
5+
cdll = ctypes.CDLL("build/libFrenetOptimalTrajectory.so")
6+
_get_fot_frenet_space = cdll.get_fot_frenet_space
7+
_get_fot_frenet_space.argtypes = (
8+
c_double, c_double, c_double, c_double, c_double, # s0, c_speed, c_d, c_d_d, c_d_dd
9+
ctypes.POINTER(c_double), ctypes.POINTER(c_double), # wx, wy
10+
ctypes.POINTER(c_double), ctypes.POINTER(c_double), # ox, oy
11+
c_int, c_int, c_double, # path length, num obstacles, target speed
12+
ctypes.POINTER(c_double), ctypes.POINTER(c_double), # result x, result y
13+
ctypes.POINTER(c_double), ctypes.POINTER(c_double) # speeds, misc
14+
)
15+
_get_fot_frenet_space.restype = ctypes.c_int
16+
_compute_initial_conditions = cdll.compute_initial_conditions
17+
_compute_initial_conditions.restype = None
18+
_compute_initial_conditions.argtypes = (
19+
c_double, c_double, c_double, c_double, c_double, c_double,
20+
ctypes.POINTER(c_double), ctypes.POINTER(c_double),
21+
c_int, ctypes.POINTER(c_double)
22+
)
23+
24+
25+
def get_fot_frenet_space(s0, c_speed, c_d, c_d_d, c_d_dd, wx, wy, obs,
26+
target_speed):
27+
""" Return the frenet optimal trajectory given initial conditions in
28+
cartesian space.
29+
30+
Args:
31+
s0 (float): initial longitudinal position
32+
c_speed (float): initial forward speed
33+
c_d (float): initial lateral position
34+
c_d_d (float): initial lateral velocity
35+
c_d_dd (float): initial lateral acceleration
36+
wx (list(float)): list of global x waypoints
37+
wy (list(float)): list of global y waypoints
38+
obs (list((float, float))): list of obstacle locations
39+
target_speed (float): target speed
40+
41+
Returns:
42+
result_x (list(float)): x positions of fot, if it exists
43+
result_y (list(float)): y positions of fot, if it exists
44+
speeds (list(float)): velocities of fot, if it exists
45+
params (dict): next frenet coordinates, if they exist
46+
success (bool): whether a fot was found or not
47+
"""
48+
result_x = np.zeros(100)
49+
result_y = np.zeros(100)
50+
speeds = np.zeros(100)
51+
misc = np.zeros(5)
52+
if obs.shape[0] == 0:
53+
obs = np.empty((0, 2))
54+
success = _get_fot_frenet_space(
55+
c_double(s0), c_double(c_speed), c_double(c_d),
56+
c_double(c_d_d), c_double(c_d_dd),
57+
wx.ctypes.data_as(ctypes.POINTER(c_double)),
58+
wy.ctypes.data_as(ctypes.POINTER(c_double)),
59+
obs[:, 0].ctypes.data_as(ctypes.POINTER(c_double)),
60+
obs[:, 1].ctypes.data_as(ctypes.POINTER(c_double)),
61+
c_int(len(wx)), c_int(len(obs)), c_double(target_speed),
62+
result_x.ctypes.data_as(ctypes.POINTER(c_double)),
63+
result_y.ctypes.data_as(ctypes.POINTER(c_double)),
64+
speeds.ctypes.data_as(ctypes.POINTER(c_double)),
65+
misc.ctypes.data_as(ctypes.POINTER(c_double))
66+
)
67+
params = {
68+
"s0": misc[0],
69+
"c_speed": misc[1],
70+
"c_d": misc[2],
71+
"c_d_d": misc[3],
72+
"c_d_dd": misc[4]
73+
}
74+
ind = -1
75+
if success:
76+
ind = np.where(np.isnan(result_x))[0][0]
77+
78+
return result_x[:ind], result_y[:ind], speeds[:ind], params, success
79+
80+
81+
def compute_initial_conditions(s0, x, y, vx, vy, forward_speed, wx, wy):
82+
""" Return the frenet optimal trajectory given initial conditions in
83+
cartesian space.
84+
85+
Args:
86+
s0 (float): previous longitudinal position
87+
x (float): initial x position
88+
y (float): initial y position
89+
vx (float): initial x velocity
90+
vy (float): initial y velocity
91+
forward_speed (float): initial speed
92+
wx (list(float)): list of global x waypoints
93+
wy (list(float)): list of global y waypoints
94+
95+
Returns:
96+
s_c (float): current longitudinal position
97+
c_speed (float): current speed
98+
c_d (float): current lateral offset
99+
c_d_d (float): current lateral velocity
100+
c_d_dd (float): current lateral acceleration
101+
"""
102+
misc = np.zeros(5)
103+
_compute_initial_conditions(
104+
c_double(s0), c_double(x), c_double(y), c_double(vx), c_double(vy),
105+
c_double(forward_speed), wx.ctypes.data_as(ctypes.POINTER(c_double)),
106+
wy.ctypes.data_as(ctypes.POINTER(c_double)), c_int(len(wx)),
107+
misc.ctypes.data_as(ctypes.POINTER(c_double))
108+
)
109+
110+
return misc[0], misc[1], misc[2], misc[3], misc[4]

‎README.md

+34-1
Original file line numberDiff line numberDiff line change
@@ -1 +1,34 @@
1-
# frenet-optimal-trajectory-planner
1+
# Frenet Optimal Trajectory
2+
![FrenetOptimalTrajectory Demo](img/fot.gif)
3+
## Overview
4+
This repository contains a fast, C++ implementation of the Frenet Optimal
5+
Trajectory algorithm with a Python wrapper. It is used as one of the motion planning models in
6+
[pylot](https://github.com/erdos-project/pylot), an [erdos](https://github.com/erdos-project) project.
7+
8+
Reference Papers:
9+
- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
10+
- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame](https://www.youtube.com/watch?v=Cj6tAQe7UCY)
11+
12+
## Profiling
13+
Some basic profiling of the code (same trajectory as demo, 10 obstacles)
14+
indicates the following expected performance:
15+
```
16+
Average Time: ~7 ms
17+
Max Time: ~20 ms
18+
```
19+
20+
## Setup
21+
```
22+
git clone https://github.com/fangedward/frenet-optimal-trajectory-planner.git
23+
./build.sh
24+
```
25+
26+
## Example Usage
27+
There is a Python wrapper and C++ API. The Python wrapper is located in
28+
`FrenetOptimalTrajectory/fot_wrapper.py` and the C++ API is under
29+
`src/FrenetOptimalTrajectory/fot_wrapper.cpp`.
30+
The following command will simulate a simple scenario to run the
31+
FrenetOptimalTrajectory planning algorithm.
32+
```
33+
python3 FrenetOptimalTrajectory/fot.py
34+
```

‎__init__.py

Whitespace-only changes.

‎build.sh

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
#!/bin/bash
2+
sudo apt-get install -y libeigen3-dev clang cmake
3+
if [ ! -d "build" ]; then
4+
mkdir build
5+
fi
6+
cd build
7+
cmake .. -DCMAKE_BUILD_TYPE=Release
8+
cmake --build . --target all -- -j 8

‎img/fot.gif

914 KB
Loading

‎src/CubicSpline/CubicSpline1D.cpp

+104
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
#include "CubicSpline1D.h"
2+
3+
#include <algorithm>
4+
#include <numeric>
5+
#include <cmath>
6+
7+
using namespace std;
8+
using namespace Eigen;
9+
10+
// Default constructor
11+
CubicSpline1D::CubicSpline1D() = default;
12+
13+
// Construct the 1-dimensional cubic spline.
14+
CubicSpline1D::CubicSpline1D(const vector<double>& v1,
15+
const vector<double>& v2):
16+
x(v1), y(v2), a(v2), nx(v1.size()) {
17+
// compute elementwise difference
18+
vector<double> deltas (nx);
19+
adjacent_difference(x.begin(), x.end(), deltas.begin());
20+
deltas.erase(deltas.begin());
21+
22+
// compute matrix a, vector b
23+
MatrixXd ma = MatrixXd::Zero(nx, nx);
24+
VectorXd vb = VectorXd::Zero(nx);
25+
matrix_a(deltas, ma);
26+
vector_b(deltas, vb);
27+
28+
// solve for c and copy to attribute vector
29+
MatrixXd ma_inv = ma.inverse();
30+
VectorXd tmp_c = ma_inv * vb;
31+
c.resize(tmp_c.size());
32+
VectorXd::Map(&c[0], tmp_c.size()) = tmp_c;
33+
34+
// construct attribute b, d
35+
for (int i = 0; i < nx - 1; i++) {
36+
d.push_back((c[i + 1] - c[i]) / (3.0 * deltas[i]));
37+
b.push_back((a[i + 1] - a[i]) / deltas[i] - deltas[i] *
38+
(c[i + 1] + 2.0 * c[i]) / 3.0);
39+
}
40+
}
41+
42+
// Calculate the 0th derivative evaluated at t
43+
double CubicSpline1D::calc_der0(double t) {
44+
if (t < x.front() || t >= x.back()) {
45+
return NAN;
46+
}
47+
48+
int i = search_index(t) - 1;
49+
double dx = t - x[i];
50+
return a[i] + b[i] * dx + c[i] * pow(dx, 2) + d[i] * pow(dx, 3);
51+
}
52+
53+
// Calculate the 1st derivative evaluated at t
54+
double CubicSpline1D::calc_der1(double t) {
55+
if (t < x.front() || t >= x.back()) {
56+
return NAN;
57+
}
58+
59+
int i = search_index(t) - 1;
60+
double dx = t - x[i];
61+
62+
return b[i] + 2.0 * c[i] * dx + 3.0 * d[i] * pow(dx, 2);
63+
}
64+
65+
// Calculate the 2nd derivative evaluated at
66+
double CubicSpline1D::calc_der2(double t) {
67+
if (t < x.front() || t >= x.back()) {
68+
return NAN;
69+
}
70+
71+
int i = search_index(t) - 1;
72+
double dx = t - x[i];
73+
74+
return 2.0 * c[i] + 6.0 * d[i] * dx;
75+
}
76+
77+
// Create the constants matrix a used in spline construction
78+
void CubicSpline1D::matrix_a(vector<double> &deltas, MatrixXd &result) {
79+
result(0, 0) = 1;
80+
for (int i = 0; i < nx - 1; i++) {
81+
if (i != nx - 2) {
82+
result(i + 1, i + 1) = 2.0 * (deltas[i] + deltas[i + 1]);
83+
}
84+
result(i + 1, i) = deltas[i];
85+
result(i, i + 1) = deltas[i];
86+
}
87+
88+
result(0, 1) = 0.0;
89+
result(nx - 1, nx - 2) = 0.0;
90+
result(nx - 1, nx - 1) = 1.0;
91+
}
92+
93+
// Create the 1st derivative vector b used in spline construction
94+
void CubicSpline1D::vector_b(vector<double> &deltas, VectorXd &result) {
95+
for (int i = 0; i < nx - 2; i++) {
96+
result(i + 1) = 3.0 * (a[i + 2] - a[i + 1]) / deltas[i + 1] - 3.0 *
97+
(a[i + 1] - a[i]) / deltas[i];
98+
}
99+
}
100+
101+
// Search the spline for index closest to t
102+
int CubicSpline1D::search_index(double t) {
103+
return std::upper_bound (x.begin(), x.end(), t) - x.begin();
104+
}

‎src/CubicSpline/CubicSpline1D.h

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
#ifndef FRENET_OPTIMAL_TRAJECTORY_CUBICSPLINE1D_H
2+
#define FRENET_OPTIMAL_TRAJECTORY_CUBICSPLINE1D_H
3+
4+
#include <Eigen/LU>
5+
#include <vector>
6+
7+
// 1-dimensional cubic spline class.
8+
// For technical details see: http://mathworld.wolfram.com/CubicSpline.html
9+
class CubicSpline1D {
10+
public:
11+
int nx{};
12+
CubicSpline1D();
13+
CubicSpline1D(const std::vector<double>& v1, const std::vector<double>& v2);
14+
double calc_der0(double t);
15+
double calc_der1(double t);
16+
double calc_der2(double t);
17+
private:
18+
std::vector<double> a, b, c, d, w, x, y;
19+
int search_index(double t);
20+
void matrix_a(std::vector<double>& deltas, Eigen::MatrixXd& result);
21+
void vector_b(std::vector<double>& deltas, Eigen::VectorXd& result);
22+
};
23+
24+
#endif //FRENET_OPTIMAL_TRAJECTORY_CUBICSPLINE1D_H

‎src/CubicSpline/CubicSpline2D.cpp

+87
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
#include "CubicSpline2D.h"
2+
#include "utils.h"
3+
4+
#include <algorithm>
5+
#include <numeric>
6+
#include <cmath>
7+
8+
using namespace std;
9+
10+
// Default constructor
11+
CubicSpline2D::CubicSpline2D() = default;
12+
13+
// Construct the 2-dimensional cubic spline
14+
CubicSpline2D::CubicSpline2D(const std::vector<double> &x,
15+
const std::vector<double> &y) {
16+
calc_s(x, y);
17+
sx = CubicSpline1D(s, x);
18+
sy = CubicSpline1D(s, y);
19+
}
20+
21+
// Calculate the s values for interpolation given x, y
22+
void CubicSpline2D::calc_s(const std::vector<double>& x,
23+
const std::vector<double>& y) {
24+
int nx = x.size();
25+
vector<double> dx (nx);
26+
vector<double> dy (nx);
27+
adjacent_difference(x.begin(), x.end(), dx.begin());
28+
adjacent_difference(y.begin(), y.end(), dy.begin());
29+
dx.erase(dx.begin());
30+
dy.erase(dy.begin());
31+
32+
double cum_sum = 0.0;
33+
s.push_back(cum_sum);
34+
for (int i = 0; i < nx - 1; i++) {
35+
cum_sum += norm(dx[i], dy[i]);
36+
s.push_back(cum_sum);
37+
}
38+
s.erase(unique(s.begin(), s.end()), s.end());
39+
}
40+
41+
// Calculate the x position along the spline at given t
42+
double CubicSpline2D::calc_x(double t) {
43+
return sx.calc_der0(t);
44+
}
45+
46+
// Calculate the y position along the spline at given t
47+
double CubicSpline2D::calc_y(double t) {
48+
return sy.calc_der0(t);
49+
}
50+
51+
// Calculate the curvature along the spline at given t
52+
double CubicSpline2D::calc_curvature(double t){
53+
double dx = sx.calc_der1(t);
54+
double ddx = sx.calc_der2(t);
55+
double dy = sy.calc_der1(t);
56+
double ddy = sy.calc_der2(t);
57+
double k = (ddy * dx - ddx * dy) /
58+
pow(pow(dx, 2) + pow(dy, 2), 1.5);
59+
return k;
60+
}
61+
62+
// Calculate the yaw along the spline at given t
63+
double CubicSpline2D::calc_yaw(double t) {
64+
double dx = sx.calc_der1(t);
65+
double dy = sy.calc_der1(t);
66+
double yaw = atan2(dy, dx);
67+
return yaw;
68+
}
69+
70+
// Given x, y positions and an initial guess s0, find the closest s value
71+
double CubicSpline2D::find_s(double x, double y, double s0) {
72+
double s_closest = s0;
73+
double closest = INFINITY;
74+
double si = s0;
75+
76+
do {
77+
double px = calc_x(si);
78+
double py = calc_y(si);
79+
double dist = norm(x - px, y - py);
80+
if (dist < closest) {
81+
closest = dist;
82+
s_closest = si;
83+
}
84+
si += 0.1;
85+
} while (si < s0 + 10);
86+
return s_closest;
87+
}

‎src/CubicSpline/CubicSpline2D.h

+27
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
#ifndef FRENET_OPTIMAL_TRAJECTORY_CUBICSPLINE2D_H
2+
#define FRENET_OPTIMAL_TRAJECTORY_CUBICSPLINE2D_H
3+
4+
#include "CubicSpline1D.h"
5+
6+
#include <vector>
7+
8+
// 2-dimensional cubic spline class.
9+
// For technical details see: http://mathworld.wolfram.com/CubicSpline.html
10+
class CubicSpline2D {
11+
public:
12+
CubicSpline2D();
13+
CubicSpline2D(const std::vector<double> &x, const std::vector<double> &y);
14+
double calc_x(double t);
15+
double calc_y(double t);
16+
double calc_curvature(double t);
17+
double calc_yaw(double t);
18+
double find_s(double x, double y, double s0);
19+
20+
private:
21+
std::vector<double> s;
22+
CubicSpline1D sx, sy;
23+
void calc_s(const std::vector<double>& x,
24+
const std::vector<double>& y);
25+
};
26+
27+
#endif //FRENET_OPTIMAL_TRAJECTORY_CUBICSPLINE2D_H

‎src/FrenetOptimalTrajectory/FrenetOptimalTrajectory.cpp

+127
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,127 @@
1+
#include "FrenetOptimalTrajectory.h"
2+
#include "QuarticPolynomial.h"
3+
#include "QuinticPolynomial.h"
4+
#include "constants.h"
5+
#include "utils.h"
6+
7+
#include <cmath>
8+
#include <iostream>
9+
#include <utility>
10+
11+
using namespace std;
12+
13+
// Compute the frenet optimal trajectory
14+
FrenetOptimalTrajectory::FrenetOptimalTrajectory(vector<double>& x_,
15+
vector<double>& y_, double s0_, double c_speed_, double c_d_,
16+
double c_d_d_, double c_d_dd_, double target_speed_,
17+
vector<tuple<double, double>>& obstacles_):
18+
x(x_), y(y_), s0(s0_), c_speed(c_speed_), c_d(c_d_), c_d_d(c_d_d_),
19+
c_d_dd(c_d_dd_), target_speed(target_speed_), obstacles(obstacles_) {
20+
csp = new CubicSpline2D(x, y);
21+
22+
// calculate the trajectories
23+
calc_frenet_paths();
24+
25+
// select the best path
26+
double mincost = INFINITY;
27+
for (FrenetPath* fp : frenet_paths) {
28+
if (fp->cf <= mincost) {
29+
mincost = fp->cf;
30+
best_frenet_path = fp;
31+
}
32+
}
33+
}
34+
35+
FrenetOptimalTrajectory::~FrenetOptimalTrajectory() {
36+
delete csp;
37+
for (FrenetPath* fp : frenet_paths) {
38+
delete fp;
39+
}
40+
}
41+
42+
// Return the best path
43+
FrenetPath* FrenetOptimalTrajectory::getBestPath() {
44+
return best_frenet_path;
45+
}
46+
47+
// Calculate frenet paths
48+
void FrenetOptimalTrajectory::calc_frenet_paths() {
49+
double t, ti, tv, jp, js, ds;
50+
FrenetPath* fp, *tfp;
51+
52+
double di = -MAX_ROAD_WIDTH;
53+
// generate path to each offset goal
54+
do {
55+
ti = MINT;
56+
// lateral motion planning
57+
do {
58+
ti += DT;
59+
fp = new FrenetPath();
60+
QuinticPolynomial lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di,
61+
0.0, 0.0, ti);
62+
t = 0;
63+
// construct frenet path
64+
do {
65+
fp->t.push_back(t);
66+
fp->d.push_back(lat_qp.calc_point(t));
67+
fp->d_d.push_back(lat_qp.calc_first_derivative(t));
68+
fp->d_dd.push_back(lat_qp.calc_second_derivative(t));
69+
fp->d_ddd.push_back(lat_qp.calc_third_derivative(t));
70+
t += DT;
71+
} while (t < ti);
72+
73+
// velocity keeping
74+
tv = target_speed - D_T_S * N_S_SAMPLE;
75+
do {
76+
jp = 0;
77+
js = 0;
78+
79+
// copy frenet path
80+
tfp = new FrenetPath();
81+
for (double tt : fp->t) {
82+
tfp->t.push_back(tt);
83+
tfp->d.push_back(lat_qp.calc_point(tt));
84+
tfp->d_d.push_back(lat_qp.calc_first_derivative(tt));
85+
tfp->d_dd.push_back(lat_qp.calc_second_derivative(tt));
86+
tfp->d_ddd.push_back(lat_qp.calc_third_derivative(tt));
87+
jp += pow(lat_qp.calc_third_derivative(tt), 2);
88+
// square jerk
89+
}
90+
QuarticPolynomial lon_qp = QuarticPolynomial(s0, c_speed,
91+
0.0, tv, 0.0, ti);
92+
93+
// longitudinal motion
94+
for (double tp : tfp->t) {
95+
tfp->s.push_back(lon_qp.calc_point(tp));
96+
tfp->s_d.push_back(lon_qp.calc_first_derivative(tp));
97+
tfp->s_dd.push_back(lon_qp.calc_second_derivative(tp));
98+
tfp->s_ddd.push_back(lon_qp.calc_third_derivative(tp));
99+
js += pow(lon_qp.calc_third_derivative(tp), 2);
100+
// square jerk
101+
}
102+
103+
// calculate costs
104+
ds = pow(target_speed - tfp->s_d.back(), 2);
105+
tfp->cd = KJ * jp + KT * ti + KD * pow(tfp->d.back(), 2);
106+
tfp->cv = KJ * js + KT * ti + KD * ds;
107+
tfp->cf = KLAT * tfp->cd + KLON * tfp->cv;
108+
109+
110+
// append
111+
tfp->to_global_path(csp);
112+
if (!tfp->is_valid_path(obstacles)) {
113+
// deallocate memory and continue
114+
delete tfp;
115+
tv += D_T_S;
116+
continue;
117+
}
118+
frenet_paths.push_back(tfp);
119+
tv += D_T_S;
120+
} while (tv < target_speed + D_T_S * N_S_SAMPLE);
121+
// make sure to deallocate
122+
delete fp;
123+
} while(ti < MAXT);
124+
di += D_ROAD_W;
125+
} while (di < MAX_ROAD_WIDTH);
126+
}
127+

‎src/FrenetOptimalTrajectory/FrenetOptimalTrajectory.h

+41
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
// Author: Edward Fang
2+
// Email: edward.fang@berkeley.edu
3+
//
4+
// This code is adapted from https://github.com/AtsushiSakai/PythonRobotics/tree/
5+
// master/PathPlanning/FrenetOptimalTrajectory.
6+
// Its author is Atsushi Sakai.
7+
//
8+
// Reference Papers:
9+
// - [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
10+
// (https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
11+
// - [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame]
12+
// (https://www.youtube.com/watch?v=Cj6tAQe7UCY)
13+
14+
#ifndef FRENET_OPTIMAL_TRAJECTORY_FRENET_OPTIMAL_TRAJECTORY_H
15+
#define FRENET_OPTIMAL_TRAJECTORY_FRENET_OPTIMAL_TRAJECTORY_H
16+
17+
#include "FrenetPath.h"
18+
#include "CubicSpline2D.h"
19+
20+
#include <vector>
21+
22+
class FrenetOptimalTrajectory {
23+
public:
24+
FrenetOptimalTrajectory(std::vector<double>& x, std::vector<double>& y,
25+
double s0, double c_speed, double c_d, double c_d_d, double
26+
c_d_dd, double target_speed,
27+
std::vector<std::tuple<double, double>>& obstacles);
28+
~FrenetOptimalTrajectory();
29+
FrenetPath* getBestPath();
30+
private:
31+
FrenetPath* best_frenet_path;
32+
CubicSpline2D* csp;
33+
std::vector<std::tuple<double, double>> obstacles;
34+
std::vector<double> x, y;
35+
double s0, c_speed, c_d, c_d_d, c_d_dd, target_speed;
36+
std::vector<FrenetPath*> frenet_paths;
37+
void calc_frenet_paths();
38+
};
39+
40+
41+
#endif //FRENET_OPTIMAL_TRAJECTORY_FRENET_OPTIMAL_TRAJECTORY_H

‎src/FrenetOptimalTrajectory/FrenetPath.cpp

+79
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
#include "FrenetPath.h"
2+
#include "constants.h"
3+
#include "utils.h"
4+
5+
#include <algorithm>
6+
7+
using namespace std;
8+
9+
// Convert the frenet path to global path in terms of x, y, yaw, velocity
10+
void FrenetPath::to_global_path(CubicSpline2D* csp) {
11+
double ix, iy, iyaw, di, fx, fy, dx, dy;
12+
// calc global positions
13+
for (int i = 0; i < s.size(); i++) {
14+
ix = csp->calc_x(s[i]);
15+
iy = csp->calc_y(s[i]);
16+
if (isnan(ix) || isnan(iy)) break;
17+
18+
iyaw = csp->calc_yaw(s[i]);
19+
di = d[i];
20+
fx = ix + di * cos(iyaw + M_PI / 2.0);
21+
fy = iy + di * sin(iyaw + M_PI / 2.0);
22+
x.push_back(fx);
23+
y.push_back(fy);
24+
}
25+
26+
// calc yaw and ds
27+
for (int i = 0; i < x.size() - 1; i++) {
28+
dx = x[i+1] - x[i];
29+
dy = y[i+1] - y[i];
30+
yaw.push_back(atan2(dy, dx));
31+
ds.push_back(hypot(dx, dy));
32+
}
33+
yaw.push_back(yaw.back());
34+
ds.push_back(ds.back());
35+
36+
// calc curvature
37+
for (int i = 0; i < yaw.size() - 1; i++) {
38+
c.push_back((yaw[i+1] - yaw[i]) / ds[i]);
39+
}
40+
}
41+
42+
// Validate the calculated frenet paths against threshold speed, acceleration,
43+
// curvature and collision checks
44+
bool FrenetPath::is_valid_path(const vector<tuple<double, double>>& obstacles) {
45+
if (any_of(s_d.begin(), s_d.end(),
46+
[](int i){return abs(i) > MAX_SPEED;})) return false;
47+
// max accel check
48+
else if (any_of(s_dd.begin(), s_dd.end(),
49+
[](int i){return abs(i) > MAX_ACCEL;})) return false;
50+
// max curvature check
51+
else if (any_of(c.begin(), c.end(),
52+
[](int i){return abs(i) > MAX_CURVATURE;})) return false;
53+
// collision check
54+
else if (is_collision(obstacles)) return false;
55+
else return true;
56+
}
57+
58+
bool FrenetPath::is_collision(const vector<tuple<double, double>>& obstacles) {
59+
// no obstacles
60+
if (obstacles.empty()) {
61+
return false;
62+
}
63+
64+
// iterate over all obstacles
65+
for (tuple<double, double> obstacle : obstacles) {
66+
// calculate distance to each point in path
67+
for (int i = 0; i < x.size(); i++) {
68+
// exit if within OBSTACLE_RADIUS
69+
double xd = x[i] - get<0>(obstacle);
70+
double yd = y[i] - get<1>(obstacle);
71+
if (norm(xd, yd) <= OBSTACLE_RADIUS) {
72+
return true;
73+
}
74+
}
75+
}
76+
77+
// no collisions
78+
return false;
79+
}

‎src/FrenetOptimalTrajectory/FrenetPath.h

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
#ifndef FRENET_OPTIMAL_TRAJECTORY_FRENETPATH_H
2+
#define FRENET_OPTIMAL_TRAJECTORY_FRENETPATH_H
3+
4+
#include "CubicSpline2D.h"
5+
6+
#include <vector>
7+
#include <tuple>
8+
9+
class FrenetPath {
10+
public:
11+
// Frenet attributes
12+
std::vector<double> t; // time
13+
std::vector<double> d; // lateral offset
14+
std::vector<double> d_d; // lateral speed
15+
std::vector<double> d_dd; // lateral acceleration
16+
std::vector<double> d_ddd; // lateral jerk
17+
std::vector<double> s; // s position along spline
18+
std::vector<double> s_d; // s speed
19+
std::vector<double> s_dd; // s acceleration
20+
std::vector<double> s_ddd; // s jerk
21+
22+
// Euclidean attributes
23+
std::vector<double> x; // x position
24+
std::vector<double> y; // y position
25+
std::vector<double> yaw; // yaw in rad
26+
std::vector<double> ds; // speed
27+
std::vector<double> c; // curvature
28+
29+
// Cost attributes
30+
double cd = 0.0; // lateral cost
31+
double cv = 0.0; // longitudinal cost
32+
double cf = 0.0; // final cost
33+
34+
FrenetPath() = default;
35+
void to_global_path(CubicSpline2D* csp);
36+
bool is_valid_path(const std::vector<std::tuple<double, double>>& obstacles);
37+
bool is_collision(const std::vector<std::tuple<double, double>>& obstacles);
38+
};
39+
40+
#endif //FRENET_OPTIMAL_TRAJECTORY_FRENETPATH_H

‎src/FrenetOptimalTrajectory/fot_wrapper.cpp

+117
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
#include "FrenetOptimalTrajectory.h"
2+
#include "FrenetPath.h"
3+
#include "CubicSpline2D.h"
4+
#include "utils.h"
5+
6+
#include <iostream>
7+
#include <vector>
8+
9+
using namespace std;
10+
11+
// C++ wrapper to expose the FrenetOptimalTrajectory class to python
12+
extern "C" {
13+
// Compute the frenet optimal trajectory given initial conditions
14+
// in frenet space.
15+
//
16+
// Arguments:
17+
// s0: initial longitudinal position along spline
18+
// c_speed: initial velocity
19+
// c_d: initial lateral offset
20+
// c_d_d: initial lateral velocity
21+
// c_d_dd: initial lateral acceleration
22+
// xp, yp: list of waypoint coordinates
23+
// xo, yo: list of obstacle coordinates
24+
// np, no: length of the waypoint list and obstacle list
25+
// target_speed: target velocity in [m/s]
26+
// Returns:
27+
// x_path, y_path: the frenet optimal trajectory in cartesian space
28+
// misc: the next states s0, c_speed, c_d, c_d_d, c_d_dd
29+
int get_fot_frenet_space(
30+
double s0, double c_speed, double c_d, double c_d_d, double c_d_dd,
31+
double* xp, double* yp, double* xo, double* yo, int np, int no,
32+
double target_speed, double* x_path, double* y_path,
33+
double* speeds, double* misc
34+
) {
35+
vector<double> wx (xp, xp + np);
36+
vector<double> wy (yp, yp + np);
37+
38+
vector<tuple<double, double>> obstacles;
39+
vector<double> ox (xo, xo + no);
40+
vector<double> oy (yo, yo + no);
41+
for (int i = 0; i < ox.size(); i++) {
42+
tuple<double, double> ob (ox[i], oy[i]);
43+
obstacles.push_back(ob);
44+
}
45+
46+
FrenetOptimalTrajectory fot = FrenetOptimalTrajectory(wx, wy, s0,
47+
c_speed, c_d, c_d_d, c_d_dd, target_speed, obstacles);
48+
FrenetPath* best_frenet_path = fot.getBestPath();
49+
50+
int success = 0;
51+
if (!best_frenet_path->x.empty()){
52+
int last = 0;
53+
for (int i = 0; i < best_frenet_path->x.size(); i++) {
54+
x_path[i] = best_frenet_path->x[i];
55+
y_path[i] = best_frenet_path->y[i];
56+
speeds[i] = best_frenet_path->s_d[i];
57+
last += 1;
58+
}
59+
60+
// indicate last point in the path
61+
x_path[last] = NAN;
62+
y_path[last] = NAN;
63+
speeds[last] = NAN;
64+
65+
misc[0] = best_frenet_path->s[1];
66+
misc[1] = best_frenet_path->s_d[1];
67+
misc[2] = best_frenet_path->d[1];
68+
misc[3] = best_frenet_path->d_d[1];
69+
misc[4] = best_frenet_path->d_dd[1];
70+
success = 1;
71+
}
72+
return success;
73+
}
74+
75+
// Convert the initial conditions from cartesian space to frenet space
76+
void compute_initial_conditions(
77+
double s0, double x, double y, double vx,
78+
double vy, double forward_speed, double* xp, double* yp, int np,
79+
double* initial_conditions
80+
) {
81+
vector<double> wx (xp, xp + np);
82+
vector<double> wy (yp, yp + np);
83+
CubicSpline2D* csp = new CubicSpline2D(wx, wy);
84+
85+
// get distance from car to spline and projection
86+
double s = csp->find_s(x, y, s0);
87+
double distance = norm(csp->calc_x(s) - x, csp->calc_y(s) - y);
88+
tuple<double, double> bvec ((csp->calc_x(s) - x) / distance,
89+
(csp->calc_y(s) - y) / distance);
90+
91+
// normal spline vector
92+
double x0 = csp->calc_x(s0);
93+
double y0 = csp->calc_y(s0);
94+
double x1 = csp->calc_x(s0 + 2);
95+
double y1 = csp->calc_y(s0 + 2);
96+
97+
// unit vector orthog. to spline
98+
tuple<double, double> tvec (y1-y0, -(x1-x0));
99+
as_unit_vector(tvec);
100+
101+
// compute tangent / normal car vectors
102+
tuple<double, double> fvec (vx, vy);
103+
as_unit_vector(fvec);
104+
105+
// get initial conditions in frenet frame
106+
initial_conditions[0] = s; // current longitudinal position s
107+
initial_conditions[1] = forward_speed; // speed [m/s]
108+
// lateral position c_d [m]
109+
initial_conditions[2] = copysign(distance, dot(tvec, bvec));
110+
// lateral speed c_d_d [m/s]
111+
initial_conditions[3] = forward_speed * dot(tvec, fvec);
112+
initial_conditions[4] = 0.0; // lateral acceleration c_d_dd [m/s^2]
113+
// TODO: add lateral acceleration when CARLA 9.7 is patched (IMU)
114+
115+
delete csp;
116+
}
117+
}

‎src/Polynomials/QuarticPolynomial.cpp

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
#include "QuarticPolynomial.h"
2+
3+
#include <Eigen/LU>
4+
#include <cmath>
5+
6+
using namespace Eigen;
7+
8+
QuarticPolynomial::QuarticPolynomial(double xs, double vxs, double axs,
9+
double vxe, double axe, double t):
10+
a0(xs), a1(vxs) {
11+
a2 = axs / 2.0;
12+
Matrix2d A;
13+
Vector2d B;
14+
A << 3 * pow(t, 2), 4 * pow(t, 3), 6 * t, 12 * pow(t, 2);
15+
B << vxe - a1 - 2 * a2 * t, axe - 2 * a2;
16+
Matrix2d A_inv = A.inverse();
17+
Vector2d x = A_inv * B;
18+
a3 = x[0];
19+
a4 = x[1];
20+
}
21+
22+
double QuarticPolynomial::calc_point(double t) {
23+
return a0 + a1 * t + a2 * pow(t, 2) + a3 * pow(t, 3) + a4 * pow(t, 4);
24+
}
25+
26+
double QuarticPolynomial::calc_first_derivative(double t) {
27+
return a1 + 2 * a2 * t + 3 * a3 * pow(t, 2) + 4 * a4 * pow(t, 3);
28+
}
29+
30+
double QuarticPolynomial::calc_second_derivative(double t) {
31+
return 2 * a2 + 6 * a3 * t + 12 * a4 * pow(t, 2);
32+
}
33+
34+
double QuarticPolynomial::calc_third_derivative(double t) {
35+
return 6 * a3 + 24 * a4 * t;
36+
}

‎src/Polynomials/QuarticPolynomial.h

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
#ifndef FRENET_OPTIMAL_TRAJECTORY_QUARTICPOLYNOMIAL_H
2+
#define FRENET_OPTIMAL_TRAJECTORY_QUARTICPOLYNOMIAL_H
3+
4+
class QuarticPolynomial {
5+
public:
6+
QuarticPolynomial() = default;
7+
QuarticPolynomial(double xs, double vxs, double axs, double vxe,
8+
double axe, double t);
9+
double calc_point(double t);
10+
double calc_first_derivative(double t);
11+
double calc_second_derivative(double t);
12+
double calc_third_derivative(double t);
13+
private:
14+
double a0, a1, a2, a3, a4;
15+
};
16+
17+
#endif //FRENET_OPTIMAL_TRAJECTORY_QUARTICPOLYNOMIAL_H

‎src/Polynomials/QuinticPolynomial.cpp

+42
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
#include "QuinticPolynomial.h"
2+
3+
#include <Eigen/LU>
4+
#include <cmath>
5+
6+
using namespace Eigen;
7+
8+
QuinticPolynomial::QuinticPolynomial(double xs, double vxs, double axs,
9+
double xe, double vxe, double axe, double t):
10+
a0(xs), a1(vxs) {
11+
a2 = axs / 2.0;
12+
Matrix3d A;
13+
Vector3d B;
14+
A << pow(t, 3), pow(t, 4), pow(t, 5), 3 * pow(t, 2),
15+
4 * pow(t, 3), 5 * pow(t, 4), 6 * t, 12 * pow(t, 2),
16+
20 * pow(t, 3);
17+
B << xe - a0 - a1 * t - a2 * pow(t, 2), vxe - a1 - 2 * a2 * t,
18+
axe - 2 * a2;
19+
Matrix3d A_inv = A.inverse();
20+
Vector3d x = A_inv * B;
21+
a3 = x[0];
22+
a4 = x[1];
23+
a5 = x[2];
24+
}
25+
26+
double QuinticPolynomial::calc_point(double t) {
27+
return a0 + a1 * t + a2 * pow(t, 2) + a3 * pow(t, 3) +
28+
a4 * pow(t, 4) + a5 * pow(t, 5);
29+
}
30+
31+
double QuinticPolynomial::calc_first_derivative(double t) {
32+
return a1 + 2 * a2 * t + 3 * a3 * pow(t, 2) + 4 * a4 * pow(t, 3) +
33+
5 * a5 * pow(t, 4);
34+
}
35+
36+
double QuinticPolynomial::calc_second_derivative(double t) {
37+
return 2 * a2 + 6 * a3 * t + 12 * a4 * pow(t, 2) + 20 * a5 * pow(t, 3);
38+
}
39+
40+
double QuinticPolynomial::calc_third_derivative(double t) {
41+
return 6 * a3 + 24 * a4 * t + 60 * a5 * pow(t, 2);
42+
}

‎src/Polynomials/QuinticPolynomial.h

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
#ifndef FRENET_OPTIMAL_TRAJECTORY_QUINTICPOLYNOMIAL_H
2+
#define FRENET_OPTIMAL_TRAJECTORY_QUINTICPOLYNOMIAL_H
3+
4+
class QuinticPolynomial {
5+
public:
6+
QuinticPolynomial() = default;
7+
QuinticPolynomial(double xs, double vxs, double axs, double xe,
8+
double vxe, double axe, double t);
9+
double calc_point(double t);
10+
double calc_first_derivative(double t);
11+
double calc_second_derivative(double t);
12+
double calc_third_derivative(double t);
13+
private:
14+
double a0, a1, a2, a3, a4, a5;
15+
};
16+
17+
#endif //FRENET_OPTIMAL_TRAJECTORY_QUINTICPOLYNOMIAL_H

‎src/constants.h

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#ifndef FRENET_OPTIMAL_TRAJECTORY_CONSTANTS_H
2+
#define FRENET_OPTIMAL_TRAJECTORY_CONSTANTS_H
3+
// Parameter
4+
const double MAX_SPEED = 25.0; // maximum speed [m/s]
5+
const double MAX_ACCEL = 6.0; // maximum acceleration [m/ss]
6+
const double MAX_CURVATURE = 10; // maximum curvature [1/m]
7+
const double MAX_ROAD_WIDTH = 6.0; // maximum road width [m]
8+
const double D_ROAD_W = 1; // road width sampling length [m]
9+
const double DT = 0.25; // time tick [s]
10+
const double MAXT = 6; // max prediction time [m]
11+
const double MINT = 4; // min prediction time [m]
12+
const double D_T_S = 1.0; // target speed sampling length [m/s]
13+
const double N_S_SAMPLE = 1; // sampling number of target speed
14+
const double OBSTACLE_RADIUS = 3; // obstacle radius [m]
15+
16+
// cost weights
17+
const double KJ = 0.1; // jerk cost
18+
const double KT = 0.1; // time cost
19+
const double KD = 1.0; // end state cost
20+
const double KLAT = 1.0; // lateral cost
21+
const double KLON = 1.0; // longitudinal cost
22+
#endif //FRENET_OPTIMAL_TRAJECTORY_CONSTANTS_H

‎src/main.cpp

+52
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
#include "FrenetOptimalTrajectory.h"
2+
#include "FrenetPath.h"
3+
4+
#include <iostream>
5+
#include <ctime>
6+
#include <vector>
7+
#include <tuple>
8+
9+
using namespace std;
10+
11+
int main() {
12+
// set up experiment
13+
clock_t start;
14+
double duration;
15+
int sim_loop = 40;
16+
double s0 = 12.6;
17+
double c_speed = 7.1;
18+
double c_d = 0.1;
19+
double c_d_d = 0.01;
20+
double c_d_dd = 0.0;
21+
vector<double> wx = {132.67, 128.67, 124.67, 120.67, 116.67, 112.67, 108.67,
22+
104.67, 101.43, 97.77, 94.84, 92.89, 92.4 , 92.4 ,
23+
92.4 , 92.4 , 92.4 , 92.4 , 92.4 , 92.39, 92.39,
24+
92.39, 92.39, 92.39, 92.39};
25+
vector<double> wy = {195.14, 195.14, 195.14, 195.14, 195.14, 195.14, 195.14,
26+
195.14, 195.14, 195.03, 193.88, 191.75, 188.72, 185.32,
27+
181.32, 177.32, 173.32, 169.32, 165.32, 161.32, 157.32,
28+
153.32, 149.32, 145.32, 141.84};
29+
vector<tuple<double, double>> obstacles;
30+
vector<double> ox = {98.2, 98.78, 99.36, 99.94, 100.52, 101.1, 101.68, 102.26, 102.85, 103.43};
31+
vector<double> oy = {198.96, 198.94, 198.91, 198.88, 198.86, 198.83, 198.81, 198.78, 198.76, 198.73};
32+
for (int i = 0; i < ox.size(); i++) {
33+
tuple<double, double> ob (ox[i], oy[i]);
34+
obstacles.push_back(ob);
35+
}
36+
37+
// run experiment
38+
start = clock();
39+
for (int i = 0; i < 40; i++) {
40+
FrenetOptimalTrajectory fot = FrenetOptimalTrajectory(wx, wy, s0, c_speed, c_d, c_d_d, c_d_dd, 10, obstacles);
41+
FrenetPath* best_frenet_path = fot.getBestPath();
42+
s0 = best_frenet_path->s[1];
43+
c_d = best_frenet_path->d[1];
44+
c_d_d = best_frenet_path->d_d[1];
45+
c_d_dd = best_frenet_path->d_dd[1];
46+
c_speed = best_frenet_path->s_d[1];
47+
}
48+
duration = (clock() - start) / (double) CLOCKS_PER_SEC;
49+
cout << "Total time taken: " << duration << endl;
50+
cout << "Time per iteration: " << duration / sim_loop << endl;
51+
return 1;
52+
}

‎src/utils.h

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
#ifndef FRENET_OPTIMAL_TRAJECTORY_UTILS_H
2+
#define FRENET_OPTIMAL_TRAJECTORY_UTILS_H
3+
4+
#include <cmath>
5+
#include <tuple>
6+
7+
inline double norm(double x, double y) {
8+
return sqrt(pow(x, 2) + pow(y, 2));
9+
}
10+
11+
inline void as_unit_vector(std::tuple<double, double>& vec) {
12+
double magnitude = norm(std::get<0>(vec), std::get<1>(vec));
13+
if (magnitude > 0) {
14+
std::get<0>(vec) = std::get<0>(vec) / magnitude;
15+
std::get<1>(vec) = std::get<1>(vec) / magnitude;
16+
}
17+
}
18+
19+
inline double dot(const std::tuple<double, double>& vec1,
20+
const std::tuple<double, double>& vec2) {
21+
return std::get<0>(vec1) * std::get<0>(vec2) +
22+
std::get<1>(vec1) * std::get<1>(vec2);
23+
}
24+
#endif //FRENET_OPTIMAL_TRAJECTORY_UTILS_H

0 commit comments

Comments
 (0)
Please sign in to comment.