Implement person following and smach FSM