Esempio n. 1
0
        public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
        {
            int    arraylength       = -1;
            bool   hasmetacomponents = false;
            object __thing;
            int    piecesize = 0;

            byte[] thischunk, scratch1, scratch2;
            IntPtr h;

            //header
            header = new Messages.std_msgs.Header(serializedMessage, ref currentIndex);
            //status
            piecesize = Marshal.SizeOf(typeof(int));
            h         = IntPtr.Zero;
            if (serializedMessage.Length - currentIndex != 0)
            {
                h = Marshal.AllocHGlobal(piecesize);
                Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
            }
            if (h == IntPtr.Zero)
            {
                throw new Exception("Memory allocation failed");
            }
            status = (int)Marshal.PtrToStructure(h, typeof(int));
            Marshal.FreeHGlobal(h);
            currentIndex += piecesize;
            //details
            details       = "";
            piecesize     = BitConverter.ToInt32(serializedMessage, currentIndex);
            currentIndex += 4;
            details       = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
            currentIndex += piecesize;
        }
Esempio n. 2
0
        public override byte[] Serialize(bool partofsomethingelse)
        {
            List <byte[]> pieces = new List <byte[]>();

            if (Header == null)
            {
                Header = new std_msgs.Header();
            }
            pieces.Add(Header.Serialize(true));

            if (GoalStatus == null)
            {
                GoalStatus = new actionlib_msgs.GoalStatus();
            }
            pieces.Add(GoalStatus.Serialize(true));

            if (Content == null)
            {
                Content = new T();
            }
            pieces.Add(Content.Serialize(true));

            // combine every array in pieces into one array and return it
            int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
            int __a_b__e = 0;

            byte[] __a_b__d = new byte[__a_b__f];
            foreach (var __p__ in pieces)
            {
                Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
                __a_b__e += __p__.Length;
            }
            return(__a_b__d);
        }
Esempio n. 3
0
        public override void Randomize()
        {
            int    arraylength = -1;
            Random rand        = new Random();
            int    strlength;

            byte[] strbuf, myByte;

            //header
            header = new Messages.std_msgs.Header();
            header.Randomize();
            //status
            status = rand.Next();
            //details
            strlength = rand.Next(100) + 1;
            strbuf    = new byte[strlength];
            rand.NextBytes(strbuf);  //fill the whole buffer with random bytes
            for (int __x__ = 0; __x__ < strlength; __x__++)
            {
                if (strbuf[__x__] == 0) //replace null chars with non-null random ones
                {
                    strbuf[__x__] = (byte)(rand.Next(254) + 1);
                }
            }
            strbuf[strlength - 1] = 0; //null terminate
            details = Encoding.ASCII.GetString(strbuf);
        }
Esempio n. 4
0
    // Update is called once per frame
    void Update()
    {
        Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage();

        Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1];
        arr[0] = new Messages.geometry_msgs.TransformStamped();

        tfmsg.transforms = arr;
        Transform   trans = trackedObject.transform;
        emTransform ta    = new emTransform(trans, ROS.GetTime(), frame_id, child_frame_id);

        Messages.std_msgs.Header hdr = new Messages.std_msgs.Header();
        hdr.frame_id = frame_id;

        hdr.stamp = ROS.GetTime();
        if (!using_gazebo)
        {
            hdr.stamp.data.sec += 18000;
        }

        tfmsg.transforms[0].header                = hdr;
        tfmsg.transforms[0].child_frame_id        = child_frame_id;
        tfmsg.transforms[0].transform             = new Messages.geometry_msgs.Transform();
        tfmsg.transforms[0].transform.translation = ta.origin.ToMsg();
        //tfmsg.transforms[0].transform.translation.z += 1.0;
        tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg();
        tfmsg.Serialized = null;

        tfPub.publish(tfmsg);
    }
