/// <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)
{
}
}
}
/* Methods */
/// <summary>
/// UDP multicast provider constructor
/// </summary>
/// <param name="lcm">LCM object</param>
/// <param name="up">URL parser object</param>
public UDPMulticastProvider(LCM lcm, URLParser up)
{
this.lcm = lcm;
string[] addrport = up.Get("network", DEFAULT_NETWORK).Split(':');
inetAddr = Dns.GetHostAddresses(addrport[0])[0];
inetPort = Int32.Parse(addrport[1]);
inetEP = new IPEndPoint(inetAddr, inetPort);
Debug.Log(inetEP);
sock = new UdpClient();
sock.MulticastLoopback = true;
sock.ExclusiveAddressUse = false;
sock.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, 1);
sock.Client.Bind(new IPEndPoint(IPAddress.Any, inetPort));
int ttl = up.Get("ttl", DEFAULT_TTL);
if (ttl == 0)
{
Console.Error.WriteLine("LCM: TTL set to zero, traffic will not leave localhost.");
}
else if (ttl > 1)
{
Console.Error.WriteLine("LCM: TTL set to > 1... That's almost never correct!");
}
else
{
Console.Error.WriteLine("LCM: TTL set to 1.");
}
sock.Ttl = (short)ttl;
sock.JoinMulticastGroup(inetAddr);
}
public GoalFlagsConnection(LCM.LCM.LCM lcm, CubeRouteField field)
{
this.lcm = lcm;
this.field = field;
this.lcm.Subscribe("MaestroDriver/Flags", this);
this.Running = false;
}
/* Methods */
/// <summary>
/// UDP multicast provider constructor
/// </summary>
/// <param name="lcm">LCM object</param>
/// <param name="up">URL parser object</param>
public UDPMulticastProvider(LCM lcm, URLParser up)
{
this.lcm = lcm;
string[] addrport = up.Get("network", DEFAULT_NETWORK).Split(':');
inetAddr = Dns.GetHostAddresses(addrport[0])[0];
inetPort = Int32.Parse(addrport[1]);
inetEP = new IPEndPoint(inetAddr, inetPort);
sock = new UdpClient();
sock.MulticastLoopback = true;
sock.ExclusiveAddressUse = false;
sock.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, 1);
sock.Client.Bind(new IPEndPoint(IPAddress.Any, inetPort));
int ttl = up.Get("ttl", DEFAULT_TTL);
if (ttl == 0)
{
Console.Error.WriteLine("LCM: TTL set to zero, traffic will not leave localhost.");
}
else if (ttl > 1)
{
Console.Error.WriteLine("LCM: TTL set to > 1... That's almost never correct!");
}
else
{
Console.Error.WriteLine("LCM: TTL set to 1.");
}
sock.Ttl = (short) ttl;
sock.JoinMulticastGroup(inetAddr);
}
public GoalFlagsConnection(LCM.LCM.LCM lcm, CubeRouteField field)
{
this.lcm = lcm;
this.field = field;
this.lcm.Subscribe("MaestroDriver/Flags", this);
this.Running = false;
}
/* Methods */
/// <summary>
/// TCP provider constructor
/// </summary>
/// <param name="lcm">LCM object</param>
/// <param name="up">URL parser object</param>
public TCPProvider(LCM lcm, URLParser up)
{
this.lcm = lcm;
string[] addrport = up.Get("network", DEFAULT_NETWORK).Split(':');
if (addrport.Length == 1)
{
inetAddr = System.Net.Dns.GetHostAddresses(addrport[0])[0];
inetPort = 7700;
}
else if (addrport.Length == 2)
{
inetAddr = System.Net.Dns.GetHostAddresses(addrport[0])[0];
inetPort = Int32.Parse(addrport[1]);
}
else
{
Console.Error.WriteLine("TCPProvider: Don't know how to parse " + up.Get("network", DEFAULT_NETWORK));
Environment.Exit(-1);
}
reader = new ReaderThread(this);
reader.Connected += OnConnect;
reader.Disconnected += OnDisconnect;
reader.Start();
}
public void MessageReceived(LCM.LCM.LCM lcm, string channel, LCMDataInputStream data_stream)
{
if (channel == Channels.discovery_res_channel)
{
discovery_response_t response = new discovery_response_t(data_stream);
Notification.Instance.GetEvent <DiscoveryResponseReceivedEvent>().Publish(response);
}
}
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 lcm, string channel, LCMDataInputStream data_stream)
{
if (channel == Channels.stream_res_channel)
{
stream_uri_response_t response = new stream_uri_response_t(data_stream);
var _ea = Notification.Instance;
_ea.GetEvent <StreamUriResponseReceivedEvent>().Publish(response);
}
}
public void MessageReceived(LCM.LCM.LCM lcm, string channel, LCMDataInputStream data_stream)
{
if (channel == Channels.end_program_mes_channel)
{
end_program_message_t response = new end_program_message_t(data_stream);
var _ea = Notification.Instance;
_ea.GetEvent <ProgramEndMessageReceivedEvent>().Publish(response);
}
}
public void MessageReceived(LCM.LCM.LCM lcm, string channel, LCMDataInputStream data_stream)
{
if (channel == Channels.output_req_channel)
{
output_request_t response = new output_request_t(data_stream);
var _ea = Notification.Instance;
_ea.GetEvent <UpdateOutputCameraReceivedEvent>().Publish(response);
}
}
public void MessageReceived(LCM.LCM.LCM lcm, string channel, LCMDataInputStream data_stream)
{
if (channel == Channels.init_session_res_channel)
{
init_session_response_t response = new init_session_response_t(data_stream);
var _ea = Notification.Instance;
_ea.GetEvent <InitSessionResponseReceivedEvent>().Publish(response);
}
}
public static void Main(string[] args)
{
LCM.LCM.LCM myLCM = null;
try
{
Console.WriteLine("Starting Maestro driver...");
myLCM = new LCM.LCM.LCM("udpm://239.255.76.67:7667?ttl=1");
Console.WriteLine("LCM Connected!");
}
catch (Exception ex)
{
Console.Error.WriteLine("Ex: " + ex);
Environment.Exit(1);
}
List <DeviceListItem> connectedDevices = Usc.getConnectedDevices();
Console.WriteLine("Number of MicroMaestros detected=" + connectedDevices.Count);
GoalFlags[] goals = new GoalFlags[4];
Dictionary <string, int> goalIDs = new Dictionary <string, int>();
goalIDs.Add("00032615", 3); //D
goalIDs.Add("00031531", 2); //C
goalIDs.Add("00032614", 1); //B
goalIDs.Add("00032598", 0); //A
foreach (DeviceListItem dli in connectedDevices)
{
Console.WriteLine(
"Detected Maestro: {serialNumber=" + dli.serialNumber +
", text=" + dli.text + "}");
goals[goalIDs[dli.serialNumber]] = new GoalFlags(new Usc(dli));
}
CubeRouteField field = new CubeRouteField(goals);
foreach (GoalFlags flags in goals)
{
for (int i = 0; i < 5; i++)
{
flags.setFlagPosition(i, 0);
}
}
// GoalLightsConnection conn = new GoalLightsConnection(null, "127.0.0.1", 9001, 9000);
GoalFlagsConnection conn = new GoalFlagsConnection(myLCM, field);
Console.WriteLine("Running...");
conn.Run();
}
public IEnumerator Listener()
{
Debug.Log ("Listener started.");
myLCM = new LCM.LCM.LCM();
myLCM.SubscribeAll(new SimpleSubscriber());
running = true;
while (running){
yield return null;
}
Debug.Log ("listener coroutine returning!");
}
public CameraExplorer(List <CameraInfo> camList, Dictionary <string, CameraInfo> ip2CameraInfo, Dictionary <string, string> IP2CameraName, ObservableCollection <CameraNameWrapper> cameraNameList)
{
this.camList = camList;
ip2CameraName = IP2CameraName;
this.cameraNameList = cameraNameList;
IP2CameraInfo = ip2CameraInfo;
_lcm = LCM.LCM.LCM.Singleton;
subscribeForResponses();
_ea = Notification.Instance;
_ea.GetEvent <CameraDiscoverEvent>().Subscribe(discover);
_ea.GetEvent <DiscoveryResponseReceivedEvent>().Subscribe(OnGetDiscoveryResponse);
}
public IEnumerator Listener()
{
Debug.Log("Listener started.");
myLCM = new LCM.LCM.LCM();
myLCM.SubscribeAll(new SimpleSubscriber());
running = true;
while (running)
{
yield return(null);
}
Debug.Log("listener coroutine returning!");
}
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 static void Main(string[] args)
{
LCM.LCM.LCM myLCM = null;
try
{
Console.WriteLine ("Starting Maestro driver...");
myLCM = new LCM.LCM.LCM("udpm://239.255.76.67:7667?ttl=1");
Console.WriteLine ("LCM Connected!");
}
catch (Exception ex)
{
Console.Error.WriteLine("Ex: " + ex);
Environment.Exit(1);
}
List<DeviceListItem> connectedDevices = Usc.getConnectedDevices();
Console.WriteLine ("Number of MicroMaestros detected=" + connectedDevices.Count);
GoalFlags[] goals = new GoalFlags[4];
Dictionary<string, int> goalIDs = new Dictionary<string, int>();
goalIDs.Add("00032615", 3);//D
goalIDs.Add("00031531", 2);//C
goalIDs.Add("00032614", 1);//B
goalIDs.Add("00032598", 0);//A
foreach (DeviceListItem dli in connectedDevices)
{
Console.WriteLine (
"Detected Maestro: {serialNumber=" + dli.serialNumber +
", text=" + dli.text + "}");
goals[goalIDs[dli.serialNumber]] = new GoalFlags(new Usc(dli));
}
CubeRouteField field = new CubeRouteField(goals);
foreach (GoalFlags flags in goals)
{
for(int i = 0; i< 5; i++)
{
flags.setFlagPosition(i, 0);
}
}
// GoalLightsConnection conn = new GoalLightsConnection(null, "127.0.0.1", 9001, 9000);
GoalFlagsConnection conn = new GoalFlagsConnection(myLCM, field);
Console.WriteLine ("Running...");
conn.Run ();
}
void setURL(string newURL)
{
print("Attempting to use LCM URL: " + newURL);
try
{
// Create new LCM object
myLCM = new LCM.LCM.LCM(newURL);
}
catch (Exception)
{
UnityEngine.Debug.LogWarning("Invalid or Unset LCM URL - reverting to default");
myLCM = new LCM.LCM.LCM("udpm://239.255.76.67:7667");
}
mySubscriber = new SimpleSubscriber();
myLCM.SubscribeAll(mySubscriber);
}
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)
{
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];
}
}
/* Methods */
/// <summary>
/// TCP provider constructor
/// </summary>
/// <param name="lcm">LCM object</param>
/// <param name="up">URL parser object</param>
public TCPProvider(LCM lcm, URLParser up)
{
this.lcm = lcm;
string[] addrport = up.Get("network", DEFAULT_NETWORK).Split(':');
if (addrport.Length == 1)
{
inetAddr = System.Net.Dns.GetHostAddresses(addrport[0])[0];
inetPort = 7700;
}
else if (addrport.Length == 2)
{
inetAddr = System.Net.Dns.GetHostAddresses(addrport[0])[0];
inetPort = Int32.Parse(addrport[1]);
}
else
{
Console.Error.WriteLine("TCPProvider: Don't know how to parse " + up.Get("network", DEFAULT_NETWORK));
Environment.Exit(-1);
}
reader = new ReaderThread(this);
reader.Start();
}
public LCMTransmitter()
{
this.appLCM = new LCM.LCM.LCM();
}
// Use this for initialization
void Start () {
Debug.Log("start");
myLCM = LCM.LCM.LCM.Singleton;
}
// Use this for initialization
void Start()
{
Debug.Log("start");
myLCM = LCM.LCM.LCM.Singleton;
}
// Start is called before the first frame update
void Start()
{
Debug.Log("publish start");
myLCM = new LCM.LCM.LCM();
}