Added SetWaypoint and WaypointCommand ServiceServers and callback functions to telemetry.h
Merge branch 'groovy-devel' of https://github.com/smd-ros-devel/asctec_drivers into groovy-devel