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