Tested anytime planner logic.
Cleaned up logic for non-anytime planners.