Go to the documentation of this file.
64 #include "../../../console_bridge/include/console_bridge/console.h"
115 std::string getFileName()
const;
123 std::tuple<std::string, uint64_t, uint64_t> getCompressionInfo()
const;
124 void setChunkThreshold(
uint32_t chunk_threshold);
148 std::shared_ptr<rs2rosinternal::M_string> connection_header = std::shared_ptr<rs2rosinternal::M_string>());
160 void write(std::string
const& topic,
rs2rosinternal::Time const& time, std::shared_ptr<T const>
const& msg,
161 std::shared_ptr<rs2rosinternal::M_string> connection_header = std::shared_ptr<rs2rosinternal::M_string>());
173 void write(std::string
const& topic,
rs2rosinternal::Time const& time, std::shared_ptr<T>
const& msg,
174 std::shared_ptr<rs2rosinternal::M_string> connection_header = std::shared_ptr<rs2rosinternal::M_string>());
179 void doWrite(std::string
const& topic,
rs2rosinternal::Time const& time,
T const& msg, std::shared_ptr<rs2rosinternal::M_string>
const& connection_header);
181 void openRead (std::string
const&
filename);
182 void openWrite (std::string
const&
filename);
183 void openAppend(std::string
const&
filename);
188 std::shared_ptr<T> instantiateBuffer(
IndexEntry const& index_entry)
const;
193 void startReadingVersion102();
194 void startReadingVersion200();
199 void writeFileHeaderRecord();
200 void writeConnectionRecord(
ConnectionInfo const* connection_info);
204 void writeIndexRecords();
205 void writeConnectionRecords();
206 void writeChunkInfoRecords();
209 void stopWritingChunk();
214 void readFileHeaderRecord();
215 void readConnectionRecord();
216 void readChunkHeader(
ChunkHeader& chunk_header)
const;
217 void readChunkInfoRecord();
218 void readConnectionIndexRecord200();
220 void readTopicIndexRecord102();
221 void readMessageDefinitionRecord102();
227 template<
typename Stream>
228 void readMessageDataIntoStream(
IndexEntry const& index_entry,
Stream& stream)
const;
230 void decompressChunk(
uint64_t chunk_pos)
const;
231 void decompressRawChunk(
ChunkHeader const& chunk_header)
const;
232 void decompressBz2Chunk(
ChunkHeader const& chunk_header)
const;
233 void decompressLz4Chunk(
ChunkHeader const& chunk_header)
const;
239 void writeDataLength(
uint32_t data_len);
246 bool readDataLength(
uint32_t& data_size)
const;
252 std::string toHeaderString(
T const* field)
const;
259 bool readField(
rs2rosinternal::M_string const& fields, std::string
const& field_name,
unsigned int min_len,
unsigned int max_len,
bool required, std::string&
data)
const;
264 rs2rosinternal::M_string::const_iterator checkField(
rs2rosinternal::M_string const& fields, std::string
const& field,
265 unsigned int min_len,
unsigned int max_len,
bool required)
const;
269 void write(
char const*
s, std::streamsize
n);
270 void write(std::string
const&
s);
271 void read(
char*
b, std::streamsize
n)
const;
272 void seek(
uint64_t pos,
int origin = std::ios_base::beg)
const;
293 std::map<std::string, uint32_t> topic_connection_ids_;
294 std::map<rs2rosinternal::M_string, uint32_t> header_connection_ids_;
295 std::map<uint32_t, ConnectionInfo*> connections_;
297 std::vector<ChunkInfo> chunks_;
299 std::map<uint32_t, std::multiset<IndexEntry> > connection_indexes_;
300 std::map<uint32_t, std::multiset<IndexEntry> > curr_chunk_connection_indexes_;
302 mutable Buffer header_buffer_;
303 mutable Buffer record_buffer_;
305 mutable Buffer chunk_buffer_;
306 mutable Buffer decompress_buffer_;
330 doWrite(topic, time, msg, connection_header);
334 void Bag::write(std::string
const& topic,
rs2rosinternal::Time const& time, std::shared_ptr<T const>
const& msg, std::shared_ptr<rs2rosinternal::M_string> connection_header) {
335 doWrite(topic, time, *msg, connection_header);
339 void Bag::write(std::string
const& topic,
rs2rosinternal::Time const& time, std::shared_ptr<T>
const& msg, std::shared_ptr<rs2rosinternal::M_string> connection_header) {
340 doWrite(topic, time, *msg, connection_header);
345 return std::string((
char*) field,
sizeof(
T));
350 rs2rosinternal::M_string::const_iterator
i =
checkField(fields, field_name,
sizeof(
T),
sizeof(
T), required);
351 if (
i == fields.end())
353 memcpy(
data,
i->second.data(),
sizeof(
T));
357 template<
typename Stream>
403 std::map<uint32_t, ConnectionInfo*>::const_iterator connection_iter =
connections_.find(connection_id);
408 std::shared_ptr<T>
p = std::make_shared<T>();
430 std::string topic, latching(
"0"), callerid;
435 std::map<std::string, uint32_t>::const_iterator topic_conn_id_iter =
topic_connection_ids_.find(topic);
438 uint32_t connection_id = topic_conn_id_iter->second;
440 std::map<uint32_t, ConnectionInfo*>::const_iterator connection_iter =
connections_.find(connection_id);
445 std::shared_ptr<T>
p = std::make_shared<T>();
448 std::shared_ptr<rs2rosinternal::M_string> message_header(std::make_shared<rs2rosinternal::M_string>());
449 for (rs2rosinternal::M_string::const_iterator
i = connection_info->
header->begin();
i != connection_info->
header->end();
i++)
450 (*message_header)[
i->first] =
i->second;
451 (*message_header)[
"latching"] = latching;
452 (*message_header)[
"callerid"] = callerid;
475 throw BagException(
"Tried to insert a message with time less than rs2rosinternal::TIME_MIN");
484 if (!connection_header) {
487 std::map<std::string, uint32_t>::iterator topic_connection_ids_iter =
topic_connection_ids_.find(topic);
493 conn_id = topic_connection_ids_iter->second;
505 connection_header_copy[
"topic"] = topic;
507 std::map<rs2rosinternal::M_string, uint32_t>::iterator header_connection_ids_iter =
header_connection_ids_.find(connection_header_copy);
513 conn_id = header_connection_ids_iter->second;
528 if (connection_info ==
NULL) {
530 connection_info->
id = conn_id;
531 connection_info->
topic = topic;
535 if (connection_header !=
NULL) {
536 connection_info->
header = connection_header;
539 connection_info->
header = std::make_shared<rs2rosinternal::M_string>();
541 (*connection_info->
header)[
"md5sum"] = connection_info->
md5sum;
542 (*connection_info->
header)[
"message_definition"] = connection_info->
msg_def;
552 index_entry.
time = time;
557 chunk_connection_index.insert(chunk_connection_index.end(), index_entry);
559 connection_index.insert(connection_index.end(), index_entry);
const ROSTIME_DECL Time TIME_MIN
std::map< std::string, uint32_t > topic_connection_ids_
ChunkedFile reads and writes files which contain interleaved chunks of compressed and uncompressed da...
Time representation. May either represent wall clock time or ROS clock time.
static void notify(const PreDeserializeParams< M > &)
void setSize(uint32_t size)
void serialize(Stream &stream, const T &t)
Serialize an object. Stream here should normally be a rs2rosinternal::serialization::OStream.
rs2rosinternal::Time start_time
std::shared_ptr< M > getMessage() const
Retrieve the message. If M is const, this returns a reference to it. If M is non const and this event...
Event type for subscriptions, const rs2rosinternal::MessageEvent<M const>& can be used in your callba...
rs2rosinternal::Time getReceiptTime() const
Returns the time at which this message was received.
A class pointing into a bag file.
void appendHeaderToBuffer(Buffer &buf, rs2rosinternal::M_string const &fields)
GLboolean GLboolean GLboolean b
void readMessageDataHeaderFromBuffer(Buffer &buffer, uint32_t offset, rs2rosinternal::Header &header, uint32_t &data_size, uint32_t &bytes_read) const
static const unsigned char OP_MSG_DATA
ChunkInfo curr_chunk_info_
void decompressChunk(uint64_t chunk_pos) const
#define ROSBAG_DECL
#include <ros/macros.h> // for the DECL's
std::string toHeaderString(T const *field) const
const std::shared_ptr< M_string > & getConnectionHeaderPtr() const
void seek(uint64_t pos, int origin=std::ios_base::beg) const
void writeMessageDataRecord(uint32_t conn_id, rs2rosinternal::Time const &time, T const &msg)
void write(std::string const &topic, rs2rosinternal::MessageEvent< T > const &event)
Write a message into the bag file.
std::map< rs2rosinternal::M_string, uint32_t > header_connection_ids_
std::map< uint32_t, ConnectionInfo * > connections_
rs2rosinternal::M_string::const_iterator checkField(rs2rosinternal::M_string const &fields, std::string const &field, unsigned int min_len, unsigned int max_len, bool required) const
uint32_t offset
absolute byte offset of the chunk record containing the message
const char * definition()
returns Definition<M>::value();
BagMode
The possible modes to open a bag in.
uint64_t getOffset() const
return current offset from the beginning of the file
Base class for rosbag exceptions.
uint64_t chunk_pos
timestamp of the message
std::map< std::string, std::string > M_string
std::shared_ptr< rs2rosinternal::M_string > header
std::map< uint32_t, std::multiset< IndexEntry > > connection_indexes_
void startWritingChunk(rs2rosinternal::Time time)
std_msgs::Header * header(M &m)
returns Header<M>::pointer(m);
static const std::string LATCHING_FIELD_NAME
void appendConnectionRecordToBuffer(Buffer &buf, ConnectionInfo const *connection_info)
std::map< uint32_t, uint32_t > connection_counts
absolute byte offset of chunk record in bag file
static const std::string CONNECTION_FIELD_NAME
void appendDataLengthToBuffer(Buffer &buf, uint32_t data_len)
uint32_t getChunkOffset() const
unsigned __int64 uint64_t
rs2rosinternal::Time end_time
earliest timestamp of a message in the chunk
std::shared_ptr< T > instantiateBuffer(IndexEntry const &index_entry) const
deserializes the message held in record_buffer_
void writeConnectionRecord(ConnectionInfo const *connection_info)
const char * datatype()
returns DataType<M>::value();
uint32_t serializationLength(const T &t)
Determine the serialized length of an object.
void readMessageDataRecord102(uint64_t offset, rs2rosinternal::Header &header) const
bool readField(rs2rosinternal::M_string const &fields, std::string const &field_name, bool required, T *data) const
void writeHeader(rs2rosinternal::M_string const &fields)
static const std::string TOPIC_FIELD_NAME
#define CONSOLE_BRIDGE_logDebug(fmt,...)
void deserialize(Stream &stream, T &t)
Deserialize an object. Stream here should normally be a rs2rosinternal::serialization::IStream.
std::shared_ptr< std::map< std::string, std::string > > connection_header
rs2rosinternal::Time time
void writeDataLength(uint32_t data_len)
std::shared_ptr< M > message
std::map< uint32_t, std::multiset< IndexEntry > > curr_chunk_connection_indexes_
void readMessageDataIntoStream(IndexEntry const &index_entry, Stream &stream) const
static const std::string TIME_FIELD_NAME
const char * md5sum()
returns MD5Sum<M>::value();
Buffer outgoing_chunk_buffer_
reusable buffer to read chunk into
static const std::string OP_FIELD_NAME
static const std::string CALLERID_FIELD_NAME
void doWrite(std::string const &topic, rs2rosinternal::Time const &time, T const &msg, std::shared_ptr< rs2rosinternal::M_string > const &connection_header)
uint64_t pos
latest timestamp of a message in the chunk
Buffer record_buffer_
reusable buffer in which to assemble the record data before writing to file
int writeHeader(roslz4_stream *str)
GLenum GLuint GLenum GLsizei const GLchar * buf
uint32_t chunk_threshold_
librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Thu Dec 22 2022 03:13:13