fix problem that isGoalReached checked goal with not current_pose_
Merge pull request #179 from YoheiKakiuchi/groovy-devel-fix-isgoalreached

fix problem that isGoalReached checked goal without updating current_pose_. 

I have this same change in a different branch.