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