/// <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)
{
}
}
}
public Flags(LCMDataInputStream ins)
{
if ((ulong) ins.ReadInt64() != LCM_FINGERPRINT)
throw new System.IO.IOException("LCM Decode error: bad fingerprint");
_decodeRecursive(ins);
}
public void MessageReceived(LCM.LCM.LCM lcm, string channel, LCM.LCM.LCMDataInputStream dins)
{
UnityEngine.Debug.LogError(channel);
if (channel == "MINIROV_ATTITUDE")
{
mwt.mini_rov_attitude_t msg = new mwt.mini_rov_attitude_t(dins);
instance.roll_deg = msg.roll_deg;
instance.pitch_deg = msg.pitch_deg;
instance.yaw_deg = msg.yaw_deg;
}
else if (channel == "MINIROV_TURNS")
{
mwt.turns_t msg = new mwt.turns_t(dins);
instance.turns = msg.turns;
}
else if (channel == "MINIROV_DEPTH")
{
mwt.mini_rov_depth_t msg = new mwt.mini_rov_depth_t(dins);
instance.depth = msg.depth;
instance.pressure = msg.pressure;
}
else if (channel == "MWT_STEREO_IMAGE")
{
mwt.stereo_image_t msg = new mwt.stereo_image_t(dins);
}
else if (channel == "MINIROV_ROWE_DVL")
{
mwt.mini_rov_rowe_dvl_t msg = new mwt.mini_rov_rowe_dvl_t(dins);
}
else if (channel == "SHIP_POSITION")
{
mwt.position_t msg = new mwt.position_t(dins);
instance.ship_lat = msg.latitude_deg;
instance.ship_lon = msg.longitude_deg;
}
else if (channel == "MINIROV_POSITION")
{
mwt.position_t msg = new mwt.position_t(dins);
instance.rov_lat = msg.latitude_deg;
instance.rov_lon = msg.longitude_deg;
}
else if (channel == "CLUMP_POSITION")
{
mwt.position_t msg = new mwt.position_t(dins);
instance.clump_lat = msg.latitude_deg;
instance.clump_lon = msg.longitude_deg;
}
else if (channel == "CLUMP_STATUS")
{
mwt.clump_status_t msg = new mwt.clump_status_t(dins);
instance.clump_delta = msg.delta_m;
}
else
{
// Debug.Log("Unknown Channel: " + channel);
}
}
public void MessageReceived(LCM.LCM lcm, string channel, LCMDataInputStream ins)
{
if (channel == "HALLWAY_TEMPERATURE")
{
try
{
temperature_t temp = new temperature_t(ins);
Console.WriteLine("The temperature is: " + temp.deg_celsius);
}
catch (System.IO.IOException ex)
{
Console.Error.WriteLine("Error decoding temperature message: " + ex);
}
}
}
public void MessageReceived(LCM.LCM.LCM lcm, string channel, LCM.LCM.LCMDataInputStream dins)
{
Debug.Log("RECV: " + channel);
if (channel == "EXAMPLE")
{
// forseti2.health msg = new forseti2.health(dins);
// String output = "CLCM received message of the type forseti2_health:\n";
// output+="msg.header.seq=\t" + msg.header.seq + "\n";
// output+="msg.header.time=\t" + msg.header.time + "\n";
// output+="msg.uptime=\t"+msg.uptime+"\n";
// Debug.Log (output);
//
// forseti2.health response = new forseti2.health();
// response.header = new forseti2.header();
// response.header.seq = seq++;
// TimeSpan span = DateTime.Now - new DateTime(1970, 1, 1);
// response.header.time = span.Ticks*100;
// response.uptime = Time.realtimeSinceStartup;
// lcm.Publish("unity/health", response);
exlcm.example_t msg = new exlcm.example_t(dins);
String message = "CLCM Received message of the type example_t:\n ";
message += String.Format(" timestamp = {0:D}", msg.timestamp);
message += String.Format(" position = ({0:N}, {1:N}, {2:N})\n ",
msg.position[0], msg.position[1], msg.position[2]);
message += String.Format(" orientation = ({0:N}, {1:N}, {2:N}, {3:N})\n",
msg.orientation[0], msg.orientation[1], msg.orientation[2],
msg.orientation[3]);
message += (" ranges = [ ");
for (int i = 0; i < msg.num_ranges; i++)
{
Console.Write(" {0:D}", msg.ranges[i]);
if (i < msg.num_ranges - 1)
{
Console.Write(", ");
}
}
message += (" ]\n");
message += (" name = '" + msg.name + "'\n");
message += (" enabled = '" + msg.enabled + "'\n");
Debug.Log(message);
}
}
public void MessageReceived(LCM.LCM.LCM lcm, string channel, LCM.LCM.LCMDataInputStream dins)
{
Console.WriteLine("RECV: " + channel);
if (channel == "MaestroDriver/Flags")
{
Flags msg = new Flags(dins);
Console.WriteLine("length1=" + msg.goals.Length);
Console.WriteLine("length2=" + msg.goals.LongLength);
for (int goal = 0; goal < 4; goal++)
{
for (int box = 0; box < 5; box++)
{
Console.WriteLine("element goal=" + goal + ",\t box=" + box + ", value=" + msg.goals[goal, box]);
this.field.Goals[goal].setFlagPosition(box, msg.goals[goal, box]);
}
}
}
}
public void MessageReceived(LCM.LCM.LCM lcm, string channel, LCM.LCM.LCMDataInputStream dins)
{
Debug.Log("RECV: " + channel);
if (channel == "body_move")
{
LCMTypes.c_test_lcmt msg = new LCMTypes.c_test_lcmt(dins);
String message = "CLCM Received message of the type c_test_lcmt:\n ";
message += String.Format(" position = ({0:N}, {1:N}, {2:N})\n ",
msg.body_pos[0], msg.body_pos[1], msg.body_pos[2]);
message += String.Format(" orientation = ({0:N}, {1:N}, {2:N})\n",
msg.body_ori[0], msg.body_ori[1], msg.body_ori[2]);
message += (" ]\n");
Debug.Log(message);
for (int i = 0; i < 3; ++i)
{
body_pos[i] = msg.body_pos[i];
}
//test_x = msg.body_pos[0];
}
}
public static LCMTypes.imageEnhanced_t _decodeRecursiveFactory(LCMDataInputStream ins)
{
LCMTypes.imageEnhanced_t o = new LCMTypes.imageEnhanced_t();
o._decodeRecursive(ins);
return o;
}
public void _decodeRecursive(LCMDataInputStream ins)
{
byte[] __strbuf = null;
this.utime = ins.ReadInt64();
this.deg_celsius = ins.ReadDouble();
}
public static LCMTypes.temperature_t _decodeRecursiveFactory(LCMDataInputStream ins)
{
LCMTypes.temperature_t o = new LCMTypes.temperature_t();
o._decodeRecursive(ins);
return o;
}
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();
}
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);
}
public static Forseti.Flags _decodeRecursiveFactory(LCMDataInputStream ins)
{
Forseti.Flags o = new Forseti.Flags();
o._decodeRecursive(ins);
return o;
}
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 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)
{
this.goals = new double[(int) 4,(int) 5];
for (int a = 0; a < 4; a++) {
for (int b = 0; b < 5; b++) {
this.goals[a,b] = ins.ReadDouble();
}
}
}
public static exlcm.example_t _decodeRecursiveFactory(LCMDataInputStream ins)
{
exlcm.example_t o = new exlcm.example_t();
o._decodeRecursive(ins);
return o;
}
