public void AddService <ArgBridgeType>(string topic, Action <JSONNode, Action <object> > callback)
{
var argType = RosUtils.GetMessageType <ArgBridgeType>();
if (Services.ContainsKey(topic))
{
throw new InvalidOperationException($"Service on {topic} topic is already registered");
}
var j = new JSONObject();
j.Add("op", "advertise_service");
j.Add("type", argType);
j.Add("service", topic);
var data = j.ToString();
lock (Setup)
{
if (Status == Status.Connected)
{
Socket.SendAsync(data, null);
}
Setup.Add(data);
}
lock (Services)
{
Services.Add(topic, callback);
}
}
public void AddSubscriber <BridgeType>(string topic, Action <JSONNode> callback)
{
var messageType = RosUtils.GetMessageType <BridgeType>();
var j = new JSONObject();
j.Add("op", "subscribe");
j.Add("topic", topic);
j.Add("type", messageType);
var data = j.ToString();
lock (Setup)
{
if (Status == Status.Connected)
{
Socket.SendAsync(data, null);
}
Setup.Add(data);
}
lock (Subscribers)
{
Subscribers.Add(topic, callback);
}
}
public virtual void Register(IBridgePlugin plugin)
{
// point cloud is special, as we use special writer for performance reasons
plugin.AddType <PointCloudData>(RosUtils.GetMessageType <Ros.PointCloud2>());
plugin.AddPublisherCreator <PointCloudData>(
(instance, topic) =>
{
var rosInstance = instance as RosBridgeInstance;
rosInstance.AddPublisher <Ros.PointCloud2>(topic);
var writer = new RosPointCloudWriter(rosInstance, topic);
return(new Publisher <PointCloudData>((data, completed) => writer.Write(data, completed)));
}
);
RegPublisher <ImageData, Ros.CompressedImage>(plugin, Conversions.ConvertFrom);
RegPublisher <CameraInfoData, Ros.CameraInfo>(plugin, Conversions.ConvertFrom);
RegPublisher <Detected2DObjectData, Lgsvl.Detection2DArray>(plugin, Conversions.ConvertFrom);
RegPublisher <ClockData, Ros.Clock>(plugin, Conversions.ConvertFrom);
RegSubscriber <VehicleStateData, Lgsvl.VehicleStateDataRos>(plugin, Conversions.ConvertTo);
RegSubscriber <Detected2DObjectArray, Lgsvl.Detection2DArray>(plugin, Conversions.ConvertTo);
RegSubscriber <Detected3DObjectArray, Lgsvl.Detection3DArray>(plugin, Conversions.ConvertTo);
// std_srvs/Empty
RegService <EmptySrv, Ros.Empty, EmptySrv, Ros.Empty>(plugin, Conversions.ConvertTo, Conversions.ConvertFrom);
// std_srvs/SetBool
RegService <SetBoolSrv, Ros.SetBool, SetBoolSrv, Ros.SetBoolResponse>(plugin, Conversions.ConvertTo, Conversions.ConvertFrom);
// std_srvs/Trigger
RegService <EmptySrv, Ros.Empty, TriggerSrv, Ros.Trigger>(plugin, Conversions.ConvertTo, Conversions.ConvertFrom);
}
public override void Register(IBridgePlugin plugin)
{
base.Register(plugin);
// gps data is special, because it actually sends two Ros.Sentence messages for each data point from simulator
plugin.AddType <GpsData>(RosUtils.GetMessageType <Ros.Sentence>());
plugin.AddPublisherCreator(
(instance, topic) =>
{
var rosInstance = instance as RosBridgeInstance;
rosInstance.AddPublisher <Ros.Sentence>(topic);
var writer = new RosNmeaWriter(rosInstance, topic);
return(new Publisher <GpsData>((data, completed) => writer.Write(data, completed)));
}
);
RegPublisher <CanBusData, Lgsvl.CanBusDataRos>(plugin, Conversions.RosConvertFrom);
RegPublisher <DetectedRadarObjectData, Lgsvl.DetectedRadarObjectArray>(plugin, Conversions.RosConvertFrom);
RegPublisher <GpsOdometryData, Ros.Odometry>(plugin, Conversions.ConvertFrom);
RegPublisher <ImuData, Ros.Imu>(plugin, Conversions.ConvertFrom);
RegPublisher <Detected3DObjectData, Lgsvl.Detection3DArray>(plugin, Conversions.ConvertFrom);
RegPublisher <SignalDataArray, Lgsvl.SignalArray>(plugin, Conversions.ConvertFrom);
RegPublisher <UltrasonicData, Lgsvl.Ultrasonic>(plugin, Conversions.ConvertFrom);
RegPublisher <VehicleOdometryData, Lgsvl.VehicleOdometry>(plugin, Conversions.ConvertFrom);
RegSubscriber <VehicleControlData, Lgsvl.VehicleControlDataRos>(plugin, Conversions.ConvertTo);
}
public void RegSubscriber <DataType, BridgeType>(IBridgePlugin plugin, Func <BridgeType, DataType> converter)
{
plugin.AddType <DataType>(RosUtils.GetMessageType <BridgeType>());
plugin.AddSubscriberCreator <DataType>(
(instance, topic, callback) => (instance as RosBridgeInstance).AddSubscriber <BridgeType>(topic,
rawData => callback(converter(RosSerialization.Unserialize <BridgeType>(rawData)))
)
);
}
public void RegPublisher <DataType, BridgeType>(IBridgePlugin plugin, Func <DataType, BridgeType> converter)
{
plugin.AddType <DataType>(RosUtils.GetMessageType <BridgeType>());
plugin.AddPublisherCreator <DataType>(
(instance, topic) =>
{
var rosInstance = instance as RosBridgeInstance;
rosInstance.AddPublisher <BridgeType>(topic);
var writer = new RosWriter <BridgeType>(rosInstance, topic);
return(new Publisher <DataType>((data, completed) => writer.Write(converter(data), completed)));
}
);
}
public void AddPublisher <BridgeType>(string topic)
{
var messageType = RosUtils.GetMessageType <BridgeType>();
var j = new JSONObject();
j.Add("op", "advertise");
j.Add("topic", topic);
j.Add("type", messageType);
var data = j.ToString();
lock (Setup)
{
if (Status == Status.Connected)
{
Socket.SendAsync(data, null);
}
Setup.Add(data);
}
}
