/// <summary>
/// Internal method, called by LCM when a message is received.
/// </summary>
public void MessageReceived(LCM lcm, string channel, LCMDataInputStream dins)
{
lock (this)
{
try
{
byte[] data = new byte[dins.Available];
dins.ReadFully(data);
messages.Enqueue(new Message(channel, data));
queueDataSize += data.Length;
while (queueDataSize > maxQueueDataSize || messages.Count > maxQueueLength)
{
Message toRemove = messages.Dequeue();
queueDataSize -= toRemove.Data.Length;
}
System.Threading.Monitor.Pulse(this);
}
catch (System.IO.IOException)
{
}
}
}
/// <summary>
/// Internal method, called by LCM when a message is received.
/// </summary>
public void MessageReceived(LCM lcm, string channel, LCMDataInputStream dins)
{
lock (this)
{
try
{
byte[] data = new byte[dins.Available];
dins.ReadFully(data);
messages.Enqueue(new Message(channel, data));
queueDataSize += data.Length;
while (queueDataSize > maxQueueDataSize || messages.Count > maxQueueLength)
{
Message toRemove = messages.Dequeue();
queueDataSize -= toRemove.Data.Length;
}
System.Threading.Monitor.Pulse(this);
}
catch (System.IO.IOException)
{
}
}
}
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 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;
__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);
}
