Example #1
0
 public TransformStorage(emTransform data, uint frame_id, uint child_frame_id)
 {
     rotation            = data.rotation;
     translation         = data.translation;
     stamp               = TimeCache.toLong(data.stamp.data);
     this.frame_id       = frame_id;
     this.child_frame_id = child_frame_id;
 }
Example #2
0
 public override uint gather(TimeCache cache, ulong time_, ref string error_str)
 {
     if (!cache.getData(time_, ref st, ref error_str))
     {
         return(0);
     }
     return(st.frame_id);
 }
Example #3
0
        public void lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform, ref string error_string)
        {
            transform = new emTransform();

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

            if (mapped_tgt == mapped_src)
            {
                transform.translation    = new emVector3();
                transform.rotation       = new emQuaternion();
                transform.child_frame_id = mapped_src;
                transform.frame_id       = mapped_tgt;
                transform.stamp          = ROS.GetTime(DateTime.Now);
                return;
            }

            lock (framemutex)
            {
                uint target_id = getFrameID(mapped_tgt);
                uint source_id = getFrameID(mapped_src);

                TransformAccum accum = new TransformAccum();

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

                    case TF_STATUS.EXTRAPOLATION_ERROR:
                        ROS.Error("EXTRAPOLATION: " + error_string);
                        break;

                    case TF_STATUS.LOOKUP_ERROR:
                        ROS.Error("LOOKUP: " + error_string);
                        break;
                    }
                }
                transform.translation    = accum.result_vec;
                transform.rotation       = accum.result_quat;
                transform.child_frame_id = mapped_src;
                transform.frame_id       = mapped_tgt;
                transform.stamp          = new Time {
                    data = new TimeData {
                        sec = (uint)(accum.time >> 32), nsec = (uint)(accum.time & 0xFFFFFFFF)
                    }
                };
            }
        }
Example #4
0
        public bool setTransform(emTransform transform)
        {
            emTransform mapped_transform = new emTransform(transform.rotation, transform.translation, 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);
            }
            lock (framemutex)
            {
                uint      frame_number = lookupOrInsertFrameNumber(mapped_transform.child_frame_id);
                TimeCache frame        = null;
                if (!frames.ContainsKey(frame_number))
                {
                    frames[frame_number] = new TimeCache(cache_time);
                    frame = frames[frame_number];
                }
                else
                {
                    frame = frames[frame_number];
                }
                if (frame.insertData(new TransformStorage(mapped_transform, lookupOrInsertFrameNumber(mapped_transform.frame_id), frame_number)))
                {
                    // authority? meh
                }
                else
                {
                    return(false);
                }
            }
            return(true);
        }
Example #5
0
        private TF_STATUS getLatestCommonTime(uint target_id, uint source_id, ref ulong time, ref string error_str)
        {
            if (target_id == source_id)
            {
                time = TimeCache.toLong(ROS.GetTime(DateTime.Now).data);
                return(TF_STATUS.NO_ERROR);
            }

            List <TimeAndFrameID> lct = new List <TimeAndFrameID>();

            uint           frame = source_id;
            TimeAndFrameID temp;
            uint           depth       = 0;
            ulong          common_time = ulong.MaxValue;

            while (frame != 0)
            {
                TimeCache cache;
                if (!frames.ContainsKey(frame))
                {
                    break;
                }
                cache = frames[frame];
                TimeAndFrameID latest = cache.getLatestTimeAndParent();
                if (latest.frame_id == 0)
                {
                    break;
                }
                if (latest.time != 0)
                {
                    common_time = Math.Min(latest.time, common_time);
                }
                lct.Add(latest);
                frame = latest.frame_id;
                if (frame == target_id)
                {
                    time = common_time;
                    if (time == ulong.MaxValue)
                    {
                        time = 0;
                    }
                    return(TF_STATUS.NO_ERROR);
                }
                ++depth;
                if (depth > MAX_GRAPH_DEPTH)
                {
                    if (error_str != null)
                    {
                        error_str = "The tf tree is invalid because it contains a loop.";
                    }
                    return(TF_STATUS.LOOKUP_ERROR);
                }
            }

            frame       = target_id;
            depth       = 0;
            common_time = ulong.MaxValue;
            uint common_parent = 0;

            while (true)
            {
                TimeCache cache;
                if (!frames.ContainsKey(frame))
                {
                    break;
                }
                cache = frames[frame];
                TimeAndFrameID latest = cache.getLatestTimeAndParent();
                if (latest.frame_id == 0)
                {
                    break;
                }
                if (latest.time != 0)
                {
                    common_time = Math.Min(latest.time, common_time);
                }

                foreach (TimeAndFrameID tf in lct)
                {
                    if (tf.frame_id == latest.frame_id)
                    {
                        common_parent = tf.frame_id;
                        break;
                    }
                }
                frame = latest.frame_id;

                if (frame == source_id)
                {
                    time = common_time;
                    if (time == uint.MaxValue)
                    {
                        time = 0;
                    }
                    return(TF_STATUS.NO_ERROR);
                }
                ++depth;
                if (depth > MAX_GRAPH_DEPTH)
                {
                    if (error_str != null)
                    {
                        error_str = "The tf tree is invalid because it contains a loop.";
                    }
                    return(TF_STATUS.LOOKUP_ERROR);
                }
            }
            if (common_parent == 0)
            {
                error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id];
                return(TF_STATUS.CONNECTIVITY_ERROR);
            }
            for (int i = 0; i < lct.Count; i++)
            {
                if (lct[i].time != 0)
                {
                    common_time = Math.Min(common_time, lct[i].time);
                }
                if (lct[i].frame_id == common_parent)
                {
                    break;
                }
            }
            if (common_time == uint.MaxValue)
            {
                common_time = 0;
            }
            time = common_time;
            return(TF_STATUS.NO_ERROR);
        }
