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