Controller::mpc_based_max_vel() and Controller::update() interaction should be refactored

[lewie-donckers]

Controller::mpc_based_max_vel() repeatedly calls Controller::update() but rolls back some of its side effects. This is a very convoluted and error-prone setup.

update() sets the following member variables:

• current_with_carrot_
• current_pos_on_plan_
• current_goal_
• distance_to_goal_ by findPositionOnPlan() - see Controller::findPositionOnPlan() should be refactored #123
• controller_state_
• current_target_x_vel_

Before mpc_based_max_vel() calls update() it creates a backup of controller_state_ and at certain points in its body (and just before the end) it overwrites the current state with this backup.

I propose the following:

• Ideally mpc_based_max_vel() should be a const function that does not modify any state. Based on the name and function documentation, this should be possible.
• If that's not feasible, update() should be refactored/split in such a way that at the very least no side effects need to be rolled back in mpc_based_max_vel().

[cesar-lopez-mar]

After our discussion indeed this can be changed by better making a copy of the state and running the simulation (calls to update included) using that copy. Currently we use the member variable in update so that part needs refactor