works kind of but only seems to send rigid body of 4, not sure if it is the mocap or the code.  Need to research more.  But /tf is publishing what looks like correct data.