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