fix problem that isGoalReached checked goal without updating current_pose_

This patch is a part of 
https://github.com/ros-planning/navigation/commit/97c2c7844cc00b489d3daa21b505fe7f080e6d5b

isGoalReached returned true, before updating current_pose.