public TransformStorage(Transform data, uint frameId, uint childFrameId) { this.Rotation = data.Basis; this.Translation = data.Origin; this.Stamp = TimeCache.ToLong(data.Stamp.data); this.FrameId = frameId; this.ChildFrameId = childFrameId; }
private bool canTransformNoLock(uint target_id, uint source_id, Time time, out string error_msg) { error_msg = null; if (target_id == 0 || source_id == 0) { return(false); } CanTransformAccum accum = new CanTransformAccum(); if (walkToTopParent(accum, TimeCache.ToLong(time.data), target_id, source_id, out error_msg) == TfStatus.NoError) { return(true); } return(false); }
private TfStatus getLatestCommonTime(uint target_id, uint source_id, ref ulong time, out string error_str) { error_str = null; if (target_id == source_id) { time = TimeCache.ToLong(ROS.GetTime().data); return(TfStatus.NoError); } List <TimeAndFrameId> lct = new List <TimeAndFrameId>(); uint frame = source_id; uint depth = 0; ulong common_time = ulong.MaxValue; while (frame != 0) { TimeCache cache; if (!frames.ContainsKey(frame)) { break; } cache = frames[frame]; TimeAndFrameId latest = cache.GetLatestTimeAndParent(); if (latest.FrameId == 0) { break; } if (latest.Time != 0) { common_time = Math.Min(latest.Time, common_time); } lct.Add(latest); frame = latest.FrameId; if (frame == target_id) { time = common_time; if (time == ulong.MaxValue) { time = 0; } return(TfStatus.NoError); } ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_str != null) { error_str = "The tf tree is invalid because it contains a loop."; } return(TfStatus.LookupError); } } frame = target_id; depth = 0; common_time = ulong.MaxValue; uint common_parent = 0; while (true) { TimeCache cache; if (!frames.ContainsKey(frame)) { break; } cache = frames[frame]; TimeAndFrameId latest = cache.GetLatestTimeAndParent(); if (latest.FrameId == 0) { break; } if (latest.Time != 0) { common_time = Math.Min(latest.Time, common_time); } foreach (TimeAndFrameId tf in lct) { if (tf.FrameId == latest.FrameId) { common_parent = tf.FrameId; break; } } frame = latest.FrameId; if (frame == source_id) { time = common_time; if (time == uint.MaxValue) { time = 0; } return(TfStatus.NoError); } ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_str != null) { error_str = "The tf tree is invalid because it contains a loop."; } return(TfStatus.LookupError); } } if (common_parent == 0) { error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id]; return(TfStatus.ConnectivityError); } for (int i = 0; i < lct.Count; i++) { if (lct[i].Time != 0) { common_time = Math.Min(common_time, lct[i].Time); } if (lct[i].FrameId == common_parent) { break; } } if (common_time == uint.MaxValue) { common_time = 0; } time = common_time; return(TfStatus.NoError); }
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); }