Fixing bugs from the action server

Hi all,

I was fixing problems with the action server so that now the human following is preemptable.
Also add a routine example.