Fixed some bugs with planner. Working on two other bugs
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.
Mades tweaks to the cycle frequencies to make trajectory execution
better. The robot still overshoots the goal and will spin
past the target orientation to reach the goal sometimes.
Moved Reflexxes calls into trajectory_generator. Removed the old
trajectory code.