First commit of beluga waypoint controller.
Fixed wrap to pi in beluga waypoint controller