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 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 Start() { TFSystem.GetOrCreateInstance().AddListener(OnChanged); HudPanel.RegisterTab(this, (int)HudTabOrdering.TF); if (color.a == 0) { color.a = 1; } }
public static TFSystem GetOrCreateInstance() { if (instance != null) { return(instance); } ROSConnection ros = ROSConnection.GetOrCreateInstance(); instance = new TFSystem(); foreach (string s in ros.TFTopics) { instance.GetOrCreateTFTopic(s); } return(instance); }
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); }