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); }