public IWriter <T> AddWriter <T>(string topic) where T : class
{
IWriter <T> writer;
var type = typeof(T);
if (type == typeof(ImageData))
{
type = typeof(CompressedImage);
writer = new Writer <ImageData, CompressedImage>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(PointCloudData))
{
type = typeof(PointCloud2);
writer = new PointCloudWriter(this, topic) as IWriter <T>;
}
else if (type == typeof(Detected3DObjectData))
{
type = typeof(LGSVL.Detection3DArray);
writer = new Writer <Detected3DObjectData, LGSVL.Detection3DArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(Detected2DObjectData))
{
type = typeof(LGSVL.Detection2DArray);
writer = new Writer <Detected2DObjectData, LGSVL.Detection2DArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(SignalDataArray))
{
type = typeof(LGSVL.SignalArray);
writer = new Writer <SignalDataArray, LGSVL.SignalArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(DetectedRadarObjectData) && Apollo)
{
type = typeof(Apollo.Drivers.ContiRadar);
writer = new Writer <DetectedRadarObjectData, Apollo.Drivers.ContiRadar>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(CanBusData))
{
if (Version == 2 && !Apollo)
{
type = typeof(VehicleStateReport);
writer = new Writer <CanBusData, VehicleStateReport>(this, topic, Conversions.ROS2ConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Apollo.ChassisMsg);
writer = new Writer <CanBusData, Apollo.ChassisMsg>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
}
else if (type == typeof(GpsData))
{
if (Apollo)
{
type = typeof(Apollo.GnssBestPose);
writer = new Writer <GpsData, Apollo.GnssBestPose>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (Version == 2)
{
type = typeof(NavSatFix);
writer = new Writer <GpsData, NavSatFix>(this, topic, Conversions.ROS2ConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Sentence);
writer = new RosNmeaWriter(this, topic) as IWriter <T>;
}
}
else if (type == typeof(ImuData))
{
if (Apollo)
{
type = typeof(Apollo.Imu);
writer = new Writer <ImuData, Apollo.Imu>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Imu);
writer = new Writer <ImuData, Imu>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
}
else if (type == typeof(CorrectedImuData))
{
type = typeof(Apollo.CorrectedImu);
writer = new Writer <CorrectedImuData, Apollo.CorrectedImu>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else if (type == typeof(GpsOdometryData))
{
if (Apollo)
{
type = typeof(Apollo.Gps);
writer = new Writer <GpsOdometryData, Apollo.Gps>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Odometry);
writer = new Writer <GpsOdometryData, Odometry>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
}
else if (type == typeof(VehicleOdometryData))
{
type = typeof(VehicleOdometry);
writer = new Writer <VehicleOdometryData, VehicleOdometry>(this, topic, Conversions.ROS2ConvertFrom) as IWriter <T>;
}
else if (type == typeof(ClockData))
{
type = typeof(Ros.Clock);
writer = new Writer <ClockData, Ros.Clock>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (BridgeConfig.bridgeConverters.ContainsKey(type))
{
writer = new Writer <T, object>(this, topic, (BridgeConfig.bridgeConverters[type] as IDataConverter <T>).GetConverter(this)) as IWriter <T>;
type = (BridgeConfig.bridgeConverters[type] as IDataConverter <T>).GetOutputType(this);
}
else
{
throw new Exception($"Unsupported message type {type} used for ROS bridge");
}
var messageType = GetMessageType(type);
var sb = new StringBuilder(1024);
sb.Append('{');
{
sb.Append("\"op\":\"advertise\",");
sb.Append("\"topic\":\"");
sb.Append(topic);
sb.Append("\",");
sb.Append("\"type\":\"");
sb.Append(messageType);
sb.Append("\"");
}
sb.Append('}');
TopicPublishers.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);
}
return(writer);
}
public IWriter <T> AddWriter <T>(string topic) where T : class
{
IWriter <T> writer;
var type = typeof(T);
if (type == typeof(ImageData))
{
type = typeof(CompressedImage);
writer = new Writer <ImageData, CompressedImage>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(PointCloudData))
{
type = typeof(PointCloud2);
writer = new PointCloudWriter(this, topic) as IWriter <T>;
}
else if (type == typeof(Detected3DObjectData))
{
type = typeof(Detection3D);
writer = new Writer <Detected3DObjectData, Detection3DArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(Detected2DObjectData))
{
type = typeof(Detection2DArray);
writer = new Writer <Detected2DObjectData, Detection2DArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(DetectedRadarObjectData) && Apollo)
{
type = typeof(Apollo.Drivers.ContiRadar);
writer = new Writer <DetectedRadarObjectData, Apollo.Drivers.ContiRadar>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(CanBusData))
{
type = typeof(Apollo.ChassisMsg);
writer = new Writer <CanBusData, Apollo.ChassisMsg>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(GpsData))
{
if (Apollo)
{
type = typeof(Apollo.GnssBestPose);
writer = new Writer <GpsData, Apollo.GnssBestPose>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Sentence);
writer = new RosNmeaWriter(this, topic) as IWriter <T>;
}
}
else if (type == typeof(ImuData))
{
if (Apollo)
{
type = typeof(Apollo.Imu);
writer = new Writer <ImuData, Apollo.Imu>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Imu);
writer = new Writer <ImuData, Imu>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
}
else if (type == typeof(CorrectedImuData))
{
type = typeof(Apollo.CorrectedImu);
writer = new Writer <CorrectedImuData, Apollo.CorrectedImu>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else if (type == typeof(GpsOdometryData))
{
if (Apollo)
{
type = typeof(Apollo.Gps);
writer = new Writer <GpsOdometryData, Apollo.Gps>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Odometry);
writer = new Writer <GpsOdometryData, Odometry>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
}
else if (type == typeof(ClockData))
{
type = typeof(Ros.Clock);
writer = new Writer <ClockData, Ros.Clock>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else
{
throw new Exception($"Unsupported message type {type} used for ROS bridge");
}
var messageType = GetMessageType(type);
var sb = new StringBuilder(1024);
sb.Append('{');
{
sb.Append("\"op\":\"advertise\",");
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);
}
return(writer);
}
public IWriter <T> AddWriter <T>(string topic) where T : class
{
IWriter <T> writer;
var type = typeof(T);
if (type == typeof(ImageData))
{
if (Version == 2)
{
type = typeof(Snowball.CompressedImage);
writer = new Writer <ImageData, Snowball.CompressedImage>(this, topic, Conversions.ConvertFromRos2, true) as IWriter <T>;
}
else
{
type = typeof(CompressedImage);
writer = new Writer <ImageData, CompressedImage>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
}
else if (type == typeof(StaticTFData))
{
type = typeof(Snowball.tfMessage);
writer = new Writer <StaticTFData, Snowball.tfMessage>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(CameraInfoData))
{
type = typeof(Snowball.CameraInfo);
writer = new Writer <CameraInfoData, Snowball.CameraInfo>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(PointCloudData))
{
if (Version == 2) // Ros2
{
type = typeof(Snowball.PointCloud2);
writer = new PointCloudWriterRos2(this, topic) as IWriter <T>;
}
else
{
type = typeof(PointCloud2);
writer = new PointCloudWriter(this, topic) as IWriter <T>;
}
}
else if (type == typeof(Detected3DObjectDataPerc))
{
type = typeof(Snowball.DetectedObjectArray);
writer = new Writer <Detected3DObjectDataPerc, Snowball.ObstacleArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(Detected3DObjectData))
{
type = typeof(LGSVL.Detection3DArray);
writer = new Writer <Detected3DObjectData, LGSVL.Detection3DArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(Detected3DObjectDataAdhoc))
{
type = typeof(Snowball.TrackedObjectArray);
writer = new Writer <Detected3DObjectDataAdhoc, Snowball.TrackedObjectArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(Detected2DObjectData))
{
type = typeof(LGSVL.Detection2DArray);
writer = new Writer <Detected2DObjectData, LGSVL.Detection2DArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(SignalDataArray))
{
type = typeof(LGSVL.SignalArray);
writer = new Writer <SignalDataArray, LGSVL.SignalArray>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(SignalDataAdhoc))
{
type = typeof(Snowball.TrafficLightData);
writer = new Writer <SignalDataAdhoc, Snowball.TrafficLightData>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(DetectedRadarObjectData) && Apollo)
{
type = typeof(Apollo.Drivers.ContiRadar);
writer = new Writer <DetectedRadarObjectData, Apollo.Drivers.ContiRadar>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(CanBusData))
{
type = typeof(Apollo.ChassisMsg);
writer = new Writer <CanBusData, Apollo.ChassisMsg>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(GpsData))
{
if (Apollo)
{
type = typeof(Apollo.GnssBestPose);
writer = new Writer <GpsData, Apollo.GnssBestPose>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Sentence);
writer = new RosNmeaWriter(this, topic) as IWriter <T>;
}
}
else if (type == typeof(ImuData))
{
if (Apollo)
{
type = typeof(Apollo.Imu);
writer = new Writer <ImuData, Apollo.Imu>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Imu);
writer = new Writer <ImuData, Imu>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
}
else if (type == typeof(CorrectedImuData))
{
type = typeof(Apollo.CorrectedImu);
writer = new Writer <CorrectedImuData, Apollo.CorrectedImu>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else if (type == typeof(GpsOdometryData))
{
if (Apollo)
{
type = typeof(Apollo.Gps);
writer = new Writer <GpsOdometryData, Apollo.Gps>(this, topic, Conversions.ApolloConvertFrom) as IWriter <T>;
}
else
{
type = typeof(Odometry);
writer = new Writer <GpsOdometryData, Odometry>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
}
else if (type == typeof(ClockData))
{
type = typeof(Ros.Clock);
writer = new Writer <ClockData, Ros.Clock>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(ChassisData))
{
type = typeof(Snowball.ChassisDataMsg);
writer = new Writer <ChassisData, Snowball.ChassisDataMsg>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(novatelInsPvaData))
{
type = typeof(Snowball.novatelInsPVA);
writer = new Writer <novatelInsPvaData, Snowball.novatelInsPVA>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(novatelRawImuXData))
{
type = typeof(Snowball.novatelRawImuX);
writer = new Writer <novatelRawImuXData, Snowball.novatelRawImuX>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(novatelMarkCountData))
{
type = typeof(Snowball.novatelMarkCount);
writer = new Writer <novatelMarkCountData, Snowball.novatelMarkCount>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(novatelBestPosData))
{
type = typeof(Snowball.novatelBestPos);
writer = new Writer <novatelBestPosData, Snowball.novatelBestPos>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(novatelCorrIMUData))
{
type = typeof(Snowball.novatelCorrIMU);
writer = new Writer <novatelCorrIMUData, Snowball.novatelCorrIMU>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else if (type == typeof(novatelHeadingData))
{
type = typeof(Snowball.novatelHeading);
writer = new Writer <novatelHeadingData, Snowball.novatelHeading>(this, topic, Conversions.ConvertFrom) as IWriter <T>;
}
else
{
throw new Exception($"Unsupported message type {type} used for ROS bridge");
}
var messageType = GetMessageType(type);
var sb = new StringBuilder(1024);
sb.Append('{');
{
sb.Append("\"op\":\"advertise\",");
sb.Append("\"topic\":\"");
sb.Append(topic);
sb.Append("\",");
sb.Append("\"type\":\"");
sb.Append(messageType);
sb.Append("\"");
}
sb.Append('}');
var pub = TopicPublishers.Find(x => x.Topic == topic);
if (pub == null)
{
TopicPublishers.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);
}
}
return(writer);
}
