Path Planning Project

In this project there was a lot to accomplish, so I had to make a check  list. A year ago this would have been totally beyond me so I am just delighted to be working at this level right now even though it is really just the beginning, but it’s still a big milestone for me. To get us started Udacity provide us all that we needed to allow us to focus on a set of problems without having to create from scratch a whole virtual world. We get a very nice 3D car simulator that looks like a computer game but with more sophistication that previous simulators on prior projects. This one has traffic in both directions and the traffic is semi random mostly other cars driving at various speeds but they also occasionally simulate real drivers quite well by cutting in front of you very close without warning or slowing down in the fast lane for no apparent reason.

Our car has to react to this environment and just going slow will not do as their is a requirement to make progress through the traffic.  We also get data from simulated sensors. In a real world scenario we would have LIADR or RADAR sensors to help us and we get data to represent other cars detected by sensors on the road from the simulator. Other projects focus on whats called sensor fusion which is combining sensor data into usable information. This project is about making good driving decisions after we have our sensor data sorted out.

The Big Green Line is the cars predicted course

Project To Do list:

  • Stay on the correct side of the road
  • Keep good position in while in lane
  • Don't Collide with other vehicles
  • Don't leave the road
  • Don't break the speed limit
  • Do go as fast as possible within the speed limit
  • Figure out the best lane for the car to be in
  • Safely move into the best lane available
  • Do not speed up or slow down too suddenly
  • Do not move side to side too suddenly
  • Do all the above for at least 6 simulated miles
  • One single violation restarts the clock

I have to say I loved this project, some of the other projects had a huge emphasis on mathematical processes to transform sensor data into useful information. In this project I got to think a lot about the behaviors of a car on the road and how to describe that in words.The beauty of software programming is that if you can express an idea in words you are well on the way to  getting it done in code. So I got to really think about describe the real wold and implementing that in C++.

So how do we drive down a 6 lane highway? Bearing in mind the points on the checklist. We want to move along at near the speed limit because its a highway but not over the speed limit. We need some kind of number in the program to keep track of speed and if possible increase the speed but not above a certain point, but also not to accelerate or decelerate too suddenly. So we make some variables for speeds and rates of acceleration. Then we need to think about  which lane to go into, well the 3 on the wrong side of the road are out and after that we should only go into a lane if it is safe to do so. But what does that really mean. After some thought and with some experimentation I decided safe would be no vehicles in the lane 50 meters ahead of me and 20 meters behind me.

What about when to change lanes? Well if you can do the speed limit you might as well stay where you are but if there is a open lane and you are doing less that the speed limit you should move into that lane. In this project we were able to create a loop that would do checks every 20th of a second and it would check the senors input, we used that to find space to move into.

Debugging in C++ or any language can often involve getting the program to display text outputs to a screen so you can see what it is doing. I got the program to display outputs telling me how far the car ahead was so I could test how it was reacting. As these outputs grew and became more complex I decided to express the outputs from the cars point of view. In a nod to Artificial Intelligence I got the outputs to say ” I am in the Center Lane” , “the right lane is clear” etc. This told me what the car was “thinking” and actually made the debugging information more clear and easier to interpret. I was quite proud of my little innovation!

Of course it was not so simple and plenty went wrong. For example our car (called “the ego car” in a touch of Udacity humor) would often crash into the back of slow moving traffic. This was because on one hand we wanted to avoid sudden acceleration and deceleration but on the other hand  not too crash. When we set up our program to do that we created the possibility that the car might not stop fast enough if there was very slow moving traffic ahead.

So I  thought about what we actually do in real driving. What I do is try to look far ahead and make small adjustments early in plenty of time, maybe just easing off the throttle a little bit. If I get closer to something I want to avoid I slow down a bit harder maybe braking or dropping gear. If I am really near I will brake and if it is an emergency I will do an emergency stop.  I took those ideas and implemented them in C++.

What that involved was doing an number of checks using sensor input to determine how close something was ahead and setting so status to “within 40m” , “within 30m”, “within  15m” or “within 5 m”. With 4 levels of distance I created 4 corresponding levels of speed adjustment . There was a penalty from the simulator if we collided or decelerated too hard but I decided it was better to get penalized for braking hard than colliding and this was the whole point of the project, to think about what decisions do you have to make and what trade offs are involved if you want to be safe but still make progress on the road.

There were other problems like the car sometimes would sit between lanes, it turned out when two adjacent lanes were equally good for progress it would alternate between them  every split second but the smoothing effect of the steering meant this created an average setting that kept the car between two lanes until traffic made one lane decisively worse than the other then it would move to the better lane. These kind of things took a lot of time to decipher what was going on but they were great learning experiences.

After a lot of work and experimentation the car was able to drive for 20 minutes without any penalties for collisions or sudden movements. We only needed to do it for about 5 minutes but I wanted to be sure. I did a number of over 15 minute tests and called that a success.

You can see the code here on Github

Some screenshots of the car in action below

It was very satisfying to get this feedback from the project reviewer