Esempio n. 5
0
 public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
 {
     Header     = new std_msgs.Header(serializedMessage, ref currentIndex);
     GoalStatus = new actionlib_msgs.GoalStatus(serializedMessage, ref currentIndex);
     //Content = (T)Activator.CreateInstance( typeof( T ), serializedMessage, currentIndex );
     Content = (T)typeof(T).GetInstance();
     Content.Deserialize(serializedMessage, ref currentIndex);
 }
		public OdomSubscriberHelper (NodeHandle nh, string topic, Pose pose, Twist twist, Accel acceleration, Header header)
		{
			pose_ = pose;
			twist_ = twist;
			acceleration_ = acceleration;
			header_ = header;
			odom_sub_ = nh.subscribe<Odometry> ( topic, 1, odomCallback );
		}
Esempio n. 7
0
    void Update()
    {
        if (publishtf)
        {
            _tfmsg.transforms[0].header.stamp           = ROS.GetTime();
            _tfmsg.transforms[0].header.stamp.data.sec += 18000; //windows time is dumb
            //Debug.Log("Current time" + ROS.GetTime().data.sec);
            tfPub.publish(_tfmsg);
        }
        if (hj == null)
        {
            hj = GameObject.Find(frame_id);
        }
        else if (!interactableItem.IsInteracting())
        {
            if (!startedInteracting)
            {
                //Debug.Log("Not interacting!");
                Vector3    t = hj.transform.position;
                Quaternion q = hj.transform.rotation;
                transform.position = t;
                transform.rotation = q;
            }
            if (startedInteracting)
            {
                Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage();

                Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1];
                arr[0] = new Messages.geometry_msgs.TransformStamped();

                tfmsg.transforms = arr;
                //Transform trans = trackedObj.transform;
                emTransform ta = new emTransform(transform, ROS.GetTime(), "/world", "/look_at_frame");

                Messages.std_msgs.Header hdr = new Messages.std_msgs.Header();
                hdr.frame_id = "/world";

                hdr.stamp           = ROS.GetTime();
                hdr.stamp.data.sec += 18000;

                tfmsg.transforms[0].header                = hdr;
                tfmsg.transforms[0].child_frame_id        = "/look_at_frame";
                tfmsg.transforms[0].transform             = new Messages.geometry_msgs.Transform();
                tfmsg.transforms[0].transform.translation = ta.origin.ToMsg();
                tfmsg.transforms[0].transform.rotation    = ta.basis.ToMsg();
                tfmsg.Serialized = null;

                tfPub.publish(tfmsg);
                _tfmsg             = tfmsg;
                publishtf          = true;
                startedInteracting = false;
            }
        }
        else
        {
            startedInteracting = true;
        }
    }
 public override void Randomize()
 {
     header = new Messages.std_msgs.Header();
     header.Randomize();
     goal_id = new Messages.actionlib_msgs.GoalID();
     goal_id.Randomize();
     goal = new LandingGoal();
     goal.Randomize();
 }
		public StateSubscriberHelper (NodeHandle nh, string topic, Pose pose, Twist twist, Accel accel, Header header)
		{
			pose_ = pose;
			twist_ = twist;
			accel_ = accel;
			header_ = header;
	    	available_ = false;
			tf_sub_ = nh.subscribe<TransformStamped> ( topic, 1, tfCb );
	  }
