Trajectory Tracking Controller

[xXBasti]

Objective: Trajectory Tracking Controller

Implement a trajectory tracking controller for the new ROS based car. There are controller available in LMS from our previous models. The related Issue can be found here:
https://github.com/tum-phoenix/drive_project_management/issues/23
lms-org/config_auto_drive#83

The implementation of the controller can be found here:
https://github.com/lms-org/trajectory_controller/blob/master/src/trajectory_point_follower.cpp

The main Idea is firstly to port the controller from LMS to ROS. We still have contract to the developer of the controller, so that we can get some information about the implementation, which is a model predictive controller.

Tasks:

• Read the description of the old controller
• Port the existing controller to ROS and check if all sensor inputs are available at the new car.
• Adjust the parameters of the controller to the new model.
• Compare the approach with different control approaches, like a flatness based controller and get more information about model predictive controller.

[Phibedy]

One mpc controller was written by Tobi (don't know his github name). The second one was from Michi (@mp4096). The simple pid was added by me. To get started I would go with the pid first. It is easy to debug, you know what happens and when we tuned the pid parameters it outperformed the mpc controllers.
We moved the "Regelpunkt" depending on the current velocity. This really improved the mpcs/pid, but if you move it to far it will short-cut S-curves

Fun-fact:
I am quite sure that there is a mistake in the mpc model from tobi which we ignored as it worked well, but I can't remember what exactly. But it worked better than the one from michi (maybe because of the moving "Regelpunkt" and velocity "hack"). You don't have to call it hack, it just looks like a hack but it describes the reality better, so maybe one can call it mpc model with improvements :)