Changes made to switch trajectories at real time
The robot package allows for a Corobot to switch its current trajectory at real-time. It still overshoots the goal some. It has not been tested on the ground yet.