Does Controller::findPositionOnPlan() do what is intended?

[lewie-donckers]

While refactoring Controller::findPositionOnPlan(), I noticed the following. I'm not sure if it is intended or not so I would like some feedback. Depending on the feedback we can determine what to do (nothing, document, fix, etc).

In that function we look ahead in the plan based on a given index. As long as each next point is a better match, we keep looking and update the given index. Once we're done with looking ahead, we start looking backward. Now one of two things will happen:

• either we found no better match when looking forward, and we start looking backward as expected
• we did find a better match when looking forward and - because we updated the given index (= starting point) - looking backward from this index will immediately fail.

So in practice we look either forward or backward but never both. Is this intended?

I've included the code below and stripped the bits that were less relevant:

  // First look in current position and in front
  for (auto idx_path = global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) {
    ...
    if (...) {
      ...
      global_plan_index = idx_path;
    } else {
      break;
    }
  }

  // Then look backwards
  for (auto idx_path = global_plan_index; idx_path > 0; --idx_path) {
    ...
  }

[cesar-lopez-mar]

At this moment I am not sure if this was the intended behavior at the moment of coding that piece, but I would say the current behavior is actually desirable. The idea is to make progress along the path, so if we find a better position ahead of us, we should not look backwards. So we should make this more explicit in the implementation.