public TransformStorage(emTransform data, uint frame_id, uint child_frame_id) { rotation = data.rotation; translation = data.translation; stamp = TimeCache.toLong(data.stamp.data); this.frame_id = frame_id; this.child_frame_id = child_frame_id; }
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) } }; } }
private TF_STATUS getLatestCommonTime(uint target_id, uint source_id, ref ulong time, ref string error_str) { if (target_id == source_id) { time = TimeCache.toLong(ROS.GetTime(DateTime.Now).data); return(TF_STATUS.NO_ERROR); } List <TimeAndFrameID> lct = new List <TimeAndFrameID>(); uint frame = source_id; TimeAndFrameID temp; 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.frame_id == 0) { break; } if (latest.time != 0) { common_time = Math.Min(latest.time, common_time); } lct.Add(latest); frame = latest.frame_id; if (frame == target_id) { time = common_time; if (time == ulong.MaxValue) { time = 0; } return(TF_STATUS.NO_ERROR); } ++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; 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.frame_id == 0) { break; } if (latest.time != 0) { common_time = Math.Min(latest.time, common_time); } foreach (TimeAndFrameID tf in lct) { if (tf.frame_id == latest.frame_id) { common_parent = tf.frame_id; break; } } frame = latest.frame_id; if (frame == source_id) { time = common_time; if (time == uint.MaxValue) { time = 0; } return(TF_STATUS.NO_ERROR); } ++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 (common_parent == 0) { error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id]; return(TF_STATUS.CONNECTIVITY_ERROR); } 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].frame_id == common_parent) { break; } } if (common_time == uint.MaxValue) { common_time = 0; } time = common_time; return(TF_STATUS.NO_ERROR); }