Exemple #1
0
        static void Main(string[] args)
        {
            ROS.Init(args, "tf_example");
            var asyncSpinner = new AsyncSpinner();

            asyncSpinner.Start();
            NodeHandle nh = new NodeHandle();

            ROS.Info()("This node will create a Transformer to compare lookup results between four source/target " +
                       "frame pairs of an OpenNI2 node's transform tree with ones observed in linux with tf_echo"
                       );

            while (ROS.OK && !ROS.ShuttingDown && (!Master.GetNodes().Result.Contains("/camera/driver")))
            {
                ROS.Error()("For this to work, you need to \"roslaunch openni2_launch openni2.launch\" on a PC with" +
                            "an ASUS Xtion or PrimeSense Carmine sensor plugged into it, and connect to the same master"
                            );
                Thread.Sleep(2000);
            }
            if (ROS.OK && !ROS.ShuttingDown)
            {
                tfer = new tf.Transformer(false);

                #region tf_echo results

                /*
                 * tf_echo camera_link camera_rgb_frame
                 *      (0.0,-0.045,0.0)
                 *      (0,0,0,1)
                 */
                tf.Transform result1 = new tf.Transform()
                {
                    basis          = new tf.Quaternion(0, 0, 0, 1),
                    origin         = new tf.Vector3(0, -0.045, 0),
                    child_frame_id = "camera_rgb_frame",
                    frame_id       = "camera_link"
                };

                /*
                 * tf_echo camera_link camera_rgb_optical_frame
                 *      (0.0,-0.045,0.0)
                 *      (-0.5,0.5,-0.5,0.5)
                 */
                tf.Transform result2 = new tf.Transform()
                {
                    basis          = new tf.Quaternion(-0.5, 0.5, -0.5, 0.5),
                    origin         = new tf.Vector3(0.0, 0.0, 0.0),
                    child_frame_id = "camera_rgb_optical_frame",
                    frame_id       = "camera_rgb_frame"
                };

                /*
                 * tf_echo camera_rgb_frame camera_depth_frame
                 *      (0.0,0.25,0.0)
                 *      (0,0,0,1)
                 */
                tf.Transform result3 = new tf.Transform()
                {
                    basis          = new tf.Quaternion(0, 0, 0, 1),
                    origin         = new tf.Vector3(0.0, -0.02, 0.0),
                    child_frame_id = "camera_depth_frame",
                    frame_id       = "camera_link"
                };

                /*
                 * tf_echo camera_rgb_optical_frame camera_depth_frame
                 *      (-0.25,0.0,0.0)
                 *      (0.5,-0.5,0.5,0.5)
                 */
                tf.Transform result4 = new tf.Transform()
                {
                    basis          = new tf.Quaternion(-0.5, 0.5, -0.5, 0.5),
                    origin         = new tf.Vector3(0.0, 0.0, 0.0),
                    child_frame_id = "camera_depth_optical_frame",
                    frame_id       = "camera_depth_frame"
                };

                #endregion

                tf.Transform test1 = null, test2 = null, test3 = null, test4 = null;
                do
                {
                    if (test1 == null || !string.Equals(result1.ToString(), test1.ToString()))
                    {
                        test1 = testLookup(result1);
                    }
                    if (!ROS.OK || ROS.ShuttingDown)
                    {
                        break;
                    }
                    if (test2 == null || !string.Equals(result2.ToString(), test2.ToString()))
                    {
                        test2 = testLookup(result2);
                    }
                    if (!ROS.OK || ROS.ShuttingDown)
                    {
                        break;
                    }
                    if (test3 == null || !string.Equals(result3.ToString(), test3.ToString()))
                    {
                        test3 = testLookup(result3);
                    }
                    if (!ROS.OK || ROS.ShuttingDown)
                    {
                        break;
                    }
                    if (test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
                    {
                        test4 = testLookup(result4);
                    }
                    Thread.Sleep(100);
                } while (ROS.OK && !ROS.ShuttingDown && (
                             test1 == null || !string.Equals(result1.ToString(), test1.ToString()) ||
                             test2 == null || !string.Equals(result2.ToString(), test2.ToString()) ||
                             test3 == null || !string.Equals(result3.ToString(), test3.ToString()) ||
                             test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
                         );
            }
            if (ROS.OK && !ROS.ShuttingDown)
            {
                Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit");
                Console.ReadLine();
                ROS.Shutdown();
            }
        }