void OnClose(object sender, CloseEventArgs args)
{
while (QueuedActions.TryDequeue(out Action action))
{
}
Status = Status.Disconnected;
Socket = null;
TopicSubscriptions.Clear();
TopicPublishers.Clear();
}
public override void AddReader <T>(string topic, Action <T> callback)
{
lock (Readers)
{
if (!Readers.ContainsKey(topic))
{
Readers.Add(topic, new HashSet <Func <object, byte[], object> >());
var channelb = System.Text.Encoding.ASCII.GetBytes(topic);
var msgType = typeof(T).ToString();
string descriptorName = NameByMsgType[msgType];
FileDescriptorProto descriptor = DescriptorByName[descriptorName].Item2;
var typeb = System.Text.Encoding.ASCII.GetBytes(msgType);
var data = new List <byte>(128);
data.Add((byte)Op.AddReader);
data.Add((byte)(channelb.Length >> 0));
data.Add((byte)(channelb.Length >> 8));
data.Add((byte)(channelb.Length >> 16));
data.Add((byte)(channelb.Length >> 24));
data.AddRange(channelb);
data.Add((byte)(typeb.Length >> 0));
data.Add((byte)(typeb.Length >> 8));
data.Add((byte)(typeb.Length >> 16));
data.Add((byte)(typeb.Length >> 24));
data.AddRange(typeb);
SendAsync(data.ToArray());
}
Readers[topic].Add((msg, bytes) =>
{
if (msg == null)
{
using (var stream = new System.IO.MemoryStream(bytes))
{
msg = ProtoBuf.Serializer.Deserialize <T>(stream);
}
}
lock (QueuedActions)
{
QueuedActions.Enqueue(() => callback((T)msg));
}
return(msg);
});
TopicSubscriptions.Add(new Topic()
{
Name = topic,
Type = typeof(T).ToString(),
});
}
}
public void Disconnect()
{
if (Socket != null)
{
if (Socket.ReadyState == WebSocketState.Open)
{
Status = Status.Disconnecting;
Socket.CloseAsync();
}
}
TopicSubscriptions.Clear();
TopicPublishers.Clear();
}
void OnClose(object sender, CloseEventArgs args)
{
//if (!args.WasClean)
//{
// UnityEngine.Debug.LogError(args.Reason);
//}
Subscriptions.Clear();
Services.Clear();
QueuedActions.Clear();
QueuedSends.Clear();
TopicSubscriptions.Clear();
TopicPublishers.Clear();
Status = BridgeStatus.Disconnected;
}
public override void AddReader <T>(string topic, Action <T> callback)
{
lock (Readers)
{
if (!Readers.ContainsKey(topic))
{
Readers.Add(topic, new HashSet <Func <IMessage, byte[], IMessage> >());
var channelb = System.Text.Encoding.ASCII.GetBytes(topic);
var descriptor = MessageHelper <T> .Descriptor;
var typeb = System.Text.Encoding.ASCII.GetBytes(descriptor.FullName);
var data = new List <byte>(128);
data.Add((byte)Op.AddReader);
data.Add((byte)(channelb.Length >> 0));
data.Add((byte)(channelb.Length >> 8));
data.Add((byte)(channelb.Length >> 16));
data.Add((byte)(channelb.Length >> 24));
data.AddRange(channelb);
data.Add((byte)(typeb.Length >> 0));
data.Add((byte)(typeb.Length >> 8));
data.Add((byte)(typeb.Length >> 16));
data.Add((byte)(typeb.Length >> 24));
data.AddRange(typeb);
SendAsync(data.ToArray());
}
Readers[topic].Add((msg, bytes) =>
{
if (msg == null)
{
msg = MessageHelper <T> .Parser.ParseFrom(bytes);
}
lock (QueuedActions)
{
QueuedActions.Enqueue(() => callback((T)msg));
}
return(msg);
});
TopicSubscriptions.Add(new Topic()
{
Name = topic,
Type = typeof(T).ToString(),
});
}
}
public TopicSubscriptionHandle CreateTopicSubscription(TopicHandle source, TopicHandle destination, SubscriptionDescription subscriptionDescription)
{
var sourceEntity = Topics.Get(source);
var destinationEntity = Topics.Get(destination);
if (sourceEntity.TopicDescription.EnablePartitioning)
{
destinationEntity.TopicDescription.EnablePartitioning = true;
}
var subscriptionEntity = new TopicSubscriptionEntity(GetNextId(), GetNextId(), sourceEntity, destinationEntity, subscriptionDescription);
return(TopicSubscriptions.GetOrAdd(subscriptionEntity));
}
public override void Disconnect()
{
if (Socket == null)
{
return;
}
QueuedActions.Clear();
Buffer.Clear();
lock (Readers)
{
Readers.Clear();
}
Status = BridgeStatus.Disconnected;
TopicSubscriptions.Clear();
TopicPublishers.Clear();
Socket.Close();
Socket = null;
}
public override void AddReader <T>(string topic, Action <T> callback)
{
if (Socket.ReadyState != WebSocketState.Open)
{
throw new InvalidOperationException("socket not open");
}
var type = GetMessageType <T>();
var sb = new StringBuilder(256);
sb.Append('{');
{
sb.Append("\"op\":\"subscribe\",");
sb.Append("\"topic\":\"");
sb.Append(topic);
sb.Append("\",");
sb.Append("\"type\":\"");
sb.Append(type);
sb.Append("\"");
}
sb.Append('}');
Subscriptions.Add(new Subscription()
{
Topic = topic,
Callback = callback,
});
TopicSubscriptions.Add(new Topic()
{
Name = topic,
Type = type,
});
// UnityEngine.Debug.Log("Adding subscriber " + sb.ToString());
Socket.SendAsync(sb.ToString(), ok => { });
}
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);
}
public void AddReader <T>(string topic, Action <T> callback) where T : class
{
var type = typeof(T);
Func <object, object> converter = null;
if (type == typeof(Detected3DObjectArray))
{
converter = (object msg) => Conversions.ConvertTo(msg as apollo.common.Detection3DArray);
type = typeof(apollo.common.Detection3DArray);
}
else if (type == typeof(Detected2DObjectArray))
{
converter = (object msg) => Conversions.ConvertTo(msg as apollo.common.Detection2DArray);
type = typeof(apollo.common.Detection2DArray);
}
else if (type == typeof(VehicleControlData))
{
type = typeof(apollo.control.ControlCommand);
converter = (object msg) => Conversions.ConvertTo(msg as apollo.control.ControlCommand);
}
else
{
throw new Exception($"Cyber bridge does not support {typeof(T).Name} type");
}
var channelBytes = Encoding.ASCII.GetBytes(topic);
var typeBytes = Encoding.ASCII.GetBytes(type.ToString());
var bytes = new List <byte>(1024);
bytes.Add((byte)BridgeOp.AddReader);
bytes.Add((byte)(channelBytes.Length >> 0));
bytes.Add((byte)(channelBytes.Length >> 8));
bytes.Add((byte)(channelBytes.Length >> 16));
bytes.Add((byte)(channelBytes.Length >> 24));
bytes.AddRange(channelBytes);
bytes.Add((byte)(typeBytes.Length >> 0));
bytes.Add((byte)(typeBytes.Length >> 8));
bytes.Add((byte)(typeBytes.Length >> 16));
bytes.Add((byte)(typeBytes.Length >> 24));
bytes.AddRange(typeBytes);
var data = bytes.ToArray();
lock (Setup)
{
if (Status == Status.Connected)
{
SendAsync(data, null);
}
Setup.Add(data);
}
lock (Readers)
{
if (!Readers.ContainsKey(topic))
{
Readers.Add(topic,
Tuple.Create <Func <byte[], object>, List <Action <object> > >(
msg =>
{
using (var stream = new MemoryStream(msg))
{
return(converter(Serializer.Deserialize(type, stream)));
}
},
new List <Action <object> >())
);
}
Readers[topic].Item2.Add(msg => callback((T)msg));
}
TopicSubscriptions.Add(new TopicUIData()
{
Topic = topic,
Type = type.ToString(),
Frequency = 0f,
});
}
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}");
}
}
public void AddReader <T>(string topic, Action <T> callback) where T : class
{
var type = typeof(T);
Func <JSONNode, object> converter;
if (type == typeof(Detected3DObjectArray))
{
type = typeof(Detection3DArray);
converter = (JSONNode json) => Conversions.ConvertTo((Detection3DArray)Unserialize(json, type));
}
else if (type == typeof(Detected2DObjectArray))
{
type = typeof(Detection2DArray);
converter = (JSONNode json) => Conversions.ConvertTo((Detection2DArray)Unserialize(json, type));
}
else if (type == typeof(VehicleControlData))
{
if (Apollo)
{
type = typeof(Apollo.control_command);
converter = (JSONNode json) => Conversions.ConvertTo((Apollo.control_command)Unserialize(json, type));
}
else if (Version == 2)
{
// Since there is no mapping acceleration to throttle, VehicleControlCommand is not supported for now.
// After supporting it, VehicleControlCommand will replace RawControlCommand.
// type = typeof(Autoware.VehicleControlCommand);
// converter = (JSONNode json) => Conversions.ConvertTo((Autoware.VehicleControlCommand)Unserialize(json, type));
type = typeof(Autoware.RawControlCommand);
converter = (JSONNode json) => Conversions.ConvertTo((Autoware.RawControlCommand)Unserialize(json, type));
}
else
{
type = typeof(Autoware.VehicleCmd);
converter = (JSONNode json) => Conversions.ConvertTo((Autoware.VehicleCmd)Unserialize(json, type));
}
}
else if (type == typeof(VehicleStateData))
{
type = typeof(Autoware.VehicleStateCommand);
converter = (JSONNode json) => Conversions.ConvertTo((Autoware.VehicleStateCommand)Unserialize(json, type));
}
else if (BridgeConfig.bridgeConverters.ContainsKey(type))
{
converter = (JSONNode json) => (BridgeConfig.bridgeConverters[type] as IDataConverter <T>).GetConverter(this);
type = (BridgeConfig.bridgeConverters[type] as IDataConverter <T>).GetOutputType(this);
}
else
{
converter = (JSONNode json) => Unserialize(json, type);
}
var messageType = GetMessageType(type);
var sb = new StringBuilder(1024);
sb.Append('{');
{
sb.Append("\"op\":\"subscribe\",");
sb.Append("\"topic\":\"");
sb.Append(topic);
sb.Append("\",");
sb.Append("\"type\":\"");
sb.Append(messageType);
sb.Append("\"");
}
sb.Append('}');
TopicSubscriptions.Add(new TopicUIData()
{
Topic = topic,
Type = messageType,
Frequency = 0f,
});
var data = sb.ToString();
lock (Setup)
{
if (Status == Status.Connected)
{
Socket.SendAsync(data, null);
}
Setup.Add(data);
}
lock (Readers)
{
if (!Readers.ContainsKey(topic))
{
Readers.Add(topic,
Tuple.Create <Func <JSONNode, object>, List <Action <object> > >(
msg => converter(msg),
new List <Action <object> >())
);
}
Readers[topic].Item2.Add(msg => callback((T)msg));
}
}
public void AddReader <T>(string topic, Action <T> callback) where T : class
{
var type = typeof(T);
Func <JSONNode, object> converter;
if (type == typeof(Detected3DObjectArray))
{
type = typeof(Detection3DArray);
converter = (JSONNode json) => Conversions.ConvertTo((Detection3DArray)Unserialize(json, type));
}
else if (type == typeof(Detected2DObjectArray))
{
type = typeof(Detection2DArray);
converter = (JSONNode json) => Conversions.ConvertTo((Detection2DArray)Unserialize(json, type));
}
else if (type == typeof(VehicleControlData))
{
if (Apollo)
{
type = typeof(Apollo.control_command);
converter = (JSONNode json) => Conversions.ConvertTo((Apollo.control_command)Unserialize(json, type));
}
else if (Version == 2)
{
type = typeof(TwistStamped);
converter = (JSONNode json) => Conversions.ConvertTo((TwistStamped)Unserialize(json, type));
}
else
{
type = typeof(Autoware.VehicleCmd);
converter = (JSONNode json) => Conversions.ConvertTo((Autoware.VehicleCmd)Unserialize(json, type));
}
}
else if (type == typeof(SnowballControlData))
{
type = typeof(Snowball.ControlData);
converter = (JSONNode json) => Conversions.ConvertTo((Snowball.ControlData)Unserialize(json, type));
}
else
{
converter = (JSONNode json) => Unserialize(json, type);
}
var messageType = GetMessageType(type);
var sb = new StringBuilder(1024);
sb.Append('{');
{
sb.Append("\"op\":\"subscribe\",");
sb.Append("\"topic\":\"");
sb.Append(topic);
sb.Append("\",");
sb.Append("\"type\":\"");
sb.Append(messageType);
sb.Append("\"");
}
sb.Append('}');
TopicSubscriptions.Add(new TopicUIData()
{
Topic = topic,
Type = messageType,
Frequency = 0f,
});
var data = sb.ToString();
lock (Setup)
{
if (Status == Status.Connected)
{
Socket.SendAsync(data, null);
}
Setup.Add(data);
}
lock (Readers)
{
if (!Readers.ContainsKey(topic))
{
Readers.Add(topic,
Tuple.Create <Func <JSONNode, object>, List <Action <object> > >(
msg => converter(msg),
new List <Action <object> >())
);
}
Readers[topic].Item2.Add(msg => callback((T)msg));
}
}
