public override void Randomize() { int arraylength = -1; Random rand = new Random(); int strlength; byte[] strbuf, myByte; //header header = new Header(); header.Randomize(); //images arraylength = rand.Next(10); if (images == null) { images = new Messages.sensor_msgs.Image[arraylength]; } else { Array.Resize(ref images, arraylength); } for (int i = 0; i < images.Length; i++) { //images[i] images[i] = new Messages.sensor_msgs.Image(); images[i].Randomize(); } //serials arraylength = rand.Next(10); if (serials == null) { serials = new string[arraylength]; } else { Array.Resize(ref serials, arraylength); } for (int i = 0; i < serials.Length; i++) { //serials[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 serials[i] = Encoding.ASCII.GetString(strbuf); } }
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 Header(serializedMessage, ref currentIndex); //images hasmetacomponents |= false; arraylength = BitConverter.ToInt32(serializedMessage, currentIndex); currentIndex += Marshal.SizeOf(typeof(System.Int32)); if (images == null) { images = new Messages.sensor_msgs.Image[arraylength]; } else { Array.Resize(ref images, arraylength); } for (int i = 0; i < images.Length; i++) { //images[i] images[i] = new Messages.sensor_msgs.Image(serializedMessage, ref currentIndex); } //serials hasmetacomponents |= false; arraylength = BitConverter.ToInt32(serializedMessage, currentIndex); currentIndex += Marshal.SizeOf(typeof(System.Int32)); if (serials == null) { serials = new string[arraylength]; } else { Array.Resize(ref serials, arraylength); } for (int i = 0; i < serials.Length; i++) { //serials[i] serials[i] = ""; piecesize = BitConverter.ToInt32(serializedMessage, currentIndex); currentIndex += 4; serials[i] = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize); currentIndex += piecesize; } }
private void updateImage(sm.Image img) { Dispatcher.Invoke(new Action(() => mGenericImage.UpdateImage(img.data, new Size((int)img.width, (int)img.height), false, img.encoding))); }
void OldPublishAll() { // pose info PoseStamped ps = new PoseStamped(); // ps.header = new Messages.std_msgs.Header (); // ps.header.Stamp = ROS.GetTime (); // ps.header.Frame_id = ""; ps.pose = new Messages.geometry_msgs.Pose(); Imu imu = new Imu(); // imu info // imu.header = new Messages.std_msgs.Header ( ps.header ); imu.angular_velocity_covariance = new double[9] { -1, 0, 0, 0, 0, 0, 0, 0, 0 }; imu.linear_acceleration_covariance = new double[9] { -1, 0, 0, 0, 0, 0, 0, 0, 0 }; imu.orientation_covariance = new double[9] { -1, 0, 0, 0, 0, 0, 0, 0, 0 }; // image info Image img = new Image(); // img.header = new Messages.std_msgs.Header ( ps.header ); img.width = (uint)QuadMotor.ImageWidth; img.height = (uint)QuadMotor.ImageHeight; img.encoding = "mono16"; // "rgba8"; img.step = img.width * 2; // * 4 img.is_bigendian = 1; // int sleep = 1000 / 30; // int sleep = 1000 / 2; int sleep = 1000 / 60; // int sleep = 1000 / 120; // int sleep = 1000 / 250; #if TIMETEST Queue <TimeData> tdq = new Queue <TimeData> (); Queue <long> tq = new Queue <long> (); #endif while (ROS.ok && !ROS.shutting_down) { #if TIMETEST tdq.Enqueue(ROS.GetTime().data); tq.Enqueue(System.DateTime.Now.Ticks); #endif // publish pose ///* // ps.header = new Messages.std_msgs.Header (); // ps.header.Stamp = ROS.GetTime (); // ps.header.Frame_id = ps.header.Stamp.data.toSec ().ToString (); // ps.header.Frame_id = ""; // ps.header.Seq = frameSeq; ps.pose.position = new Messages.geometry_msgs.Point(droneController.Position.ToRos()); ps.pose.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos()); posePub.publish(ps); if (ps.header != null) { Debug.Log("Send Time " + ps.header.Stamp.data.toSec()); } else { Debug.Log("Send time null header"); } // */ // publish imu ///* // imu.header = new Messages.std_msgs.Header (); // imu.header.Frame_id = ""; // imu.header.Seq = frameSeq++; // imu.header.Stamp = ROS.GetTime (); // imu.header.Stamp = ps.header.Stamp; imu.angular_velocity = new GVector3(droneController.AngularVelocity.ToRos()); imu.linear_acceleration = new GVector3(droneController.LinearAcceleration.ToRos()); imu.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos()); imuPub.publish(imu); // Debug.Log ( "Send Imu " + imu.header.Seq ); // publish image // img.data = droneController.GetImageData (); // imgPub.publish ( img ); // */ Thread.Sleep(sleep); } #if TIMETEST while (tdq.Count > 0) { TimeData td = tdq.Dequeue(); Debug.Log("t: " + td.sec + " " + td.nsec); } while (tq.Count > 0) { Debug.Log("ticks: " + tq.Dequeue()); } #endif }
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 Header(); } pieces.Add(header.Serialize(true)); //images hasmetacomponents |= false; if (images == null) { images = new Messages.sensor_msgs.Image[0]; } pieces.Add(BitConverter.GetBytes(images.Length)); for (int i = 0; i < images.Length; i++) { //images[i] if (images[i] == null) { images[i] = new Messages.sensor_msgs.Image(); } pieces.Add(images[i].Serialize(true)); } //serials hasmetacomponents |= false; if (serials == null) { serials = new string[0]; } pieces.Add(BitConverter.GetBytes(serials.Length)); for (int i = 0; i < serials.Length; i++) { //serials[i] if (serials[i] == null) { serials[i] = ""; } scratch1 = Encoding.ASCII.GetBytes((string)serials[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); }