public void lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform, ref string error_string) { transform = new emTransform(); string mapped_tgt = resolve(tf_prefix, target_frame); string mapped_src = resolve(tf_prefix, source_frame); if (mapped_tgt == mapped_src) { transform.translation = new emVector3(); transform.rotation = new emQuaternion(); transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = ROS.GetTime(DateTime.Now); return; } lock (framemutex) { uint target_id = getFrameID(mapped_tgt); uint source_id = getFrameID(mapped_src); TransformAccum accum = new TransformAccum(); TF_STATUS retval = walkToTopParent(accum, TimeCache.toLong(time.data), target_id, source_id, ref error_string); if (error_string != null && retval != TF_STATUS.NO_ERROR) { switch (retval) { case TF_STATUS.CONNECTIVITY_ERROR: ROS.Error("NO CONNECTIONSZSZ: " + error_string); break; case TF_STATUS.EXTRAPOLATION_ERROR: ROS.Error("EXTRAPOLATION: " + error_string); break; case TF_STATUS.LOOKUP_ERROR: ROS.Error("LOOKUP: " + error_string); break; } } transform.translation = accum.result_vec; transform.rotation = accum.result_quat; transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = new Time { data = new TimeData { sec = (uint)(accum.time >> 32), nsec = (uint)(accum.time & 0xFFFFFFFF) } }; } }
public TF_STATUS walkToTopParent <F>(F f, ulong time, uint target_id, uint source_id, out string error_str) where F : ATransformAccum { error_str = null; if (target_id == source_id) { f.finalize(WalkEnding.Identity, time); return(TF_STATUS.NO_ERROR); } if (time == 0) { TF_STATUS retval = getLatestCommonTime(target_id, source_id, ref time, out error_str); if (retval != TF_STATUS.NO_ERROR) { return(retval); } } uint frame = source_id; uint top_parent = frame; uint depth = 0; while (frame != 0) { if (!frames.ContainsKey(frame)) { top_parent = frame; break; } TimeCache cache = frames[frame]; uint parent = f.gather(cache, time, out error_str); if (parent == 0) { top_parent = frame; break; } if (frame == target_id) { f.finalize(WalkEnding.TargetParentOfSource, time); return(TF_STATUS.NO_ERROR); } f.accum(true); top_parent = frame; frame = parent; ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_str != null) { error_str = "The tf tree is invalid because it contains a loop."; } return(TF_STATUS.LOOKUP_ERROR); } } frame = target_id; depth = 0; while (frame != top_parent) { if (!frames.ContainsKey(frame)) { break; } TimeCache cache = frames[frame]; uint parent = f.gather(cache, time, out error_str); if (parent == 0) { if (error_str != null) { error_str += ", when looking up transform from frame [" + frameids_reverse[source_id] + "] to [" + frameids_reverse[target_id] + "]"; } return(TF_STATUS.EXTRAPOLATION_ERROR); } if (frame == source_id) { f.finalize(WalkEnding.SourceParentOfTarget, time); return(TF_STATUS.NO_ERROR); } f.accum(false); frame = parent; ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_str != null) { error_str = "The tf tree is invalid because it contains a loop."; } return(TF_STATUS.LOOKUP_ERROR); } } if (frame != top_parent) { if (error_str != null) { error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id]; } return(TF_STATUS.CONNECTIVITY_ERROR); } f.finalize(WalkEnding.FullPath, time); return(TF_STATUS.NO_ERROR); }