public bool Equals(TimeData timer)
{
return(sec == timer.sec && nsec == timer.nsec);
}
public bool Equals(TimeData timer)
{
return (sec == timer.sec && nsec == timer.nsec);
}
private static object _deserialize(Type T, Type container, byte[] bytes, out int amountread, bool sizeknown)
{
if (bytes == null && !WHAT_IS_HAPPENING || bytes.Length == 0 && !WHAT_IS_HAPPENING)
{
// Console.WriteLine("Deserializing empty array?");
amountread = 0;
return(null);
}
object thestructure = null;
if (T.FullName.Contains("System.") && !T.IsCOMObject && !T.IsArray && T != typeof(string))
{
thestructure = new object();
int size = Marshal.SizeOf(T);
IntPtr mem = IntPtr.Zero;
if (bytes.Length != 0)
{
mem = Marshal.AllocHGlobal(size);
Marshal.Copy(bytes, 0, mem, size);
}
amountread = size;
if (WHAT_IS_HAPPENING)
{
/*
* Console.WriteLine("//deserialize: " + T.FullName);
* /*Console.WriteLine(string.Format(@" $A = new {0}();
* IntPtr $B = Marshal.AllocHGlobal({1})
* Marshal.Copy(bytes, 0, $B, {1});
* $A = Marshal.PtrToStructure($B, typeof({0}));
* ", T.FullName, Marshal.SizeOf(T)));*/
}
if (mem == IntPtr.Zero)
{
return(null);
}
object obj = Marshal.PtrToStructure(mem, T);
Marshal.FreeHGlobal(mem);
return(obj);
}
IRosMessage MSG;
int startingpos = 0, currpos = 0;
/*if (container==null)
* currpos = 4;*/
MsgTypes MT = MsgTypes.Unknown;
FieldInfo[] infos = GetFields(T, ref thestructure, out MSG);
if (MSG != null)
{
MT = MSG.msgtype;
}
startingpos = currpos;
int currinfo = 0;
while ((currpos < bytes.Length || WHAT_IS_HAPPENING) && currinfo < infos.Length)
{
// Console.WriteLine(infos[currinfo].Name + "(" + currpos + "/" + bytes.Length + ")");
Type type = GetType(infos[currinfo].FieldType.FullName);
//Console.WriteLine("GetType returned: " + type.FullName);
Type realtype = infos[currinfo].FieldType;
MsgTypes msgtype = GetMessageType(type);
bool knownpiecelength = IsSizeKnown(realtype, true) && MSG.Fields != null && !MSG.Fields[infos[currinfo].Name].IsArray || MSG.Fields[infos[currinfo].Name].Length != -1;
if (realtype == typeof(Byte[])) //this is deplorable.
{
//SHORTCUT
byte[] PWNED;
MsgFieldInfo mfi = MSG.Fields[infos[currinfo].Name];
int num = mfi.Length;
if (mfi.Length == -1) //if -1, then array length not in definition
{
num = BitConverter.ToInt32(bytes, currpos);
currpos += 4;
}
PWNED = new byte[num];
Array.Copy(bytes, currpos, PWNED, 0, num);
currpos += PWNED.Length;
infos[currinfo].SetValue(thestructure, PWNED);
PWNED = null;
}
else if (realtype == typeof(Boolean[]))
{
bool[] PWNED;
MsgFieldInfo mfi = MSG.Fields[infos[currinfo].Name];
int num = mfi.Length;
if (mfi.Length == -1) //if -1, then array length not in definition
{
num = BitConverter.ToInt32(bytes, currpos);
currpos += 4;
}
PWNED = new bool[num];
for (int i = 0; i < num; i++)
{
PWNED[i] = bytes[i + currpos] > 0;
}
currpos += PWNED.Length;
infos[currinfo].SetValue(thestructure, PWNED);
PWNED = null;
}
else if (knownpiecelength)
{
if (infos[currinfo].FieldType.IsArray && msgtype != MsgTypes.std_msgs__String)
//must have length defined, or else knownpiecelength would be false... so look it up in the dict!
{
Type TT = GetType(infos[currinfo].FieldType.GetElementType().FullName);
if (TT.IsArray)
{
throw new Exception("ERIC, YOU NEED TO MAKE DESERIALIZATION RECURSE!!!");
}
Array vals = (infos[currinfo].GetValue(thestructure) as Array);
if (vals != null)
{
for (int i = 0; i < vals.Length; i++)
{
MsgTypes mt = GetMessageType(TT);
int leng = 0;
Type et = realtype.GetElementType();
if (mt == MsgTypes.Unknown)
{
leng = Marshal.SizeOf(et);
}
if (leng == 0)
{
leng = Marshal.SizeOf(vals.GetValue(i));
}
if (leng == 0)
{
throw new Exception("LENGTH ENUMERATION FAIL IN DESERIALIZE!");
}
if (leng + currpos <= bytes.Length)
{
IntPtr pIP = Marshal.AllocHGlobal(leng);
Marshal.Copy(bytes, currpos, pIP, leng);
object o = Marshal.PtrToStructure(pIP, TT);
vals.SetValue(o, i);
Marshal.FreeHGlobal(pIP);
}
else
{
vals.SetValue(null, i);
}
currpos += leng;
}
}
infos[currinfo].SetValue(thestructure, vals);
}
else
{
if (type.FullName != null && type.FullName.Contains("Message"))
{
if (GetMessageType(realtype) == MsgTypes.std_msgs__Time || GetMessageType(realtype) == MsgTypes.std_msgs__Duration || infos[currinfo].FieldType == typeof(TimeData))
{
TimeData td;
if (currpos + 8 <= bytes.Length)
{
uint u1 = BitConverter.ToUInt32(bytes, currpos);
uint u2 = BitConverter.ToUInt32(bytes, currpos + 4);
td = new TimeData(u1, u2);
}
else
{
td = new TimeData(0, 0);
}
currpos += 8;
if (infos[currinfo].FieldType == typeof(TimeData))
{
infos[currinfo].SetValue(thestructure, td);
}
else if (GetMessageType(realtype) == MsgTypes.std_msgs__Time)
{
infos[currinfo].SetValue(thestructure, (object)new std_msgs.Time(td));
}
else
{
infos[currinfo].SetValue(thestructure, (object)new std_msgs.Duration(td));
}
}
else
{
byte[] piece = new byte[bytes.Length != 0 ? bytes.Length - currpos : 0];
if (bytes.Length != 0)
{
Array.Copy(bytes, currpos, piece, 0, piece.Length);
}
int len = 0;
object obj = _deserialize(realtype, T, piece, out len,
IsSizeKnown(realtype, true));
//if ((int)(infos[currinfo].Attributes & FieldAttributes.InitOnly) != 0)
infos[currinfo].SetValue(thestructure, obj);
currpos += len;
}
}
else if (msgtype == MsgTypes.std_msgs__Bool)
{
infos[currinfo].SetValue(thestructure, bytes[currpos] != 0);
currpos++;
}
else
{
int len = Marshal.SizeOf(infos[currinfo].GetValue(thestructure));
IntPtr pIP = Marshal.AllocHGlobal(len);
object obj = null;
if (currpos + len <= bytes.Length)
{
Marshal.Copy(bytes, currpos, pIP, len);
obj = Marshal.PtrToStructure(pIP, infos[currinfo].FieldType);
}
infos[currinfo].SetValue(thestructure, obj);
currpos += len;
}
}
}
else
{
Type ft = realtype;
if (ft.IsArray)
{
Type TT = ft.GetElementType();
Array val = infos[currinfo].GetValue(thestructure) as Array;
int chunklen = 0;
if (val == null)
{
if (currpos + 4 <= bytes.Length)
{
chunklen = BitConverter.ToInt32(bytes, currpos);
currpos += 4;
val = Array.CreateInstance(TT, chunklen);
}
else
{
currpos += 4;
val = Array.CreateInstance(TT, 0);
}
}
else
{
chunklen = val.Length;
}
if (TT == null)
{
throw new Exception("LENGTHLESS ARRAY FAIL -- ELEMENT TYPE IS NULL!");
}
if (TT.FullName.Contains("Message."))
{
throw new Exception("NOT YET, YOUNG PATAWAN");
}
MsgTypes mt = GetMessageType(TT);
if (mt == MsgTypes.std_msgs__Byte || mt == MsgTypes.std_msgs__UInt8 || mt == MsgTypes.std_msgs__Int8)
{
int num = val.Length;
byte[] PWNED = new byte[num];
if (currpos + num <= bytes.Length)
{
Array.Copy(bytes, currpos, PWNED, 0, num);
}
currpos += num;
infos[currinfo].SetValue(thestructure, PWNED);
}
else if (TT.FullName != null && TT.FullName.Contains("Message"))
{
for (int i = 0; i < chunklen; i++)
{
byte[] chunk = new byte[bytes.Length - currpos];
Array.Copy(bytes, currpos, chunk, 0, chunk.Length);
int len = 0;
object data = _deserialize(TT, T, chunk, out len,
IsSizeKnown(TT, false));
val.SetValue(data, i);
currpos += len;
}
infos[currinfo].SetValue(thestructure, val);
}
else if (TT == typeof(string))
{
for (int i = 0; i < chunklen; i++)
{
int len = 0;
if (currpos + 4 <= bytes.Length)
{
len = BitConverter.ToInt32(bytes, currpos);
}
byte[] piece = new byte[len];
currpos += 4;
if (currpos + len <= bytes.Length)
{
Array.Copy(bytes, currpos, piece, 0, len);
}
string str = Encoding.ASCII.GetString(piece);
val.SetValue(str, i);
currpos += len;
}
infos[currinfo].SetValue(thestructure, val);
}
else
{
int len = Marshal.SizeOf(TT);
IntPtr pIP = IntPtr.Zero;
if (currpos + len * chunklen <= bytes.Length)
{
pIP = Marshal.AllocHGlobal(len * chunklen);
Marshal.Copy(bytes, currpos, pIP, len * chunklen);
}
object o = null;
for (int i = 0; i < chunklen * len; i += len)
{
if (pIP != IntPtr.Zero)
{
o = Marshal.PtrToStructure(pIP, TT);
}
val.SetValue(o, i / len);
if (pIP != IntPtr.Zero)
{
pIP = new IntPtr(pIP.ToInt32() + len);
}
}
infos[currinfo].SetValue(thestructure, val);
currpos += chunklen * len;
}
}
else
{
if (ft.FullName != null && ft.FullName.Contains("Message"))
{
IRosMessage msg = (IRosMessage)Activator.CreateInstance(ft);
Type t = GetType(msg.GetType().FullName);
bool knownsize = IsSizeKnown(t, false) && MSG.Fields != null && !MSG.Fields[infos[currinfo].Name].IsArray || MSG.Fields[infos[currinfo].Name].Length != -1;
if (!knownsize && t.GetField("data").FieldType == typeof(string))
{
int len = -4;
if (currpos + 4 <= bytes.Length)
{
len = BitConverter.ToInt32(bytes, currpos);
}
byte[] smallerpiece = new byte[len + 4];
if (currpos + 4 <= bytes.Length)
{
Array.Copy(bytes, currpos, smallerpiece, 0, smallerpiece.Length);
}
int dontcare = 0;
msg = _deserialize(t, T, smallerpiece, out dontcare, false) as IRosMessage;
if (bytes.Length != 0 && dontcare != len + 4)
{
throw new Exception("WTF?!");
}
infos[currinfo].SetValue(thestructure, msg);
currpos += len + 4;
}
else // if (!knownsize)
{
byte[] smallerpiece = new byte[bytes.Length != 0 ? bytes.Length - currpos : 0];
if (bytes.Length != 0)
{
Array.Copy(bytes, currpos, smallerpiece, 0, smallerpiece.Length);
}
int len = 0;
msg = _deserialize(t, T, smallerpiece, out len, knownsize) as IRosMessage;
infos[currinfo].SetValue(thestructure, msg);
currpos += len;
}
/*else
* {
* throw new Exception("THIS BROKE SOMEHOW! FIX IT!");
* }*/
}
else
{
if (infos[currinfo].FieldType == typeof(string))
{
int len = 0;
if (currpos + 4 <= bytes.Length)
{
len = BitConverter.ToInt32(bytes, currpos);
}
byte[] piece = new byte[len];
currpos += 4;
if (currpos + len <= bytes.Length)
{
Array.Copy(bytes, currpos, piece, 0, len);
}
string str = Encoding.ASCII.GetString(piece);
infos[currinfo].SetValue(thestructure, str);
currpos += len;
}
else
{
//Console.WriteLine("ZOMG HANDLE: " + infos[currinfo].FieldType.FullName);
amountread = currpos - startingpos;
return(thestructure);
}
}
}
}
currinfo++;
}
amountread = currpos - startingpos;
infos = null;
bytes = null;
return(thestructure);
}
public static ulong toLong(TimeData td)
{
return (ulong)ROS.ticksFromData(td);
}
public uint getParent(TimeData time_, ref string error_str)
{
return getParent(toLong(time_), ref error_str);
}
public bool getData(TimeData time_, ref TransformStorage data_out, ref string error_str)
{
return getData(toLong(time_), ref data_out, ref error_str);
}
private static void _deserialize(ref object thestructure, ref int currpos, Type T, Type container, byte[] bytes, out int amountread, bool sizeknown)
{
if (bytes == null || bytes.Length - currpos == 0)
{
amountread = 0;
return;
}
if (T.FullName.StartsWith("System.") && !T.IsCOMObject && !T.IsArray)
{
int size;
if (T == typeof(string))
{
//it's a string!
size = 0;
if (currpos + 4 > bytes.Length)
{
throw new Exception("No room in array to read string length");
}
size = BitConverter.ToInt32(bytes, currpos) + 4;
if (currpos + size > bytes.Length)
{
throw new Exception("No room in array to read string");
}
string str = Encoding.ASCII.GetString(bytes, currpos + 4, size - 4);
thestructure = str;
}
else if (T == typeof(char))
{
//.NET chars are 16bits -- the one we're receiving is 8bits
thestructure = (char)bytes[currpos];
size = 1;
}
else if (T == typeof(bool))
{
//1 byte in size, but a bit derpy as far as marshallability
thestructure = bytes[currpos] != 0;
size = 1;
}
else
{
size = Marshal.SizeOf(T);
IntPtr mem = IntPtr.Zero;
if (bytes.Length != 0)
{
mem = Marshal.AllocHGlobal(size);
Marshal.Copy(bytes, currpos, mem, size);
}
if (mem == IntPtr.Zero)
{
throw new Exception("Alloc failed");
}
thestructure = Marshal.PtrToStructure(mem, T);
Marshal.FreeHGlobal(mem);
}
currpos += size;
amountread = size;
return;
}
IRosMessage MSG;
int startingpos = currpos;
MsgTypes MT = MsgTypes.Unknown;
FieldInfo[] infos = GetFields(T, ref thestructure, out MSG);
if (MSG != null)
{
MT = MSG.msgtype;
}
//iterate over all fields of MSG, recursing for a sub-message where necessary
int currinfo = 0;
while ((currpos < bytes.Length) && currinfo < infos.Length)
{
FieldInfo info = infos[currinfo];
MsgFieldInfo mfi = MSG.Fields[info.Name];
if (mfi.IsConst)
{
continue;
}
Type realtype = info.FieldType;
Type type = GetType(realtype.FullName);
bool isMessage = realtype.FullName != null && realtype.FullName.StartsWith("Message");
MsgTypes msgtype = GetMessageType(type);
MsgTypes realmsgtype = GetMessageType(realtype);
bool knownpiecelength = IsSizeKnown(realtype, true);
if (realtype.IsArray)
{
#region ARRAY
Type ArrayElementType = realtype.GetElementType();
#region ARRAY THAT CAN BE HANDLED AS A WHOLE
if (ArrayElementType == typeof(byte))
{
//SHORTCUT FOR BYTE ARRAYS
int num = mfi.Length;
if (mfi.Length == -1) //if -1, then array length not in definition
{
num = BitConverter.ToInt32(bytes, currpos);
currpos += 4;
}
byte[] PWNED = new byte[num];
Array.Copy(bytes, currpos, PWNED, 0, num);
currpos += PWNED.Length;
info.SetValue(thestructure, PWNED);
}
else if (ArrayElementType == typeof(bool))
{
//SHORTCUT FOR BOOL ARRAYS
int num = mfi.Length;
if (mfi.Length == -1) //if -1, then array length not in definition
{
num = BitConverter.ToInt32(bytes, currpos);
currpos += 4;
}
bool[] PWNED = new bool[num];
for (int i = 0; i < num; i++)
{
PWNED[i] = bytes[i + currpos] != 0;
}
currpos += PWNED.Length;
info.SetValue(thestructure, PWNED);
}
else if (ArrayElementType == typeof(char))
{
//make sure we handle chars correctly (8bit in serialized message, vs 16bit struct size)
int num = mfi.Length;
if (mfi.Length == -1)
{
num = BitConverter.ToInt32(bytes, currpos);
currpos += 4;
}
char[] PWNED = new char[num];
for (int i = 0; i < num; i++)
{
PWNED[i] = (char)bytes[currpos + i];
}
currpos += PWNED.Length;
info.SetValue(thestructure, PWNED);
}
#endregion
else
{
MsgTypes ArrayElementMsgType = GetMessageType(ArrayElementType);
bool ElementSizeKnown = IsSizeKnown(ArrayElementType, true);
bool isElementMessage = (ArrayElementMsgType != MsgTypes.Unknown);
Array vals = (info.GetValue(thestructure) as Array);
#region determine array element size (if determinable)
int chunklen = mfi.Length;
if (vals == null)
{
if (chunklen != -1)
{
//if not -1, then size of array hardcoded in msg definition
vals = Array.CreateInstance(ArrayElementType, chunklen);
}
else if (currpos + 4 <= bytes.Length)
{
chunklen = BitConverter.ToInt32(bytes, currpos);
vals = Array.CreateInstance(ArrayElementType, chunklen);
currpos += 4;
}
else
{
throw new Exception("32bit array length extends beyond serialized data");
}
}
else
{
chunklen = vals.Length;
}
if (chunklen == -1)
{
throw new Exception("Could not determine number of array elements to deserialize");
}
#endregion
if (isElementMessage || ArrayElementType == typeof(string))
{
//handle each message or string in arrays of either as independent non-array things
int len;
for (int i = 0; i < chunklen; i++)
{
len = 0;
object data = null;
_deserialize(ref data, ref currpos, ArrayElementType, T, bytes, out len, ElementSizeKnown);
if (data.GetType() != vals.GetType().GetElementType())
{
throw new Exception("Type missmatch fail");
}
vals.SetValue(data, i);
}
}
else
{
//Array of literals with known size
int len = Marshal.SizeOf(ArrayElementType);
IntPtr pIP = IntPtr.Zero, sIP = IntPtr.Zero;
if (currpos + len * chunklen <= bytes.Length)
{
sIP = pIP = Marshal.AllocHGlobal(len * chunklen);
Marshal.Copy(bytes, currpos, pIP, len * chunklen);
}
object o = null;
for (int i = 0; i < chunklen * len; i += len)
{
if (pIP != IntPtr.Zero)
{
o = Marshal.PtrToStructure(pIP, ArrayElementType);
}
vals.SetValue(o, i / len);
if (pIP != IntPtr.Zero)
{
pIP = new IntPtr(pIP.ToInt32() + len);
}
}
if (sIP != IntPtr.Zero)
{
Marshal.FreeHGlobal(sIP);
}
currpos += chunklen * len;
}
info.SetValue(thestructure, vals);
}
#endregion
}
else
{
#region NON-ARRAY
if (isMessage)
{
if (realmsgtype == MsgTypes.std_msgs__Time || realmsgtype == MsgTypes.std_msgs__Duration || info.FieldType == typeof(TimeData))
{
//special cased because of custom 8-byte time struct
TimeData td;
if (currpos + 8 <= bytes.Length)
{
uint u1 = BitConverter.ToUInt32(bytes, currpos);
uint u2 = BitConverter.ToUInt32(bytes, currpos + 4);
td = new TimeData(u1, u2);
}
else
{
throw new Exception("TimeData fail");
}
currpos += 8;
if (info.FieldType == typeof(TimeData))
{
info.SetValue(thestructure, td);
}
else if (GetMessageType(realtype) == MsgTypes.std_msgs__Time)
{
info.SetValue(thestructure, (object)new std_msgs.Time(td));
}
else
{
info.SetValue(thestructure, (object)new std_msgs.Duration(td));
}
}
else
{
//all other message types, with extra verification for the length of strings
int len = 0;
object obj = null;
FieldInfo datafield = null;
bool isastring = false;
if (!knownpiecelength)
{
datafield = realtype.GetField("data");
isastring = (datafield != null && datafield.FieldType == typeof(string));
}
if (isastring)
{
if (currpos + 4 <= bytes.Length)
{
len = BitConverter.ToInt32(bytes, currpos);
}
}
int dontcare = 0;
_deserialize(ref obj, ref currpos, realtype, T, bytes, out dontcare, knownpiecelength);
if (isastring && bytes.Length != 0 && dontcare != len + 4)
{
throw new Exception("WTF?!");
}
info.SetValue(thestructure, obj);
}
}
else //if (!isMessage)
{
if (!knownpiecelength || realtype == typeof(bool) || realtype == typeof(char))
{
//handle special cases (strings, bools, chars) specially
int dontcare = 0;
object obj = "";
_deserialize(ref obj, ref currpos, realtype, T, bytes, out dontcare, false);
info.SetValue(thestructure, obj);
}
else
{
if (info.FieldType == typeof(bool))
{
info.SetValue(thestructure, bytes[currpos] != 0);
currpos += 1;
}
else
{
//non-array literal
int len = Marshal.SizeOf(info.GetValue(thestructure));
IntPtr pIP = Marshal.AllocHGlobal(len);
object obj = null;
if (currpos + len <= bytes.Length)
{
Marshal.Copy(bytes, currpos, pIP, len);
obj = Marshal.PtrToStructure(pIP, info.FieldType);
}
info.SetValue(thestructure, obj);
Marshal.FreeHGlobal(pIP);
currpos += len;
}
}
}
#endregion
}
currinfo++;
}
amountread = currpos - startingpos;
infos = null;
bytes = null;
}