Example #6
0
        public TF_STATUS walkToTopParent <F>(F f, ulong time, uint target_id, uint source_id, ref string error_str) where F : ATransformAccum
        {
            if (target_id == source_id)
            {
                f.finalize(WalkEnding.Identity, time);
                return(TF_STATUS.NO_ERROR);
            }
            if (time == 0)
            {
                TF_STATUS retval = getLatestCommonTime(target_id, source_id, ref time, ref error_str);
                if (retval != TF_STATUS.NO_ERROR)
                {
                    return(retval);
                }
            }
            uint frame      = source_id;
            uint top_parent = frame;
            uint depth      = 0;

            while (frame != 0)
            {
                if (!frames.ContainsKey(frame))
                {
                    top_parent = frame;
                    break;
                }
                TimeCache cache  = frames[frame];
                uint      parent = f.gather(cache, time, ref error_str);
                if (parent == 0)
                {
                    top_parent = frame;
                    break;
                }

                if (frame == target_id)
                {
                    f.finalize(WalkEnding.TargetParentOfSource, time);
                    return(TF_STATUS.NO_ERROR);
                }

                f.accum(true);

                top_parent = frame;
                frame      = parent;
                ++depth;
                if (depth > MAX_GRAPH_DEPTH)
                {
                    if (error_str != null)
                    {
                        error_str = "The tf tree is invalid because it contains a loop.";
                    }
                    return(TF_STATUS.LOOKUP_ERROR);
                }
            }

            frame = target_id;
            depth = 0;
            while (frame != top_parent)
            {
                if (!frames.ContainsKey(frame))
                {
                    break;
                }
                TimeCache cache = frames[frame];

                uint parent = f.gather(cache, time, ref error_str);

                if (parent == 0)
                {
                    if (error_str != null)
                    {
                        error_str += ", when looking up transform from frame [" + frameids_reverse[source_id] + "] to [" + frameids_reverse[target_id] + "]";
                    }
                    return(TF_STATUS.EXTRAPOLATION_ERROR);
                }

                if (frame == source_id)
                {
                    f.finalize(WalkEnding.SourceParentOfTarget, time);
                    return(TF_STATUS.NO_ERROR);
                }

                f.accum(false);

                frame = parent;
                ++depth;
                if (depth > MAX_GRAPH_DEPTH)
                {
                    if (error_str != null)
                    {
                        error_str = "The tf tree is invalid because it contains a loop.";
                    }
                    return(TF_STATUS.LOOKUP_ERROR);
                }
            }


            if (frame != top_parent)
            {
                if (error_str != null)
                {
                    error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id];
                }
                return(TF_STATUS.CONNECTIVITY_ERROR);
            }

            f.finalize(WalkEnding.FullPath, time);

            return(TF_STATUS.NO_ERROR);
        }
Example #7
0
 public override uint gather(TimeCache cache, ulong time, ref string error_str)
 {
     return(cache.getParent(time, ref error_str));
 }
Example #8
0
 public abstract uint gather(TimeCache cache, ulong time, ref string error_str);