Ejemplo n.º 1
0
    void SendImage(byte[] data, int length)
    {
        if (Bridge == null || Bridge.Status != Ros.Status.Connected)
        {
            return;
        }

#if USE_COMPRESSED
        var msg = new Ros.CompressedImage()
        {
            header = new Ros.Header()
            {
                stamp    = Ros.Time.Now(),
                seq      = seqId++,
                frame_id = FrameId,
            },
            format = "jpeg",
            data   = new Ros.PartialByteArray()
            {
                Array  = data,
                Length = length,
            }
        };
#else
        byte[] temp   = new byte[videoWidth * Reader.BytesPerPixel];
        int    stride = videoWidth * Reader.BytesPerPixel;
        for (int y = 0; y < videoHeight / 2; y++)
        {
            int row1 = stride * y;
            int row2 = stride * (videoHeight - 1 - y);
            System.Array.Copy(data, row1, temp, 0, stride);
            System.Array.Copy(data, row2, data, row1, stride);
            System.Array.Copy(temp, 0, data, row2, stride);
        }
        var msg = new Ros.Image()
        {
            header = new Ros.Header()
            {
                stamp    = Ros.Time.Now(),
                seq      = seqId++,
                frame_id = FrameId,
            },
            height       = (uint)videoHeight,
            width        = (uint)videoWidth,
            encoding     = Reader.BytesPerPixel == 3 ? "rgb8" : "rgba8",
            is_bigendian = 0,
            step         = (uint)stride,
            data         = data,
        };
#endif
        ImageIsBeingSent = true;
        Bridge.Publish(TopicName, msg, () => ImageIsBeingSent = false);
    }
Ejemplo n.º 2
0
    void SendImage(byte[] data, int length)
    {
        if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
        {
            return;
        }

        if (TargetEnvironment == ROSTargetEnvironment.APOLLO35)
        {
            System.DateTime Unixepoch        = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
            double          measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;

#if USE_COMPRESSED
            var msg = new Apollo.Drivers.CompressedImage()
            {
                Header = new Apollo.Common.Header()
                {
                    TimestampSec = measurement_time,
                    Version      = 1,
                    Status       = new Apollo.Common.StatusPb()
                    {
                        ErrorCode = Apollo.Common.ErrorCode.Ok,
                    },
                },
                MeasurementTime = measurement_time,
                FrameId         = FrameId,
                // Format = "png",
                Format = "jpg",

                Data = ByteString.CopyFrom(data, 0, length),
            };
#else
            // TODO
#endif

            ImageIsBeingSent = true;
            CyberVideoWriter.Publish(msg, () => ImageIsBeingSent = false);
        }

        else
        {
#if USE_COMPRESSED
            var msg = new Ros.CompressedImage()
            {
                header = new Ros.Header()
                {
                    stamp    = Ros.Time.Now(),
                    seq      = seqId++,
                    frame_id = FrameId,
                },
                format = "jpeg",
                data   = new Ros.PartialByteArray()
                {
                    Array  = data,
                    Length = length,
                }
            };
#else
            byte[] temp   = new byte[videoWidth * Reader.BytesPerPixel];
            int    stride = videoWidth * Reader.BytesPerPixel;
            for (int y = 0; y < videoHeight / 2; y++)
            {
                int row1 = stride * y;
                int row2 = stride * (videoHeight - 1 - y);
                System.Array.Copy(data, row1, temp, 0, stride);
                System.Array.Copy(data, row2, data, row1, stride);
                System.Array.Copy(temp, 0, data, row2, stride);
            }
            var msg = new Ros.Image()
            {
                header = new Ros.Header()
                {
                    stamp    = Ros.Time.Now(),
                    seq      = seqId++,
                    frame_id = FrameId,
                },
                height       = (uint)videoHeight,
                width        = (uint)videoWidth,
                encoding     = Reader.BytesPerPixel == 3 ? "rgb8" : "rgba8",
                is_bigendian = 0,
                step         = (uint)stride,
                data         = data,
            };
#endif
            ImageIsBeingSent = true;
            VideoWriter.Publish(msg, () => ImageIsBeingSent = false);
        }
    }