public void ReceiveTF_MultipleMessages_InterpolatesTimes()
{
ROSConnection ros = ROSConnection.GetOrCreateInstance();
ros.ConnectOnStart = false;
TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic();
TFStream stream = topic.GetOrCreateFrame(simple_frame_id);
int time1_secs = 1000;
int time2_secs = 2000;
TimeMsg time1 = MakeTimeMsg(time1_secs, 0);
Vector3 unityPosition1 = new Vector3(1, 2, 3);
topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg(
MakeHeaderMsg(time1, parent_frame_id),
simple_frame_id,
new TransformMsg(unityPosition1.To <FLU>(), new QuaternionMsg()
)) }));
TimeMsg time2 = MakeTimeMsg(time2_secs, 0);
Vector3 unityPosition2 = new Vector3(2, 3, 4);
topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg(
MakeHeaderMsg(time2, parent_frame_id),
simple_frame_id,
new TransformMsg(unityPosition2.To <FLU>(), new QuaternionMsg()
)) }));
TimeMsg time1_5 = MakeTimeMsg((time1_secs + time2_secs) / 2, 0);
Vector3 pointAtTime1 = stream.GetWorldTF(time1).translation;
Vector3 pointAtTime1_5 = stream.GetWorldTF(time1_5).translation;
Vector3 pointAtTime2 = stream.GetWorldTF(time2).translation;
Vector3 unityPosition1_5 = (unityPosition1 + unityPosition2) / 2;
Assert.AreEqual(pointAtTime1, unityPosition1);
Assert.AreEqual(pointAtTime1_5, unityPosition1_5);
Assert.AreEqual(pointAtTime2, unityPosition2);
}
public void GetOrCreateFrame_Hierarchy_BuildsValidHierarchy()
{
ROSConnection ros = ROSConnection.GetOrCreateInstance();
ros.ConnectOnStart = false;
TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic();
TFStream stream = topic.GetOrCreateFrame(composite_frame_id);
GameObject gameObject = stream.GameObject;
Assert.IsNotNull(stream.GameObject);
Assert.AreEqual(stream.GameObject.name, simple_frame_id);
Assert.AreEqual(stream.GameObject.transform.parent.name, parent_frame_id);
TFStream stream2 = topic.GetOrCreateFrame(composite_frame_id2);
Assert.IsNotNull(stream2.GameObject);
Assert.AreEqual(stream2.GameObject.name, simple_frame_id2);
Assert.AreNotEqual(stream.GameObject, stream2.GameObject);
Assert.AreEqual(stream.GameObject.transform.parent, stream2.GameObject.transform.parent);
}