Functioning version of convert_tf_to_pose developed - CL (other stuff also modified)