public RosMessage GetFieldAsRosMessage(RosMessageDefinition def, string indexString) { // get the data and offset var fieldData = this.messageType.GetData(this.RawData, indexString); return(new RosMessage(def, this.Time, this.Conn, fieldData.Item2)); }
private void parseDefinitionText(string text) { string definitionSplit = "================================================================================"; var sentences = new List <string>(text.Split('\n')); if (sentences.Contains(definitionSplit)) { // This means there are multiple definitions this.ParseSingleDefinitonText(sentences.Take(sentences.IndexOf(definitionSplit)).ToList()); sentences.RemoveRange(0, sentences.IndexOf(definitionSplit) + 1); while (sentences.Contains(definitionSplit)) { var subSentences = sentences.Take(sentences.IndexOf(definitionSplit)).ToList(); var loopRosDef = new RosMessageDefinition(subSentences); if (!this.KnownDefinitions.ContainsKey(loopRosDef.Type)) { this.KnownDefinitions.Add(loopRosDef.Type, loopRosDef); } sentences.RemoveRange(0, sentences.IndexOf(definitionSplit) + 1); } var newRosDef = new RosMessageDefinition(sentences); if (!this.KnownDefinitions.ContainsKey(newRosDef.Type)) { this.KnownDefinitions.Add(newRosDef.Type, newRosDef); } } else { this.ParseSingleDefinitonText(sentences); } }
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; }
private Tuple <int, dynamic> ParseSingleFieldType(byte[] rawData, string type, int offset = 0) { if (RosMessageDefinition.IsBuiltInType(type)) { return(this.ParseBuildInTypes(rawData, type, offset)); } // This is not a standard type // TODO Figure out a way to handle nested types return(null); }
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; }
private (int, dynamic) ParseSingleFieldType(byte[] rawData, string type, int offset = 0) { if (RosMessageDefinition.IsBuiltInType(type)) { var result = this.ParseBuildInTypes(rawData, type, offset); return(result.Item1, result.Item2); } else { RosMessageDefinition msgDef = null; // If not built in type // get the message definition of the type if (this.KnownMsgDefinitions.ContainsKey(type)) { msgDef = this.KnownMsgDefinitions[type]; } else { // Its possible the name isn't format correctly. Lack the beginning part foreach (var name in this.KnownMsgDefinitions.Keys) { if (name.EndsWith(type)) { msgDef = this.KnownMsgDefinitions[name]; break; } } if (msgDef == null) { throw new KeyNotFoundException($"{type} not found among known message definitions."); } } var subMessageResult = msgDef.ParseMessage(rawData, offset); return(subMessageResult); } }
/// <summary> /// Return the data of the field as a RosMessage object. This is useful in cases /// where the field is another custom message type. /// </summary> /// <param name="def">Definition of the Field</param> /// <param name="indexString">Name of the Field</param> /// <returns>The data in the field as its own RosMessage</returns> public RosMessage GetFieldAsRosMessage(RosMessageDefinition def, string indexString) { return(new RosMessage(def, this.Time, this.Conn, this.fields[indexString])); }
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; }
private void contructRosMessageDefinition(Dictionary <string, RosMessageDefinition> knownDef) { MessageDefinition = new RosMessageDefinition(Type, MessageDefinitionText, knownDef); }