Waypoint Updater Node - Ridgebeck/CarND-Capstone-Team-Herbie GitHub Wiki

Loading the Base Waypoints

• waypoint_loader.py loads x and y coordinates of map waypoints from a csv file in the data folder (by default from wp_yaw_const.csv for the highway simulation). This has to be changed for the parking lot simulation to churchlot_with_cars.csv. The path can be modified in the file waypoint_loader.launch.
• waypoint_loader.py publishes a topic with the message type "lane" called /base_waypoints
• The target speed (speed limit) is also defined in the file waypoint_loader.launch in km/h, which is then converted into m/s and attached as individual velocity to each waypoint under lane.waypoints.waypoints.twist.twist.linear.x. This value should be changed for the different simulations.
• At the end of the course the velocity of each waypoint is decreased linear until it reaches 0 at the last waypoint. This is a default by Udacity to stop the car after one round. This can be commented out in order to test the car for multiple laps. It also helps to avoid additional efforts when reading the target velocity from /base_waypoints.

Linear Deceleration

The idea is to decelerate the car with a linear profile or constant deceleration. The profile has to be created with modified waypoints based on the position of the red traffic light relative to the car's current position. Following the basic formula for constant acceleration:

In order to create a linear deceleration profile the velocity of each waypoint has to be changed based on the distance to the car. The location of the waypoints as well as the location of the car is given and therefore the distance to each waypoint can be calculated. With this distance value, the constant deceleration, and the current car speed the target velocity for every point in between car and stopping position can be calculated as follows:

The velocity of all waypoints which are in front of the car and in proximity (length is defined in the constant LOOKAHEAD_WPS) is changed via this formula to shape the linear deceleration profile. All waypoints after the stopping position should be set to zero velocity.

The question is now, when to slow down and when to accelerate or keep the target speed. Based on the distance to the red light as well as the speed of the car, a decision can be made if the car should slow down, should keep the current speed, or accelerate (as it is far away from the light and doesn't have to slow down yet or it is too close to the red light and cannot safely brake). This decision can be made based on the steepness of the velocity profile, which is the deceleration of the car over time. If it is in between two limits, the car should slow down. This is shown in the following diagram.

Let's look at three different exemplary scenarios what happens once a red light is detected in front of the car.

1. The calculated deceleration is below the ideal deceleration rate. The car accelerates at a comfortable rate to the target speed or keeps the target speed if it is already there. Once the needed deceleration passes the ideal deceleration rate the velocity of the waypoints is adjusted to follow this deceleration profile, which leads to corresponding velocity commands that are sent to the drive-by-wire node which converts it into throttle and brake commands.
2. The car is already close and fast enough that the calculated deceleration is larger than the ideal deceleration rate when the light turns red. The steeper linear deceleration profile is taken right away to decelerate the car.
3. The car is close to the traffic light when it turns red and the current speed would not allow a save stopping maneuver. The car therefore keeps the target speed or accelerates fast to the target speed if it is currently slower. This prevents an unsafe condition where the car could end up in the middle of the intersection or would slow down, pass the stopping line, and then accelerate again. Ideally, this would already be captured by looking at a change from green to yellow, but in this project our model is only looking at the light being red or not for simplicity reasons.

The different scenarios are illustrated below:

The decision making process is shown in the following flowchart. When a red light is detected, the first thing is to look at the distance to the stopping position, which is slightly before the stopping line belonging to the red traffic light. It is then checked if the car is close to stopping and all waypoints are set to zero if that is the case. This prevents possible misinterpretations once the car is at the target position or slightly behind. If the car is not yet that close to a stop the deceleration which is needed to stop the car on time will be calculated. If this value is higher than the maximum limit (means the car cannot physically stop in time), the car will be accelerated quickly to the target speed or keep the target speed if already there. If it is below the limit it will be checked if the red light was already detected before and the car is already in a deceleration process. If that is the case and there is no problem with the original trajectory, the waypoints with the assigned target speeds will be kept. If there is a problem with it a new trajectory can be calculated. Using the existing trajectory helps the controller to stabilize as otherwise the input to the PID loop would be constantly changed by calculating a slightly different trajectory up to 50 times a second. If the car is not yet decelerating (first time recognizing the red light), it will be checked if the deceleration has reached the minimum deceleration rate. If not, the car can accelerate comfortably or keep the target speed. Once it is reached the car will calculate a trajectory very close to the ideal deceleration rate to allow a comfortable stopping process.

After implementing the code for linear deceleration, we tested it in the simulator, still with the red light data coming directly from the simulator. The goal was to see how input and output of the velocity loop would look like.

Therefore we plotted both values (/twist_cmd/twist/linear/x and /current_velocity/twist/linear/x) via rqt_plot and received the following result. It has to be noted that in order to be able to plot a topic it has to be an array of values and has to have a header with a timestamp, which was not the case for /current_velocity/twist/linear/x. The timestamp was added to the header of the topic. For that we had to modify the function create_twist() in bridge.py.

We noticed a few problems while running the simulator and looking at the plot, which are shown in the video here: https://youtu.be/opf0Wxundx0

The most problematic situation seemed to be at low speeds, where the velocity command had large steps and the PID could not follow, which either led to an early stop or a jerky motion right before the stop. The reason for that is how the speed command is processed. The waypoints with the target speeds are published to the topic /final_waypoints which the node /pure_pursuit subscribes to. The flow of data looks like this:

We therefore had to look closer at the /pure_pursuit node and how the target velocity is processed inside the node that then publishes a velocity command that the node /dbw_node uses as the input for the PID controller. We noticed that the node took the speed of the next waypoint in front of the car as an input and only modified it once the car was far away from the waypoints. If the car stayed close to the waypoints the velocity command was the same as the target speed of the next waypoint. This explained perfectly the pattern we had seen in the velocity graph, as the steps got larger the slower the car was because it was passing the waypoints slower and the target velocity stayed longer the same.

In order to optimize that, a completely new approach for setting the target speed was needed, which also allowed us to make it a lot simpler. The velocity curve can be simply calculated as v2=v1+/-a*t depending on if the car is accelerating or decelerating. Just looking at delta t allows us to calculate the target velocity at any point in time accurately, even if the car is in between waypoints which results in a smooth velocity profile without any steps.

This results in a much cleaner signal which can be seen in the following picture (blue).

The next step was to change the linear profile to a S curve velocity profile with limited jerk values. The first version was a simple s curve with no linear velocity section in between. The calculations were adapted taken from the following document: http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf

The implementation yielded results as seen in the following graph:

This method has still a disadvantage, which is especially noticeable at higher speed deltas, when the car has to stop in a specified distance. In order to achieve this, the jerk values have to be calculated so that the concave and convex periods are taking half of the total time respectively. This can lead to a fairly long period at the beginning and end of the velocity profile, as acceleration and jerk can not be chosen independently. An approach that is closer to how a human driver behavior would therefore be an S curve profile with a linear acceleration period in between the concave and convex periods.

This was therefore implemented. The results can be seen here:

The curves only calculated a limited jerk profile between the current velocity of the car and the target velocity (either the speed limit or zero) without looking at the current acceleration of the car. Therefore the transitions between accelerating and decelerating, e.g. when a red light was recognized while the car was still accelerating were not yet jerk limited.

The next step was to properly tune the PID parameters in the node /DBW_node.