public void AddService <Argument, Result>(string topic, Func <Argument, Result> callback)
{
var argtype = typeof(Argument);
var restype = typeof(Result);
Func <object, object> converter = null;
if (argtype == typeof(EmptySrv))
{
argtype = typeof(Empty);
converter = (object obj) => Conversions.ConvertTo((Empty)obj);
}
else if (argtype == typeof(SetBoolSrv))
{
argtype = typeof(SetBool);
converter = (object obj) => Conversions.ConvertTo((SetBool)obj);
}
Func <object, object> convertResult = null;
if (restype == typeof(EmptySrv))
{
restype = typeof(Empty);
convertResult = (object obj) => Conversions.ConvertFrom((EmptySrv)obj);
}
else if (restype == typeof(SetBoolSrv))
{
restype = typeof(SetBoolResponse);
convertResult = (object obj) => Conversions.ConvertFrom((SetBoolSrv)obj);
}
else if (restype == typeof(TriggerSrv))
{
restype = typeof(Trigger);
convertResult = (object obj) => Conversions.ConvertFrom((TriggerSrv)obj);
}
var argType = GetMessageType(argtype);
var resType = GetMessageType(restype);
if (Services.ContainsKey(topic))
{
throw new Exception($"Topic {topic} already has service registered");
}
var sb = new StringBuilder(1024);
sb.Append('{');
{
sb.Append("\"op\":\"advertise_service\",");
sb.Append("\"type\":\"");
sb.Append(argType);
sb.Append("\",");
sb.Append("\"service\":\"");
sb.Append(topic);
sb.Append("\"");
}
sb.Append('}');
var data = sb.ToString();
lock (Setup)
{
if (Status == Status.Connected)
{
Socket.SendAsync(data, null);
}
Setup.Add(data);
}
lock (Services)
{
Services.Add(topic, Tuple.Create <Type, Type, Func <object, object> >(argtype, restype, (object argObj) => {
var argData = (Argument)converter(argObj);
var resData = callback(argData);
var resObj = convertResult(resData);
return(resObj);
}));
}
}
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
{
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('}');
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)
{
// 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));
}
}
