public void ReceiveTF_FrameWithParent_ReturnsSameTranslation()
{
ROSConnection ros = ROSConnection.GetOrCreateInstance();
ros.ConnectOnStart = false;
TFSystem system = TFSystem.GetOrCreateInstance();
TFSystem.TFTopicState topic = system.GetOrCreateTFTopic();
TFStream stream = system.GetOrCreateFrame(composite_frame_id);
Assert.AreEqual(stream.Name, simple_frame_id);
Assert.AreEqual(stream.Parent.Name, parent_frame_id);
TimeMsg time = MakeTimeMsg(4567, 890);
Vector3 unityPosition = new Vector3(1, 2, 3);
topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg(
MakeHeaderMsg(time, parent_frame_id),
composite_frame_id,
new TransformMsg(unityPosition.To <FLU>(), new QuaternionMsg()
)) }));
TFFrame frame = stream.GetWorldTF(time.ToLongTime());
Assert.AreEqual(frame.translation.x, unityPosition.x);
Assert.AreEqual(frame.translation.y, unityPosition.y);
Assert.AreEqual(frame.translation.z, unityPosition.z);
Vector3 gameObjectPos = stream.GameObject.transform.position;
Assert.AreEqual(gameObjectPos.x, unityPosition.x);
Assert.AreEqual(gameObjectPos.y, unityPosition.y);
Assert.AreEqual(gameObjectPos.z, unityPosition.z);
}
public void Add(long timestamp, Vector3 translation, Quaternion rotation)
{
TFFrame newEntry = new TFFrame(translation, rotation);
// most likely case: we're just adding a newer transform to the end of the list
if (m_Timestamps.Count == 0 || m_Timestamps[m_Timestamps.Count - 1] < timestamp)
{
m_Timestamps.Add(timestamp);
m_Frames.Add(newEntry);
m_GameObject.transform.localPosition = translation;
m_GameObject.transform.localRotation = rotation;
}
else
{
int index = m_Timestamps.BinarySearch(timestamp);
if (index < 0)
{
// no preexisting entry, but ~index gives us the position to insert the new entry
m_Timestamps.Insert(~index, timestamp);
m_Frames.Insert(~index, newEntry);
}
else
{
// we found an existing entry at the same timestamp!? Just replace the old one, I guess.
m_Frames[index] = newEntry;
}
}
// for now, just a lazy way to keep the buffer from growing infinitely: every 50 updates, discard the oldest 50
if (m_Timestamps.Count > 100)
{
m_Timestamps.RemoveRange(0, 50);
m_Frames.RemoveRange(0, 50);
}
}
public void SetTFTrackingSettings(TFTrackingSettings tfTrackingType, HeaderMsg headerMsg)
{
switch (tfTrackingType.type)
{
case TFTrackingType.Exact:
{
TFFrame frame = TFSystem.instance.GetTransform(headerMsg, tfTrackingType.tfTopic);
transform.position = frame.translation;
transform.rotation = frame.rotation;
}
break;
case TFTrackingType.TrackLatest:
{
transform.parent = TFSystem.instance.GetTransformObject(headerMsg.frame_id, tfTrackingType.tfTopic).transform;
transform.localPosition = Vector3.zero;
transform.localRotation = Quaternion.identity;
}
break;
case TFTrackingType.None:
transform.localPosition = Vector3.zero;
transform.localRotation = Quaternion.identity;
break;
}
}
public static TFFrame Lerp(TFFrame a, TFFrame b, float lerp)
{
return(new TFFrame
{
translation = Vector3.Lerp(a.translation, b.translation, lerp),
rotation = Quaternion.Lerp(a.rotation, b.rotation, lerp)
});
}
//添加元素
public void draw_Elements(Element[] elements, string name)
{
TFFrameList frame_list = new TFFrameListClass();
TFFrame frame = frame_list.AsTFFrame;
for (int i = 0; i < elements.Length; i++)
{
frame.Add3DElement(elements[i]);
}
frame.SetName(name);
TFApplicationList tfapp_list = new TFApplicationList();
TFApplication tfapp = tfapp_list.TFApplication;
tfapp.ModelReferenceAddFrame(app.ActiveModelReference, frame);
}
public TFFrame GetLocalTF(long time = 0)
{
// this stream has no data at all, so just report identity.
if (m_Frames.Count == 0)
{
return(TFFrame.identity);
}
// if time is 0, just get the newest position
if (time == 0)
{
return(m_Frames[m_Frames.Count - 1]);
}
int index = m_Timestamps.BinarySearch(time);
if (index >= 0)
{
// no problem, we have an entry at this time
return(m_Frames[index]);
}
index = ~index;
if (index == 0)
{
// older than our first entry: just use the first one
return(m_Frames[0]);
}
else if (index == m_Frames.Count)
{
// newer than our last entry: just use the last one
return(m_Frames[m_Frames.Count - 1]);
}
else
{
// between two entries: interpolate
float lerpValue = (time - m_Timestamps[index - 1]) / (float)(m_Timestamps[index] - m_Timestamps[index - 1]);
return(TFFrame.Lerp(m_Frames[index - 1], m_Frames[index], lerpValue));
}
}
public void OnChanged(TFStream stream)
{
Drawing3d drawing;
if (!drawings.TryGetValue(stream.Name, out drawing))
{
drawing = Drawing3d.Create();
drawings[stream.Name] = drawing;
if (stream.Parent != null)
{
OnChanged(stream.Parent);
Drawing3d parentStream;
if (drawings.TryGetValue(stream.Parent.Name, out parentStream))
{
drawing.transform.parent = parentStream.transform;
}
}
}
TFFrame frame = stream.GetLocalTF();
drawing.transform.localPosition = frame.translation;
drawing.transform.localRotation = frame.rotation;
drawing.Clear();
EnsureSettings(stream);
if (m_ShowAxes[stream])
{
VisualizationUtils.DrawAxisVectors <FLU>(drawing, Vector3.zero.To <FLU>(), Quaternion.identity.To <FLU>(), axesScale, false);
}
if (m_ShowLinks[stream])
{
drawing.DrawLine(Quaternion.Inverse(frame.rotation) * -frame.translation, Vector3.zero, color, lineThickness);
}
if (m_ShowNames[stream])
{
drawing.DrawLabel(stream.Name, Vector3.zero, color);
}
}
public TFFrame Compose(TFFrame child)
{
return(new TFFrame(TransformPoint(child.translation), rotation * child.rotation));
}
