42 #include <boost/foreach.hpp> 44 #include "console_bridge/console.h" 46 #define foreach BOOST_FOREACH 49 using std::priority_queue;
65 Bag::Bag(
string const& filename, uint32_t mode)
71 #ifndef BOOST_NO_CXX11_RVALUE_REFERENCES 83 #endif // BOOST_NO_CXX11_RVALUE_REFERENCES 116 throw BagException((format(
"Unknown mode: %1%") % (
int) mode).str());
120 seek(0, std::ios::end);
163 seek(0, std::ios::end);
214 (format(
"Unknown compression type: %i") % compression).str());
223 string version = string(
"#ROSBAG V") +
VERSION + string(
"\n");
225 CONSOLE_BRIDGE_logDebug(
"Writing VERSION [%llu]: %s", (
unsigned long long)
file_.
getOffset(), version.c_str());
237 char logtypename[100];
238 int version_major, version_minor;
239 #if defined(_MSC_VER) 240 if (sscanf_s(version_line.c_str(),
"#ROS%s V%d.%d", logtypename,
sizeof(logtypename), &version_major, &version_minor) != 3)
242 if (sscanf(version_line.c_str(),
"#ROS%99s V%d.%d", logtypename, &version_major, &version_minor) != 3)
246 version_ = version_major * 100 + version_minor;
248 CONSOLE_BRIDGE_logDebug(
"Read VERSION: version=%d",
version_);
266 seek(0, std::ios::end);
322 seek(0, std::ios::end);
334 multiset<IndexEntry>
const& index = i->second;
335 IndexEntry const& first_entry = *index.begin();
337 CONSOLE_BRIDGE_logDebug(
"Reading message definition for connection %d at %llu", i->first, (
unsigned long long) first_entry.
chunk_pos);
351 CONSOLE_BRIDGE_logDebug(
"Writing FILE_HEADER [%llu]: index_pos=%llu connection_count=%d chunk_count=%d",
361 boost::shared_array<uint8_t> header_buffer;
364 uint32_t data_len = 0;
367 write((
char*) &header_len, 4);
368 write((
char*) header_buffer.get(), header_len);
369 write((
char*) &data_len, 4);
374 padding.resize(data_len,
' ');
402 CONSOLE_BRIDGE_logDebug(
"Read FILE_HEADER: index_pos=%llu connection_count=%d chunk_count=%d",
406 seek(data_size, std::ios::cur);
450 seek(end_of_chunk_pos);
463 switch (compression) {
472 CONSOLE_BRIDGE_logDebug(
"Writing CHUNK [%llu]: compression=%s compressed=%d uncompressed=%d",
504 uint32_t connection_id = i->first;
505 multiset<IndexEntry>
const& index = i->second;
508 uint32_t index_size = index.size();
518 CONSOLE_BRIDGE_logDebug(
"Writing INDEX_DATA: connection=%d ver=%d count=%d", connection_id,
INDEX_VERSION, index_size);
541 uint32_t index_version;
548 CONSOLE_BRIDGE_logDebug(
"Read INDEX_DATA: ver=%d topic=%s count=%d", index_version, topic.c_str(), count);
550 if (index_version != 0)
551 throw BagFormatException((format(
"Unsupported INDEX_DATA version: %1%") % index_version).str());
553 uint32_t connection_id;
558 CONSOLE_BRIDGE_logDebug(
"Creating connection: id=%d topic=%s", connection_id, topic.c_str());
560 connection_info->id = connection_id;
561 connection_info->topic = topic;
567 connection_id = topic_conn_id_iter->second;
571 for (uint32_t i = 0; i < count; i++) {
575 read((
char*) &sec, 4);
576 read((
char*) &nsec, 4);
581 CONSOLE_BRIDGE_logDebug(
" - %d.%d: %llu", sec, nsec, (
unsigned long long) index_entry.
chunk_pos);
588 connection_index.insert(connection_index.end(), index_entry);
603 uint32_t index_version;
604 uint32_t connection_id;
610 CONSOLE_BRIDGE_logDebug(
"Read INDEX_DATA: ver=%d connection=%d count=%d", index_version, connection_id, count);
612 if (index_version != 1)
613 throw BagFormatException((format(
"Unsupported INDEX_DATA version: %1%") % index_version).str());
619 for (uint32_t i = 0; i < count; i++) {
624 read((
char*) &sec, 4);
625 read((
char*) &nsec, 4);
629 CONSOLE_BRIDGE_logDebug(
" - %d.%d: %llu+%d", sec, nsec, (
unsigned long long) index_entry.
chunk_pos, index_entry.
offset);
636 connection_index.insert(connection_index.end(), index_entry);
651 CONSOLE_BRIDGE_logDebug(
"Writing CONNECTION [%llu:%d]: topic=%s id=%d",
692 map<uint32_t, ConnectionInfo*>::iterator key =
connections_.find(
id);
695 connection_info->
id = id;
696 connection_info->
topic = topic;
697 connection_info->
header = boost::make_shared<M_string>();
698 for (M_string::const_iterator i = connection_header.
getValues()->begin(); i != connection_header.
getValues()->end(); i++)
699 (*connection_info->
header)[i->first] = i->second;
700 connection_info->
msg_def = (*connection_info->
header)[
"message_definition"];
702 connection_info->
md5sum = (*connection_info->
header)[
"md5sum"];
705 CONSOLE_BRIDGE_logDebug(
"Read CONNECTION: topic=%s id=%d", topic.c_str(), id);
731 CONSOLE_BRIDGE_logDebug(
"Creating connection: topic=%s md5sum=%s datatype=%s", topic.c_str(), md5sum.c_str(), datatype.c_str());
733 connection_info->id = id;
734 connection_info->topic = topic;
740 connection_info =
connections_[topic_conn_id_iter->second];
742 connection_info->
msg_def = message_definition;
743 connection_info->
datatype = datatype;
744 connection_info->
md5sum = md5sum;
745 connection_info->
header = boost::make_shared<ros::M_string>();
747 (*connection_info->
header)[
"md5sum"] = connection_info->
md5sum;
748 (*connection_info->
header)[
"message_definition"] = connection_info->
msg_def;
750 CONSOLE_BRIDGE_logDebug(
"Read MSG_DEF: topic=%s md5sum=%s datatype=%s", topic.c_str(), md5sum.c_str(), datatype.c_str());
785 CONSOLE_BRIDGE_logDebug(
"readMessageDataRecord: offset=%llu", (
unsigned long long) offset);
840 CONSOLE_BRIDGE_logDebug(
"lz4 compressed_size: %d uncompressed_size: %d",
901 CONSOLE_BRIDGE_logDebug(
"Writing CHUNK_INFO [%llu]: ver=%d pos=%llu start=%d.%d end=%d.%d",
912 uint32_t connection_id = i->first;
913 uint32_t count = i->second;
915 write((
char*) &connection_id, 4);
916 write((
char*) &count, 4);
918 CONSOLE_BRIDGE_logDebug(
" - %d: %d", connection_id, count);
934 uint32_t chunk_info_version;
944 uint32_t chunk_connection_count = 0;
947 CONSOLE_BRIDGE_logDebug(
"Read CHUNK_INFO: chunk_pos=%llu connection_count=%d start=%d.%d end=%d.%d",
948 (
unsigned long long) chunk_info.
pos, chunk_connection_count,
953 for (uint32_t i = 0; i < chunk_connection_count; i ++) {
954 uint32_t connection_id, connection_count;
955 read((
char*) &connection_id, 4);
956 read((
char*) &connection_count, 4);
958 CONSOLE_BRIDGE_logDebug(
" %d: %d messages", connection_id, connection_count);
975 boost::shared_array<uint8_t> header_buffer;
978 write((
char*) &header_len, 4);
979 write((
char*) header_buffer.get(), header_len);
983 write((
char*) &data_len, 4);
987 boost::shared_array<uint8_t> header_buffer;
991 uint32_t offset = buf.
getSize();
995 memcpy(buf.
getData() + offset, &header_len, 4);
997 memcpy(buf.
getData() + offset, header_buffer.get(), header_len);
1001 uint32_t offset = buf.
getSize();
1005 memcpy(buf.
getData() + offset, &data_len, 4);
1012 uint8_t* start = (uint8_t*) buffer.
getData() + offset;
1014 uint8_t* ptr = start;
1017 uint32_t header_len;
1018 memcpy(&header_len, ptr, 4);
1023 bool parsed = header.
parse(ptr, header_len, error_msg);
1029 memcpy(&data_size, ptr, 4);
1032 bytes_read = ptr - start;
1037 total_bytes_read = 0;
1040 CONSOLE_BRIDGE_logDebug(
"reading header from buffer: offset=%d", offset);
1041 uint32_t bytes_read;
1044 offset += bytes_read;
1045 total_bytes_read += bytes_read;
1057 uint32_t header_len;
1058 read((
char*) &header_len, 4);
1074 read((
char*) &data_size, 4);
1078 M_string::const_iterator
Bag::checkField(
M_string const& fields,
string const& field,
unsigned int min_len,
unsigned int max_len,
bool required)
const {
1079 M_string::const_iterator fitr = fields.find(field);
1080 if (fitr == fields.end()) {
1084 else if ((fitr->second.size() < min_len) || (fitr->second.size() > max_len))
1085 throw BagFormatException((format(
"Field '%1%' is wrong size (%2% bytes)") % field % (uint32_t) fitr->second.size()).str());
1090 bool Bag::readField(
M_string const& fields,
string const& field_name,
bool required,
string& data)
const {
1091 return readField(fields, field_name, 1, UINT_MAX, required, data);
1094 bool Bag::readField(
M_string const& fields,
string const& field_name,
unsigned int min_len,
unsigned int max_len,
bool required,
string& data)
const {
1095 M_string::const_iterator fitr =
checkField(fields, field_name, min_len, max_len, required);
1096 if (fitr == fields.end())
1099 data = fitr->second;
1104 uint64_t packed_time;
1105 if (!
readField(fields, field_name, required, &packed_time))
1108 uint64_t bitmask = (1LL << 33) - 1;
1109 data.
sec = (uint32_t) (packed_time & bitmask);
1110 data.
nsec = (uint32_t) (packed_time >> 32);
1116 uint64_t packed_time = (((uint64_t) field->
nsec) << 32) + field->
sec;
BagMode getMode() const
Get the mode the bag is in.
uint32_t readMessageDataSize(IndexEntry const &index_entry) const
static const std::string COMPRESSION_BZ2
void openRead(std::string const &filename)
open file for reading
static const std::string END_TIME_FIELD_NAME
void startWritingChunk(ros::Time time)
static const std::string CHUNK_COUNT_FIELD_NAME
void read(void *ptr, size_t size)
read size bytes from the file into ptr
static const std::string COMPRESSION_FIELD_NAME
std::map< std::string, uint32_t > topic_connection_ids_
Bag & operator=(Bag &&other)
void decompressChunk(uint64_t chunk_pos) const
uint64_t pos
latest timestamp of a message in the chunk
void openRead(std::string const &filename)
static const std::string VER_FIELD_NAME
ros::M_string::const_iterator checkField(ros::M_string const &fields, std::string const &field, unsigned int min_len, unsigned int max_len, bool required) const
static const std::string COMPRESSION_LZ4
void open(std::string const &filename, uint32_t mode=bagmode::Read)
Open a bag file.
uint32_t getChunkThreshold() const
Get the threshold for creating new chunks.
std::string getFileName() const
Get the filename of the bag.
std::map< uint32_t, ConnectionInfo * > connections_
static const std::string CONNECTION_FIELD_NAME
void startReadingVersion102()
void write(std::string const &s)
std::map< ros::M_string, uint32_t > header_connection_ids_
static const std::string INDEX_POS_FIELD_NAME
void setChunkThreshold(uint32_t chunk_threshold)
Set the threshold for creating new chunks.
void readTopicIndexRecord102()
uint64_t getOffset() const
return current offset from the beginning of the file
std::map< uint32_t, uint32_t > connection_counts
absolute byte offset of chunk record in bag file
void readMessageDataHeaderFromBuffer(Buffer &buffer, uint32_t offset, ros::Header &header, uint32_t &data_size, uint32_t &bytes_read) const
CompressionType getCompression() const
Get the compression method to use for writing chunks.
Buffer header_buffer_
reusable buffer in which to assemble the record header before writing to file
static const unsigned char OP_CONNECTION
void openWrite(std::string const &filename)
void seek(uint64_t pos, int origin=std::ios_base::beg) const
Base class for rosbag exceptions.
bool truncate(uint64_t length)
void seek(uint64_t offset, int origin=std::ios_base::beg)
seek to given offset from origin
void writeConnectionRecords()
uint32_t getChunkOffset() const
static const std::string DEF_FIELD_NAME
uint32_t getMajorVersion() const
Get the major-version of the open bag file.
void readChunkHeader(ChunkHeader &chunk_header) const
void writeConnectionRecord(ConnectionInfo const *connection_info)
std_msgs::Header * header(M &m)
void openAppend(std::string const &filename)
void close()
close the file
void swap(Bag &a, Bag &b)
void appendDataLengthToBuffer(Buffer &buf, uint32_t data_len)
uint32_t getCompressedBytesIn() const
return the number of bytes written to current compressed stream
BagMode
The possible modes to open a bag in.
void setWriteMode(CompressionType type)
static const unsigned char OP_MSG_DATA
void close()
Close the bag file.
static const unsigned char OP_CHUNK
bool readField(ros::M_string const &fields, std::string const &field_name, bool required, T *data) const
static const std::string VERSION
Buffer decompress_buffer_
reusable buffer to decompress chunks into
void appendConnectionRecordToBuffer(Buffer &buf, ConnectionInfo const *connection_info)
void decompressRawChunk(ChunkHeader const &chunk_header) const
std::map< std::string, std::string > M_string
void readMessageDefinitionRecord102()
void writeFileHeaderRecord()
std::string toHeaderString(T const *field) const
static const std::string CHUNK_POS_FIELD_NAME
uint64_t decompressed_chunk_
position of decompressed chunk
bool readHeader(ros::Header &header) const
void readHeaderFromBuffer(Buffer &buffer, uint32_t offset, ros::Header &header, uint32_t &data_size, uint32_t &bytes_read) const
void writeHeader(ros::M_string const &fields)
static const std::string START_TIME_FIELD_NAME
Exception thrown on problems reading the bag index.
static const uint32_t CHUNK_INFO_VERSION
void decompressBz2Chunk(ChunkHeader const &chunk_header) const
Exception thrown when on IO problems.
void appendHeaderToBuffer(Buffer &buf, ros::M_string const &fields)
bool isOp(ros::M_string &fields, uint8_t reqOp) const
void setCompression(CompressionType compression)
Set the compression method to use for writing chunks.
void decompressLz4Chunk(ChunkHeader const &chunk_header) const
void writeChunkHeader(CompressionType compression, uint32_t compressed_size, uint32_t uncompressed_size)
static const std::string OP_FIELD_NAME
uint64_t file_header_pos_
bool isOpen() const
return true if file is open for reading or writing
Buffer record_buffer_
reusable buffer in which to assemble the record data before writing to file
static const std::string TYPE_FIELD_NAME
ROSTIME_DECL const Time TIME_MAX
Buffer chunk_buffer_
reusable buffer to read chunk into
ChunkInfo curr_chunk_info_
uint32_t getMinorVersion() const
Get the minor-version of the open bag file.
static const std::string COUNT_FIELD_NAME
std::map< uint32_t, std::multiset< IndexEntry > > connection_indexes_
uint64_t chunk_pos
timestamp of the message
static const unsigned char OP_FILE_HEADER
static const unsigned char OP_INDEX_DATA
uint32_t connection_count_
void readConnectionIndexRecord200()
void readChunkInfoRecord()
void openWrite(std::string const &filename)
open file for writing
uint32_t offset
absolute byte offset of the chunk record containing the message
bool readDataLength(uint32_t &data_size) const
uint32_t chunk_threshold_
std::vector< ChunkInfo > chunks_
ROSTIME_DECL const Time TIME_MIN
boost::shared_ptr< ros::M_string > header
void startReadingVersion200()
void writeDataLength(uint32_t data_len)
CompressionType compression_
void decompress(CompressionType compression, uint8_t *dest, unsigned int dest_len, uint8_t *source, unsigned int source_len)
std::map< uint32_t, std::multiset< IndexEntry > > curr_chunk_connection_indexes_
void read(char *b, std::streamsize n) const
static const std::string CONNECTION_COUNT_FIELD_NAME
static const uint32_t FILE_HEADER_LENGTH
uint64_t curr_chunk_data_pos_
static const std::string MD5_FIELD_NAME
Buffer outgoing_chunk_buffer_
reusable buffer to read chunk into
static const unsigned char OP_CHUNK_INFO
void readMessageDataRecord102(uint64_t offset, ros::Header &header) const
static const std::string SIZE_FIELD_NAME
void readConnectionRecord()
static const std::string COMPRESSION_NONE
ros::Time end_time
earliest timestamp of a message in the chunk
uint64_t getSize() const
Get the current size of the bag file (a lower bound)
void writeChunkInfoRecords()
std::string getFileName() const
return path of currently open file
static const std::string TOPIC_FIELD_NAME
void openReadWrite(std::string const &filename)
open file for reading & writing
static const unsigned char OP_MSG_DEF
void setSize(uint32_t size)
void write(std::string const &topic, ros::MessageEvent< T > const &event)
Write a message into the bag file.
void readFileHeaderRecord()
static const uint32_t INDEX_VERSION
ros::Header readMessageDataHeader(IndexEntry const &index_entry)