Fix the bug in the planner as discussed with ErikZalm