Esempio n. 10
0
        internal bool EnqueueMessage(MessageAndSerializerFunc holder)
        {
            lock (gate)
            {
                if (Dropped)
                {
                    return(false);
                }
            }

            uint seq = IncrementSequence();

            if (HasHeader)
            {
                object h = holder.msg.GetType().GetTypeInfo().GetField("header").GetValue(holder.msg);

                std_msgs.Header header;
                if (h == null)
                {
                    header = new std_msgs.Header();
                }
                else
                {
                    header = (std_msgs.Header)h;
                }

                header.seq = seq;
                if (header.stamp == null)
                {
                    header.stamp = ROS.GetTime();
                }
                if (header.frame_id == null)
                {
                    header.frame_id = "";
                }
                holder.msg.GetType().GetTypeInfo().GetField("header").SetValue(holder.msg, header);
            }
            holder.msg.connection_header = connectionHeader.Values;

            lock (gate)
            {
                foreach (SubscriberLink link in subscriberLinks)
                {
                    link.EnqueueMessage(holder);
                }
            }

            if (Latch)
            {
                lastMessage = new MessageAndSerializerFunc(holder.msg, holder.serfunc, false, true);
            }

            return(true);
        }
		void tfCb (TransformStamped transform)
		{
			header_ = transform.header;
			pose_.position.x = transform.transform.translation.x;
			pose_.position.y = transform.transform.translation.y;
			pose_.position.z = transform.transform.translation.z;
			pose_.orientation = transform.transform.rotation;

			diff_.updateAndEstimate ( header_.Stamp, pose_, twist_, accel_ );
			available_ = true;
		}
Esempio n. 12
0
        internal bool EnqueueMessage(MessageAndSerializerFunc holder)
        {
            lock (subscriber_links_mutex)
            {
                if (Dropped)
                {
                    return(false);
                }
            }

            uint seq = incrementSequence();

            if (HasHeader)
            {
                object   h = holder.msg.GetType().GetField("header").GetValue(holder.msg);
                m.Header header;
                if (h == null)
                {
                    header = new m.Header();
                }
                else
                {
                    header = (m.Header)h;
                }
                header.Seq = seq;
                if (header.Stamp == null)
                {
                    header.Stamp = ROS.GetTime();
                }
                if (header.Frame_id == null)
                {
                    header.Frame_id = "";
                }
                holder.msg.GetType().GetField("header").SetValue(holder.msg, header);
            }
            holder.msg.connection_header = connection_header.Values;

            lock (subscriber_links_mutex)
                foreach (SubscriberLink sub_link in subscriber_links)
                {
                    sub_link.enqueueMessage(holder);
                }

            if (Latch)
            {
                last_message = new MessageAndSerializerFunc(holder.msg, holder.serfunc, false, true);
            }
            return(true);
        }
Esempio n. 13
0
        public override byte[] Serialize(bool partofsomethingelse)
        {
            int  currentIndex = 0, length = 0;
            bool hasmetacomponents = false;

            byte[]        thischunk, scratch1, scratch2;
            List <byte[]> pieces = new List <byte[]>();
            GCHandle      h;
            IntPtr        ptr;
            int           x__size;

            //header
            if (header == null)
            {
                header = new Messages.std_msgs.Header();
            }
            pieces.Add(header.Serialize(true));
            //status
            scratch1 = new byte[Marshal.SizeOf(typeof(int))];
            h        = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
            Marshal.StructureToPtr(status, h.AddrOfPinnedObject(), false);
            h.Free();
            pieces.Add(scratch1);
            //details
            if (details == null)
            {
                details = "";
            }
            scratch1  = Encoding.ASCII.GetBytes((string)details);
            thischunk = new byte[scratch1.Length + 4];
            scratch2  = BitConverter.GetBytes(scratch1.Length);
            Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
            Array.Copy(scratch2, thischunk, 4);
            pieces.Add(thischunk);
            // combine every array in pieces into one array and return it
            int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
            int __a_b__e = 0;

            byte[] __a_b__d = new byte[__a_b__f];
            foreach (var __p__ in pieces)
            {
                Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
                __a_b__e += __p__.Length;
            }
            return(__a_b__d);
        }
