public bool lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform, ref string error_string) { transform = null; string mapped_tgt = resolve(tf_prefix, target_frame); string mapped_src = resolve(tf_prefix, source_frame); if (mapped_tgt == mapped_src) { transform = new emTransform(); transform.origin = new emVector3(); transform.basis = new emQuaternion(); transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = ROS.GetTime(DateTime.Now); return(true); } TF_STATUS retval; uint target_id = getFrameIDInternal(mapped_tgt); uint source_id = getFrameIDInternal(mapped_src); TransformAccum accum = new TransformAccum(); retval = walkToTopParent(accum, TimeCache.toLong(time.data), target_id, source_id, ref error_string); if (retval != TF_STATUS.NO_ERROR) { error_string = error_string ?? "UNSPECIFIED"; switch (retval) { case TF_STATUS.CONNECTIVITY_ERROR: error_string = "NO CONNECTIONSZSZ: " + error_string; break; case TF_STATUS.EXTRAPOLATION_ERROR: error_string = "EXTRAPOLATION: " + error_string; break; case TF_STATUS.LOOKUP_ERROR: error_string = "LOOKUP: " + error_string; break; default: if (accum.result_quat == null || accum.result_vec == null) { error_string = "ACCUM WALK FAIL!"; } break; } } if (accum.result_vec != null && accum.result_quat != null) { transform = new emTransform(); transform.origin = accum.result_vec; transform.basis = accum.result_quat; transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = new Time(ROS.ticksToData((long)accum.time)); } return(retval == TF_STATUS.NO_ERROR); }