moved clearCostmapWindows before first plan attempt