-
Notifications
You must be signed in to change notification settings - Fork 3
Waypoint Updater
The waypoint updater node has two main tasks:
1. Waypoint Preparation:
- Load the waypoints from the /base_waypoints topic and prepare them for use during planning
2. Planning:
-
Use pose messages from /current_pose topic to identify the location of the closest waypoint in front of the car.
-
Use messages from /traffic_waypoint topic to decide whether the car should speed up, maintain current speed, or needs to come to a stop and calculate an appropriate trajectory to accomplish this. A set of waypoints and velocities is then published to the /final_waypoints topic for the waypoint follower to consume
The decision was made to use jerk minimizing trajectory planning to be able to produce speedup and slowdown profiles that can be merged at any point based on changing traffic signal status without exceeding the 10m/s2 acceleration or 10 m/s3 jerk criteria.
The waypoints are loaded into a custom structure which allows us to store the waypoint data, the distance of the waypoint (through preceding waypoints) from the start of the track, the maximum velocity allowed for that waypoint, and the velocity, acceleration, jerk, and elapsed time variables used in the jerk minimizing trajectory planning calculations.
The cartesian (x,y) co-ordinates of each of the waypoints are also loaded into a kd-tree using scipy.spatial.KDTree to enable finding the closest waypoint to the car. This search is used as a backup method to find the closest waypoint. Our tests have shown kdtree to be very fast and usable for our requirements with 11K waypoints, however, it is generally only used by the waypoint_updater at startup.
Once the waypoints have been loaded and at least one pose message has been received, the updater loop begins. The loop is set to run at approximately 30 hz, or about 0.033 s intervals
The first stage of the cycle uses the most recent pose data to locate the closest waypoint ahead of the car on the waypoint list. Since the car is expected to follow the waypoints, a local search of waypoints ahead and behind the waypoint identified in the last cycle is conducted to try to find the car location as quickly as possible.
A check is then made to see if a traffic_light message has been received from the traffic light detection system since the last cycle. Messages are stored and only updated at the start of each cycle to prevent race conditions when the traffic signal information changes during an ongoing cycle.
The car can be in 1 of 5 states:
| State | Description |
|---|---|
| Stopped | Car is not moving and is waiting at a red or yellow light |
| Creeping | Car is moving at a steady minimum velocity up to a stopline |
| Speedup | Car is accelerating toward the default cruising velocity |
| Maintainspeed | Car is maintaining constant default cruising speed |
| Slowdown | Car is decelerating to stop just behind the stopline |
If the car is moving (in speedup,maintainspeed, or slowdown states), the minimum stopping distances are calculated to prevent exceeding maximum desired jerk and maximum allowed jerk using the current velocity and acceleration of the car as starting conditions. This remains an area of investigation in this project, as a simple fast and accurate method to determine the minimum distance required to prevent exceeding a specific jerk level has not been achieved. The current method is functional, but is thought to overestimate the necessary distance.* This is a critical issue, as it is key in the decision to whether the car can stop for a signal.
If the distance to a signalling light is between the 2 calculated stopping distances, the time required to complete the slowdown is optimised to produce the lowest jerk along the trajectory, and the slowdown trajectory is calculated and published. If the distance to the traffic light is greater than the larger stopping distance, then the car will continue on its current plan. If the distance to the traffic light is less than the minimum calculated stopping distance, then the planner will proceed past the signal - (this should correspond to driving through a yellow light).
A decelerating message is published when the planner has put the car in slowdown or stopped to assist the DBW Node with braking control.
In the event that the car is in slowdown or stopped, and the distance to the next signalling traffic light increases beyond the calculated stopping distance, a new trajectory is calculated to accelerate the car to the default velocity. Maximum desired jerk is used as the criteria to calculate the distance and time over which this trajectory should be completed. This algorithm is not optimal, but overestimating the minimum distance required to accelerate without exceeding a specific jerk limit is not critical like being able to stop in time for a stop signal.
A special creeping state was used for the condition where the car is stopped a short distance from a stop signal. If the car is within the creeping setback from the light, the planner will just use the minimum moving velocity on waypoints until the car gets to the traffic light buffer setback where velocity is returned to zero. This prevents short acceleration/braking cycles at the light, simulating just releasing the brake to ease forward.
The upcoming waypoints are published to the /final_waypoints topic. We found that using 40 waypoints on track 1 of the simulator was sufficient for use by the waypoint follower.
The calculation time required in each cycle was low enough in the conditions tested that we did not use any algorithm to account for latency when publishing the waypoints.
- During tests on the simulator, we found that the minimum stopping distance calculated at higher speeds (~ 60 m for 60 km/hr) to meet maximum jerk limits occasionally resulted in not being able to stop for a red traffic light which was detected within 60m of the line. At 40 km/hr the minimum stopping distance was below 30m which could still result in the car going through a yellow light, but was much less likely to result in running a red. The slower speed also gave more time for the traffic light classifier to classify the light signal.