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