adding lock to planner makePlan fail case

Same issue described (here)[https://github.com/ros-planning/navigation/pull/280]. As I say in the previous thread, I believe this is a fix for #131.