Path planning project — Udacity’s self-driving car nanodegree

Image for post
Image for post
From Udacity lecture

Route planning

Prediction

{
"timestamp" : 34512.21,
"vehicles" : [
{
"id" : 0,
"x" : -10.0,
"y" : 8.1,
"v_x" : 8.0,
"v_y" : 0.0,
"sigma_x" : 0.031,
"sigma_y" : 0.040,
"sigma_v_x" : 0.12,
"sigma_v_y" : 0.03,
},
{
"id" : 1,
"x" : 10.0,
"y" : 12.1,
"v_x" : -8.0,
"v_y" : 0.0,
"sigma_x" : 0.031,
"sigma_y" : 0.040,
"sigma_v_x" : 0.12,
"sigma_v_y" : 0.03,
},
]
}
{
"timestamp" : 34512.21,
"vehicles" : [
{
"id" : 0,
"length": 3.4,
"width" : 1.5,
"predictions" : [
{
"probability" : 0.781,
"trajectory" : [
{
"x": -10.0,
"y": 8.1,
"yaw": 0.0,
"timestamp": 34512.71
},
{
"x": -6.0,
"y": 8.1,
"yaw": 0.0,
"timestamp": 34513.21
},
{
"x": -2.0,
"y": 8.1,
"yaw": 0.0,
"timestamp": 34513.71
},
{
"x": 2.0,
"y": 8.1,
"yaw": 0.0,
"timestamp": 34514.21
},
{
"x": 6.0,
"y": 8.1,
"yaw": 0.0,
"timestamp": 34514.71
},
{
"x": 10.0,
"y": 8.1,
"yaw": 0.0,
"timestamp": 34515.21
},
]
},
{
"probability" : 0.219,
"trajectory" : [
{
"x": -10.0,
"y": 8.1,
"yaw": 0.0,
"timestamp": 34512.71
},
{
"x": -7.0,
"y": 7.5,
"yaw": -5.2,
"timestamp": 34513.21
},
{
"x": -4.0,
"y": 6.1,
"yaw": -32.0,
"timestamp": 34513.71
},
{
"x": -3.0,
"y": 4.1,
"yaw": -73.2,
"timestamp": 34514.21
},
{
"x": -2.0,
"y": 1.2,
"yaw": -90.0,
"timestamp": 34514.71
},
{
"x": -2.0,
"y":-2.8,
"yaw": -90.0,
"timestamp": 34515.21
},
]
}
]
},
{
"id" : 1,
"length": 3.4,
"width" : 1.5,
"predictions" : [
{
"probability" : 1.0,
"trajectory" : [
{
"x": 10.0,
"y": 12.1,
"yaw": -180.0,
"timestamp": 34512.71
},
{
"x": 6.0,
"y": 12.1,
"yaw": -180.0,
"timestamp": 34513.21
},
{
"x": 2.0,
"y": 12.1,
"yaw": -180.0,
"timestamp": 34513.71
},
{
"x": -2.0,
"y": 12.1,
"yaw": -180.0,
"timestamp": 34514.21
},
{
"x": -6.0,
"y": 12.1,
"yaw": -180.0,
"timestamp": 34514.71
},
{
"x": -10.0,
"y": 12.1,
"yaw": -180.0,
"timestamp": 34515.21
}
]
}
]
}
]
}
bool car_ahead = false;
bool car_left = false;
bool car_right = false;
if(d > 0 && d < 4) {
check_car_lane = 0;
} else if(d > 4 && d < 8) {
check_car_lane = 1;
} else if(d > 8 and d < 12) {
check_car_lane = 2;
}
if(check_car_lane == lane) {
//A vehicle is on the same line and check the car is in front of the ego car
car_ahead |= check_car_s > car_s && (check_car_s - car_s) < 30;
} else if((check_car_lane - lane) == -1) {
//A vehicle is on the left lane and check that is in 30 meter range
car_left |= (car_s+30) > check_car_s && (car_s-30) < check_car_s;
} else if((check_car_lane - lane) == 1) {
//A vehicle is on the right lane and check that is in 30 meter range
car_right |= (car_s+30) > check_car_s && (car_s-30) < check_car_s;
}
check_car_s += ((double)prev_size*0.02*check_speed);

Behavioral planning

Image for post
Image for post
From udacity lecture
Image for post
Image for post
Typical cost functions from udacity
if(car_ahead) {
if(!car_left && lane > 0) {
lane--;
} else if(!car_right && lane !=2) {
lane++;
} else if(!car_left && lane !=2) {
lane++;
}else {
ref_vel -= speed_diff;
}
} else if(ref_vel < max_accel){
ref_vel += speed_diff;
}

Trajectory planning

int prev_size = previous_path_x.size();
double ref_x = car_x;
double ref_y = car_y;
double ref_yaw = deg2rad(car_yaw);
if ( prev_size < 2 ) {    //Use two points thats makes path tangent to the car
double prev_car_x = car_x - cos(car_yaw);
double prev_car_y = car_y - sin(car_yaw);
ptsx.push_back(prev_car_x);
ptsx.push_back(car_x);
ptsy.push_back(prev_car_y);
ptsy.push_back(car_y);
}
//Redefine the reference point to previous point
ref_x = previous_path_x[prev_size - 1];
ref_y = previous_path_y[prev_size - 1];
double ref_x_prev = previous_path_x[prev_size - 2];
double ref_y_prev = previous_path_y[prev_size - 2];
ref_yaw = atan2(ref_y-ref_y_prev, ref_x-ref_x_prev);
ptsx.push_back(ref_x_prev);
ptsx.push_back(ref_x);
ptsy.push_back(ref_y_prev);
ptsy.push_back(ref_y);
vector<double> next_wp0 = getXY(car_s + 30, 2 + 4*lane, map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> next_wp1 = getXY(car_s + 60, 2 + 4*lane, map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> next_wp2 = getXY(car_s + 90, 2 + 4*lane, map_waypoints_s, map_waypoints_x, map_waypoints_y);
ptsx.push_back(next_wp0[0]);
ptsx.push_back(next_wp1[0]);
ptsx.push_back(next_wp2[0]);
ptsy.push_back(next_wp0[1]);
ptsy.push_back(next_wp1[1]);
ptsy.push_back(next_wp2[1]);
tk::spline s;
s.set_points(ptsx, ptsy);
vector<double> next_x_vals;
vector<double> next_y_vals;
//For the smooth transition, we are adding previous path points
for ( int i = 0; i < prev_size; i++ ) {
next_x_vals.push_back(previous_path_x[i]);
next_y_vals.push_back(previous_path_y[i]);
}
double target_x = 30.0;
double target_y = s(target_x);
double target_dist = sqrt(target_x*target_x + target_y*target_y);
Image for post
Image for post
for( int i = 1; i < 50 - prev_size; i++ ) {

double N = target_dist/(0.02*ref_vel/2.24);
double x_point = x_add_on + target_x/N;
double y_point = s(x_point);
x_add_on = x_point; double x_ref = x_point;
double y_ref = y_point;
//Rotate back to normal after rotating it earlier
x_point = x_ref * cos(ref_yaw) - y_ref * sin(ref_yaw);
y_point = x_ref * sin(ref_yaw) + y_ref * cos(ref_yaw);
x_point += ref_x;
y_point += ref_y;
next_x_vals.push_back(x_point);
next_y_vals.push_back(y_point);
}

Written by

Robotics PhD candidate@USYD, Software Engineer, Self Driving cars nanodegree holder@ Udacity

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store