Esempio n. 14
0
        public bool EnqueueMessage(IRosMessage msg)
        {
            lock (subscriber_links_mutex)
            {
                if (Dropped)
                {
                    return(false);
                }
            }

            uint seq = incrementSequence();

            if (HasHeader)
            {
                object   h = msg.GetType().GetField("header").GetValue(msg);
                m.Header header;
                if (h == null)
                {
                    header = new m.Header();
                }
                else
                {
                    header = (m.Header)h;
                }
                header.seq      = seq;
                header.stamp    = ROS.GetTime();
                header.frame_id = new m.String();
                msg.GetType().GetField("header").SetValue(msg, header);
            }
            msg.connection_header = connection_header.Values;

            lock (subscriber_links_mutex)
                foreach (SubscriberLink sub_link in subscriber_links)
                {
                    sub_link.enqueueMessage(msg, true, false);
                }

            if (Latch)
            {
                last_message = msg;
            }
            return(true);
        }
		void odomCallback (Odometry odom)
		{
			// calculate acceleration
			if ( header_.Stamp.data.toSec () != 0.0 && odom.header.Stamp.data.toSec () != 0.0 )
			{
				double acceleration_time_constant = 0.1;
				double dt = ( ( odom.header.Stamp - header_.Stamp ).data.toSec () );
				if ( dt > 0.0 )
				{
					acceleration_.linear.x = ( ( odom.twist.twist.linear.x - twist_.linear.x ) + acceleration_time_constant * acceleration_.linear.x ) / ( dt + acceleration_time_constant );
					acceleration_.linear.y = ( ( odom.twist.twist.linear.y - twist_.linear.y ) + acceleration_time_constant * acceleration_.linear.y ) / ( dt + acceleration_time_constant );
					acceleration_.linear.z = ( ( odom.twist.twist.linear.z - twist_.linear.z ) + acceleration_time_constant * acceleration_.linear.z ) / ( dt + acceleration_time_constant );
					acceleration_.angular.x = ( ( odom.twist.twist.angular.x - twist_.angular.x ) + acceleration_time_constant * acceleration_.angular.x ) / ( dt + acceleration_time_constant );
					acceleration_.angular.y = ( ( odom.twist.twist.angular.y - twist_.angular.y ) + acceleration_time_constant * acceleration_.angular.y ) / ( dt + acceleration_time_constant );
					acceleration_.angular.z = ( ( odom.twist.twist.angular.z - twist_.angular.z ) + acceleration_time_constant * acceleration_.angular.z ) / ( dt + acceleration_time_constant );
				}
			}

			header_ = odom.header;
			pose_ = odom.pose.pose;
			twist_ = odom.twist.twist;
		}
Esempio n. 16
0
        internal bool EnqueueMessage(MessageAndSerializerFunc holder)
        {
            lock (subscriber_links_mutex)
            {
                if (Dropped) return false;
            }

            uint seq = incrementSequence();

            if (HasHeader)
            {
                object h = holder.msg.GetType().GetField("header").GetValue(holder.msg);
                m.Header header;
                if (h == null)
                    header = new m.Header();
                else
                    header = (m.Header) h;
                header.seq = seq;
                if (header.stamp == null)
                {
                    header.stamp = ROS.GetTime();
                }
                if (header.frame_id == null)
                {
                    header.frame_id = "";
                }
                holder.msg.GetType().GetField("header").SetValue(holder.msg, header);
            }
            holder.msg.connection_header = connection_header.Values;

            lock (subscriber_links_mutex)
                foreach (SubscriberLink sub_link in subscriber_links)
                {
                    sub_link.enqueueMessage(holder);
                }

            if (Latch)
            {
                last_message = new MessageAndSerializerFunc(holder.msg, holder.serfunc, false, true);
            }
            return true;
        }
