private void HandleShortMessage(byte[] packetData, IPEndPoint from, LCMDataInputStream ins)
{
int msgSeqNumber = ins.ReadInt32();
string channel = ins.ReadStringZ();
lcm.ReceiveMessage(channel, ins.Buffer, ins.BufferOffset, ins.Available);
}
private void HandlePacket(byte[] packetData, IPEndPoint from)
{
LCMDataInputStream ins = new LCMDataInputStream(packetData, 0, packetData.Length);
int magic = ins.ReadInt32();
if (magic == UDPMulticastProvider.MAGIC_SHORT)
{
HandleShortMessage(packetData, from, ins);
}
else if (magic == UDPMulticastProvider.MAGIC_LONG)
{
HandleFragment(packetData, from, ins);
}
else
{
Console.Error.WriteLine("Bad magic: " + System.Convert.ToString(magic, 16));
return;
}
}
private void HandleFragment(byte[] packetData, IPEndPoint from, LCMDataInputStream ins)
{
int msgSeqNumber = ins.ReadInt32();
int msgSize = ins.ReadInt32() & unchecked ((int)0xffffffff);
int fragmentOffset = ins.ReadInt32() & unchecked ((int)0xffffffff);
int fragmentId = ins.ReadInt16() & 0xffff;
int fragmentsInMsg = ins.ReadInt16() & 0xffff;
// read entire packet payload
byte[] payload = new byte[ins.Available];
ins.ReadFully(payload);
if (ins.Available > 0)
{
System.Console.Error.WriteLine("Unread data! " + ins.Available);
}
int dataStart = 0;
int fragSize = payload.Length;
FragmentBuffer fbuf;
fragBufs.TryGetValue(from.Serialize(), out fbuf);
if (fbuf != null && ((fbuf.msgSeqNumber != msgSeqNumber) || (fbuf.data_size != msgSize)))
{
fragBufs.Remove(fbuf.from);
fbuf = null;
}
if (fbuf == null && fragmentId == 0)
{
// extract channel name
int channelLen = 0;
for (; channelLen < payload.Length; channelLen++)
{
if (payload[channelLen] == 0)
{
break;
}
}
dataStart = channelLen + 1;
fragSize -= (channelLen + 1);
string tempStr;
tempStr = System.Text.Encoding.GetEncoding("US-ASCII").GetString(payload);
string channel = new string(tempStr.ToCharArray(), 0, channelLen);
fbuf = new FragmentBuffer(from.Serialize(), channel, msgSeqNumber, msgSize, fragmentsInMsg);
fragBufs.Add(fbuf.from, fbuf);
}
if (fbuf == null)
{
// TODO
return;
}
if (fragmentOffset + fragSize > fbuf.data_size)
{
System.Console.Error.WriteLine("LC: dropping invalid fragment");
fragBufs.Remove(fbuf.from);
return;
}
Array.Copy(payload, dataStart, fbuf.data, fragmentOffset, fragSize);
fbuf.fragments_remaining--;
if (0 == fbuf.fragments_remaining)
{
lcm.ReceiveMessage(fbuf.channel, fbuf.data, 0, fbuf.data_size);
fragBufs.Remove(fbuf.from);
}
}
public void _decodeRecursive(LCMDataInputStream ins)
{
byte[] __strbuf = null;
this.timestamp = ins.ReadInt64();
this.position = new double[(int) 3];
for (int a = 0; a < 3; a++) {
this.position[a] = ins.ReadDouble();
}
this.orientation = new double[(int) 4];
for (int a = 0; a < 4; a++) {
this.orientation[a] = ins.ReadDouble();
}
this.num_ranges = ins.ReadInt32();
this.ranges = new short[(int) num_ranges];
for (int a = 0; a < this.num_ranges; a++) {
this.ranges[a] = ins.ReadInt16();
}
__strbuf = new byte[ins.ReadInt32()-1]; ins.ReadFully(__strbuf); ins.ReadByte(); this.name = System.Text.Encoding.GetEncoding("US-ASCII").GetString(__strbuf);
this.enabled = ins.ReadBoolean();
}
private void HandlePacket(byte[] packetData, IPEndPoint from)
{
LCMDataInputStream ins = new LCMDataInputStream(packetData, 0, packetData.Length);
int magic = ins.ReadInt32();
if (magic == UDPMulticastProvider.MAGIC_SHORT)
{
HandleShortMessage(packetData, from, ins);
}
else if (magic == UDPMulticastProvider.MAGIC_LONG)
{
HandleFragment(packetData, from, ins);
}
else
{
Console.Error.WriteLine("Bad magic: " + System.Convert.ToString(magic, 16));
return;
}
}
private void HandleFragment(byte[] packetData, IPEndPoint from, LCMDataInputStream ins)
{
int msgSeqNumber = ins.ReadInt32();
int msgSize = ins.ReadInt32() & unchecked((int) 0xffffffff);
int fragmentOffset = ins.ReadInt32() & unchecked((int) 0xffffffff);
int fragmentId = ins.ReadInt16() & 0xffff;
int fragmentsInMsg = ins.ReadInt16() & 0xffff;
// read entire packet payload
byte[] payload = new byte[ins.Available];
ins.ReadFully(payload);
if (ins.Available > 0)
{
System.Console.Error.WriteLine("Unread data! " + ins.Available);
}
int dataStart = 0;
int fragSize = payload.Length;
FragmentBuffer fbuf;
fragBufs.TryGetValue(from.Serialize(), out fbuf);
if (fbuf != null && ((fbuf.msgSeqNumber != msgSeqNumber) || (fbuf.data_size != msgSize)))
{
fragBufs.Remove(fbuf.from);
fbuf = null;
}
if (fbuf == null && fragmentId == 0)
{
// extract channel name
int channelLen = 0;
for (; channelLen < payload.Length; channelLen++)
{
if (payload[channelLen] == 0)
{
break;
}
}
dataStart = channelLen + 1;
fragSize -= (channelLen + 1);
string tempStr;
tempStr = System.Text.Encoding.GetEncoding("US-ASCII").GetString(payload);
string channel = new string(tempStr.ToCharArray(), 0, channelLen);
fbuf = new FragmentBuffer(from.Serialize(), channel, msgSeqNumber, msgSize, fragmentsInMsg);
fragBufs.Add(fbuf.from, fbuf);
}
if (fbuf == null)
{
// TODO
return ;
}
if (fragmentOffset + fragSize > fbuf.data_size)
{
System.Console.Error.WriteLine("LC: dropping invalid fragment");
fragBufs.Remove(fbuf.from);
return ;
}
Array.Copy(payload, dataStart, fbuf.data, fragmentOffset, fragSize);
fbuf.fragments_remaining--;
if (0 == fbuf.fragments_remaining)
{
lcm.ReceiveMessage(fbuf.channel, fbuf.data, 0, fbuf.data_size);
fragBufs.Remove(fbuf.from);
}
}
private void HandleShortMessage(byte[] packetData, IPEndPoint from, LCMDataInputStream ins)
{
int msgSeqNumber = ins.ReadInt32();
string channel = ins.ReadStringZ();
lcm.ReceiveMessage(channel, ins.Buffer, ins.BufferOffset, ins.Available);
}
public void _decodeRecursive(LCMDataInputStream ins)
{
byte[] __strbuf = null;
__strbuf = new byte[ins.ReadInt32() - 1]; ins.ReadFully(__strbuf); ins.ReadByte(); this.imageFilename = System.Text.Encoding.GetEncoding("US-ASCII").GetString(__strbuf);
this.jointX = ins.ReadInt16();
this.jointY = ins.ReadInt16();
this.radius = ins.ReadInt16();
}
public void _decodeRecursive(LCMDataInputStream ins)
{
byte[] __strbuf = null;
__strbuf = new byte[ins.ReadInt32() - 1]; ins.ReadFully(__strbuf); ins.ReadByte(); this.imageFilename = System.Text.Encoding.GetEncoding("US-ASCII").GetString(__strbuf);
this.imageNumber = ins.ReadInt16();
__strbuf = new byte[ins.ReadInt32() - 1]; ins.ReadFully(__strbuf); ins.ReadByte(); this.agentName = System.Text.Encoding.GetEncoding("US-ASCII").GetString(__strbuf);
__strbuf = new byte[ins.ReadInt32() - 1]; ins.ReadFully(__strbuf); ins.ReadByte(); this.agentAuthor = System.Text.Encoding.GetEncoding("US-ASCII").GetString(__strbuf);
__strbuf = new byte[ins.ReadInt32() - 1]; ins.ReadFully(__strbuf); ins.ReadByte(); this.vote = System.Text.Encoding.GetEncoding("US-ASCII").GetString(__strbuf);
}
