bool ReceivePublish()
{
if (1 + 2 * 4 > Buffer.Count)
{
return(false);
}
int offset = 1;
int channel_size = Get32le(offset);
offset += 4;
if (offset + channel_size > Buffer.Count)
{
return(false);
}
int channel_offset = offset;
offset += channel_size;
int message_size = Get32le(offset);
offset += 4;
if (offset + message_size > Buffer.Count)
{
return(false);
}
int message_offset = offset;
offset += message_size;
var channel = Encoding.ASCII.GetString(Buffer.Skip(channel_offset).Take(channel_size).ToArray());
if (Readers.TryGetValue(channel, out var readersPair))
{
var parser = readersPair.Item1;
var readers = readersPair.Item2;
var bytes = Buffer.Skip(message_offset).Take(message_size).ToArray();
var message = parser(bytes);
foreach (var reader in readers)
{
QueuedActions.Enqueue(() => reader(message));
}
if (!string.IsNullOrEmpty(channel))
{
TopicSubscriptions.Find(x => x.Topic == channel).Count++;
}
}
else
{
Debug.Log($"Received message on channel '{channel}' which nobody subscribed");
}
Buffer.RemoveRange(0, offset);
return(true);
}
void OnMessage(object sender, MessageEventArgs args)
{
var json = JSONNode.Parse(args.Data);
string op = json["op"];
if (op == "publish")
{
string topic = json["topic"];
Tuple <Func <JSONNode, object>, List <Action <object> > > readerPair;
lock (Readers)
{
if (!Readers.TryGetValue(topic, out readerPair))
{
UnityEngine.Debug.Log($"Received message on topic '{topic}' which nobody subscribed");
return;
}
}
var parse = readerPair.Item1;
var readers = readerPair.Item2;
var msg = parse(json["msg"]);
foreach (var reader in readers)
{
QueuedActions.Enqueue(() => reader(msg));
}
if (!string.IsNullOrEmpty(topic))
{
var topicSub = TopicSubscriptions.Find(x => x.Topic == topic);
if (topicSub != null)
{
topicSub.Count++;
}
}
}
else if (op == "call_service")
{
var service = json["service"];
var id = json["id"];
Tuple <Type, Type, Func <object, object> > serviceTuple;
lock (Services)
{
if (!Services.TryGetValue(service, out serviceTuple))
{
return;
}
}
var(argumentType, resultType, convertResult) = serviceTuple;
var arg = Unserialize(json["args"], argumentType);
QueuedActions.Enqueue(() =>
{
var result = convertResult.DynamicInvoke(arg);
var sb = new StringBuilder(1024);
sb.Append('{');
{
sb.Append("\"op\":\"service_response\",");
sb.Append("\"id\":");
sb.Append(id.ToString());
sb.Append(",");
sb.Append("\"service\":\"");
sb.Append(service.Value);
sb.Append("\",");
sb.Append("\"values\":");
Serialize(result, resultType, sb);
sb.Append(",");
sb.Append("\"result\":true");
}
sb.Append('}');
var data = sb.ToString();
Socket.SendAsync(data, null);
});
}
else if (op == "set_level")
{
// ignore these
}
else
{
UnityEngine.Debug.Log($"Unknown operation from rosbridge: {op}");
}
}