public TransformStorage(Transform data, uint frameId, uint childFrameId)
{
this.rotation = data.basis;
this.translation = data.origin;
this.stamp = TimeCache.toLong(data.stamp.data);
this.frame_id = frameId;
this.child_frame_id = childFrameId;
}
public override uint Gather(TimeCache cache, ulong time, out string errorMessage)
{
if (!cache.GetData(time, ref st, out errorMessage))
{
return(0);
}
return(st.FrameId);
}
public override uint gather(TimeCache cache, ulong time_, out string error_str)
{
if (!cache.getData(time_, ref st, out error_str))
{
return(0);
}
return(st.frame_id);
}
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) == TF_STATUS.NO_ERROR)
{
return(true);
}
return(false);
}
public bool setTransform(Transform transform)
{
Transform mapped_transform = new Transform(transform.basis, transform.origin, transform.stamp, transform.frame_id, transform.child_frame_id);
mapped_transform.child_frame_id = resolve(tf_prefix, transform.child_frame_id);
mapped_transform.frame_id = resolve(tf_prefix, transform.frame_id);
if (mapped_transform.child_frame_id == mapped_transform.frame_id)
{
return(false);
}
if (mapped_transform.child_frame_id == "/")
{
return(false);
}
if (mapped_transform.frame_id == "/")
{
return(false);
}
uint frame_number = lookupOrInsertFrameNumber(mapped_transform.frame_id);
uint child_frame_number = lookupOrInsertFrameNumber(mapped_transform.child_frame_id);
TimeCache parent_frame = null, frame = null;
if (!frames.ContainsKey(frame_number))
{
parent_frame = frames[frame_number] = new TimeCache(cache_time);
}
if (!frames.ContainsKey(child_frame_number))
{
frame = frames[child_frame_number] = new TimeCache(cache_time);
}
else
{
//if we're revising a frame, that was previously labelled as having no parent, clear that knowledge from the time cache
frame = frames[child_frame_number];
}
return(frame.insertData(new TransformStorage(mapped_transform, frame_number, child_frame_number)));
}
private TF_STATUS 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(DateTime.UtcNow).data);
return(TF_STATUS.NO_ERROR);
}
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.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);
}
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);
}
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 override uint Gather(TimeCache cache, ulong time, out string errorMessage)
{
return(cache.GetParent(time, out errorMessage));
}
public abstract uint Gather(TimeCache cache, ulong time, out string errorMessage);
public override uint gather(TimeCache cache, ulong time, out string error_str)
{
return(cache.getParent(time, out error_str));
}
public abstract uint gather(TimeCache cache, ulong time, out string error_str);
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 TfStatus 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(TfStatus.NoError);
}
if (time == 0)
{
TfStatus retval = getLatestCommonTime(target_id, source_id, ref time, out error_str);
if (retval != TfStatus.NoError)
{
return(retval);
}
}
uint frame = source_id;
uint top_parent = frame;
uint depth = 0;
var extrapolationErrorMightHaveOccurred = false;
string extrapolationErrorString = null;
while (frame != 0)
{
if (!frames.ContainsKey(frame))
{
top_parent = frame;
break;
}
TimeCache cache = frames[frame];
uint parent = f.Gather(cache, time, out extrapolationErrorString);
if (parent == 0)
{
top_parent = frame;
extrapolationErrorMightHaveOccurred = true;
break;
}
if (frame == target_id)
{
f.Finalize(WalkEnding.TargetParentOfSource, time);
error_str = null;
return(TfStatus.NoError);
}
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(TfStatus.LookupError);
}
}
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(TfStatus.ExtrapolationError);
}
if (frame == source_id)
{
f.Finalize(WalkEnding.SourceParentOfTarget, time);
error_str = null;
return(TfStatus.NoError);
}
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(TfStatus.LookupError);
}
}
if (frame != top_parent)
{
if (extrapolationErrorMightHaveOccurred)
{
if (extrapolationErrorString != null)
{
error_str = extrapolationErrorString + ", when looking up transform from frame [" + frameids_reverse[source_id] + "] to [" + frameids_reverse[target_id] + "]";
}
}
error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id];
return(TfStatus.ConnectivityError);
}
f.Finalize(WalkEnding.FullPath, time);
error_str = null;
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);
}
