Esempio n. 1
0
 public TransformStorage(Transform data, uint frameId, uint childFrameId)
 {
     this.Rotation     = data.Basis;
     this.Translation  = data.Origin;
     this.Stamp        = TimeCache.ToLong(data.Stamp.data);
     this.FrameId      = frameId;
     this.ChildFrameId = childFrameId;
 }
Esempio n. 2
0
        private bool canTransformNoLock(uint target_id, uint source_id, Time time, out string error_msg)
        {
            error_msg = null;
            if (target_id == 0 || source_id == 0)
            {
                return(false);
            }

            CanTransformAccum accum = new CanTransformAccum();

            if (walkToTopParent(accum, TimeCache.ToLong(time.data), target_id, source_id, out error_msg) == TfStatus.NoError)
            {
                return(true);
            }
            return(false);
        }
Esempio n. 3
0
        private TfStatus getLatestCommonTime(uint target_id, uint source_id, ref ulong time, out string error_str)
        {
            error_str = null;
            if (target_id == source_id)
            {
                time = TimeCache.ToLong(ROS.GetTime().data);
                return(TfStatus.NoError);
            }

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

            uint  frame       = source_id;
            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.FrameId == 0)
                {
                    break;
                }
                if (latest.Time != 0)
                {
                    common_time = Math.Min(latest.Time, common_time);
                }
                lct.Add(latest);
                frame = latest.FrameId;
                if (frame == target_id)
                {
                    time = common_time;
                    if (time == ulong.MaxValue)
                    {
                        time = 0;
                    }
                    return(TfStatus.NoError);
                }
                ++depth;
                if (depth > MAX_GRAPH_DEPTH)
                {
                    if (error_str != null)
                    {
                        error_str = "The tf tree is invalid because it contains a loop.";
                    }
                    return(TfStatus.LookupError);
                }
            }

            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.FrameId == 0)
                {
                    break;
                }
                if (latest.Time != 0)
                {
                    common_time = Math.Min(latest.Time, common_time);
                }

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

                if (frame == source_id)
                {
                    time = common_time;
                    if (time == uint.MaxValue)
                    {
                        time = 0;
                    }
                    return(TfStatus.NoError);
                }
                ++depth;
                if (depth > MAX_GRAPH_DEPTH)
                {
                    if (error_str != null)
                    {
                        error_str = "The tf tree is invalid because it contains a loop.";
                    }
                    return(TfStatus.LookupError);
                }
            }
            if (common_parent == 0)
            {
                error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id];
                return(TfStatus.ConnectivityError);
            }
            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].FrameId == common_parent)
                {
                    break;
                }
            }
            if (common_time == uint.MaxValue)
            {
                common_time = 0;
            }
            time = common_time;
            return(TfStatus.NoError);
        }
Esempio n. 4
0
        public bool lookupTransform(string target_frame, string source_frame, Time time, out Transform transform, out string error_string)
        {
            transform    = null;
            error_string = null;

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

            if (mapped_tgt == mapped_src)
            {
                transform              = new Transform();
                transform.Origin       = new Vector3();
                transform.Basis        = new Quaternion();
                transform.ChildFrameId = mapped_src;
                transform.FrameId      = mapped_tgt;
                transform.Stamp        = DateTime.UtcNow.ToTimeMessage();
                return(true);
            }

            TfStatus 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, out error_string);
            if (retval != TfStatus.NoError)
            {
                error_string = error_string ?? "UNSPECIFIED";
                switch (retval)
                {
                case TfStatus.ConnectivityError:
                    error_string = "NO CONNECTION: " + error_string;
                    break;

                case TfStatus.ExtrapolationError:
                    error_string = "EXTRAPOLATION: " + error_string;
                    break;

                case TfStatus.LookupError:
                    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 Transform();
                transform.Origin       = accum.result_vec;
                transform.Basis        = accum.result_quat;
                transform.ChildFrameId = mapped_src;
                transform.FrameId      = mapped_tgt;
                transform.Stamp        = new Time(TimeData.FromTicks((long)accum.time));
            }
            return(retval == TfStatus.NoError);
        }