public RosMessage(RosMessageDefinition type, RosTime time, int conn, Dictionary <string, dynamic> fields)
{
this.messageType = type;
this.Time = time;
this.Conn = conn;
this.fields = fields;
}
public RosMessage(RosMessageDefinition type, RosTime time, int conn, byte[] data)
{
this.messageType = type;
this.Time = time;
this.Conn = conn;
this.RawData = data;
}
public static RosHeader FromRosBytes(byte[] headerBytes, int offset = 0)
{
// calculate the size of the frame string
var frameStringLength = BitConverter.ToInt32(headerBytes, offset + 12);
return(new RosHeader(BitConverter.ToUInt32(headerBytes, offset), RosTime.FromRosBytes(headerBytes, offset + 4), Encoding.UTF8.GetString(headerBytes, offset + 16, frameStringLength), frameStringLength + 16));
}
public RosHeader(uint seq, RosTime time, string frameId, int headerSize)
{
this.Seq = seq;
this.Time = time;
this.FrameId = frameId;
this.HeaderByteSize = headerSize;
}
public RosMessage(RosMessageDefinition type, RosTime time, int conn, byte[] data)
{
this.messageType = type;
this.Time = time;
this.Conn = conn;
// parse message
var parseResult = type.ParseMessage(data);
this.fields = parseResult.Item2;
}
/// <summary>
/// Parse the build in type and return the length of object in the bytes and record
/// </summary>
/// <param name="rawData"></param>
/// <param name="type"></param>
/// <param name="offset"></param>
/// <returns></returns>
private Tuple <int, dynamic> ParseBuildInTypes(byte[] rawData, string type, int offset = 0)
{
switch (type)
{
case "string":
var len = (int)BitConverter.ToInt32(rawData, offset);
return(new Tuple <int, dynamic>(len + 4, Encoding.UTF8.GetString(rawData, offset + 4, len)));
case "bool":
return(new Tuple <int, dynamic>(1, BitConverter.ToBoolean(rawData, offset)));
case "int8":
return(new Tuple <int, dynamic>(1, rawData[offset]));
case "uint8":
return(new Tuple <int, dynamic>(1, rawData[offset]));
case "int16":
return(new Tuple <int, dynamic>(2, BitConverter.ToInt16(rawData, offset)));
case "uint16":
return(new Tuple <int, dynamic>(2, BitConverter.ToUInt16(rawData, offset)));
case "int32":
return(new Tuple <int, dynamic>(4, BitConverter.ToInt32(rawData, offset)));
case "uint32":
return(new Tuple <int, dynamic>(4, BitConverter.ToUInt32(rawData, offset)));
case "int64":
return(new Tuple <int, dynamic>(8, BitConverter.ToInt64(rawData, offset)));
case "uint64":
return(new Tuple <int, dynamic>(8, BitConverter.ToUInt64(rawData, offset)));
case "float32":
return(new Tuple <int, dynamic>(4, BitConverter.ToSingle(rawData, offset)));
case "float64":
return(new Tuple <int, dynamic>(8, BitConverter.ToDouble(rawData, offset)));
case "time":
return(new Tuple <int, dynamic>(8, RosTime.FromRosBytes(rawData, offset)));
case "duration":
return(new Tuple <int, dynamic>(8, RosDuration.FromRosBytes(rawData, offset)));
case "header":
case "Header":
var header = RosHeader.FromRosBytes(rawData, offset);
return(new Tuple <int, dynamic>(header.HeaderByteSize, header));
}
return(null);
}
private Tuple <int, dynamic> ParseBuildInTypes(byte[] rawData, string type, int offset = 0)
{
switch (type)
{
case "string":
int len = BitConverter.ToInt32(rawData, offset);
return(new Tuple <int, dynamic>(len + 4, Encoding.UTF8.GetString(rawData.Skip(offset + 4).Take(len).ToArray())));
case "bool":
return(new Tuple <int, dynamic>(1, BitConverter.ToBoolean(rawData, offset)));
case "int8":
return(new Tuple <int, dynamic>(1, rawData[0]));
case "uint8":
return(new Tuple <int, dynamic>(1, rawData[0]));
case "int16":
return(new Tuple <int, dynamic>(2, BitConverter.ToInt16(rawData, offset)));
case "uint16":
return(new Tuple <int, dynamic>(2, BitConverter.ToUInt16(rawData, offset)));
case "int32":
return(new Tuple <int, dynamic>(4, BitConverter.ToInt32(rawData, offset)));
case "uint32":
return(new Tuple <int, dynamic>(4, BitConverter.ToUInt32(rawData, offset)));
case "int64":
return(new Tuple <int, dynamic>(8, BitConverter.ToInt64(rawData, offset)));
case "uint64":
return(new Tuple <int, dynamic>(8, BitConverter.ToUInt64(rawData, offset)));
case "float32":
return(new Tuple <int, dynamic>(4, BitConverter.ToSingle(rawData, offset)));
case "float64":
return(new Tuple <int, dynamic>(8, BitConverter.ToDouble(rawData, offset)));
case "time":
return(new Tuple <int, dynamic>(8, RosTime.FromRosBytes(rawData, offset)));
case "duration":
return(new Tuple <int, dynamic>(8, RosDuration.FromRosBytes(rawData, offset)));
}
return(null);
}
public ChunkInfo(Dictionary <string, byte[]> fields, byte[] data)
{
//StartTime = BitConverter.ToInt64(fields["start_time"], 0);
StartTime = RosTime.FromRosBytes(fields["start_time"]);
//EndTime = BitConverter.ToInt64(fields["end_time"], 0);
EndTime = RosTime.FromRosBytes(fields["end_time"]);
ChunkPos = BitConverter.ToInt64(fields["chunk_pos"], 0);
Count = BitConverter.ToInt32(fields["count"], 0);
for (var i = 0; i < Count; i++)
{
var conn = BitConverter.ToInt32(data, (i * 8));
var count = BitConverter.ToInt32(data, (i * 8) + 4);
MessageCount.Add(conn, count);
}
}
public int CompareTo(object obj)
{
if (obj == null)
{
return(1);
}
RosTime otherTime = obj as RosTime;
if (otherTime.Seconds == this.Seconds)
{
return((int)this.NanoSeconds - (int)otherTime.NanoSeconds);
}
else
{
return((int)this.Seconds - (int)otherTime.Seconds);
}
}
public static RosTime FromRosBytes(byte[] timeBytes)
{
return(RosTime.FromRosBytes(timeBytes, 0));
}
public RosMessage(RosMessageDefinition type, Dictionary <string, byte[]> headerField, byte[] data)
: this(type, RosTime.FromRosBytes(headerField["time"]), BitConverter.ToInt32(headerField["conn"], 0), data)
{
}
public IEnumerable <RosMessage> ReadTopic(List <string> topicNames)
{
byte[] indexDataBytes = new byte[12];
byte[] intBytes = new byte[4];
RosMessageDefinition msgDef = null;
Dictionary <string, byte[]> headerField;
byte[] data = null;
List <RosMessage> msgList = new List <RosMessage>();
// We assume the file streams are all sorted
for (var bi = 0; bi < this.bagFileStreams.Count; bi++)
{
var fs = this.bagFileStreams[bi];
// first figure out which chunk has the information
List <int> savedConnections = new List <int>();
// first figure out which connection we want
foreach (var conn in this.EachBagConnections[bi])
{
if (topicNames.Contains(conn.Value.Topic))
{
savedConnections.Add(conn.Key);
}
}
//now we figure out which chunk we want
for (var i = 0; i < this.EachChunkInfoList[bi].Count(); i++)
{
var chunkInfo = this.EachChunkInfoList[bi][i];
foreach (var connId in savedConnections)
{
if (chunkInfo.MessageCount.ContainsKey(connId))
{
// read the chunk data
(var header, var chuckDataOffset, var chunkDataLen) = this.ReadNextRecord(fs, chunkInfo.ChunkPos);
// check to make sure its a chunk
if (header["op"][0] != (byte)0x05)
{
throw new Exception($"Except to see chunk(0x05) but got {header["op"][0]}");
}
// read the next records which are index records for all messages in the chunk
long indexDataOffset = -1;
int indexDataLen = -1;
int msgCount = -1;
Dictionary <string, byte[]> indexHeader;
long nextRecordPos = chuckDataOffset + chunkDataLen;
while (true)
{
(indexHeader, indexDataOffset, indexDataLen) = this.ReadNextRecord(fs, nextRecordPos);
if (indexHeader["op"][0] != (byte)0x04)
{
throw new Exception($"Except to see Index data Record (0x04) but got {indexHeader["op"][0]}");
}
msgCount = BitConverter.ToInt32(indexHeader["count"], 0);
// if this index header belongs to the one we are working on break out
if (savedConnections.Contains(BitConverter.ToInt32(indexHeader["conn"], 0)))
{
break;
}
nextRecordPos = indexDataOffset + indexDataLen;
}
// loop through and read each message
long messageOffset = chuckDataOffset;
for (var msgIndex = 0; msgIndex < msgCount; msgIndex++)
{
lock (this.mutex)
{
// seek to the correct part
fs.Seek(indexDataOffset + msgIndex * 12, SeekOrigin.Begin);
// from the index, read the time and offset into chunk
fs.Read(indexDataBytes, 0, 12);
var messageTime = RosTime.FromRosBytes(indexDataBytes, 0);
var offset = BitConverter.ToInt32(indexDataBytes, 8);
// now we can look into the chunk and read data
fs.Seek(chuckDataOffset + offset, SeekOrigin.Begin);
// read header
fs.Read(intBytes, 0, 4);
var headerLen = BitConverter.ToInt32(intBytes, 0);
byte[] headerBytes = new byte[headerLen];
fs.Read(headerBytes, 0, headerLen);
headerField = Utils.ParseHeaderData(headerBytes);
// read data len
fs.Read(intBytes, 0, 4);
var dataLen = BitConverter.ToInt32(intBytes, 0);
// read data
data = new byte[dataLen];
fs.Read(data, 0, dataLen);
msgDef = EachBagConnections[bi][BitConverter.ToInt32(headerField["conn"], 0)].MessageDefinition;
}
yield return(new RosMessage(msgDef, headerField, data));
// msgList.Add(new RosMessage(msgDef, headerField, data));
}
}
}
}
}
//return msgList;
}
