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