CarND-Path-Planning-Project-P1

Udacity Self-Driving Car Nanodegree - Path Planning Project

In this project, we need to implement a path planning algorithms to drive a car on a highway on a simulator provided by Udacity(the simulator could be downloaded here). The simulator sends car telemetry information (car's position and velocity) and sensor fusion information about the rest of the cars in the highway (Ex. car id, velocity, position). It expects a set of points spaced in time at 0.02 seconds representing the car's trajectory. The communication between the simulator and the path planner is done using WebSocket. The path planner uses the uWebSockets WebSocket implementation to handle this communication. Udacity provides a seed project to start from on this project (here).

Prerequisites

The project has the following dependencies (from Udacity's seed project):

cmake >= 3.5
make >= 4.1
gcc/g++ >= 5.4
libuv 1.12.0
Udacity's simulator.

For instructions on how to install these components on different operating systems, please, visit Udacity's seed project. As this particular implementation was done on Mac OS, the rest of this documentation will be focused on Mac OS. I am sorry to be that restrictive.

In order to install the necessary libraries, use the install-mac.sh.

Compiling and executing the project

In order to build the project there is a ./build.sh script on the repo root. It will create the ./build directory and compile de code. This is an example of the output of this script:

> sh ./build.sh
-- The C compiler identification is AppleClang 8.0.0.8000042
-- The CXX compiler identification is AppleClang 8.0.0.8000042
-- Check for working C compiler: /Library/Developer/CommandLineTools/usr/bin/cc
-- Check for working C compiler: /Library/Developer/CommandLineTools/usr/bin/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /Library/Developer/CommandLineTools/usr/bin/c++
-- Check for working CXX compiler: /Library/Developer/CommandLineTools/usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Configuring done
-- Generating done
-- Build files have been written to: REPO_ROOT/CarND-Path-Planning-Project-P1/build
Scanning dependencies of target path_planning
[ 50%] Building CXX object CMakeFiles/path_planning.dir/src/main.cpp.o
[100%] Linking CXX executable path_planning
[100%] Built target path_planning

The project could be executed directly using ./build/path_planning

> cd build
> ./path_planning
Listening to port 4567

Prediction

This part of the code deal with the telemetry and sensor fusion data. It intents to reason about the environment. In the case, we want to know three aspects of it:

Is there a car in front of us blocking the traffic.
Is there a car to the right of us making a lane change not safe.
Is there a car to the left of us making a lane change not safe.

These questions are answered by calculating the lane each other car is and the position it will be at the end of the last plan trajectory. A car is considered "dangerous" when its distance to our car is less than 30 meters in front or behind us.

bool car_ahead = false;
bool car_left = false;
bool car_righ = false;
for ( int i = 0; i < sensor_fusion.size(); i++ ) {
    float d = sensor_fusion[i][6];
    int car_lane = -1;
    // is it on the same lane we are
    if ( d > 0 && d < 4 ) {
        car_lane = 0;
    } else if ( d > 4 && d < 8 ) {
        car_lane = 1;
    } else if ( d > 8 && d < 12 ) {
        car_lane = 2;
    }
    if (car_lane < 0) {
        continue;
    }
    // Find car speed.
    double vx = sensor_fusion[i][3];
    double vy = sensor_fusion[i][4];
    double check_speed = sqrt(vx*vx + vy*vy);
    double check_car_s = sensor_fusion[i][5];
    // Estimate car s position after executing previous trajectory.
    check_car_s += ((double)prev_size*0.02*check_speed);

    if ( car_lane == lane ) {
        // Car in our lane.
        car_ahead |= check_car_s > car_s && check_car_s - car_s < 30;
    } else if ( car_lane - lane == -1 ) {
        // Car left
        car_left |= car_s - 30 < check_car_s && car_s + 30 > check_car_s;
    } else if ( car_lane - lane == 1 ) {
        // Car right
        car_righ |= car_s - 30 < check_car_s && car_s + 30 > check_car_s;
    }
}

Behavior

This part decides what to do:

If we have a car in front of us, do we change lanes?
Do we speed up or slow down?

Based on the prediction of the situation we are in, this code increases the speed, decrease speed, or make a lane change when it is safe. Instead of increasing the speed at this part of the code, a speed_diff is created to be used for speed changes when generating the trajectory in the last part of the code. This approach makes the car more responsive acting faster to changing situations like a car in front of it trying to apply breaks to cause a collision.

// Behavior : Let's see what to do.
double speed_diff = 0;
const double MAX_SPEED = 49.5;
const double MAX_ACC = .224;
if ( car_ahead ) { // Car ahead
    if ( !car_left && lane > 0 ) {
        // if there is no car left and there is a left lane.
        lane--; // Change lane left.
    } else if ( !car_righ && lane != 2 ){
        // if there is no car right and there is a right lane.
        lane++; // Change lane right.
    } else {
        speed_diff -= MAX_ACC;
    }
} else {
	if ( lane != 1 ) { // if we are not on the center lane.
		if ( ( lane == 0 && !car_righ ) || ( lane == 2 && !car_left ) ) {
			lane = 1; // Back to center.
		}
	}
	if ( ref_vel < MAX_SPEED ) {
		speed_diff += MAX_ACC;
	}
}

Trajectory

This code does the calculation of the trajectory based on the speed and lane output from the behavior, car coordinates and past path points.

First, the last two points of the previous trajectory (or the car position if there are no previous trajectory, lines 321 to 345) are used in conjunction three points at a far distance (lines 348 to 350) to initialize the spline calculation (line 370 and 371). To make the work less complicated to the spline calculation based on those points, the coordinates are transformed (shift and rotation) to local car coordinates (lines 361 to 367).

In order to ensure more continuity on the trajectory (in addition to adding the last two point of the pass trajectory to the spline adjustment), the pass trajectory points are copied to the new trajectory (lines 374 to 379). The rest of the points are calculated by evaluating the spline and transforming the output coordinates to not local coordinates (lines 388 to 407). Worth noticing the change in the velocity of the car from line 393 to 398. The speed change is decided on the behavior part of the code, but it is used in that part to increase/decrease speed on every trajectory points instead of doing it for the complete trajectory.