Figure 1: Snapshot from Video of Path Planner Code successfully navigating car though traffic
The program consists of the following code modules
| module | Description |
|---|---|
| spline.h | external code used to build splines. |
| json.hpp | external code used to decode json in websocket messages. |
| main.cpp | main module which initiates logging and websocket |
| WebSocketMessageHandler.cpp WebSocketMessageHandler.h | module which decodes websocket json messages, and passes them to the PathPlanner, and returns new path produced by PathPlanner to simulator. |
| CartesianPoint.h | defines structure of cartesian point with (x, y, theta). |
| FrenetPoint.h | defines structures of FrenetPoint with S,D, and FrenetDescription with Frenet references to position, velocity, acceleration and jerk at any point in time. |
| HighwayMap.cpp HighwayMap.h | Module which maps out the Frenet to Cartesian co-ordinates of the centreline of the highway track. |
| PathPlannerInput.h | defines the structure that encompasses all the information passed from the Websocket handler to the Pathplanner. |
| OtherCar.h | defines the structure of details of other cars reported from the simulator. |
| PathPlanner.cpp PathPlanner.h | base class for PathPlanners |
| KeepLanePathPlanner.cpp KeepLanePathPlanner.h | PathPlanner which produces a fixed velocity straight line path along the central lane without concern for acceleration or other cars. This code is all present in the module. |
| SimpleSplineBasedPlanner.cpp SimpleSplineBasedPlanner.h | PathPlanner which produces a path using the spline libraryand takes into account other cars, acceleration and velocity. |
| JMTBasedPlanner.cpp JMTBasedPlanner.h | PathPlanner that uses Jerk minimizing Trajectory (JMT) code to build trajectories directly, taking into account velocity, acceleration and other cars. |
| RoadMap.cpp RoadMap.h | Module used by JMTBasedPathPlanner to map out Other Cars and project their locations in the future to decide if lane or velocity changes should be made. |
| Trajectory.cpp Trajectory.h | Module used by JMTBasedPathPlanner to build and connect JMT trajectories |
| JMT.cpp JMT.h | Module used by Trajectory to calculate JMT Frenet paths |
| PathTracking.cpp PathTracking.h | Module implementing a deque which is used to store Cartesian and Frenet information regarding the Trajectory. |
Initially, I worked on developing a method to use the spline to generate trajectories in SimpleSplineBasedPlanner. This was relatively easy, and worked well to adjoin new trajectories onto the existing path. By experimenting with different path lengths when moving between lanes I was able to determine appropriate time/distance to prevent excessive acceleration or jerk during lane changes.
I also developed rudimentary logic to change lanes and prevent rear-ending other cars. This code could generally satisfy the project requirements. However I decided to develop a planner which used JMT trajectories.
I moved on to building a planner which used Jerk minimizing trajectory (JMT) code to generate the path without using the spline code, and which is used in this project submission. This proved to be a lot more tricky and finicky than I had expected. I eventually realized that I needed to keep track of the JMT Frenet details of vehicle pose at each point in the path (location, velocity, and acceleration) between steps. I built a structure called FrenetDescriptors to store these details so that they could be used as the starting values when generating new paths.
I developed PathTracking code which stored the FrenetDescriptors generated by the JMT::JMTFrenetDescriptorsAt() code in a deque. A deque was used since we frequently need to trim it at both ends when matching it with the Path handed back from the simulator, or inserting a new path into it. Cutting elements from the start of a deque is much less costly than with a vector.
I determined that I needed a state machine design with only the following three states: - Uninitialized, DriveInLane, and ChangeLane.
The role of the Uninitialized state is to start the car moving forward in a straight line with no change in direction. Once the car has moved forward along almost all the path generated in the first iteration of this state (after about 1.2 seconds when there are < 20 pathpoints left), the car is moved into the 'DriveInLane' state.
In the DriveInLane state the car moves along the centreline of the lane it is in while attempting to achieve the maximum speed of 49 mph (unless there is a slower car ahead, which it will attempt to match velocity with). The code will evaluate if it should switch to ChangeLane to move to a lane with greater clearance to any cars ahead. I added a restriction that the car had to maintain this state for a minimum time period KEEP_LANE_MINIMUM_TIME of 2 seconds before it could look to switch to 'ChangeLane` in order to prevent cutting across multiple lanes.
In this state the car travels along a trajectory between the centrelines of two adjacent lanes. The car is returned to the DriveinLane state when it is found to be near the centre of the target lane.
* Currently there are no collision avoidance measures in this state which can lead to issues if a car changes lane into the path of the lane change while it is ongoing.
Custom trajectory code was designed to build trajectories for each state Trajectory::InitiateTrajectory(), Trajectory::GenerateKeepInLaneTrajectory(), & Trajectory::GenerateJMTLaneChangeTrajectory(). These methods could/should be generalized into a single function, possibly with wrapper functions, to make them more maintainable.
By ensuring that the conditions at the start of any path generated by the JMT code matched the conditions at the point where it meshed with a previously generated trajectory, and by limiting the acceleration/deceleration rate to 0.225 * g, the code prevented excessive acceleration or jerk. Trajectory::CheckPath() is used to assess the trajectory after it has been mapped to Cartesian co-ordinates, and to detect any acceleration or velocity exceeding the restrictions.
Excessive values found with CheckPath() cause the trajectory to be recalculated in JMT with a modified acceleration rate. Testing has demonstrated that this effectively keeps the car within the required velocity, acceleration, and jerk restrictions.
The RoadMap::CreateRoadMap() code was used to build a RoadMapDeck which consisting of one layer containing the position of the EgoCar as time progressed, and one layer that contained the position of all the other cars as time progressed. The relative car positions were mapped through time at 0.1 s intervals for the next 2 seconds, with each time interval mapping as a separate dimension on the respective EgoCar and OtherCar layers. In this way the relative clearances in each lane between the EgoCar and any other car at any time interval could be easily calculated. Furthermore the minimum clearances over the next 2 seconds could be used both to evaluate if lane changes were possible, and if they might be advantageous. At present the algorithm only considers motion along the lane the car is currently in and does not predict lane changes of other cars that could be detected based on their velocity.
The RoadMap::CheckForSlowCarsAhead() and RoadMap::CheckForSlowCarInOtherLane() methods also look for cars ahead of the EgoCar within a certain distance threshold, to determine if the EgoCar should be decelerating, and by how much to avoid a rear end collision.
I thought that the RoadMapDeck structure could also be used to set up a grid for an A* style pathfinding algorithm, but did not have time to follow this up.
LaneChange options are evaluated in RoadMap::CheckForLaneChange(). The following decision tree was used to determine if a lanechange should be made:
-
If the current lane is clear for at least 60m over the next 2 seconds:
- Stay in lane if you are in the center lane.
- If in an outside lane, move to center lane if lanechange clearance is met and middle lane is also clear ahead for at least 60m for next 2 seconds.
-
If The current lane has a minimum clearance over next 2 seconds of less than 60m:
- Move from outside lanes to middle lane if lanechange clearance is met and:
- Middle lane has greater length of clear lane than current lane over next 2 seconds, or
- Middle lane has at least 40 m of clearance over next 2 seconds, or
- Current outside lane has less than 40 m clearance, but opposite outside lane has more than 55 m clearance over next 2 seconds (middle lane is just a transit to other outside lane).
- Move from the centre lane to the outside lane which has the greater min clearance exceeding that of the centre lane over the next 2 seconds.
- Move from outside lanes to middle lane if lanechange clearance is met and:
The minimum clearances assessed to be relatively safe to permit lane changes were 6m behind the car location point and 16 m in front of the car location point over the next 2 seconds in the target lane. These are more aggressive than would be recommended in a driving school, but were effective to move through tight traffic when possible in the simulator.
The car was heavily biased to move to the center lane, as it appeared that this would allow greater degree of choice, and reduce the possibility of getting caught without lane change possibilities that would occur more often in outside lanes.
Several possible improvements have been identified above. Additional possible improvements include:
- development of cost functions to evaluate potential alternative paths at each iteration.
- Adding simulation of reducing EgoCar velocity when stuck in traffic in an outside lane, while other Outside lane is clear of traffic to evaluate if a lanechange would become possible.
The code can be built by running
mkdir build
cmake..
makeThe model can be run from the build directory by calling:
./path-planThe program was built starting with Stanislav Olekhnovich's path planning starter code. I used this code so that I started with a cleaner separation of code than is present in the default code provided by Udacity.
I integrated spdlog, a "Very fast, header only, C++ logging library", to write output to the screen and logfiles. The executable writes out to a logfile PPlan.log in the local directory where the program is run.
I also integrated the "header only" spline code spline.h from http://kluge.in-chemnitz.de/opensource/spline/ which I did not use directly in my submission to generate paths, but used to implement better Frenet to Cartesian co-ordinate mapping by interpolating values of parameters at 1m intervals between the waypoints provided in "highway_map.csv".
The "header only" json.hpp from https://github.com/nlohmann/json is included by default with the project to decode and encode json messages between the program and the simulator.
The code is also dependent on the uWebSockets library to open a websocket connection to the simulator.