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