Added predictive steering:
From twist_command, we get the target angular speed (which is due to road curvature). We used to calculate that in our code, but it's done by the system, so using that one instead. From that one, and yaw controller, we calculate a steering angle, due to road curvature (predictive steering). We add that to the corrective steering (due to cte) to get our new steering.
The assistance of predictive steering is evident if we switch to high speeds
When I add those steering angles I do steer + PREDICTIVE_STEERING * yaw_steer. PREDICTIVE STEERING is the ratio of yaw_steer you want to add. 0.0 to disable. 1.0 to include all (1.0 is default
Practically disabled filtering in steering_angle (twist_controller line 51). The LowPassFilter just interpolates the previous value to the current one, with some weighting, to have smooth transitions.
I saw in higher speeds, in turns, that this actually hurts driving, because in turns we want sharp reactions, and slower reactions due to the filter lead to oscillations.
Tested with speeds up to 80mph, and it drives fine (with minor oscillations)
The following changes were made
Tested with speeds up to 80mph, and it drives fine (with minor oscillations)