Add velocity to waypoint generator

[jackparsonss]

Can be an easy new member task. All we need is to add velocity to the waypoint generator. This data should be right into the odometry message, and then just feed it into the csv