Beispiel #1
0
        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));
        }
Beispiel #2
0
        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);
            }
        }
Beispiel #3
0
 public RosMessage(RosMessageDefinition type, RosTime time, int conn, Dictionary <string, dynamic> fields)
 {
     this.messageType = type;
     this.Time        = time;
     this.Conn        = conn;
     this.fields      = fields;
 }
Beispiel #4
0
 public RosMessage(RosMessageDefinition type, RosTime time, int conn, byte[] data)
 {
     this.messageType = type;
     this.Time        = time;
     this.Conn        = conn;
     this.RawData     = data;
 }
Beispiel #5
0
 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);
 }
Beispiel #6
0
        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;
        }
Beispiel #7
0
 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);
     }
 }
Beispiel #8
0
 /// <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]));
 }
Beispiel #9
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;
        }
 private void contructRosMessageDefinition(Dictionary <string, RosMessageDefinition> knownDef)
 {
     MessageDefinition = new RosMessageDefinition(Type, MessageDefinitionText, knownDef);
 }