Beispiel #1
0
 public TransformStorage(emTransform data, uint frame_id, uint child_frame_id)
 {
     rotation            = data.basis;
     translation         = data.origin;
     stamp               = TimeCache.toLong(data.stamp.data);
     this.frame_id       = frame_id;
     this.child_frame_id = child_frame_id;
 }
Beispiel #2
0
        public void transformQuaternion(string target_frame, Stamped <emQuaternion> stamped_in, ref Stamped <emQuaternion> stamped_out)
        {
            emTransform trans = new emTransform();

            lookupTransform(target_frame, stamped_in.frame_id, stamped_in.stamp, out trans);
            stamped_out.data     = trans * stamped_in.data;
            stamped_out.stamp    = trans.stamp;
            stamped_out.frame_id = target_frame;
        }
Beispiel #3
0
        public void transformVector(string target_frame, Stamped <emVector3> stamped_in, ref Stamped <emVector3> stamped_out)
        {
            emTransform trans = new emTransform();

            lookupTransform(target_frame, stamped_in.frame_id, stamped_in.stamp, out trans);
            emVector3 end    = stamped_in.data;
            emVector3 origin = new emVector3(0, 0, 0);
            emVector3 output = (trans * end) - (trans * origin);

            stamped_out.data     = output;
            stamped_out.stamp    = trans.stamp;
            stamped_out.frame_id = target_frame;
        }
Beispiel #4
0
 private static emTransform testLookup(emTransform intendedResult)
 {
     if (!tfer.waitForTransform(intendedResult.frame_id, intendedResult.child_frame_id, intendedResult.stamp, new Duration(new TimeData(1, 0)), null))
         return null;
     emTransform ret = new emTransform();
     if (tfer.lookupTransform(intendedResult.frame_id, intendedResult.child_frame_id, intendedResult.stamp, out ret))
     {
         Console.WriteLine("***  " + intendedResult.frame_id + " ==> " + intendedResult.child_frame_id + " ***");
         Console.WriteLine("********************** IDEAL *********************");
         Console.WriteLine(intendedResult);
         Console.WriteLine("********************** ACTUAL ********************");
         Console.WriteLine(ret);
         Console.WriteLine("***************************************************\n\n");
         return ret;
     }
     return null;
 }
Beispiel #5
0
        public bool setTransform(emTransform transform)
        {
            emTransform mapped_transform = new emTransform(transform.basis, transform.origin, transform.stamp, transform.frame_id, transform.child_frame_id);

            mapped_transform.child_frame_id = resolve(tf_prefix, transform.child_frame_id);
            mapped_transform.frame_id       = resolve(tf_prefix, transform.frame_id);

            if (mapped_transform.child_frame_id == mapped_transform.frame_id)
            {
                return(false);
            }
            if (mapped_transform.child_frame_id == "/")
            {
                return(false);
            }
            if (mapped_transform.frame_id == "/")
            {
                return(false);
            }
            uint      frame_number = lookupOrInsertFrameNumber(mapped_transform.frame_id);
            uint      child_frame_number = lookupOrInsertFrameNumber(mapped_transform.child_frame_id);
            TimeCache parent_frame = null, frame = null;

            if (!frames.ContainsKey(frame_number))
            {
                parent_frame = frames[frame_number] = new TimeCache(cache_time);
            }
            if (!frames.ContainsKey(child_frame_number))
            {
                frame = frames[child_frame_number] = new TimeCache(cache_time);
            }
            else
            {
                //if we're revising a frame, that was previously labelled as having no parent, clear that knowledge from the time cache
                frame = frames[child_frame_number];
            }
            return(frame.insertData(new TransformStorage(mapped_transform, frame_number, child_frame_number)));
        }
