Add kinematic bicycle model and some example plots

(@dejanpan @jitrc @s-mlr @JWhitleyWork FYI)

This is the next step after !330 (closed) is merged into https://gitlab.com/smerkli-embotech/AutowareAuto/-/commits/408-design-and-create-controller-test-env. I based my changes on @s-mlr's state in !330 (closed), so if !330 (closed) is merged into branch 408 first and then the MR here, there should be no merge-related issues.

The code performs a simulation with a reference trajectory of a straight line and shows an X-Y plot of the CoG (on the left) as well as a velocity-versus-time plot (on the right) when it's done. Behavior is still quite iffy: (I forgot to label the axes, it's all in SI units). The reference velocity is 1 meter per second, and the reference trajectory goes from (x,y) = (0,0) to (x,y) = (100,0), where I added an offset of 0.1m in y direction to the initial state to make the behavior a bit more interesting. At this point I attribute that to my code still being buggy and not really MPC. However, now we have a basis of very little code that we can incrementally debug and look for issues in.

The code here is still in pretty bad shape, but it was all I could do with the rest of the hours available to me this week. Some issues in particular are:

• The conversion from the complex heading to an angle and back may be wrong (I've left TODOs in the code)
• The concept of simulation time vs ROS time has to be discussed and looked at. Right now the code uses a mix, and that alone could be the source of problems.
• Interfaces were not thought about enough yet for time reasons.
• Very little testing has gone into this so far overall, for time reasons again.
• There is no real-time publishing of the state and control messages yet, they get logged to memory and analyzed afterwards.
• There are plenty of magic numbers around. They are OK to understand from context, but should be moved to more appropriate places. I think the worst offender is the car dimensions, I just hardcoded something in that'll obviously be wrong.
• The mode of simulation is to be discussed as well. We currently control the simulation progress from ROS topic callbacks. This is OK in terms of determinism and data flow, but may be a bit more tricky to understand than having the simulation main loop take control over the thread and just block waiting for ROS messages.

Things can be run by building the code using colcon build --packages-up-to mpc_controller_node motion_model_testing_simulator controller_testing, then sourcing the setup file source install/setup.bash, then ros2 launch controller_testing controller_testing_node.launch.py. @JWhitleyWork one thing that won't work yet in the current situation is that Ubuntu's python3-scipy package is a version from 2017, which is too old for us to use. I've installed it with pip3 install --user scipy instead, that worked out.

The next steps are to merge these things into 408 so we can have a good base to go off and work in parallel again on Monday (Friday is a public holiday in Switzerland and Germany). In order to try and organize that, I'll talk to @JWhitleyWork later today to coordinate.