public void Register(IBridgePlugin plugin)
{
// point cloud is special, as we use special writer for performance reasons
plugin.AddType <PointCloudData>(Ros2Utils.GetMessageType <Ros.PointCloud2>());
plugin.AddPublisherCreator(
(instance, topic) =>
{
var ros2Instance = instance as Ros2BridgeInstance;
ros2Instance.AddPublisher <Ros.PointCloud2>(topic);
var writer = new Ros2PointCloudWriter(ros2Instance, topic);
return(new Publisher <PointCloudData>((data, completed) => writer.Write(data, completed)));
}
);
RegPublisher <ImageData, Ros.CompressedImage>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <Detected3DObjectData, Lgsvl.Detection3DArray>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <Detected2DObjectData, Lgsvl.Detection2DArray>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <SignalDataArray, Lgsvl.SignalArray>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <CanBusData, Lgsvl.CanBusData>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <UltrasonicData, Lgsvl.Ultrasonic>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <GpsData, Ros.NavSatFix>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <GpsOdometryData, Ros.Odometry>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <ImuData, Ros.Imu>(plugin, Ros2Conversions.ConvertFrom);
RegPublisher <ClockData, Ros.Clock>(plugin, Ros2Conversions.ConvertFrom);
RegSubscriber <VehicleStateData, Lgsvl.VehicleStateData>(plugin, Ros2Conversions.ConvertTo);
RegSubscriber <VehicleControlData, Lgsvl.VehicleControlData>(plugin, Ros2Conversions.ConvertTo);
RegSubscriber <Detected2DObjectArray, Lgsvl.Detection2DArray>(plugin, Ros2Conversions.ConvertTo);
RegSubscriber <Detected3DObjectArray, Lgsvl.Detection3DArray>(plugin, Ros2Conversions.ConvertTo);
}
public void RegSubscriber <DataType, BridgeType>(IBridgePlugin plugin, Func <BridgeType, DataType> converter)
{
plugin.AddType <DataType>(Ros2Utils.GetMessageType <BridgeType>());
plugin.AddSubscriberCreator <DataType>(
(instance, topic, callback) => (instance as Ros2BridgeInstance).AddSubscriber <BridgeType>(topic,
rawData => callback(converter(Ros2Serialization.Unserialize <BridgeType>(rawData)))
)
);
}
public void RegPublisher <DataType, BridgeType>(IBridgePlugin plugin, Func <DataType, BridgeType> converter)
{
plugin.AddType <DataType>(Ros2Utils.GetMessageType <BridgeType>());
plugin.AddPublisherCreator(
(instance, topic) =>
{
var ros2Instance = instance as Ros2BridgeInstance;
ros2Instance.AddPublisher <BridgeType>(topic);
var writer = new Ros2Writer <BridgeType>(ros2Instance, topic);
return(new Publisher <DataType>((data, completed) => writer.Write(converter(data), completed)));
}
);
}
public void AddSubscriber <BridgeType>(string topic, Action <ArraySegment <byte> > callback)
{
if (topic.Split('/').Any(x => char.IsDigit(x.FirstOrDefault())))
{
throw new ArgumentException($"ROS2 does not allow part topic name start with digit - '{topic}'");
}
var topicBytes = Encoding.ASCII.GetBytes(topic);
var messageType = Ros2Utils.GetMessageType <BridgeType>();
var typeBytes = Encoding.ASCII.GetBytes(messageType);
var bytes = new List <byte>(1 + 4 + topicBytes.Length + 4 + typeBytes.Length);
bytes.Add((byte)BridgeOp.AddSubscriber);
bytes.Add((byte)(topicBytes.Length >> 0));
bytes.Add((byte)(topicBytes.Length >> 8));
bytes.Add((byte)(topicBytes.Length >> 16));
bytes.Add((byte)(topicBytes.Length >> 24));
bytes.AddRange(topicBytes);
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 (Subscribers)
{
Subscribers.Add(topic, callback);
}
}