srv done. Tf error exists: TF_NAN_INPUT during setInitPose