private void Send(RosMessage_old message)
{
MaybeLog("Send ROSmessage...");
byte[] buffer = encoder.GetBytes(CreateRosMessageString(message));
/*
* rosBridgeWebSocket.Send(buffer);
*/
}
public void EnqueRosCommand(RosMessage_old message)
{
//if (verbose) this.debugHUDText.text = "\n Try to get token..." + this.debugHUDText.text;
// lock (syncObjCommandQueue) {
//if (verbose) this.debugHUDText.text = "\n success" + this.debugHUDText.text;
this.rosCommandQueue.Enqueue(message);
//if (verbose) this.debugHUDText.text = "\n Enqueued message succesfully" + this.debugHUDText.text;
//MaybeLog("" + rosCommandQueue.Count());
// }
}
// works
private async void Send(RosMessage_old message)
{
MaybeLog("Send ROSmessage...");
DataWriter messageWriter = new DataWriter(webSock.OutputStream);
string messageString = CreateRosMessageString(message);
messageWriter.WriteString(messageString);
await messageWriter.StoreAsync();
await messageWriter.FlushAsync();
messageWriter.DetachStream();
}
// serializes a RosMessage to a string
// TODO: generic
public string CreateRosMessageString(RosMessage_old msg)
{
string messageString = "";
if (msg.op == "subscribe")
{
MaybeLog("CreateRosMessageStrings...");
messageString = "{ \"op\": \"";
messageString += msg.op;
messageString += "\", ";
messageString += "\"topic\": \"";
messageString += msg.topic;
messageString += "\", ";
messageString += "\"throttle_rate\": ";
messageString += ((RosSubscribe_old)msg).throttle_rate;
messageString += "}";
}
else if (msg.op == "publish")
{
if (msg.topic == "/hololens/plan_pick")
{
messageString = "{ \"op\": \"";
messageString += msg.op;
messageString += "\", ";
messageString += "\"topic\": \"";
messageString += msg.topic;
messageString += "\", ";
messageString += "\"msg\": ";
messageString += "{";
RosMessages_old.geometry_msgs.PointStamped_old ps = (RosMessages_old.geometry_msgs.PointStamped_old)((RosPublish_old)msg).msg;
messageString += "\"header\": ";
messageString += "{";
messageString += "\"stamp\": ";
messageString += "{";
messageString += "\"secs\": ";
messageString += 0;
messageString += " ,";
messageString += "\"nsecs\": ";
messageString += 0;
messageString += "}";
messageString += " ,";
messageString += "\"frame_id\": ";
messageString += "\"" + ps.header.frame_id + "\"";
messageString += " ,";
messageString += "\"seq\": ";
messageString += ps.header.seq;
messageString += "}";
messageString += " ,";
messageString += "\"point\": ";
messageString += "{";
messageString += "\"x\": ";
messageString += ps.point.x;
messageString += " ,";
messageString += "\"y\": ";
messageString += ps.point.y;
messageString += " ,";
messageString += "\"z\": ";
messageString += ps.point.z;
messageString += "}";
messageString += "}";
messageString += "}";
}
else if (msg.topic == "/hololens/plan_place")
{
messageString = "{ \"op\": \"";
messageString += msg.op;
messageString += "\", ";
messageString += "\"topic\": \"";
messageString += msg.topic;
messageString += "\", ";
messageString += "\"msg\": ";
messageString += "{";
RosMessages_old.geometry_msgs.PointStamped_old ps = (RosMessages_old.geometry_msgs.PointStamped_old)((RosPublish_old)msg).msg;
messageString += "\"header\": ";
messageString += "{";
messageString += "\"stamp\": ";
messageString += "{";
messageString += "\"secs\": ";
messageString += 0;
messageString += " ,";
messageString += "\"nsecs\": ";
messageString += 0;
messageString += "}";
messageString += " ,";
messageString += "\"seq\": ";
messageString += ps.header.seq;
messageString += "}";
messageString += " ,";
messageString += "\"point\": ";
messageString += "{";
messageString += "\"x\": ";
messageString += ps.point.x;
messageString += " ,";
messageString += "\"y\": ";
messageString += ps.point.y;
messageString += " ,";
messageString += "\"z\": ";
messageString += ps.point.z;
messageString += "}";
messageString += "}";
messageString += "}";
}
else if (msg.topic == "/hololens/execute_pick")
{
messageString = "{ \"op\": \"";
messageString += msg.op;
messageString += "\", ";
messageString += "\"topic\": \"";
messageString += msg.topic;
messageString += "\", ";
messageString += "\"msg\": ";
messageString += "{";
messageString += "}";
messageString += "}";
}
else if (msg.topic == "/hololens/execute_place")
{
messageString = "{ \"op\": \"";
messageString += msg.op;
messageString += "\", ";
messageString += "\"topic\": \"";
messageString += msg.topic;
messageString += "\", ";
messageString += "\"msg\": ";
messageString += "{";
messageString += "}";
messageString += "}";
}
else if (msg.topic == "/hololens/reset")
{
messageString = "{ \"op\": \"";
messageString += msg.op;
messageString += "\", ";
messageString += "\"topic\": \"";
messageString += msg.topic;
messageString += "\", ";
messageString += "\"msg\": ";
messageString += "{";
messageString += "}";
messageString += "}";
}
else if (msg.topic == "/hololens_open_gripper")
{
messageString = "{ \"op\": \"";
messageString += msg.op;
messageString += "\", ";
messageString += "\"topic\": \"";
messageString += msg.topic;
messageString += "\", ";
messageString += "\"msg\": ";
messageString += "{";
messageString += "}";
messageString += "}";
}
}
return(messageString);
}
