adding support for when the optimum is actually found
Merge branch 'master' of https://github.com/utexas-bwi/bwi_experimental
updated planner cost copy in the real robot case to correctly handle the situation where the optimum is found