bool
SimpleReadWriter::writeHeader(
const ReaderContext& context,
bool sorted,
int argc,
const char **argv,
const char *version,
const char *rgLine,
bool omitSQLines)
{
char* buffer;
size_t size;
size_t used;
char *localBuffer = NULL;
writer->inHeader(true);
if (! writer->getBuffer(&buffer, &size)) {
return false;
}
char *writerBuffer = buffer;
size_t writerBufferSize = size;
while (!format->writeHeader(context, buffer, size, &used, sorted, argc, argv, version, rgLine, omitSQLines)) {
delete[] localBuffer;
size = 2 * size;
localBuffer = new char[size];
buffer = localBuffer;
}
if (NULL == localBuffer) {
_ASSERT(writerBuffer == buffer);
writer->advance((unsigned)used, 0);
writer->nextBatch();
} else {
size_t bytesRemainingToWrite = used;
size_t bytesWritten = 0;
while (bytesRemainingToWrite > 0) {
size_t bytesToWrite = __min(bytesRemainingToWrite, writerBufferSize);
memcpy(writerBuffer, localBuffer + bytesWritten, bytesToWrite);
writer->advance(bytesToWrite);
writer->nextBatch();
if (!writer->getBuffer(&writerBuffer, &writerBufferSize)) {
return false;
}
bytesWritten += bytesToWrite;
bytesRemainingToWrite -= bytesToWrite;
}
delete[] localBuffer;
}
writer->inHeader(false);
return true;
}
bool PlotIO::write(const QString& path, const ros::Time& start, const ros::Time& end)
{
FileFormat* io = 0;
if(path.endsWith(".bag"))
io = new BagFormat;
else if(path.endsWith(".csv"))
io = new CSVFormat;
if(!io)
{
fprintf(stderr, "Could not find output format\n");
return false;
}
if(!io->init(path, start))
return false;
QLinkedList<Plot::LinkedBufferIterator> its = m_rootPlot->iterators(io->defaultFlags());
io->writeHeader(its);
QVector<Plot::LinkedBufferIterator*> to_write(its.size(), 0);
while(1)
{
ros::Time stamp = ros::Time(0);
qFill(to_write, (Plot::LinkedBufferIterator*)0);
// Get a list of iterators with the next timestamp
int i = 0;
for(QLinkedList<Plot::LinkedBufferIterator>::iterator it = its.begin(); it != its.end(); ++it, ++i)
{
Plot::LinkedBufferIterator& buf_it = *it;
if(!buf_it.isValid())
continue;
const Plot::DataPoint& point = *buf_it;
if(point.time < stamp || stamp == ros::Time(0))
{
qFill(to_write, (Plot::LinkedBufferIterator*)0);
to_write[i] = &buf_it;
stamp = point.time;
}
else if(point.time == stamp)
to_write[i] = &buf_it;
}
if(stamp == ros::Time(0))
break;
if(end != ros::Time(0) && stamp > end)
break;
if(stamp >= start)
io->writeData(stamp, to_write);
// Advance all used iterators
Q_FOREACH(Plot::LinkedBufferIterator* it, to_write)
{
if(!it)
continue;
++(*it);
}
}
delete io;
return true;
}