Beispiel #6
0
        public bool lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform, ref string error_string)
        {
            transform = null;

            string mapped_tgt = resolve(tf_prefix, target_frame);
            string mapped_src = resolve(tf_prefix, source_frame);

            if (mapped_tgt == mapped_src)
            {
                transform                = new emTransform();
                transform.origin         = new emVector3();
                transform.basis          = new emQuaternion();
                transform.child_frame_id = mapped_src;
                transform.frame_id       = mapped_tgt;
                transform.stamp          = ROS.GetTime(DateTime.Now);
                return(true);
            }

            TF_STATUS retval;
            uint      target_id = getFrameIDInternal(mapped_tgt);
            uint      source_id = getFrameIDInternal(mapped_src);

            TransformAccum accum = new TransformAccum();

            retval = walkToTopParent(accum, TimeCache.toLong(time.data), target_id, source_id, ref error_string);
            if (retval != TF_STATUS.NO_ERROR)
            {
                error_string = error_string ?? "UNSPECIFIED";
                switch (retval)
                {
                case TF_STATUS.CONNECTIVITY_ERROR:
                    error_string = "NO CONNECTIONSZSZ: " + error_string;
                    break;

                case TF_STATUS.EXTRAPOLATION_ERROR:
                    error_string = "EXTRAPOLATION: " + error_string;
                    break;

                case TF_STATUS.LOOKUP_ERROR:
                    error_string = "LOOKUP: " + error_string;
                    break;

                default:
                    if (accum.result_quat == null || accum.result_vec == null)
                    {
                        error_string = "ACCUM WALK FAIL!";
                    }
                    break;
                }
            }
            if (accum.result_vec != null && accum.result_quat != null)
            {
                transform                = new emTransform();
                transform.origin         = accum.result_vec;
                transform.basis          = accum.result_quat;
                transform.child_frame_id = mapped_src;
                transform.frame_id       = mapped_tgt;
                transform.stamp          = new Time(ROS.ticksToData((long)accum.time));
            }
            return(retval == TF_STATUS.NO_ERROR);
        }
Beispiel #7
0
        public bool lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform)
        {
            string error_string = null;
            bool   result       = lookupTransform(target_frame, source_frame, time, out transform, ref error_string);

            if (!result && error_string != null)
            {
                ROS.Error(error_string);
            }
            return(result);
        }
Beispiel #8
0
        static void Main(string[] args)
        {
            ROS.Init(args, "tf_example");

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

            string[] nodes = null;
            while (ROS.ok && !ROS.shutting_down && (!master.getNodes(ref nodes) || !nodes.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.shutting_down)
            {
                tfer = new Transformer(false);

                #region tf_echo results

                /*
             * tf_echo camera_link camera_rgb_frame
             *      (0.0,-0.045,0.0)
             *      (0,0,0,1)
             */
                emTransform result1 = new emTransform() {basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(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)
             */
                emTransform result2 = new emTransform() {basis = new emQuaternion(-0.5, 0.5, -0.5, 0.5), origin = new emVector3(0.0, -0.045, 0.0), child_frame_id = "camera_rgb_optical_frame", frame_id = "camera_link"};

                /*
             * tf_echo camera_rgb_frame camera_depth_frame
             *      (0.0,0.25,0.0)
             *      (0,0,0,1)
             */
                emTransform result3 = new emTransform() {basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(0.0, 0.025, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_frame"};

                /*
             * tf_echo camera_rgb_optical_frame camera_depth_frame
             *      (-0.25,0.0,0.0)
             *      (0.5,-0.5,0.5,0.5)
             */
                emTransform result4 = new emTransform() {basis = new emQuaternion(0.5, -0.5, 0.5, 0.5), origin = new emVector3(-0.025, 0.0, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_optical_frame"};

                #endregion

                emTransform 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.shutting_down)
                        break;
                    if (test2 == null || !string.Equals(result2.ToString(), test2.ToString()))
                        test2 = testLookup(result2);
                    if (!ROS.ok || ROS.shutting_down)
                        break;
                    if (test3 == null || !string.Equals(result3.ToString(), test3.ToString()))
                        test3 = testLookup(result3);
                    if (!ROS.ok || ROS.shutting_down)
                        break;
                    if (test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
                        test4 = testLookup(result4);
                    Thread.Sleep(100);
                } while (ROS.ok && !ROS.shutting_down && (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.shutting_down)
            {
                Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit");
                Console.ReadLine();
                ROS.shutdown();
            }
        }