public bool lookupTransform(string target_frame, string source_frame, Time time, out Transform transform, out string error_string) { transform = null; error_string = null; string mapped_tgt = resolve(tf_prefix, target_frame); string mapped_src = resolve(tf_prefix, source_frame); if (mapped_tgt == mapped_src) { transform = new Transform(); transform.origin = new Vector3(); transform.basis = new Quaternion(); transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = ROS.GetTime(DateTime.UtcNow); 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, out 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 Transform(); 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(TimeData.FromTicks((long)accum.time)); } return(retval == TF_STATUS.NO_ERROR); }
public bool lookupTransform(string target_frame, string source_frame, Time time, out Transform transform, out string error_string) { transform = null; error_string = null; string mapped_tgt = resolve(tf_prefix, target_frame); string mapped_src = resolve(tf_prefix, source_frame); if (mapped_tgt == mapped_src) { transform = new Transform(); transform.Origin = new Vector3(); transform.Basis = new Quaternion(); transform.ChildFrameId = mapped_src; transform.FrameId = mapped_tgt; transform.Stamp = DateTime.UtcNow.ToTimeMessage(); return(true); } TfStatus 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, out error_string); if (retval != TfStatus.NoError) { error_string = error_string ?? "UNSPECIFIED"; switch (retval) { case TfStatus.ConnectivityError: error_string = "NO CONNECTION: " + error_string; break; case TfStatus.ExtrapolationError: error_string = "EXTRAPOLATION: " + error_string; break; case TfStatus.LookupError: 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 Transform(); transform.Origin = accum.result_vec; transform.Basis = accum.result_quat; transform.ChildFrameId = mapped_src; transform.FrameId = mapped_tgt; transform.Stamp = new Time(TimeData.FromTicks((long)accum.time)); } return(retval == TfStatus.NoError); }