Fixed bugs to move the robot along a reflexxes trajectory.
Currently it seems to work fine when executing a single trajectory, but
it does not work well with the planner. The robot often fails to meet the
target theta to drive towards the goal. When it does meet it, the robot
does not drive straight for long - it will stop and start driving
erratically. I think this may be due to using time synchronization for
all segments of a trajectory, rather than just the segments that are
driving in a straight line.