planner: add check before filling objects_failed vector

[gsaponaro]

This fixes the problems that were occurring near the message 'horizon too large, jumping to next goal'.

Thanks @AlexAntn