Esempio n. 1
0
        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);
        }
Esempio n. 2
0
        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);
        }
Esempio n. 3
0
        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);
        }