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