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