Made trajectory slower. Preempt and send new trajectory.
Adjusted getCurrentStepNo. Seems to work now!!!
Tried to fix getCurrentStepNo