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