Add option to specify yaw at waypoints in the trajectory tracker

[kartikmohta]

Currently the trajectory tracker does not touch the yaw, it maintains the initial yaw. The yaw should be allowed to change along the trajectory by specifying the yaw at the waypoints. This would require taking the time to yaw into account when computing the segment times.

Implementing the option to have yaw be tangential to the trajectory at every point is a bit hard in a simple tracker like this, so that is a task for another time 🙂

[tdinesh]

How about adding a bool flag in TrajectoryTracker.action to parse yaw from waypoints?