Controller::findPositionOnPlan() should be refactored

[lewie-donckers]

Controller::findPositionOnPlan() should be refactored:

1. Its name suggests it only finds a position but it also sets member variable distance_to_goal_. It is incorrect (or at least odd) to only change the distance but not any other state (current plan index, etc). I propose we make this function const and do not set any data members. It should just do as the name suggests and find something. If required this value can be made part of the return value. (But perhaps it might be easy to calculate by the caller.)
2. Parameter path_pose_idx is an output parameter. I propose we change it to be part of the return value.
3. Parameter controller_state contains way too much state. Only the following members are used:
   • last_visited_pose_index is used as an output parameter. I propose we change it to be part of the return value.
   • current_global_plan_index is used as an in-/output parameter. I propose we split this into a pure input parameter and make the output part of the return value.

[cesar-lopez-mar]

Seems a good idea!