Esempio n. 17
0
        public override byte[] Serialize(bool partofsomethingelse)
        {
            int  currentIndex = 0, length = 0;
            bool hasmetacomponents = false;

            byte[]        thischunk, scratch1, scratch2;
            List <byte[]> pieces = new List <byte[]>();
            GCHandle      h;
            IntPtr        ptr;
            int           x__size;

            //header
            if (header == null)
            {
                header = new Messages.std_msgs.Header();
            }
            pieces.Add(header.Serialize(true));
            //system_status
            scratch1 = new byte[Marshal.SizeOf(typeof(int))];
            h        = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
            Marshal.StructureToPtr(system_status, h.AddrOfPinnedObject(), false);
            h.Free();
            pieces.Add(scratch1);
            //topics
            hasmetacomponents |= false;
            if (topics == null)
            {
                topics = new string[0];
            }
            pieces.Add(BitConverter.GetBytes(topics.Length));
            for (int i = 0; i < topics.Length; i++)
            {
                //topics[i]
                if (topics[i] == null)
                {
                    topics[i] = "";
                }
                scratch1  = Encoding.ASCII.GetBytes((string)topics[i]);
                thischunk = new byte[scratch1.Length + 4];
                scratch2  = BitConverter.GetBytes(scratch1.Length);
                Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
                Array.Copy(scratch2, thischunk, 4);
                pieces.Add(thischunk);
            }
            //err_code
            hasmetacomponents |= false;
            if (err_code == null)
            {
                err_code = new int[0];
            }
            pieces.Add(BitConverter.GetBytes(err_code.Length));
// Start Xamla
            //err_code
            x__size  = Marshal.SizeOf(typeof(int)) * err_code.Length;
            scratch1 = new byte[x__size];
            Buffer.BlockCopy(err_code, 0, scratch1, 0, x__size);
            pieces.Add(scratch1);
// End Xamla

            //topic_msg
            hasmetacomponents |= false;
            if (topic_msg == null)
            {
                topic_msg = new string[0];
            }
            pieces.Add(BitConverter.GetBytes(topic_msg.Length));
            for (int i = 0; i < topic_msg.Length; i++)
            {
                //topic_msg[i]
                if (topic_msg[i] == null)
                {
                    topic_msg[i] = "";
                }
                scratch1  = Encoding.ASCII.GetBytes((string)topic_msg[i]);
                thischunk = new byte[scratch1.Length + 4];
                scratch2  = BitConverter.GetBytes(scratch1.Length);
                Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
                Array.Copy(scratch2, thischunk, 4);
                pieces.Add(thischunk);
            }
            // combine every array in pieces into one array and return it
            int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
            int __a_b__e = 0;

            byte[] __a_b__d = new byte[__a_b__f];
            foreach (var __p__ in pieces)
            {
                Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
                __a_b__e += __p__.Length;
            }
            return(__a_b__d);
        }
Esempio n. 18
0
        public override void Randomize()
        {
            int    arraylength = -1;
            Random rand        = new Random();
            int    strlength;

            byte[] strbuf, myByte;

            //header
            header = new Messages.std_msgs.Header();
            header.Randomize();
            //system_status
            system_status = rand.Next();
            //topics
            arraylength = rand.Next(10);
            if (topics == null)
            {
                topics = new string[arraylength];
            }
            else
            {
                Array.Resize(ref topics, arraylength);
            }
            for (int i = 0; i < topics.Length; i++)
            {
                //topics[i]
                strlength = rand.Next(100) + 1;
                strbuf    = new byte[strlength];
                rand.NextBytes(strbuf);  //fill the whole buffer with random bytes
                for (int __x__ = 0; __x__ < strlength; __x__++)
                {
                    if (strbuf[__x__] == 0) //replace null chars with non-null random ones
                    {
                        strbuf[__x__] = (byte)(rand.Next(254) + 1);
                    }
                }
                strbuf[strlength - 1] = 0; //null terminate
                topics[i]             = Encoding.ASCII.GetString(strbuf);
            }
            //err_code
            arraylength = rand.Next(10);
            if (err_code == null)
            {
                err_code = new int[arraylength];
            }
            else
            {
                Array.Resize(ref err_code, arraylength);
            }
            for (int i = 0; i < err_code.Length; i++)
            {
                //err_code[i]
                err_code[i] = rand.Next();
            }
            //topic_msg
            arraylength = rand.Next(10);
            if (topic_msg == null)
            {
                topic_msg = new string[arraylength];
            }
            else
            {
                Array.Resize(ref topic_msg, arraylength);
            }
            for (int i = 0; i < topic_msg.Length; i++)
            {
                //topic_msg[i]
                strlength = rand.Next(100) + 1;
                strbuf    = new byte[strlength];
                rand.NextBytes(strbuf);  //fill the whole buffer with random bytes
                for (int __x__ = 0; __x__ < strlength; __x__++)
                {
                    if (strbuf[__x__] == 0) //replace null chars with non-null random ones
                    {
                        strbuf[__x__] = (byte)(rand.Next(254) + 1);
                    }
                }
                strbuf[strlength - 1] = 0; //null terminate
                topic_msg[i]          = Encoding.ASCII.GetString(strbuf);
            }
        }
