public emTransform transformFrame(string source, string target, out gm.Vector3 vec, out gm.Quaternion quat)
{
emTransform trans = new emTransform();
try
{
transformer.lookupTransform(target, source, new Time(new TimeData()), out trans);
}
catch (Exception e)
{
ROS.Error(e.ToString());
trans = null;
}
if (trans != null)
{
vec = trans.translation != null?trans.translation.ToMsg() : new emVector3().ToMsg();
quat = trans.rotation != null?trans.rotation.ToMsg() : new emQuaternion().ToMsg();
}
else
{
vec = null;
quat = null;
}
return(trans);
}
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)
}
};
}
}
public void lookupTransform(String t, String s, Time time, out emTransform transform)
{
try
{
lookupTransform(t.data, s.data, time, out transform);
}
catch (Exception e)
{
transform = null;
ROS.Error(e);
throw e;
}
}
public bool setTransform(emTransform transform)
{
emTransform mapped_transform = new emTransform(transform.rotation, transform.translation, 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);
}
lock (framemutex)
{
uint frame_number = lookupOrInsertFrameNumber(mapped_transform.child_frame_id);
TimeCache frame = null;
if (!frames.ContainsKey(frame_number))
{
frames[frame_number] = new TimeCache(cache_time);
frame = frames[frame_number];
}
else
{
frame = frames[frame_number];
}
if (frame.insertData(new TransformStorage(mapped_transform, lookupOrInsertFrameNumber(mapped_transform.frame_id), frame_number)))
{
// authority? meh
}
else
{
return(false);
}
}
return(true);
}
public void lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform)
{
string error_string = null;
lookupTransform(target_frame, source_frame, time, out transform, ref error_string);
}