Now publishing human position, velocity on the same topic as a PoseArray
The human safety signal works now. Only need to tune parameters next.