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);
}
