Merge pull request #58 from BFALacerda/hydro-devel

cancelling action properly
making sure robot gets to the correct pose on goal waypoint
replanning added for unexpected state transitions