Fixed bug where agents trying to explore one more node would not be able to plan
Merge branch 'Release/MileStone3/debug' of https://github.com/srnjcbsn/Beduin into Release/MileStone3/debug