internal MessageAndSerializerFunc(IRosMessage msg, TopicManager.SerializeFunc serfunc, bool ser, bool nc)
{
this.msg = msg;
this.serfunc = serfunc;
serialize = ser;
nocopy = nc;
}
public static FieldInfo[] GetFields(Type T, ref object instance, out IRosMessage msg)
{
if (instance == null)
{
if (T.IsArray)
{
instance = Array.CreateInstance(T.GetElementType(), 0);
}
else
{
instance = Activator.CreateInstance(T);
}
}
IRosMessage MSG = instance as IRosMessage;
if (MSG == null)
{
msg = MSG;
return(instance.GetType().GetFields());
}
MsgTypes MT = MSG.msgtype;
if (MT != MsgTypes.Unknown)
{
msg = MSG;
return(msg.GetType().GetFields().Where((fi => MSG.Fields.Keys.Contains(fi.Name) && !fi.IsStatic)).ToArray());
}
throw new Exception("GetFields is weaksauce");
}
public static string Sum(MsgTypes m)
{
if (!md5memo.ContainsKey(m))
{
IRosMessage irm = IRosMessage.generate(m);
string hashme = PrepareToHash(irm);
md5memo.Add(m, Sum(hashme));
}
return(md5memo[m]);
}
public static Type GetType(string s)
{
Type ret;
MsgTypes mt = GetMessageType(s);
if (mt == MsgTypes.Unknown)
{
ret = Type.GetType(s, true, true);
}
else
{
ret = IRosMessage.generate(mt).GetType();
}
// Console.WriteLine(s + "=" + ret.Name);
return(ret);
}
public static FieldInfo[] GetFields(Type T, ref object instance, out IRosMessage msg)
{
if (T.IsArray)
{
throw new Exception("Called GetFields for an array type. Bad form!");
}
if (instance == null)
{
if (T.IsArray)
{
T = T.GetElementType();
instance = Array.CreateInstance(T, 0);
}
else if (T != typeof(string))
{
instance = Activator.CreateInstance(T);
}
else
{
instance = (object)"";
}
}
IRosMessage MSG = instance as IRosMessage;
if (instance != null && MSG == null)
{
throw new Exception("Garbage");
}
msg = MSG;
lock (speedyFields)
{
if (speedyFields.ContainsKey(T))
{
return(speedyFields[T]);
}
if (MSG == null || MSG.msgtype == MsgTypes.Unknown)
{
return(speedyFields[T] = instance.GetType().GetFields());
}
else if (MSG != null)
{
return(speedyFields[T] = MSG.GetType().GetFields().Where((fi => MSG.Fields.Keys.Contains(fi.Name) && !fi.IsStatic)).ToArray());
}
}
throw new Exception("GetFields is weaksauce");
}
public static IRosService generate <RequestType> (SrvTypes t) where RequestType : IRosMessage
{
lock ( constructors )
{
if (constructors.ContainsKey(t))
{
return(constructors [t].Invoke(t));
}
Type thistype = typeof(IRosService);
foreach (Type othertype in Assembly.GetAssembly(typeof(RequestType)).GetTypes())
{
if (thistype == othertype || !othertype.IsSubclassOf(thistype) || othertype.IsAbstract || othertype.Name.Contains("Decorator"))
{
continue;
}
IRosService srv = Activator.CreateInstance(othertype) as IRosService;
if (srv != null)
{
if (srv.srvtype() == SrvTypes.Unknown)
{
throw new Exception("OH NOES IRosService.generate is borked!");
}
if (!_typeregistry.ContainsKey(srv.srvtype()))
{
_typeregistry.Add(srv.srvtype(), srv.GetType());
}
if (!constructors.ContainsKey(srv.srvtype()))
{
constructors.Add(srv.srvtype(), T => Activator.CreateInstance(_typeregistry [T]) as IRosService);
}
srv.RequestMessage = IRosMessage.generate <RequestType> ((MsgTypes)Enum.Parse(typeof(MsgTypes), srv.srvtype().ToString() + "__Request"));
srv.ResponseMessage = IRosMessage.generate <RequestType> ((MsgTypes)Enum.Parse(typeof(MsgTypes), srv.srvtype().ToString() + "__Response"));
}
}
if (constructors.ContainsKey(t))
{
return(constructors [t].Invoke(t));
}
else
{
throw new Exception("OH NOES IRosService.generate is borked!");
}
}
}
public static IRosMessage generate <MessageType> (MsgTypes t)
{
lock ( constructors )
{
if (constructors.ContainsKey(t))
{
return(constructors [t].Invoke(t));
}
Type thistype = typeof(IRosMessage);
foreach (Type othertype in Assembly.GetAssembly(typeof(MessageType)).GetTypes())
{
if (thistype == othertype || !othertype.IsSubclassOf(thistype) || othertype.IsAbstract || othertype.Name.Contains("Decorator"))
{
continue;
}
IRosMessage msg = Activator.CreateInstance(othertype) as IRosMessage;
if (msg != null)
{
if (msg.msgtype() == MsgTypes.Unknown)
{
throw new Exception("OH NOES IRosMessage.generate is borked!");
}
if (!_typeregistry.ContainsKey(msg.msgtype()))
{
_typeregistry.Add(msg.msgtype(), msg.GetType());
}
if (!constructors.ContainsKey(msg.msgtype()))
{
constructors.Add(msg.msgtype(), T => Activator.CreateInstance(_typeregistry [T]) as IRosMessage);
}
}
}
if (constructors.ContainsKey(t))
{
return(constructors [t].Invoke(t));
}
else
{
throw new Exception("OH NOES IRosMessage.generate is borked!");
}
}
}
public static bool IsSizeKnown(Type T, bool recurse)
{
if (T == typeof(string) || T == typeof(String) ||
(T.FullName != null && T.FullName.Contains("Messages.std_msgs.String")) /*|| T.IsArray ERIC*/)
{
return(false);
}
if (T.FullName.Contains("System.") || T.FullName.Contains("Messages.std_msgs.Time") || T.FullName.Contains("Messages.std_msgs.Duration"))
{
return(true);
}
if (!recurse || !T.FullName.Contains("Messages"))
{
return(true);
}
FieldInfo[] infos = T.GetFields();
bool b = true;
foreach (FieldInfo info in infos)
{
string fullName = info.FieldType.FullName;
if (fullName != null)
{
if (fullName.Contains("Messages."))
{
MsgTypes MT = GetMessageType(info.FieldType);
IRosMessage TI = IRosMessage.generate(MT);
b &= IsSizeKnown(TI.GetType(), true); //TI.Fields[info.Name].Type != typeof(string) && TI.Fields[info.Name].Type != typeof(String) && (!TI.Fields[info.Name].IsArray || TI.Fields[info.Name].Length != -1);
}
else
{
b &= !info.FieldType.IsArray && info.FieldType != typeof(string);
}
}
if (!b)
{
break;
}
}
return(b);
}
public static Type GetType(string s)
{
lock (GetTypeTypeMemo)
{
if (GetTypeTypeMemo.ContainsKey(s))
{
return(GetTypeTypeMemo[s]);
}
Type ret;
MsgTypes mt = GetMessageType(s);
if (mt == MsgTypes.Unknown)
{
ret = Type.GetType(s, true, true);
}
else
{
ret = IRosMessage.generate(mt).GetType();
}
return(GetTypeTypeMemo[s] = ret);
}
}
public static byte[] NeedsMoreChunks(Type T, object val, ref bool knownlength)
{
byte[] thischunk = null;
if (!T.IsArray)
{
if (T != typeof(TimeData) && T.Namespace.Contains("Message"))
{
IRosMessage msg = null;
if (val != null)
{
msg = val as IRosMessage;
}
else
{
msg = (IRosMessage)Activator.CreateInstance(T);
}
thischunk = msg.Serialize(true);
}
else if (T == typeof(byte) || T.HasElementType && T.GetElementType() == typeof(byte))
{
if (T.IsArray)
{
if (!knownlength)
{
Array ar = (val as Array);
byte[] nolen = new byte[ar.Length];
Array.Copy(ar, 0, nolen, 0, ar.Length);
thischunk = new byte[nolen.Length + 4];
byte[] bylen2 = BitConverter.GetBytes(nolen.Length);
Array.Copy(nolen, 0, thischunk, 4, nolen.Length);
Array.Copy(bylen2, thischunk, 4);
}
else
{
Array ar = (val as Array);
thischunk = new byte[ar.Length];
Array.Copy(ar, 0, thischunk, 0, ar.Length);
knownlength = false;
}
}
else
{
thischunk = new[] { (byte)val };
}
}
else if (val is string || T == typeof(string))
{
if (!knownlength)
{
if (val == null)
{
val = "";
}
byte[] nolen = Encoding.ASCII.GetBytes((string)val);
thischunk = new byte[nolen.Length + 4];
byte[] bylen2 = BitConverter.GetBytes(nolen.Length);
Array.Copy(nolen, 0, thischunk, 4, nolen.Length);
Array.Copy(bylen2, thischunk, 4);
}
else
{
thischunk = Encoding.ASCII.GetBytes((string)val);
knownlength = false;
}
}
else if (val is bool || T == typeof(bool))
{
thischunk = new byte[1];
thischunk[0] = (byte)((bool)val ? 1 : 0);
}
else
{
byte[] temp = new byte[Marshal.SizeOf(T)];
GCHandle h = GCHandle.Alloc(temp, GCHandleType.Pinned);
Marshal.StructureToPtr(val, h.AddrOfPinnedObject(), false);
h.Free();
thischunk = new byte[temp.Length + (knownlength ? 0 : 4)];
if (!knownlength)
{
byte[] bylen = BitConverter.GetBytes(temp.Length);
Array.Copy(bylen, 0, thischunk, 0, 4);
}
Array.Copy(temp, 0, thischunk, (knownlength ? 0 : 4), temp.Length);
}
}
else
{
int arraylength = 0;
Array valArray = val as Array;
List <byte> thechunk = new List <byte>();
for (int i = 0; i < valArray.GetLength(0); i++)
{
if (valArray.GetValue(i) == null)
{
valArray.SetValue(Activator.CreateInstance(T.GetElementType()), i);
}
Type TT = valArray.GetValue(i).GetType();
MsgTypes mt = GetMessageType(TT);
bool piecelengthknown = mt != MsgTypes.std_msgs__String;
thechunk.AddRange(NeedsMoreChunks(TT, valArray.GetValue(i), ref piecelengthknown));
}
if (!knownlength)
{
thechunk.InsertRange(0, BitConverter.GetBytes(valArray.GetLength(0)));
}
return(thechunk.ToArray());
}
return(thischunk);
}
protected void InitSubtypes(IRosMessage request, IRosMessage response)
{
RequestMessage = request;
ResponseMessage = response;
}
protected IRosMessage GeneralInvoke(RosServiceDelegate invocation, IRosMessage m)
{
return(invocation.Invoke(m));
}
public virtual bool Equals(IRosMessage msg)
{
throw new NotImplementedException();
}
public static FieldInfo[] GetFields(Type T, ref object instance)
{
IRosMessage dontcare = null;
return(GetFields(T, ref instance, out dontcare));
}
public static byte[] NeedsMoreChunks(Type T, object val, ref bool knownlength)
{
byte[] thischunk = null;
if (!T.IsArray)
{
if (T != typeof(TimeData) && T.Namespace.Contains("Message"))
{
IRosMessage msg = null;
if (val != null)
{
msg = val as IRosMessage;
}
else
{
msg = (IRosMessage)Activator.CreateInstance(T);
}
thischunk = msg.Serialize(true);
}
else if (T == typeof(byte) || T.HasElementType && T.GetElementType() == typeof(byte))
{
if (T.IsArray)
{
if (!knownlength)
{
Array ar = (val as Array);
byte[] nolen = new byte[ar.Length];
Array.Copy(ar, 0, nolen, 0, ar.Length);
thischunk = new byte[nolen.Length + 4];
byte[] bylen2 = BitConverter.GetBytes(nolen.Length);
Array.Copy(nolen, 0, thischunk, 4, nolen.Length);
Array.Copy(bylen2, thischunk, 4);
}
else
{
Array ar = (val as Array);
thischunk = new byte[ar.Length];
Array.Copy(ar, 0, thischunk, 0, ar.Length);
knownlength = false;
}
}
else
{
thischunk = new[] { (byte)val };
}
}
else if (val is string || T == typeof(string))
{
if (!knownlength)
{
if (val == null)
{
val = "";
}
byte[] nolen = Encoding.ASCII.GetBytes((string)val);
thischunk = new byte[nolen.Length + 4];
byte[] bylen2 = BitConverter.GetBytes(nolen.Length);
Array.Copy(nolen, 0, thischunk, 4, nolen.Length);
Array.Copy(bylen2, thischunk, 4);
}
else
{
thischunk = Encoding.ASCII.GetBytes((string)val);
knownlength = false;
}
}
else if (val is bool || T == typeof(bool))
{
thischunk = new byte[1];
thischunk[0] = (byte)((bool)val ? 1 : 0);
}
else
{
byte[] temp = new byte[Marshal.SizeOf(T)];
GCHandle h = GCHandle.Alloc(temp, GCHandleType.Pinned);
Marshal.StructureToPtr(val, h.AddrOfPinnedObject(), false);
h.Free();
thischunk = new byte[temp.Length + (knownlength ? 0 : 4)];
if (!knownlength)
{
byte[] bylen = BitConverter.GetBytes(temp.Length);
Array.Copy(bylen, 0, thischunk, 0, 4);
}
Array.Copy(temp, 0, thischunk, (knownlength ? 0 : 4), temp.Length);
}
}
else
{
int arraylength = 0;
object[] vals = (val as Array).Cast <object>().ToArray();
Queue <byte[]> arraychunks = new Queue <byte[]>();
for (int i = 0; i < vals.Length; i++)
{
if (vals[i] == null)
{
vals[i] = Activator.CreateInstance(T.GetElementType());
}
Type TT = vals[i].GetType();
MsgTypes mt = GetMessageType(TT);
bool piecelengthknown = mt != MsgTypes.std_msgs__String;
byte[] chunk = NeedsMoreChunks(TT, vals[i], ref piecelengthknown);
arraychunks.Enqueue(chunk);
arraylength += chunk.Length;
}
thischunk = new byte[knownlength ? arraylength : (arraylength + 4)];
if (!knownlength)
{
byte[] bylen = BitConverter.GetBytes(vals.Length);
Array.Copy(bylen, 0, thischunk, 0, 4);
}
int arraypos = knownlength ? 0 : 4;
while (arraychunks.Count > 0)
{
byte[] chunk = arraychunks.Dequeue();
Array.Copy(chunk, 0, thischunk, arraypos, chunk.Length);
arraypos += chunk.Length;
}
}
return(thischunk);
}
protected IRosMessage GeneralInvoke(RosServiceDelegate invocation, IRosMessage m)
{
return invocation.Invoke(m);
}
public virtual void processResponse(IRosMessage msg, bool success)
{
msg.Serialize();
byte[] buf = new byte[msg.Serialized.Length + 1];
buf[0] = (byte) (success ? 0x01 : 0x00);
msg.Serialized.CopyTo(buf, 1);
connection.write(buf, (uint) buf.Length, onResponseWritten, true);
}
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 virtual bool Equals(IRosMessage msg)
{
throw new NotImplementedException();
}
public static byte[] NeedsMoreChunks(Type T, object val, FieldInfo fi, object topmost, bool knownlength)
{
byte[] thischunk = null;
if (!T.IsArray)
{
if (T.Namespace.Contains("Message"))
{
IRosMessage msg = null;
if (val != null)
{
msg = (IRosMessage)Activator.CreateInstance(typeof(TypedMessage <>).MakeGenericType(T), val);
}
else
{
msg = (IRosMessage)Activator.CreateInstance(typeof(TypedMessage <>).MakeGenericType(T));
}
thischunk = msg.Serialize();
}
else if (val is string || T == typeof(string))
{
if (val == null)
{
val = "";
}
byte[] nolen = Encoding.ASCII.GetBytes((string)val);
thischunk = new byte[nolen.Length + 4];
byte[] bylen2 = BitConverter.GetBytes(nolen.Length);
Array.Copy(nolen, 0, thischunk, 4, nolen.Length);
Array.Copy(bylen2, thischunk, 4);
}
else
{
byte[] temp = new byte[Marshal.SizeOf(T)];
GCHandle h = GCHandle.Alloc(temp, GCHandleType.Pinned);
Marshal.StructureToPtr(val, h.AddrOfPinnedObject(), false);
h.Free();
thischunk = new byte[temp.Length + (knownlength?0:4)];
if (!knownlength)
{
byte[] bylen = BitConverter.GetBytes(temp.Length);
Array.Copy(bylen, 0, thischunk, 0, 4);
}
Array.Copy(temp, 0, thischunk, (knownlength?0:4), temp.Length);
}
}
else
{
int arraylength = 0;
List <object> valslist = new List <object>();
foreach (object o in (val as Array))
{
valslist.Add(o);
}
object[] vals = valslist.ToArray();
Queue <byte[]> arraychunks = new Queue <byte[]>();
for (int i = 0; i < vals.Length; i++)
{
Type TT = vals[i].GetType();
#if arraypiecesneedlengthtoo
bool pieceknownlength =;
byte[] chunkwithoutlen = NeedsMoreChunks(TT, vals[i], fi, topmost, pieceknownlength);
byte[] chunklen = BitConverter.GetBytes(chunkwithoutlen.Length);
byte[] chunk = new byte[chunkwithoutlen.Length + (pieceknownlength ? 0 : 4)];
if (!pieceknownlength)
{
Array.Copy(chunklen, 0, chunk, 0, 4);
}
Array.Copy(chunkwithoutlen, 0, chunk, (pieceknownlength ? 0 : 4), chunkwithoutlen.Length);
#else
byte[] chunk = NeedsMoreChunks(TT, vals[i], fi, topmost, true);
#endif
arraychunks.Enqueue(chunk);
arraylength += chunk.Length;
}
thischunk = new byte[knownlength?arraylength:(arraylength + 4)];
if (!knownlength)
{
byte[] bylen = BitConverter.GetBytes(vals.Length);
Array.Copy(bylen, 0, thischunk, 0, 4);
}
int arraypos = knownlength?0:4;
while (arraychunks.Count > 0)
{
byte[] chunk = arraychunks.Dequeue();
Array.Copy(chunk, 0, thischunk, arraypos, chunk.Length);
arraypos += chunk.Length;
}
}
return(thischunk);
}
protected void InitSubtypes(IRosMessage request, IRosMessage response)
{
RequestMessage = request;
ResponseMessage = response;
}
static string PrepareToHash(IRosMessage irm)
{
MsgTypes m = irm.msgtype;
string hashme = irm.MessageDefinition.Trim('\n', '\t', '\r', ' ');
while (hashme.Contains(" "))
{
hashme = hashme.Replace(" ", " ");
}
while (hashme.Contains("\r\n"))
{
hashme = hashme.Replace("\r\n", "\n");
}
hashme = hashme.Trim();
string[] lines = hashme.Split('\n');
//this shit is bananas.
Queue <string> haves = new Queue <string>(), havenots = new Queue <string>();
for (int i = 0; i < lines.Length; i++)
{
string l = lines[i];
if (l.Contains("="))
{
haves.Enqueue(l);
}
else
{
havenots.Enqueue(l);
}
}
hashme = "";
while (haves.Count + havenots.Count > 0)
{
hashme += (haves.Count > 0 ? haves.Dequeue() : havenots.Dequeue()) + (haves.Count + havenots.Count >= 1 ? "\n" : "");
}
/*if (irm.IsServiceComponent)
* {
* object o = irm;
* FieldInfo[] infos = SerializationHelper.GetFields(irm.GetType(), ref o, out irm);
* if (infos.Length == 1 && infos[0].FieldType.FullName.Contains("Messages"))
* {
*
* }
* }*/
if (irm.IsMetaType || irm.IsServiceComponent)
{
Type t = irm.GetType();
object o = irm;
FieldInfo[] fields = SerializationHelper.GetFields(t, ref o, out irm);;
for (int i = 0; i < fields.Length; i++)
{
Type FieldType = fields[i].FieldType;
if (!FieldType.Namespace.Contains("Messages"))
{
continue;
}
while (FieldType.IsArray)
{
FieldType = FieldType.GetElementType();
}
MsgTypes T =
(MsgTypes)
Enum.Parse(typeof(MsgTypes), FieldType.FullName.Replace("Messages.", "").Replace(".", "__"));
string[] BLADAMN = hashme.Replace(FieldType.Name, Sum(T)).Split('\n');
hashme = "";
for (int x = 0; x < BLADAMN.Length; x++)
{
if (BLADAMN[x].Contains(fields[i].Name))
{
if (BLADAMN[x].Contains("/"))
{
BLADAMN[x] = BLADAMN[x].Split('/')[1];
}
if (BLADAMN[x].Contains("[]") && !irm.Fields[fields[i].Name].IsLiteral)
{
BLADAMN[x] = BLADAMN[x].Replace("[]", "");
}
}
hashme += BLADAMN[x];
if (x < BLADAMN.Length - 1)
{
hashme += "\n";
}
}
}
}
return(hashme);
}
static string PrepareToHash(IRosMessage irm)
{
MsgTypes m = irm.msgtype;
string hashme = irm.MessageDefinition.Trim('\n', '\t', '\r', ' ');
while (hashme.Contains(" "))
hashme = hashme.Replace(" ", " ");
while (hashme.Contains("\r\n"))
hashme = hashme.Replace("\r\n", "\n");
hashme = hashme.Trim();
string[] lines = hashme.Split('\n');
//this shit is bananas.
Queue<string> haves = new Queue<string>(), havenots = new Queue<string>();
for (int i = 0; i < lines.Length; i++)
{
string l = lines[i];
if (l.Contains("=")) haves.Enqueue(l); else havenots.Enqueue(l);
} hashme = "";
while (haves.Count + havenots.Count > 0) hashme += (haves.Count > 0 ? haves.Dequeue() : havenots.Dequeue()) + (haves.Count + havenots.Count >= 1 ? "\n" : "");
/*if (irm.IsServiceComponent)
{
object o = irm;
FieldInfo[] infos = SerializationHelper.GetFields(irm.GetType(), ref o, out irm);
if (infos.Length == 1 && infos[0].FieldType.FullName.Contains("Messages"))
{
}
}*/
if (irm.IsMetaType || irm.IsServiceComponent)
{
Type t = irm.GetType();
object o = irm;
FieldInfo[] fields = SerializationHelper.GetFields(t, ref o, out irm); ;
for (int i = 0; i < fields.Length; i++)
{
Type FieldType = fields[i].FieldType;
if (!FieldType.Namespace.Contains("Messages")) continue;
while (FieldType.IsArray) FieldType = FieldType.GetElementType();
MsgTypes T =
(MsgTypes)
Enum.Parse(typeof(MsgTypes), FieldType.FullName.Replace("Messages.", "").Replace(".", "__"));
string[] BLADAMN = hashme.Replace(FieldType.Name, Sum(T)).Split('\n');
hashme = "";
for (int x = 0; x < BLADAMN.Length; x++)
{
if (BLADAMN[x].Contains(fields[i].Name))
{
if (BLADAMN[x].Contains("/"))
{
BLADAMN[x] = BLADAMN[x].Split('/')[1];
}
if (BLADAMN[x].Contains("[]") && !irm.Fields[fields[i].Name].IsLiteral)
{
BLADAMN[x] = BLADAMN[x].Replace("[]", "");
}
}
hashme += BLADAMN[x];
if (x < BLADAMN.Length - 1)
hashme += "\n";
}
}
}
return hashme;
}
public static FieldInfo[] GetFields(Type T, out IRosMessage msg)
{
object dontcare = null;
return(GetFields(T, ref dontcare, out msg));
}
