Added Waypoint class changes, added back RvizWidget.