Output debug code
Added the ROS_INFO for testing waypoints navigation
Fixed a problem that failed save waypoints
Avoid an error that 'Map update control missing'
Merge pull request #19 from open-rdc/improve-nav-error

Avoid an error that 'Map update control missing'