Esempio n. 19
0
        public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
        {
            int    arraylength       = -1;
            bool   hasmetacomponents = false;
            object __thing;
            int    piecesize = 0;

            byte[] thischunk, scratch1, scratch2;
            IntPtr h;

            //header
            header = new Messages.std_msgs.Header(serializedMessage, ref currentIndex);
            //system_status
            piecesize = Marshal.SizeOf(typeof(int));
            h         = IntPtr.Zero;
            if (serializedMessage.Length - currentIndex != 0)
            {
                h = Marshal.AllocHGlobal(piecesize);
                Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
            }
            if (h == IntPtr.Zero)
            {
                throw new Exception("Memory allocation failed");
            }
            system_status = (int)Marshal.PtrToStructure(h, typeof(int));
            Marshal.FreeHGlobal(h);
            currentIndex += piecesize;
            //topics
            hasmetacomponents |= false;
            arraylength        = BitConverter.ToInt32(serializedMessage, currentIndex);
            currentIndex      += Marshal.SizeOf(typeof(System.Int32));
            if (topics == null)
            {
                topics = new string[arraylength];
            }
            else
            {
                Array.Resize(ref topics, arraylength);
            }
            for (int i = 0; i < topics.Length; i++)
            {
                //topics[i]
                topics[i]     = "";
                piecesize     = BitConverter.ToInt32(serializedMessage, currentIndex);
                currentIndex += 4;
                topics[i]     = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
                currentIndex += piecesize;
            }
            //err_code
            hasmetacomponents |= false;
            arraylength        = BitConverter.ToInt32(serializedMessage, currentIndex);
            currentIndex      += Marshal.SizeOf(typeof(System.Int32));
            if (err_code == null)
            {
                err_code = new int[arraylength];
            }
            else
            {
                Array.Resize(ref err_code, arraylength);
            }
// Start Xamla
            //err_code
            piecesize = Marshal.SizeOf(typeof(int)) * err_code.Length;
            if (currentIndex + piecesize > serializedMessage.Length)
            {
                throw new Exception("Memory allocation failed: Ran out of bytes to read.");
            }
            Buffer.BlockCopy(serializedMessage, currentIndex, err_code, 0, piecesize);
            currentIndex += piecesize;
// End Xamla

            //topic_msg
            hasmetacomponents |= false;
            arraylength        = BitConverter.ToInt32(serializedMessage, currentIndex);
            currentIndex      += Marshal.SizeOf(typeof(System.Int32));
            if (topic_msg == null)
            {
                topic_msg = new string[arraylength];
            }
            else
            {
                Array.Resize(ref topic_msg, arraylength);
            }
            for (int i = 0; i < topic_msg.Length; i++)
            {
                //topic_msg[i]
                topic_msg[i]  = "";
                piecesize     = BitConverter.ToInt32(serializedMessage, currentIndex);
                currentIndex += 4;
                topic_msg[i]  = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
                currentIndex += piecesize;
            }
        }
 public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
 {
     header  = new Messages.std_msgs.Header(SERIALIZEDSTUFF, ref currentIndex);
     goal_id = new Messages.actionlib_msgs.GoalID(SERIALIZEDSTUFF, ref currentIndex);
     goal    = new LandingGoal(SERIALIZEDSTUFF, ref currentIndex);
 }