pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
thirdparty/ros/ros_comm/tools/rosbag_storage/CHANGELOG.rst vendored Normal file
View File

@@ -0,0 +1,145 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package rosbag_storage
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.12.14 (2018-08-23)
--------------------
1.12.13 (2018-02-21)
--------------------
* performance improvement for lower/upper bound (`#1223 <https://github.com/ros/ros_comm/issues/1223>`_)
* place console_bridge macros definition in header and use it everywhere console_bridge is included (`#1238 <https://github.com/ros/ros_comm/issues/1238>`_)
1.12.12 (2017-11-16)
--------------------
* backward compatibility with libconsole-bridge in Jessie (take 3) (`#1235 <https://github.com/ros/ros_comm/issues/1235>`_)
1.12.11 (2017-11-07)
--------------------
1.12.10 (2017-11-06)
--------------------
* fix compatibility with libconsole-bridge in Jessie, take 2
1.12.9 (2017-11-06)
-------------------
* fix compatibility with libconsole-bridge in Jessie (`#1219 <https://github.com/ros/ros_comm/issues/1219>`_, regression from 1.12.8)
1.12.8 (2017-11-06)
-------------------
* check if bzfile\_ and lz4s\_ handle is valid before reading/writing/closing (`#1183 <https://github.com/ros/ros_comm/issues/1183>`_)
* fix an out of bounds read in rosbag::View::iterator::increment() (`#1191 <https://github.com/ros/ros_comm/issues/1191>`_)
* replace usage deprecated console_bridge macros (`#1149 <https://github.com/ros/ros_comm/issues/1149>`_)
* fix whitespace warnings with g++ 7 (`#1138 <https://github.com/ros/ros_comm/issues/1138>`_)
* remove deprecated dynamic exception specifications (`#1137 <https://github.com/ros/ros_comm/issues/1137>`_)
* fix buffer overflow vulnerability (`#1092 <https://github.com/ros/ros_comm/issues/1092>`_)
* fix rosbag::View::iterator copy assignment operator (`#1017 <https://github.com/ros/ros_comm/issues/1017>`_)
* fix open mode on Windows (`#1005 <https://github.com/ros/ros_comm/pull/1005>`_)
* add swap function instead of copy constructor / assignment operator for rosbag::Bag (`#1000 <https://github.com/ros/ros_comm/issues/1000>`_)
1.12.7 (2017-02-17)
-------------------
1.12.6 (2016-10-26)
-------------------
1.12.5 (2016-09-30)
-------------------
1.12.4 (2016-09-19)
-------------------
1.12.3 (2016-09-17)
-------------------
* make Bag constructor explicit (`#835 <https://github.com/ros/ros_comm/pull/835>`_)
1.12.2 (2016-06-03)
-------------------
1.12.1 (2016-04-18)
-------------------
* use directory specific compiler flags (`#785 <https://github.com/ros/ros_comm/pull/785>`_)
1.12.0 (2016-03-18)
-------------------
1.11.18 (2016-03-17)
--------------------
* fix compiler warnings
1.11.17 (2016-03-11)
--------------------
* use boost::make_shared instead of new for constructing boost::shared_ptr (`#740 <https://github.com/ros/ros_comm/issues/740>`_)
1.11.16 (2015-11-09)
--------------------
1.11.15 (2015-10-13)
--------------------
1.11.14 (2015-09-19)
--------------------
1.11.13 (2015-04-28)
--------------------
1.11.12 (2015-04-27)
--------------------
1.11.11 (2015-04-16)
--------------------
* support large bagfiles (>2GB) on 32-bit systems (`#464 <https://github.com/ros/ros_comm/issues/464>`_)
1.11.10 (2014-12-22)
--------------------
* fix various defects reported by coverity
1.11.9 (2014-08-18)
-------------------
1.11.8 (2014-08-04)
-------------------
1.11.7 (2014-07-18)
-------------------
1.11.6 (2014-07-10)
-------------------
1.11.5 (2014-06-24)
-------------------
* convert to use console bridge from upstream debian package (`ros/rosdistro#4633 <https://github.com/ros/rosdistro/issues/4633>`_)
1.11.4 (2014-06-16)
-------------------
1.11.3 (2014-05-21)
-------------------
1.11.2 (2014-05-08)
-------------------
1.11.1 (2014-05-07)
-------------------
* add lz4 compression to rosbag (Python and C++) (`#356 <https://github.com/ros/ros_comm/issues/356>`_)
* move rosbag dox to rosbag_storage (`#389 <https://github.com/ros/ros_comm/issues/389>`_)
1.11.0 (2014-03-04)
-------------------
1.10.0 (2014-02-11)
-------------------
* remove use of __connection header
1.9.54 (2014-01-27)
-------------------
1.9.53 (2014-01-14)
-------------------
1.9.52 (2014-01-08)
-------------------
1.9.51 (2014-01-07)
-------------------
* move several client library independent parts from ros_comm into roscpp_core, split rosbag storage specific stuff from client library usage (`#299 <https://github.com/ros/ros_comm/issues/299>`_)

52
thirdparty/ros/ros_comm/tools/rosbag_storage/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,52 @@
cmake_minimum_required(VERSION 2.8.3)
project(rosbag_storage)
if(NOT WIN32)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
endif()
find_package(console_bridge REQUIRED)
find_package(catkin REQUIRED COMPONENTS cpp_common roscpp_serialization roscpp_traits rostime roslz4)
find_package(Boost REQUIRED COMPONENTS date_time filesystem program_options regex)
find_package(BZip2 REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES rosbag_storage
CATKIN_DEPENDS roslz4
DEPENDS console_bridge Boost
)
# Support large bags (>2GB) on 32-bit systems
add_definitions(-D_FILE_OFFSET_BITS=64)
include_directories(include ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${BZIP2_INCLUDE_DIR})
add_definitions(${BZIP2_DEFINITIONS})
add_library(rosbag_storage
src/bag.cpp
src/bag_player.cpp
src/buffer.cpp
src/bz2_stream.cpp
src/lz4_stream.cpp
src/chunked_file.cpp
src/message_instance.cpp
src/query.cpp
src/stream.cpp
src/view.cpp
src/uncompressed_stream.cpp
)
target_link_libraries(rosbag_storage ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BZIP2_LIBRARIES} ${console_bridge_LIBRARIES})
install(TARGETS rosbag_storage
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)

638
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/bag.h vendored Normal file
View File

@@ -0,0 +1,638 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSBAG_BAG_H
#define ROSBAG_BAG_H
#include "rosbag/macros.h"
#include "rosbag/buffer.h"
#include "rosbag/chunked_file.h"
#include "rosbag/constants.h"
#include "rosbag/exceptions.h"
#include "rosbag/structures.h"
#include "ros/header.h"
#include "ros/time.h"
#include "ros/message_traits.h"
#include "ros/message_event.h"
#include "ros/serialization.h"
//#include "ros/subscription_callback_helper.h"
#include <ios>
#include <map>
#include <queue>
#include <set>
#include <stdexcept>
#include <boost/format.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include "console_bridge/console.h"
// Remove this include when no longer supporting platforms with libconsole-bridge-dev < 0.3.0,
// in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
#include "rosbag/console_bridge_compatibility.h"
namespace rosbag {
namespace bagmode
{
//! The possible modes to open a bag in
enum BagMode
{
Write = 1,
Read = 2,
Append = 4
};
}
typedef bagmode::BagMode BagMode;
class MessageInstance;
class View;
class Query;
class ROSBAG_DECL Bag
{
friend class MessageInstance;
friend class View;
public:
Bag();
//! Open a bag file
/*!
* \param filename The bag file to open
* \param mode The mode to use (either read, write or append)
*
* Can throw BagException
*/
explicit Bag(std::string const& filename, uint32_t mode = bagmode::Read);
~Bag();
//! Open a bag file.
/*!
* \param filename The bag file to open
* \param mode The mode to use (either read, write or append)
*
* Can throw BagException
*/
void open(std::string const& filename, uint32_t mode = bagmode::Read);
//! Close the bag file
void close();
std::string getFileName() const; //!< Get the filename of the bag
BagMode getMode() const; //!< Get the mode the bag is in
uint32_t getMajorVersion() const; //!< Get the major-version of the open bag file
uint32_t getMinorVersion() const; //!< Get the minor-version of the open bag file
uint64_t getSize() const; //!< Get the current size of the bag file (a lower bound)
void setCompression(CompressionType compression); //!< Set the compression method to use for writing chunks
CompressionType getCompression() const; //!< Get the compression method to use for writing chunks
void setChunkThreshold(uint32_t chunk_threshold); //!< Set the threshold for creating new chunks
uint32_t getChunkThreshold() const; //!< Get the threshold for creating new chunks
//! Write a message into the bag file
/*!
* \param topic The topic name
* \param event The message event to be added
*
* Can throw BagIOException
*/
template<class T>
void write(std::string const& topic, ros::MessageEvent<T> const& event);
//! Write a message into the bag file
/*!
* \param topic The topic name
* \param time Timestamp of the message
* \param msg The message to be added
* \param connection_header A connection header.
*
* Can throw BagIOException
*/
template<class T>
void write(std::string const& topic, ros::Time const& time, T const& msg,
boost::shared_ptr<ros::M_string> connection_header = boost::shared_ptr<ros::M_string>());
//! Write a message into the bag file
/*!
* \param topic The topic name
* \param time Timestamp of the message
* \param msg The message to be added
* \param connection_header A connection header.
*
* Can throw BagIOException
*/
template<class T>
void write(std::string const& topic, ros::Time const& time, boost::shared_ptr<T const> const& msg,
boost::shared_ptr<ros::M_string> connection_header = boost::shared_ptr<ros::M_string>());
//! Write a message into the bag file
/*!
* \param topic The topic name
* \param time Timestamp of the message
* \param msg The message to be added
* \param connection_header A connection header.
*
* Can throw BagIOException
*/
template<class T>
void write(std::string const& topic, ros::Time const& time, boost::shared_ptr<T> const& msg,
boost::shared_ptr<ros::M_string> connection_header = boost::shared_ptr<ros::M_string>());
void swap(Bag&);
private:
Bag(const Bag&);
Bag& operator=(const Bag&);
// This helper function actually does the write with an arbitrary serializable message
template<class T>
void doWrite(std::string const& topic, ros::Time const& time, T const& msg, boost::shared_ptr<ros::M_string> const& connection_header);
void openRead (std::string const& filename);
void openWrite (std::string const& filename);
void openAppend(std::string const& filename);
void closeWrite();
template<class T>
boost::shared_ptr<T> instantiateBuffer(IndexEntry const& index_entry) const; //!< deserializes the message held in record_buffer_
void startWriting();
void stopWriting();
void startReadingVersion102();
void startReadingVersion200();
// Writing
void writeVersion();
void writeFileHeaderRecord();
void writeConnectionRecord(ConnectionInfo const* connection_info);
void appendConnectionRecordToBuffer(Buffer& buf, ConnectionInfo const* connection_info);
template<class T>
void writeMessageDataRecord(uint32_t conn_id, ros::Time const& time, T const& msg);
void writeIndexRecords();
void writeConnectionRecords();
void writeChunkInfoRecords();
void startWritingChunk(ros::Time time);
void writeChunkHeader(CompressionType compression, uint32_t compressed_size, uint32_t uncompressed_size);
void stopWritingChunk();
// Reading
void readVersion();
void readFileHeaderRecord();
void readConnectionRecord();
void readChunkHeader(ChunkHeader& chunk_header) const;
void readChunkInfoRecord();
void readConnectionIndexRecord200();
void readTopicIndexRecord102();
void readMessageDefinitionRecord102();
void readMessageDataRecord102(uint64_t offset, ros::Header& header) const;
ros::Header readMessageDataHeader(IndexEntry const& index_entry);
uint32_t readMessageDataSize(IndexEntry const& index_entry) const;
template<typename Stream>
void readMessageDataIntoStream(IndexEntry const& index_entry, Stream& stream) const;
void decompressChunk(uint64_t chunk_pos) const;
void decompressRawChunk(ChunkHeader const& chunk_header) const;
void decompressBz2Chunk(ChunkHeader const& chunk_header) const;
void decompressLz4Chunk(ChunkHeader const& chunk_header) const;
uint32_t getChunkOffset() const;
// Record header I/O
void writeHeader(ros::M_string const& fields);
void writeDataLength(uint32_t data_len);
void appendHeaderToBuffer(Buffer& buf, ros::M_string const& fields);
void appendDataLengthToBuffer(Buffer& buf, uint32_t data_len);
void readHeaderFromBuffer(Buffer& buffer, uint32_t offset, ros::Header& header, uint32_t& data_size, uint32_t& bytes_read) const;
void readMessageDataHeaderFromBuffer(Buffer& buffer, uint32_t offset, ros::Header& header, uint32_t& data_size, uint32_t& bytes_read) const;
bool readHeader(ros::Header& header) const;
bool readDataLength(uint32_t& data_size) const;
bool isOp(ros::M_string& fields, uint8_t reqOp) const;
// Header fields
template<typename T>
std::string toHeaderString(T const* field) const;
std::string toHeaderString(ros::Time const* field) const;
template<typename T>
bool readField(ros::M_string const& fields, std::string const& field_name, bool required, T* data) const;
bool readField(ros::M_string const& fields, std::string const& field_name, unsigned int min_len, unsigned int max_len, bool required, std::string& data) const;
bool readField(ros::M_string const& fields, std::string const& field_name, bool required, std::string& data) const;
bool readField(ros::M_string const& fields, std::string const& field_name, bool required, ros::Time& data) const;
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;
// Low-level I/O
void write(char const* s, std::streamsize n);
void write(std::string const& s);
void read(char* b, std::streamsize n) const;
void seek(uint64_t pos, int origin = std::ios_base::beg) const;
private:
BagMode mode_;
mutable ChunkedFile file_;
int version_;
CompressionType compression_;
uint32_t chunk_threshold_;
uint32_t bag_revision_;
uint64_t file_size_;
uint64_t file_header_pos_;
uint64_t index_data_pos_;
uint32_t connection_count_;
uint32_t chunk_count_;
// Current chunk
bool chunk_open_;
ChunkInfo curr_chunk_info_;
uint64_t curr_chunk_data_pos_;
std::map<std::string, uint32_t> topic_connection_ids_;
std::map<ros::M_string, uint32_t> header_connection_ids_;
std::map<uint32_t, ConnectionInfo*> connections_;
std::vector<ChunkInfo> chunks_;
std::map<uint32_t, std::multiset<IndexEntry> > connection_indexes_;
std::map<uint32_t, std::multiset<IndexEntry> > curr_chunk_connection_indexes_;
mutable Buffer header_buffer_; //!< reusable buffer in which to assemble the record header before writing to file
mutable Buffer record_buffer_; //!< reusable buffer in which to assemble the record data before writing to file
mutable Buffer chunk_buffer_; //!< reusable buffer to read chunk into
mutable Buffer decompress_buffer_; //!< reusable buffer to decompress chunks into
mutable Buffer outgoing_chunk_buffer_; //!< reusable buffer to read chunk into
mutable Buffer* current_buffer_;
mutable uint64_t decompressed_chunk_; //!< position of decompressed chunk
};
} // namespace rosbag
#include "rosbag/message_instance.h"
namespace rosbag {
// Templated method definitions
template<class T>
void Bag::write(std::string const& topic, ros::MessageEvent<T> const& event) {
doWrite(topic, event.getReceiptTime(), *event.getMessage(), event.getConnectionHeaderPtr());
}
template<class T>
void Bag::write(std::string const& topic, ros::Time const& time, T const& msg, boost::shared_ptr<ros::M_string> connection_header) {
doWrite(topic, time, msg, connection_header);
}
template<class T>
void Bag::write(std::string const& topic, ros::Time const& time, boost::shared_ptr<T const> const& msg, boost::shared_ptr<ros::M_string> connection_header) {
doWrite(topic, time, *msg, connection_header);
}
template<class T>
void Bag::write(std::string const& topic, ros::Time const& time, boost::shared_ptr<T> const& msg, boost::shared_ptr<ros::M_string> connection_header) {
doWrite(topic, time, *msg, connection_header);
}
template<typename T>
std::string Bag::toHeaderString(T const* field) const {
return std::string((char*) field, sizeof(T));
}
template<typename T>
bool Bag::readField(ros::M_string const& fields, std::string const& field_name, bool required, T* data) const {
ros::M_string::const_iterator i = checkField(fields, field_name, sizeof(T), sizeof(T), required);
if (i == fields.end())
return false;
memcpy(data, i->second.data(), sizeof(T));
return true;
}
template<typename Stream>
void Bag::readMessageDataIntoStream(IndexEntry const& index_entry, Stream& stream) const {
ros::Header header;
uint32_t data_size;
uint32_t bytes_read;
switch (version_)
{
case 200:
{
decompressChunk(index_entry.chunk_pos);
readMessageDataHeaderFromBuffer(*current_buffer_, index_entry.offset, header, data_size, bytes_read);
if (data_size > 0)
memcpy(stream.advance(data_size), current_buffer_->getData() + index_entry.offset + bytes_read, data_size);
break;
}
case 102:
{
readMessageDataRecord102(index_entry.chunk_pos, header);
data_size = record_buffer_.getSize();
if (data_size > 0)
memcpy(stream.advance(data_size), record_buffer_.getData(), data_size);
break;
}
default:
throw BagFormatException((boost::format("Unhandled version: %1%") % version_).str());
}
}
template<class T>
boost::shared_ptr<T> Bag::instantiateBuffer(IndexEntry const& index_entry) const {
switch (version_)
{
case 200:
{
decompressChunk(index_entry.chunk_pos);
// Read the message header
ros::Header header;
uint32_t data_size;
uint32_t bytes_read;
readMessageDataHeaderFromBuffer(*current_buffer_, index_entry.offset, header, data_size, bytes_read);
// Read the connection id from the header
uint32_t connection_id;
readField(*header.getValues(), CONNECTION_FIELD_NAME, true, &connection_id);
std::map<uint32_t, ConnectionInfo*>::const_iterator connection_iter = connections_.find(connection_id);
if (connection_iter == connections_.end())
throw BagFormatException((boost::format("Unknown connection ID: %1%") % connection_id).str());
ConnectionInfo* connection_info = connection_iter->second;
boost::shared_ptr<T> p = boost::make_shared<T>();
ros::serialization::PreDeserializeParams<T> predes_params;
predes_params.message = p;
predes_params.connection_header = connection_info->header;
ros::serialization::PreDeserialize<T>::notify(predes_params);
// Deserialize the message
ros::serialization::IStream s(current_buffer_->getData() + index_entry.offset + bytes_read, data_size);
ros::serialization::deserialize(s, *p);
return p;
}
case 102:
{
// Read the message record
ros::Header header;
readMessageDataRecord102(index_entry.chunk_pos, header);
ros::M_string& fields = *header.getValues();
// Read the connection id from the header
std::string topic, latching("0"), callerid;
readField(fields, TOPIC_FIELD_NAME, true, topic);
readField(fields, LATCHING_FIELD_NAME, false, latching);
readField(fields, CALLERID_FIELD_NAME, false, callerid);
std::map<std::string, uint32_t>::const_iterator topic_conn_id_iter = topic_connection_ids_.find(topic);
if (topic_conn_id_iter == topic_connection_ids_.end())
throw BagFormatException((boost::format("Unknown topic: %1%") % topic).str());
uint32_t connection_id = topic_conn_id_iter->second;
std::map<uint32_t, ConnectionInfo*>::const_iterator connection_iter = connections_.find(connection_id);
if (connection_iter == connections_.end())
throw BagFormatException((boost::format("Unknown connection ID: %1%") % connection_id).str());
ConnectionInfo* connection_info = connection_iter->second;
boost::shared_ptr<T> p = boost::make_shared<T>();
// Create a new connection header, updated with the latching and callerid values
boost::shared_ptr<ros::M_string> message_header(boost::make_shared<ros::M_string>());
for (ros::M_string::const_iterator i = connection_info->header->begin(); i != connection_info->header->end(); i++)
(*message_header)[i->first] = i->second;
(*message_header)["latching"] = latching;
(*message_header)["callerid"] = callerid;
ros::serialization::PreDeserializeParams<T> predes_params;
predes_params.message = p;
predes_params.connection_header = message_header;
ros::serialization::PreDeserialize<T>::notify(predes_params);
// Deserialize the message
ros::serialization::IStream s(record_buffer_.getData(), record_buffer_.getSize());
ros::serialization::deserialize(s, *p);
return p;
}
default:
throw BagFormatException((boost::format("Unhandled version: %1%") % version_).str());
}
}
template<class T>
void Bag::doWrite(std::string const& topic, ros::Time const& time, T const& msg, boost::shared_ptr<ros::M_string> const& connection_header) {
if (time < ros::TIME_MIN)
{
throw BagException("Tried to insert a message with time less than ros::TIME_MIN");
}
// Whenever we write we increment our revision
bag_revision_++;
// Get ID for connection header
ConnectionInfo* connection_info = NULL;
uint32_t conn_id = 0;
if (!connection_header) {
// No connection header: we'll manufacture one, and store by topic
std::map<std::string, uint32_t>::iterator topic_connection_ids_iter = topic_connection_ids_.find(topic);
if (topic_connection_ids_iter == topic_connection_ids_.end()) {
conn_id = connections_.size();
topic_connection_ids_[topic] = conn_id;
}
else {
conn_id = topic_connection_ids_iter->second;
connection_info = connections_[conn_id];
}
}
else {
// Store the connection info by the address of the connection header
// Add the topic name to the connection header, so that when we later search by
// connection header, we can disambiguate connections that differ only by topic name (i.e.,
// same callerid, same message type), #3755. This modified connection header is only used
// for our bookkeeping, and will not appear in the resulting .bag.
ros::M_string connection_header_copy(*connection_header);
connection_header_copy["topic"] = topic;
std::map<ros::M_string, uint32_t>::iterator header_connection_ids_iter = header_connection_ids_.find(connection_header_copy);
if (header_connection_ids_iter == header_connection_ids_.end()) {
conn_id = connections_.size();
header_connection_ids_[connection_header_copy] = conn_id;
}
else {
conn_id = header_connection_ids_iter->second;
connection_info = connections_[conn_id];
}
}
{
// Seek to the end of the file (needed in case previous operation was a read)
seek(0, std::ios::end);
file_size_ = file_.getOffset();
// Write the chunk header if we're starting a new chunk
if (!chunk_open_)
startWritingChunk(time);
// Write connection info record, if necessary
if (connection_info == NULL) {
connection_info = new ConnectionInfo();
connection_info->id = conn_id;
connection_info->topic = topic;
connection_info->datatype = std::string(ros::message_traits::datatype(msg));
connection_info->md5sum = std::string(ros::message_traits::md5sum(msg));
connection_info->msg_def = std::string(ros::message_traits::definition(msg));
if (connection_header != NULL) {
connection_info->header = connection_header;
}
else {
connection_info->header = boost::make_shared<ros::M_string>();
(*connection_info->header)["type"] = connection_info->datatype;
(*connection_info->header)["md5sum"] = connection_info->md5sum;
(*connection_info->header)["message_definition"] = connection_info->msg_def;
}
connections_[conn_id] = connection_info;
writeConnectionRecord(connection_info);
appendConnectionRecordToBuffer(outgoing_chunk_buffer_, connection_info);
}
// Add to topic indexes
IndexEntry index_entry;
index_entry.time = time;
index_entry.chunk_pos = curr_chunk_info_.pos;
index_entry.offset = getChunkOffset();
std::multiset<IndexEntry>& chunk_connection_index = curr_chunk_connection_indexes_[connection_info->id];
chunk_connection_index.insert(chunk_connection_index.end(), index_entry);
std::multiset<IndexEntry>& connection_index = connection_indexes_[connection_info->id];
connection_index.insert(connection_index.end(), index_entry);
// Increment the connection count
curr_chunk_info_.connection_counts[connection_info->id]++;
// Write the message data
writeMessageDataRecord(conn_id, time, msg);
// Check if we want to stop this chunk
uint32_t chunk_size = getChunkOffset();
CONSOLE_BRIDGE_logDebug(" curr_chunk_size=%d (threshold=%d)", chunk_size, chunk_threshold_);
if (chunk_size > chunk_threshold_) {
// Empty the outgoing chunk
stopWritingChunk();
outgoing_chunk_buffer_.setSize(0);
// We no longer have a valid curr_chunk_info
curr_chunk_info_.pos = -1;
}
}
}
template<class T>
void Bag::writeMessageDataRecord(uint32_t conn_id, ros::Time const& time, T const& msg) {
ros::M_string header;
header[OP_FIELD_NAME] = toHeaderString(&OP_MSG_DATA);
header[CONNECTION_FIELD_NAME] = toHeaderString(&conn_id);
header[TIME_FIELD_NAME] = toHeaderString(&time);
// Assemble message in memory first, because we need to write its length
uint32_t msg_ser_len = ros::serialization::serializationLength(msg);
record_buffer_.setSize(msg_ser_len);
ros::serialization::OStream s(record_buffer_.getData(), msg_ser_len);
// todo: serialize into the outgoing_chunk_buffer & remove record_buffer_
ros::serialization::serialize(s, msg);
// We do an extra seek here since writing our data record may
// have indirectly moved our file-pointer if it was a
// MessageInstance for our own bag
seek(0, std::ios::end);
file_size_ = file_.getOffset();
CONSOLE_BRIDGE_logDebug("Writing MSG_DATA [%llu:%d]: conn=%d sec=%d nsec=%d data_len=%d",
(unsigned long long) file_.getOffset(), getChunkOffset(), conn_id, time.sec, time.nsec, msg_ser_len);
writeHeader(header);
writeDataLength(msg_ser_len);
write((char*) record_buffer_.getData(), msg_ser_len);
// todo: use better abstraction than appendHeaderToBuffer
appendHeaderToBuffer(outgoing_chunk_buffer_, header);
appendDataLengthToBuffer(outgoing_chunk_buffer_, msg_ser_len);
uint32_t offset = outgoing_chunk_buffer_.getSize();
outgoing_chunk_buffer_.setSize(outgoing_chunk_buffer_.getSize() + msg_ser_len);
memcpy(outgoing_chunk_buffer_.getData() + offset, record_buffer_.getData(), msg_ser_len);
// Update the current chunk time range
if (time > curr_chunk_info_.end_time)
curr_chunk_info_.end_time = time;
else if (time < curr_chunk_info_.start_time)
curr_chunk_info_.start_time = time;
}
inline void swap(Bag& a, Bag& b) {
a.swap(b);
}
} // namespace rosbag
#endif

135
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/bag_player.h vendored Normal file
View File

@@ -0,0 +1,135 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSBAG_BAG_PLAYER_H
#define ROSBAG_BAG_PLAYER_H
#include <boost/foreach.hpp>
#include "rosbag/bag.h"
#include "rosbag/view.h"
namespace rosbag
{
// A helper struct
struct BagCallback
{
virtual ~BagCallback() {};
virtual void call(MessageInstance m) = 0;
};
// A helper class for the callbacks
template<class T>
class BagCallbackT : public BagCallback
{
public:
typedef boost::function<void (const boost::shared_ptr<const T>&)> Callback;
BagCallbackT(Callback cb) :
cb_(cb)
{}
void call(MessageInstance m) {
cb_(m.instantiate<T>());
}
private:
Callback cb_;
};
/* A class for playing back bag files at an API level. It supports
relatime, as well as accelerated and slowed playback. */
class BagPlayer
{
public:
/* Constructor expecting the filename of a bag */
BagPlayer(const std::string &filename);
/* Register a callback for a specific topic and type */
template<class T>
void register_callback(const std::string &topic,
typename BagCallbackT<T>::Callback f);
/* Unregister a callback for a topic already registered */
void unregister_callback(const std::string &topic);
/* Set the time in the bag to start.
* Default is the first message */
void set_start(const ros::Time &start);
/* Set the time in the bag to stop.
* Default is the last message */
void set_end(const ros::Time &end);
/* Set the speed to playback. 1.0 is the default.
* 2.0 would be twice as fast, 0.5 is half realtime. */
void set_playback_speed(double scale);
/* Start playback of the bag file using the parameters previously
set */
void start_play();
/* Get the current time of the playback */
ros::Time get_time();
// Destructor
virtual ~BagPlayer();
// The bag file interface loaded in the constructor.
Bag bag;
private:
ros::Time real_time(const ros::Time &msg_time);
std::map<std::string, BagCallback *> cbs_;
ros::Time bag_start_;
ros::Time bag_end_;
ros::Time last_message_time_;
double playback_speed_;
ros::Time play_start_;
};
template<class T>
void BagPlayer::register_callback(const std::string &topic,
typename BagCallbackT<T>::Callback cb) {
cbs_[topic] = new BagCallbackT<T>(cb);
}
}
#endif

73
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/buffer.h vendored Normal file
View File

@@ -0,0 +1,73 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#ifndef ROSBAG_BUFFER_H
#define ROSBAG_BUFFER_H
#include <stdint.h>
#include "macros.h"
namespace rosbag {
class ROSBAG_DECL Buffer
{
public:
Buffer();
~Buffer();
uint8_t* getData();
uint32_t getCapacity() const;
uint32_t getSize() const;
void setSize(uint32_t size);
void swap(Buffer& other);
private:
Buffer(const Buffer&);
Buffer& operator=(const Buffer&);
void ensureCapacity(uint32_t capacity);
private:
uint8_t* buffer_;
uint32_t capacity_;
uint32_t size_;
};
inline void swap(Buffer& a, Buffer& b) {
a.swap(b);
}
} // namespace rosbag
#endif

111
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/chunked_file.h vendored Normal file
View File

@@ -0,0 +1,111 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#ifndef ROSBAG_CHUNKED_FILE_H
#define ROSBAG_CHUNKED_FILE_H
#include <ios>
#include <stdint.h>
#include <string>
#include "macros.h"
#include <boost/shared_ptr.hpp>
#include <bzlib.h>
#include "rosbag/stream.h"
namespace rosbag {
//! ChunkedFile reads and writes files which contain interleaved chunks of compressed and uncompressed data.
class ROSBAG_DECL ChunkedFile
{
friend class Stream;
public:
ChunkedFile();
~ChunkedFile();
void openWrite (std::string const& filename); //!< open file for writing
void openRead (std::string const& filename); //!< open file for reading
void openReadWrite(std::string const& filename); //!< open file for reading & writing
void close(); //!< close the file
std::string getFileName() const; //!< return path of currently open file
uint64_t getOffset() const; //!< return current offset from the beginning of the file
uint32_t getCompressedBytesIn() const; //!< return the number of bytes written to current compressed stream
bool isOpen() const; //!< return true if file is open for reading or writing
bool good() const; //!< return true if hasn't reached end-of-file and no error
void setReadMode(CompressionType type);
void setWriteMode(CompressionType type);
// File I/O
void write(std::string const& s);
void write(void* ptr, size_t size); //!< write size bytes from ptr to the file
void read(void* ptr, size_t size); //!< read size bytes from the file into ptr
std::string getline();
bool truncate(uint64_t length);
void seek(uint64_t offset, int origin = std::ios_base::beg); //!< seek to given offset from origin
void decompress(CompressionType compression, uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len);
void swap(ChunkedFile& other);
private:
ChunkedFile(const ChunkedFile&);
ChunkedFile& operator=(const ChunkedFile&);
void open(std::string const& filename, std::string const& mode);
void clearUnused();
private:
std::string filename_; //!< path to file
FILE* file_; //!< file pointer
uint64_t offset_; //!< current position in the file
uint64_t compressed_in_; //!< number of bytes written to current compressed stream
char* unused_; //!< extra data read by compressed stream
int nUnused_; //!< number of bytes of extra data read by compressed stream
boost::shared_ptr<StreamFactory> stream_factory_;
boost::shared_ptr<Stream> read_stream_;
boost::shared_ptr<Stream> write_stream_;
};
inline void swap(ChunkedFile& a, ChunkedFile& b) {
a.swap(b);
}
} // namespace rosbag
#endif

62
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/console_bridge_compatibility.h vendored Normal file
View File

@@ -0,0 +1,62 @@
/*
* Copyright (c) 2017, Open Source Robotics Foundation, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Open Source Robotics Foundation, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROSBAG__CONSOLE_BRIDGE_COMPATIBILITY_H_
#define ROSBAG__CONSOLE_BRIDGE_COMPATIBILITY_H_
#include <console_bridge/console.h>
// Remove this file and it's inclusions when no longer supporting platforms with
// libconsole-bridge-dev < 0.3.0,
// in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
#ifndef CONSOLE_BRIDGE_logError
# define CONSOLE_BRIDGE_logError(fmt, ...) \
console_bridge::log( \
__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, fmt, ##__VA_ARGS__)
#endif
#ifndef CONSOLE_BRIDGE_logWarn
# define CONSOLE_BRIDGE_logWarn(fmt, ...) \
console_bridge::log( \
__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, fmt, ##__VA_ARGS__)
#endif
#ifndef CONSOLE_BRIDGE_logInform
# define CONSOLE_BRIDGE_logInform(fmt, ...) \
console_bridge::log( \
__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, fmt, ##__VA_ARGS__)
#endif
#ifndef CONSOLE_BRIDGE_logDebug
# define CONSOLE_BRIDGE_logDebug(fmt, ...) \
console_bridge::log( \
__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, fmt, ##__VA_ARGS__)
#endif
#endif // ROSBAG__CONSOLE_BRIDGE_COMPATIBILITY_H_

101
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/constants.h vendored Normal file
View File

@@ -0,0 +1,101 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSBAG_CONSTANTS_H
#define ROSBAG_CONSTANTS_H
#include <string>
#include <stdint.h>
namespace rosbag {
// Bag file version to write
static const std::string VERSION = "2.0";
// Header field delimiter
static const unsigned char FIELD_DELIM = '=';
// Current header fields
static const std::string OP_FIELD_NAME = "op";
static const std::string TOPIC_FIELD_NAME = "topic";
static const std::string VER_FIELD_NAME = "ver";
static const std::string COUNT_FIELD_NAME = "count";
static const std::string INDEX_POS_FIELD_NAME = "index_pos"; // 1.2+
static const std::string CONNECTION_COUNT_FIELD_NAME = "conn_count"; // 2.0+
static const std::string CHUNK_COUNT_FIELD_NAME = "chunk_count"; // 2.0+
static const std::string CONNECTION_FIELD_NAME = "conn"; // 2.0+
static const std::string COMPRESSION_FIELD_NAME = "compression"; // 2.0+
static const std::string SIZE_FIELD_NAME = "size"; // 2.0+
static const std::string TIME_FIELD_NAME = "time"; // 2.0+
static const std::string START_TIME_FIELD_NAME = "start_time"; // 2.0+
static const std::string END_TIME_FIELD_NAME = "end_time"; // 2.0+
static const std::string CHUNK_POS_FIELD_NAME = "chunk_pos"; // 2.0+
// Legacy header fields
static const std::string MD5_FIELD_NAME = "md5"; // <2.0
static const std::string TYPE_FIELD_NAME = "type"; // <2.0
static const std::string DEF_FIELD_NAME = "def"; // <2.0
static const std::string SEC_FIELD_NAME = "sec"; // <2.0
static const std::string NSEC_FIELD_NAME = "nsec"; // <2.0
static const std::string LATCHING_FIELD_NAME = "latching"; // <2.0
static const std::string CALLERID_FIELD_NAME = "callerid"; // <2.0
// Current "op" field values
static const unsigned char OP_MSG_DATA = 0x02;
static const unsigned char OP_FILE_HEADER = 0x03;
static const unsigned char OP_INDEX_DATA = 0x04;
static const unsigned char OP_CHUNK = 0x05;
static const unsigned char OP_CHUNK_INFO = 0x06;
static const unsigned char OP_CONNECTION = 0x07;
// Legacy "op" field values
static const unsigned char OP_MSG_DEF = 0x01;
// Bytes reserved for file header record (4KB)
static const uint32_t FILE_HEADER_LENGTH = 4 * 1024;
// Index data record version to write
static const uint32_t INDEX_VERSION = 1;
// Chunk info record version to write
static const uint32_t CHUNK_INFO_VERSION = 1;
// Compression types
static const std::string COMPRESSION_NONE = "none";
static const std::string COMPRESSION_BZ2 = "bz2";
static const std::string COMPRESSION_LZ4 = "lz4";
} // namespace rosbag
#endif

72
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/exceptions.h vendored Normal file
View File

@@ -0,0 +1,72 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSBAG_EXCEPTIONS_H
#define ROSBAG_EXCEPTIONS_H
#include <ros/exception.h>
namespace rosbag {
//! Base class for rosbag exceptions
class BagException : public ros::Exception
{
public:
BagException(std::string const& msg) : ros::Exception(msg) { }
};
//! Exception thrown when on IO problems
class BagIOException : public BagException
{
public:
BagIOException(std::string const& msg) : BagException(msg) { }
};
//! Exception thrown on problems reading the bag format
class BagFormatException : public BagException
{
public:
BagFormatException(std::string const& msg) : BagException(msg) { }
};
//! Exception thrown on problems reading the bag index
class BagUnindexedException : public BagException
{
public:
BagUnindexedException() : BagException("Bag unindexed") { }
};
} // namespace rosbag
#endif

45
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/macros.h vendored Normal file
View File

@@ -0,0 +1,45 @@
/*
* Copyright (C) 2008, Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROSBAG_MACROS_H_
#define ROSBAG_MACROS_H_
#include <ros/macros.h> // for the DECL's
// Import/export for windows dll's and visibility for gcc shared libraries.
#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
#ifdef rosbag_EXPORTS // we are building a shared lib/dll
#define ROSBAG_DECL ROS_HELPER_EXPORT
#else // we are using shared lib/dll
#define ROSBAG_DECL ROS_HELPER_IMPORT
#endif
#else // ros is being built around static libraries
#define ROSBAG_DECL
#endif
#endif /* ROSBAG_MACROS_H_ */

175
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/message_instance.h vendored Normal file
View File

@@ -0,0 +1,175 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSBAG_MESSAGE_INSTANCE_H
#define ROSBAG_MESSAGE_INSTANCE_H
#include <ros/message_traits.h>
#include <ros/serialization.h>
//#include <ros/ros.h>
#include <ros/time.h>
#include "rosbag/structures.h"
#include "rosbag/macros.h"
namespace rosbag {
class Bag;
//! A class pointing into a bag file
/*!
* The MessageInstance class itself is fairly light weight. It
* simply contains a pointer to a bag-file and the index_entry
* necessary to get access to the corresponding data.
*
* It adheres to the necessary ros::message_traits to be directly
* serializable.
*/
class ROSBAG_DECL MessageInstance
{
friend class View;
public:
ros::Time const& getTime() const;
std::string const& getTopic() const;
std::string const& getDataType() const;
std::string const& getMD5Sum() const;
std::string const& getMessageDefinition() const;
boost::shared_ptr<ros::M_string> getConnectionHeader() const;
std::string getCallerId() const;
bool isLatching() const;
//! Test whether the underlying message of the specified type.
/*!
* returns true iff the message is of the template type
*/
template<class T>
bool isType() const;
//! Templated call to instantiate a message
/*!
* returns NULL pointer if incompatible
*/
template<class T>
boost::shared_ptr<T> instantiate() const;
//! Write serialized message contents out to a stream
template<typename Stream>
void write(Stream& stream) const;
//! Size of serialized message
uint32_t size() const;
private:
MessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index, Bag const& bag);
ConnectionInfo const* connection_info_;
IndexEntry const index_entry_;
Bag const* bag_;
};
} // namespace rosbag
namespace ros {
namespace message_traits {
template<>
struct MD5Sum<rosbag::MessageInstance>
{
static const char* value(const rosbag::MessageInstance& m) { return m.getMD5Sum().c_str(); }
};
template<>
struct DataType<rosbag::MessageInstance>
{
static const char* value(const rosbag::MessageInstance& m) { return m.getDataType().c_str(); }
};
template<>
struct Definition<rosbag::MessageInstance>
{
static const char* value(const rosbag::MessageInstance& m) { return m.getMessageDefinition().c_str(); }
};
} // namespace message_traits
namespace serialization
{
template<>
struct Serializer<rosbag::MessageInstance>
{
template<typename Stream>
inline static void write(Stream& stream, const rosbag::MessageInstance& m) {
m.write(stream);
}
inline static uint32_t serializedLength(const rosbag::MessageInstance& m) {
return m.size();
}
};
} // namespace serialization
} // namespace ros
#include "rosbag/bag.h"
namespace rosbag {
template<class T>
bool MessageInstance::isType() const {
char const* md5sum = ros::message_traits::MD5Sum<T>::value();
return md5sum == std::string("*") || md5sum == getMD5Sum();
}
template<class T>
boost::shared_ptr<T> MessageInstance::instantiate() const {
if (!isType<T>())
return boost::shared_ptr<T>();
return bag_->instantiateBuffer<T>(index_entry_);
}
template<typename Stream>
void MessageInstance::write(Stream& stream) const {
bag_->readMessageDataIntoStream(index_entry_, stream);
}
} // namespace rosbag
#endif

138
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/query.h vendored Normal file
View File

@@ -0,0 +1,138 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSBAG_QUERY_H
#define ROSBAG_QUERY_H
#include "ros/time.h"
#include <vector>
#include <map>
#include <set>
#include <boost/function.hpp>
#include "rosbag/macros.h"
#include "rosbag/structures.h"
namespace rosbag {
class Bag;
class ROSBAG_DECL Query
{
public:
//! The base query takes an optional time-range
/*!
* param start_time the beginning of the time_range for the query
* param end_time the end of the time_range for the query
*/
Query(boost::function<bool(ConnectionInfo const*)>& query,
ros::Time const& start_time = ros::TIME_MIN,
ros::Time const& end_time = ros::TIME_MAX);
boost::function<bool(ConnectionInfo const*)> const& getQuery() const; //!< Get the query functor
ros::Time const& getStartTime() const; //!< Get the start-time
ros::Time const& getEndTime() const; //!< Get the end-time
private:
boost::function<bool(ConnectionInfo const*)> query_;
ros::Time start_time_;
ros::Time end_time_;
};
class ROSBAG_DECL TopicQuery
{
public:
TopicQuery(std::string const& topic);
TopicQuery(std::vector<std::string> const& topics);
bool operator()(ConnectionInfo const*) const;
private:
std::vector<std::string> topics_;
};
class ROSBAG_DECL TypeQuery
{
public:
TypeQuery(std::string const& type);
TypeQuery(std::vector<std::string> const& types);
bool operator()(ConnectionInfo const*) const;
private:
std::vector<std::string> types_;
};
//! Pairs of queries and the bags they come from (used internally by View)
struct ROSBAG_DECL BagQuery
{
BagQuery(Bag const* _bag, Query const& _query, uint32_t _bag_revision);
Bag const* bag;
Query query;
uint32_t bag_revision;
};
struct ROSBAG_DECL MessageRange
{
MessageRange(std::multiset<IndexEntry>::const_iterator const& _begin,
std::multiset<IndexEntry>::const_iterator const& _end,
ConnectionInfo const* _connection_info,
BagQuery const* _bag_query);
std::multiset<IndexEntry>::const_iterator begin;
std::multiset<IndexEntry>::const_iterator end;
ConnectionInfo const* connection_info;
BagQuery const* bag_query; //!< pointer to vector of queries in View
};
//! The actual iterator data structure
struct ROSBAG_DECL ViewIterHelper
{
ViewIterHelper(std::multiset<IndexEntry>::const_iterator _iter, MessageRange const* _range);
std::multiset<IndexEntry>::const_iterator iter;
MessageRange const* range; //!< pointer to vector of ranges in View
};
struct ROSBAG_DECL ViewIterHelperCompare
{
bool operator()(ViewIterHelper const& a, ViewIterHelper const& b);
};
} // namespace rosbag
#endif

200
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/stream.h vendored Normal file
View File

@@ -0,0 +1,200 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#ifndef ROSBAG_STREAM_H
#define ROSBAG_STREAM_H
#include <ios>
#include <stdint.h>
#include <string>
#include <boost/shared_ptr.hpp>
#include <bzlib.h>
#include <roslz4/lz4s.h>
#include "rosbag/exceptions.h"
#include "rosbag/macros.h"
namespace rosbag {
namespace compression
{
enum CompressionType
{
Uncompressed = 0,
BZ2 = 1,
LZ4 = 2,
};
}
typedef compression::CompressionType CompressionType;
class ChunkedFile;
class FileAccessor;
class ROSBAG_DECL Stream
{
friend class FileAccessor;
public:
Stream(ChunkedFile* file);
virtual ~Stream();
virtual CompressionType getCompressionType() const = 0;
virtual void write(void* ptr, size_t size) = 0;
virtual void read (void* ptr, size_t size) = 0;
virtual void decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) = 0;
virtual void startWrite();
virtual void stopWrite();
virtual void startRead();
virtual void stopRead();
protected:
FILE* getFilePointer();
uint64_t getCompressedIn();
void setCompressedIn(uint64_t nbytes);
void advanceOffset(uint64_t nbytes);
char* getUnused();
int getUnusedLength();
void setUnused(char* unused);
void setUnusedLength(int nUnused);
void clearUnused();
protected:
ChunkedFile* file_;
};
class ROSBAG_DECL StreamFactory
{
public:
StreamFactory(ChunkedFile* file);
boost::shared_ptr<Stream> getStream(CompressionType type) const;
private:
boost::shared_ptr<Stream> uncompressed_stream_;
boost::shared_ptr<Stream> bz2_stream_;
boost::shared_ptr<Stream> lz4_stream_;
};
class FileAccessor {
friend class ChunkedFile;
static void setFile(Stream& a, ChunkedFile* file) {
a.file_ = file;
}
};
class ROSBAG_DECL UncompressedStream : public Stream
{
public:
UncompressedStream(ChunkedFile* file);
CompressionType getCompressionType() const;
void write(void* ptr, size_t size);
void read(void* ptr, size_t size);
void decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len);
};
/*!
* BZ2Stream uses libbzip2 (http://www.bzip.org) for reading/writing compressed data in the BZ2 format.
*/
class ROSBAG_DECL BZ2Stream : public Stream
{
public:
BZ2Stream(ChunkedFile* file);
CompressionType getCompressionType() const;
void startWrite();
void write(void* ptr, size_t size);
void stopWrite();
void startRead();
void read(void* ptr, size_t size);
void stopRead();
void decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len);
private:
int verbosity_; //!< level of debugging output (0-4; 0 default). 0 is silent, 4 is max verbose debugging output
int block_size_100k_; //!< compression block size (1-9; 9 default). 9 is best compression, most memory
int work_factor_; //!< compression behavior for worst case, highly repetitive data (0-250; 30 default)
BZFILE* bzfile_; //!< bzlib compressed file stream
int bzerror_; //!< last error from bzlib
};
// LZ4Stream reads/writes compressed datat in the LZ4 format
// https://code.google.com/p/lz4/
class ROSBAG_DECL LZ4Stream : public Stream
{
public:
LZ4Stream(ChunkedFile* file);
~LZ4Stream();
CompressionType getCompressionType() const;
void startWrite();
void write(void* ptr, size_t size);
void stopWrite();
void startRead();
void read(void* ptr, size_t size);
void stopRead();
void decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len);
private:
LZ4Stream(const LZ4Stream&);
LZ4Stream operator=(const LZ4Stream&);
void writeStream(int action);
char *buff_;
int buff_size_;
int block_size_id_;
roslz4_stream lz4s_;
};
} // namespace rosbag
#endif

93
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/structures.h vendored Normal file
View File

@@ -0,0 +1,93 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSBAG_STRUCTURES_H
#define ROSBAG_STRUCTURES_H
#include <map>
#include <vector>
#include "ros/time.h"
#include "ros/datatypes.h"
#include "macros.h"
namespace rosbag {
struct ROSBAG_DECL ConnectionInfo
{
ConnectionInfo() : id(-1) { }
uint32_t id;
std::string topic;
std::string datatype;
std::string md5sum;
std::string msg_def;
boost::shared_ptr<ros::M_string> header;
};
struct ChunkInfo
{
ros::Time start_time; //! earliest timestamp of a message in the chunk
ros::Time end_time; //! latest timestamp of a message in the chunk
uint64_t pos; //! absolute byte offset of chunk record in bag file
std::map<uint32_t, uint32_t> connection_counts; //! number of messages in each connection stored in the chunk
};
struct ROSBAG_DECL ChunkHeader
{
std::string compression; //! chunk compression type, e.g. "none" or "bz2" (see constants.h)
uint32_t compressed_size; //! compressed size of the chunk in bytes
uint32_t uncompressed_size; //! uncompressed size of the chunk in bytes
};
struct ROSBAG_DECL IndexEntry
{
ros::Time time; //! timestamp of the message
uint64_t chunk_pos; //! absolute byte offset of the chunk record containing the message
uint32_t offset; //! relative byte offset of the message record (either definition or data) in the chunk
bool operator<(IndexEntry const& b) const { return time < b.time; }
};
struct ROSBAG_DECL IndexEntryCompare
{
bool operator()(ros::Time const& a, IndexEntry const& b) const { return a < b.time; }
bool operator()(IndexEntry const& a, ros::Time const& b) const { return a.time < b; }
};
} // namespace rosbag
#endif

179
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/view.h vendored Normal file
View File

@@ -0,0 +1,179 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSBAG_VIEW_H
#define ROSBAG_VIEW_H
#include <boost/function.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include "rosbag/message_instance.h"
#include "rosbag/query.h"
#include "rosbag/macros.h"
#include "rosbag/structures.h"
namespace rosbag {
class ROSBAG_DECL View
{
friend class Bag;
public:
//! An iterator that points to a MessageInstance from a bag
/*!
* This iterator stores the MessageInstance that it is returning a
* reference to. If you increment the iterator that
* MessageInstance is destroyed. You should never store the
* pointer to this reference.
*/
class iterator : public boost::iterator_facade<iterator,
MessageInstance,
boost::forward_traversal_tag>
{
public:
iterator(iterator const& i);
iterator &operator=(iterator const& i);
iterator();
~iterator();
protected:
iterator(View* view, bool end = false);
private:
friend class View;
friend class boost::iterator_core_access;
void populate();
void populateSeek(std::multiset<IndexEntry>::const_iterator iter);
bool equal(iterator const& other) const;
void increment();
MessageInstance& dereference() const;
private:
View* view_;
std::vector<ViewIterHelper> iters_;
uint32_t view_revision_;
mutable MessageInstance* message_instance_;
};
typedef iterator const_iterator;
struct TrueQuery {
bool operator()(ConnectionInfo const*) const { return true; };
};
//! Create a view on a bag
/*!
* param reduce_overlap If multiple views return the same messages, reduce them to a single message
*/
View(bool const& reduce_overlap = false);
//! Create a view on a bag
/*!
* param bag The bag file on which to run this query
* param start_time The beginning of the time range for the query
* param end_time The end of the time range for the query
* param reduce_overlap If multiple views return the same messages, reduce them to a single message
*/
View(Bag const& bag, ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);
//! Create a view and add a query
/*!
* param bag The bag file on which to run this query
* param query The actual query to evaluate which connections to include
* param start_time The beginning of the time range for the query
* param end_time The end of the time range for the query
* param reduce_overlap If multiple views return the same messages, reduce them to a single message
*/
View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query,
ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);
~View();
iterator begin();
iterator end();
uint32_t size();
//! Add a query to a view
/*!
* param bag The bag file on which to run this query
* param start_time The beginning of the time range for the query
* param end_time The end of the time range for the query
*/
void addQuery(Bag const& bag, ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX);
//! Add a query to a view
/*!
* param bag The bag file on which to run this query
* param query The actual query to evaluate which connections to include
* param start_time The beginning of the time range for the query
* param end_time The end of the time range for the query
*/
void addQuery(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query,
ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX);
std::vector<const ConnectionInfo*> getConnections();
ros::Time getBeginTime();
ros::Time getEndTime();
protected:
friend class iterator;
void updateQueries(BagQuery* q);
void update();
MessageInstance* newMessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index, Bag const& bag);
private:
View(View const& view);
View& operator=(View const& view);
protected:
std::vector<MessageRange*> ranges_;
std::vector<BagQuery*> queries_;
uint32_t view_revision_;
uint32_t size_cache_;
uint32_t size_revision_;
bool reduce_overlap_;
};
} // namespace rosbag
#endif

66
thirdparty/ros/ros_comm/tools/rosbag_storage/mainpage.dox vendored Normal file
View File

@@ -0,0 +1,66 @@
/**
\mainpage
\htmlinclude manifest.html
\b rosbag_storage is a set of tools and API's for recording/writing messages to bag files and playing/reading them back without relying on the ROS client library.
The code is still in the rosbag namespace since it was extracted from the <a href="../../../rosbag/html/c++/">rosbag</a> package without renaming any API.
\section codeapi Code API
The C++ and Python API's are provided for serializing bag files. The C++ API consists of the following classes:
- rosbag::Bag - Serializes to/from a bag file on disk.
- rosbag::View - Specifies a view into a bag file to allow for querying for messages on specific connections withn a time range.
Here's a simple example of writing to a bag file:
\verbatim
#include "rosbag/bag.h"
...
rosbag::Bag bag("test.bag", rosbag::bagmode::Write);
std_msgs::Int32 i;
i.data = 42;
bag.write("numbers", ros::Time::now(), i);
bag.close();
\endverbatim
Likewise, to read from that bag file:
\verbatim
#include "rosbag/bag.h"
...
rosbag::Bag bag("test.bag");
rosbag::View view(bag, rosbag::TopicQuery("numbers"));
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
std_msgs::Int32::ConstPtr i = m.instantiate<std_msgs::Int32>();
if (i != NULL)
std::cout << i->data << std::endl;
}
bag.close();
\endverbatim
The Python API is similar. Writing to a bag file:
\verbatim
import rosbag
from std_msgs.msg import Int32, String
bag = rosbag.Bag('test.bag', 'w')
i = Int32()
i.data = 42
bag.write('numbers', i);
bag.close();
\endverbatim
Example usage for read:
\verbatim
import rosbag
bag = rosbag.Bag('test.bag')
for topic, msg, t in bag.read_messages('numbers'):
print msg.data
bag.close();
\endverbatim
*/

34
thirdparty/ros/ros_comm/tools/rosbag_storage/package.xml vendored Normal file
View File

@@ -0,0 +1,34 @@
<package>
<name>rosbag_storage</name>
<version>1.12.14</version>
<description>
This is a set of tools for recording from and playing back ROS
message without relying on the ROS client library.
</description>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>boost</build_depend>
<build_depend>bzip2</build_depend>
<build_depend version_gte="0.3.17">cpp_common</build_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<build_depend>roscpp_serialization</build_depend>
<build_depend version_gte="0.3.17">roscpp_traits</build_depend>
<build_depend>rostime</build_depend>
<build_depend>roslz4</build_depend>
<run_depend>boost</run_depend>
<run_depend>bzip2</run_depend>
<run_depend version_gte="0.3.17">cpp_common</run_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<run_depend>roscpp_serialization</run_depend>
<run_depend version_gte="0.3.17">roscpp_traits</run_depend>
<run_depend>rostime</run_depend>
<run_depend>roslz4</run_depend>
<export>
<rosdoc config="${prefix}/rosdoc.yaml"/>
</export>
</package>

4
thirdparty/ros/ros_comm/tools/rosbag_storage/rosdoc.yaml vendored Normal file
View File

@@ -0,0 +1,4 @@
- builder: doxygen
name: C++ API
output_dir: c++
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'

1154
thirdparty/ros/ros_comm/tools/rosbag_storage/src/bag.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

71
thirdparty/ros/ros_comm/tools/rosbag_storage/src/bag_player.cpp vendored Normal file
View File

@@ -0,0 +1,71 @@
#include "rosbag/bag_player.h"
#define foreach BOOST_FOREACH
namespace rosbag
{
BagPlayer::BagPlayer(const std::string &fname) {
bag.open(fname, rosbag::bagmode::Read);
ros::Time::init();
View v(bag);
bag_start_ = v.getBeginTime();
bag_end_ = v.getEndTime();
last_message_time_ = ros::Time(0);
playback_speed_ = 1.0;
}
BagPlayer::~BagPlayer() {
bag.close();
}
ros::Time BagPlayer::get_time() {
return last_message_time_;
}
void BagPlayer::set_start(const ros::Time &start) {
bag_start_ = start;
}
void BagPlayer::set_end(const ros::Time &end) {
bag_end_ = end;
}
void BagPlayer::set_playback_speed(double scale) {
if (scale > 0.0)
playback_speed_ = scale;
}
ros::Time BagPlayer::real_time(const ros::Time &msg_time) {
return play_start_ + (msg_time - bag_start_) * (1 / playback_speed_);
}
void BagPlayer::start_play() {
std::vector<std::string> topics;
std::pair<std::string, BagCallback *> cb;
foreach(cb, cbs_)
topics.push_back(cb.first);
View view(bag, TopicQuery(topics), bag_start_, bag_end_);
play_start_ = ros::Time::now();
foreach(MessageInstance const m, view)
{
if (cbs_.find(m.getTopic()) == cbs_.end())
continue;
ros::Time::sleepUntil(real_time(m.getTime()));
last_message_time_ = m.getTime(); /* this is the recorded time */
cbs_[m.getTopic()]->call(m);
}
}
void BagPlayer::unregister_callback(const std::string &topic) {
delete cbs_[topic];
cbs_.erase(topic);
}
}

82
thirdparty/ros/ros_comm/tools/rosbag_storage/src/buffer.cpp vendored Normal file
View File

@@ -0,0 +1,82 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include <stdlib.h>
#include <assert.h>
#include <utility>
#include "rosbag/buffer.h"
//#include <ros/ros.h>
namespace rosbag {
Buffer::Buffer() : buffer_(NULL), capacity_(0), size_(0) { }
Buffer::~Buffer() {
free(buffer_);
}
uint8_t* Buffer::getData() { return buffer_; }
uint32_t Buffer::getCapacity() const { return capacity_; }
uint32_t Buffer::getSize() const { return size_; }
void Buffer::setSize(uint32_t size) {
size_ = size;
ensureCapacity(size);
}
void Buffer::ensureCapacity(uint32_t capacity) {
if (capacity <= capacity_)
return;
if (capacity_ == 0)
capacity_ = capacity;
else {
while (capacity_ < capacity)
capacity_ *= 2;
}
buffer_ = (uint8_t*) realloc(buffer_, capacity_);
assert(buffer_);
}
void Buffer::swap(Buffer& other) {
using std::swap;
swap(buffer_, other.buffer_);
swap(capacity_, other.capacity_);
swap(size_, other.size_);
}
} // namespace rosbag

178
thirdparty/ros/ros_comm/tools/rosbag_storage/src/bz2_stream.cpp vendored Normal file
View File

@@ -0,0 +1,178 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "rosbag/chunked_file.h"
#include <iostream>
#include <cstring>
#include "console_bridge/console.h"
// Remove this include when no longer supporting platforms with libconsole-bridge-dev < 0.3.0,
// in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
#include "rosbag/console_bridge_compatibility.h"
using std::string;
namespace rosbag {
BZ2Stream::BZ2Stream(ChunkedFile* file) :
Stream(file),
verbosity_(0),
block_size_100k_(9),
work_factor_(30),
bzfile_(NULL),
bzerror_(0)
{ }
CompressionType BZ2Stream::getCompressionType() const {
return compression::BZ2;
}
void BZ2Stream::startWrite() {
bzfile_ = BZ2_bzWriteOpen(&bzerror_, getFilePointer(), block_size_100k_, verbosity_, work_factor_);
switch (bzerror_) {
case BZ_OK: break;
default: {
BZ2_bzWriteClose(&bzerror_, bzfile_, 0, NULL, NULL);
throw BagException("Error opening file for writing compressed stream");
}
}
setCompressedIn(0);
}
void BZ2Stream::write(void* ptr, size_t size) {
if (!bzfile_) {
throw BagException("cannot write to unopened bzfile");
}
BZ2_bzWrite(&bzerror_, bzfile_, ptr, size);
switch (bzerror_) {
case BZ_IO_ERROR: throw BagException("BZ_IO_ERROR: error writing the compressed file");
}
setCompressedIn(getCompressedIn() + size);
}
void BZ2Stream::stopWrite() {
if (!bzfile_) {
throw BagException("cannot close unopened bzfile");
}
unsigned int nbytes_in;
unsigned int nbytes_out;
BZ2_bzWriteClose(&bzerror_, bzfile_, 0, &nbytes_in, &nbytes_out);
switch (bzerror_) {
case BZ_IO_ERROR: throw BagIOException("BZ_IO_ERROR");
}
advanceOffset(nbytes_out);
setCompressedIn(0);
}
void BZ2Stream::startRead() {
bzfile_ = BZ2_bzReadOpen(&bzerror_, getFilePointer(), verbosity_, 0, getUnused(), getUnusedLength());
switch (bzerror_) {
case BZ_OK: break;
default: {
BZ2_bzReadClose(&bzerror_, bzfile_);
throw BagException("Error opening file for reading compressed stream");
}
}
clearUnused();
}
void BZ2Stream::read(void* ptr, size_t size) {
if (!bzfile_) {
throw BagException("cannot read from unopened bzfile");
}
BZ2_bzRead(&bzerror_, bzfile_, ptr, size);
advanceOffset(size);
switch (bzerror_) {
case BZ_OK: return;
case BZ_STREAM_END:
if (getUnused() || getUnusedLength() > 0)
CONSOLE_BRIDGE_logError("unused data already available");
else {
char* unused;
int nUnused;
BZ2_bzReadGetUnused(&bzerror_, bzfile_, (void**) &unused, &nUnused);
setUnused(unused);
setUnusedLength(nUnused);
}
return;
case BZ_IO_ERROR: throw BagIOException("BZ_IO_ERROR: error reading from compressed stream"); break;
case BZ_UNEXPECTED_EOF: throw BagIOException("BZ_UNEXPECTED_EOF: compressed stream ended before logical end-of-stream detected"); break;
case BZ_DATA_ERROR: throw BagIOException("BZ_DATA_ERROR: data integrity error detected in compressed stream"); break;
case BZ_DATA_ERROR_MAGIC: throw BagIOException("BZ_DATA_ERROR_MAGIC: stream does not begin with requisite header bytes"); break;
case BZ_MEM_ERROR: throw BagIOException("BZ_MEM_ERROR: insufficient memory available"); break;
}
}
void BZ2Stream::stopRead() {
if (!bzfile_) {
throw BagException("cannot close unopened bzfile");
}
BZ2_bzReadClose(&bzerror_, bzfile_);
switch (bzerror_) {
case BZ_IO_ERROR: throw BagIOException("BZ_IO_ERROR");
}
}
void BZ2Stream::decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
int result = BZ2_bzBuffToBuffDecompress((char*) dest, &dest_len, (char*) source, source_len, 0, verbosity_);
switch (result) {
case BZ_OK: break;
case BZ_CONFIG_ERROR: throw BagException("library has been mis-compiled"); break;
case BZ_PARAM_ERROR: throw BagException("dest is NULL or destLen is NULL or small != 0 && small != 1 or verbosity < 0 or verbosity > 4"); break;
case BZ_MEM_ERROR: throw BagException("insufficient memory is available"); break;
case BZ_OUTBUFF_FULL: throw BagException("size of the compressed data exceeds *destLen"); break;
case BZ_DATA_ERROR: throw BagException("data integrity error was detected in the compressed data"); break;
case BZ_DATA_ERROR_MAGIC: throw BagException("compressed data doesn't begin with the right magic bytes"); break;
case BZ_UNEXPECTED_EOF: throw BagException("compressed data ends unexpectedly"); break;
}
}
} // namespace rosbag

255
thirdparty/ros/ros_comm/tools/rosbag_storage/src/chunked_file.cpp vendored Normal file
View File

@@ -0,0 +1,255 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "rosbag/chunked_file.h"
#include <iostream>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
//#include <ros/ros.h>
#ifdef _WIN32
# ifdef __MINGW32__
# define fseeko fseeko64
# define ftello ftello64
// not sure if we need a ftruncate here yet or not
# else
# include <io.h>
# define fseeko _fseeki64
# define ftello _ftelli64
# define fileno _fileno
# define ftruncate _chsize
# endif
#endif
using std::string;
using boost::format;
using boost::shared_ptr;
using ros::Exception;
namespace rosbag {
ChunkedFile::ChunkedFile() :
file_(NULL),
offset_(0),
compressed_in_(0),
unused_(NULL),
nUnused_(0)
{
stream_factory_ = boost::make_shared<StreamFactory>(this);
}
ChunkedFile::~ChunkedFile() {
close();
}
void ChunkedFile::openReadWrite(string const& filename) { open(filename, "r+b"); }
void ChunkedFile::openWrite (string const& filename) { open(filename, "w+b"); }
void ChunkedFile::openRead (string const& filename) { open(filename, "rb"); }
void ChunkedFile::open(string const& filename, string const& mode) {
// Check if file is already open
if (file_)
throw BagIOException((format("File already open: %1%") % filename_.c_str()).str());
// Open the file
if (mode == "r+b") {
// check if file already exists
#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
fopen_s( &file_, filename.c_str(), "r" );
#else
file_ = fopen(filename.c_str(), "r");
#endif
if (file_ == NULL)
// create an empty file and open it for update
#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
fopen_s( &file_, filename.c_str(), "w+b" );
#else
file_ = fopen(filename.c_str(), "w+b");
#endif
else {
fclose(file_);
// open existing file for update
#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
fopen_s( &file_, filename.c_str(), "r+b" );
#else
file_ = fopen(filename.c_str(), "r+b");
#endif
}
}
else
#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
fopen_s( &file_, filename.c_str(), mode.c_str() );
#else
file_ = fopen(filename.c_str(), mode.c_str());
#endif
if (!file_)
throw BagIOException((format("Error opening file: %1%") % filename.c_str()).str());
read_stream_ = boost::make_shared<UncompressedStream>(this);
write_stream_ = boost::make_shared<UncompressedStream>(this);
filename_ = filename;
offset_ = ftello(file_);
}
bool ChunkedFile::good() const {
return feof(file_) == 0 && ferror(file_) == 0;
}
bool ChunkedFile::isOpen() const { return file_ != NULL; }
string ChunkedFile::getFileName() const { return filename_; }
void ChunkedFile::close() {
if (!file_)
return;
// Close any compressed stream by changing to uncompressed mode
setWriteMode(compression::Uncompressed);
// Close the file
int success = fclose(file_);
if (success != 0)
throw BagIOException((format("Error closing file: %1%") % filename_.c_str()).str());
file_ = NULL;
filename_.clear();
clearUnused();
}
// Read/write modes
void ChunkedFile::setWriteMode(CompressionType type) {
if (!file_)
throw BagIOException("Can't set compression mode before opening a file");
if (type != write_stream_->getCompressionType()) {
write_stream_->stopWrite();
shared_ptr<Stream> stream = stream_factory_->getStream(type);
stream->startWrite();
write_stream_ = stream;
}
}
void ChunkedFile::setReadMode(CompressionType type) {
if (!file_)
throw BagIOException("Can't set compression mode before opening a file");
if (type != read_stream_->getCompressionType()) {
read_stream_->stopRead();
shared_ptr<Stream> stream = stream_factory_->getStream(type);
stream->startRead();
read_stream_ = stream;
}
}
void ChunkedFile::seek(uint64_t offset, int origin) {
if (!file_)
throw BagIOException("Can't seek - file not open");
setReadMode(compression::Uncompressed);
int success = fseeko(file_, offset, origin);
if (success != 0)
throw BagIOException("Error seeking");
offset_ = ftello(file_);
}
uint64_t ChunkedFile::getOffset() const { return offset_; }
uint32_t ChunkedFile::getCompressedBytesIn() const { return compressed_in_; }
void ChunkedFile::write(string const& s) { write((void*) s.c_str(), s.size()); }
void ChunkedFile::write(void* ptr, size_t size) { write_stream_->write(ptr, size); }
void ChunkedFile::read(void* ptr, size_t size) { read_stream_->read(ptr, size); }
bool ChunkedFile::truncate(uint64_t length) {
int fd = fileno(file_);
return ftruncate(fd, length) == 0;
}
//! \todo add error handling
string ChunkedFile::getline() {
char buffer[1024];
if(fgets(buffer, 1024, file_))
{
string s(buffer);
offset_ += s.size();
return s;
}
else
return string("");
}
void ChunkedFile::decompress(CompressionType compression, uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
stream_factory_->getStream(compression)->decompress(dest, dest_len, source, source_len);
}
void ChunkedFile::clearUnused() {
unused_ = NULL;
nUnused_ = 0;
}
void ChunkedFile::swap(ChunkedFile& other) {
using std::swap;
using boost::swap;
swap(filename_, other.filename_);
swap(file_, other.file_);
swap(offset_, other.offset_);
swap(compressed_in_, other.compressed_in_);
swap(unused_, other.unused_);
swap(nUnused_, other.nUnused_);
swap(stream_factory_, other.stream_factory_);
FileAccessor::setFile(*stream_factory_->getStream(compression::Uncompressed), this);
FileAccessor::setFile(*stream_factory_->getStream(compression::BZ2), this);
FileAccessor::setFile(*stream_factory_->getStream(compression::LZ4), this);
FileAccessor::setFile(*other.stream_factory_->getStream(compression::Uncompressed), &other);
FileAccessor::setFile(*other.stream_factory_->getStream(compression::BZ2), &other);
FileAccessor::setFile(*other.stream_factory_->getStream(compression::LZ4), &other);
swap(read_stream_, other.read_stream_);
FileAccessor::setFile(*read_stream_, this);
FileAccessor::setFile(*other.read_stream_, &other);
swap(write_stream_, other.write_stream_);
FileAccessor::setFile(*write_stream_, this);
FileAccessor::setFile(*other.write_stream_, &other);
}
} // namespace rosbag

233
thirdparty/ros/ros_comm/tools/rosbag_storage/src/lz4_stream.cpp vendored Normal file
View File

@@ -0,0 +1,233 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Ben Charrow
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "rosbag/chunked_file.h"
#include <iostream>
#include <cstring>
#include "console_bridge/console.h"
// Remove this include when no longer supporting platforms with libconsole-bridge-dev < 0.3.0,
// in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
#include "rosbag/console_bridge_compatibility.h"
using std::string;
namespace rosbag {
LZ4Stream::LZ4Stream(ChunkedFile* file)
: Stream(file), block_size_id_(6) {
buff_size_ = roslz4_blockSizeFromIndex(block_size_id_) + 64;
buff_ = new char[buff_size_];
lz4s_.state = NULL;
}
LZ4Stream::~LZ4Stream() {
delete[] buff_;
}
CompressionType LZ4Stream::getCompressionType() const {
return compression::LZ4;
}
void LZ4Stream::startWrite() {
if (lz4s_.state) {
throw BagException("cannot start writing to already opened lz4 stream");
}
setCompressedIn(0);
int ret = roslz4_compressStart(&lz4s_, block_size_id_);
switch(ret) {
case ROSLZ4_OK: break;
case ROSLZ4_MEMORY_ERROR: throw BagIOException("ROSLZ4_MEMORY_ERROR: insufficient memory available"); break;
case ROSLZ4_PARAM_ERROR: throw BagIOException("ROSLZ4_PARAM_ERROR: bad block size"); break;
default: throw BagException("Unhandled return code");
}
lz4s_.output_next = buff_;
lz4s_.output_left = buff_size_;
}
void LZ4Stream::write(void* ptr, size_t size) {
if (!lz4s_.state) {
throw BagException("cannot write to unopened lz4 stream");
}
lz4s_.input_left = size;
lz4s_.input_next = (char*) ptr;
writeStream(ROSLZ4_RUN);
setCompressedIn(getCompressedIn() + size);
}
void LZ4Stream::writeStream(int action) {
int ret = ROSLZ4_OK;
while (lz4s_.input_left > 0 ||
(action == ROSLZ4_FINISH && ret != ROSLZ4_STREAM_END)) {
ret = roslz4_compress(&lz4s_, action);
switch(ret) {
case ROSLZ4_OK: break;
case ROSLZ4_OUTPUT_SMALL:
if (lz4s_.output_next - buff_ == buff_size_) {
throw BagIOException("ROSLZ4_OUTPUT_SMALL: output buffer is too small");
} else {
// There's data to be written in buff_; this will free up space
break;
}
case ROSLZ4_STREAM_END: break;
case ROSLZ4_PARAM_ERROR: throw BagIOException("ROSLZ4_PARAM_ERROR: bad block size"); break;
case ROSLZ4_ERROR: throw BagIOException("ROSLZ4_ERROR: compression error"); break;
default: throw BagException("Unhandled return code");
}
// If output data is ready, write to disk
int to_write = lz4s_.output_next - buff_;
if (to_write > 0) {
if (fwrite(buff_, 1, to_write, getFilePointer()) != static_cast<size_t>(to_write)) {
throw BagException("Problem writing data to disk");
}
advanceOffset(to_write);
lz4s_.output_next = buff_;
lz4s_.output_left = buff_size_;
}
}
}
void LZ4Stream::stopWrite() {
if (!lz4s_.state) {
throw BagException("cannot close unopened lz4 stream");
}
writeStream(ROSLZ4_FINISH);
setCompressedIn(0);
roslz4_compressEnd(&lz4s_);
}
void LZ4Stream::startRead() {
if (lz4s_.state) {
throw BagException("cannot start reading from already opened lz4 stream");
}
int ret = roslz4_decompressStart(&lz4s_);
switch(ret) {
case ROSLZ4_OK: break;
case ROSLZ4_MEMORY_ERROR: throw BagException("ROSLZ4_MEMORY_ERROR: insufficient memory available"); break;
default: throw BagException("Unhandled return code");
}
if (getUnusedLength() > buff_size_) {
throw BagException("Too many unused bytes to decompress");
}
// getUnused() could be pointing to part of buff_, so don't use memcpy
memmove(buff_, getUnused(), getUnusedLength());
lz4s_.input_next = buff_;
lz4s_.input_left = getUnusedLength();
clearUnused();
}
void LZ4Stream::read(void* ptr, size_t size) {
if (!lz4s_.state) {
throw BagException("cannot read from unopened lz4 stream");
}
// Setup stream by filling buffer with data from file
int to_read = buff_size_ - lz4s_.input_left;
char *input_start = buff_ + lz4s_.input_left;
int nread = fread(input_start, 1, to_read, getFilePointer());
if (ferror(getFilePointer())) {
throw BagIOException("Problem reading from file");
}
lz4s_.input_next = buff_;
lz4s_.input_left += nread;
lz4s_.output_next = (char*) ptr;
lz4s_.output_left = size;
// Decompress. If reach end of stream, store unused data
int ret = roslz4_decompress(&lz4s_);
switch (ret) {
case ROSLZ4_OK: break;
case ROSLZ4_STREAM_END:
if (getUnused() || getUnusedLength() > 0)
CONSOLE_BRIDGE_logError("unused data already available");
else {
setUnused(lz4s_.input_next);
setUnusedLength(lz4s_.input_left);
}
return;
case ROSLZ4_ERROR: throw BagException("ROSLZ4_ERROR: decompression error"); break;
case ROSLZ4_MEMORY_ERROR: throw BagException("ROSLZ4_MEMORY_ERROR: insufficient memory available"); break;
case ROSLZ4_OUTPUT_SMALL: throw BagException("ROSLZ4_OUTPUT_SMALL: output buffer is too small"); break;
case ROSLZ4_DATA_ERROR: throw BagException("ROSLZ4_DATA_ERROR: malformed data to decompress"); break;
default: throw BagException("Unhandled return code");
}
if (feof(getFilePointer())) {
throw BagIOException("Reached end of file before reaching end of stream");
}
size_t total_out = lz4s_.output_next - (char*)ptr;
advanceOffset(total_out);
// Shift input buffer if there's unconsumed data
if (lz4s_.input_left > 0) {
memmove(buff_, lz4s_.input_next, lz4s_.input_left);
}
}
void LZ4Stream::stopRead() {
if (!lz4s_.state) {
throw BagException("cannot close unopened lz4 stream");
}
roslz4_decompressEnd(&lz4s_);
}
void LZ4Stream::decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
unsigned int actual_dest_len = dest_len;
int ret = roslz4_buffToBuffDecompress((char*)source, source_len,
(char*)dest, &actual_dest_len);
switch(ret) {
case ROSLZ4_OK: break;
case ROSLZ4_ERROR: throw BagException("ROSLZ4_ERROR: decompression error"); break;
case ROSLZ4_MEMORY_ERROR: throw BagException("ROSLZ4_MEMORY_ERROR: insufficient memory available"); break;
case ROSLZ4_OUTPUT_SMALL: throw BagException("ROSLZ4_OUTPUT_SMALL: output buffer is too small"); break;
case ROSLZ4_DATA_ERROR: throw BagException("ROSLZ4_DATA_ERROR: malformed data to decompress"); break;
default: throw BagException("Unhandled return code");
}
if (actual_dest_len != dest_len) {
throw BagException("Decompression size mismatch in LZ4 chunk");
}
}
} // namespace rosbag

65
thirdparty/ros/ros_comm/tools/rosbag_storage/src/message_instance.cpp vendored Normal file
View File

@@ -0,0 +1,65 @@
// Copyright (c) 2009, Willow Garage, Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of Willow Garage, Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#include "rosbag/message_instance.h"
#include "ros/message_event.h"
using std::string;
using ros::Time;
using boost::shared_ptr;
namespace rosbag {
MessageInstance::MessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index_entry, Bag const& bag) :
connection_info_(connection_info), index_entry_(index_entry), bag_(&bag)
{
}
Time const& MessageInstance::getTime() const { return index_entry_.time; }
string const& MessageInstance::getTopic() const { return connection_info_->topic; }
string const& MessageInstance::getDataType() const { return connection_info_->datatype; }
string const& MessageInstance::getMD5Sum() const { return connection_info_->md5sum; }
string const& MessageInstance::getMessageDefinition() const { return connection_info_->msg_def; }
shared_ptr<ros::M_string> MessageInstance::getConnectionHeader() const { return connection_info_->header; }
string MessageInstance::getCallerId() const {
ros::M_string::const_iterator header_iter = connection_info_->header->find("callerid");
return header_iter != connection_info_->header->end() ? header_iter->second : string("");
}
bool MessageInstance::isLatching() const {
ros::M_string::const_iterator header_iter = connection_info_->header->find("latching");
return header_iter != connection_info_->header->end() && header_iter->second == "1";
}
uint32_t MessageInstance::size() const {
return bag_->readMessageDataSize(index_entry_);
}
} // namespace rosbag

114
thirdparty/ros/ros_comm/tools/rosbag_storage/src/query.cpp vendored Normal file
View File

@@ -0,0 +1,114 @@
// Copyright (c) 2009, Willow Garage, Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of Willow Garage, Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#include "rosbag/query.h"
#include "rosbag/bag.h"
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
using std::map;
using std::string;
using std::vector;
using std::multiset;
namespace rosbag {
// Query
Query::Query(boost::function<bool(ConnectionInfo const*)>& query, ros::Time const& start_time, ros::Time const& end_time)
: query_(query), start_time_(start_time), end_time_(end_time)
{
}
boost::function<bool(ConnectionInfo const*)> const& Query::getQuery() const {
return query_;
}
ros::Time const& Query::getStartTime() const { return start_time_; }
ros::Time const& Query::getEndTime() const { return end_time_; }
// TopicQuery
TopicQuery::TopicQuery(std::string const& topic) {
topics_.push_back(topic);
}
TopicQuery::TopicQuery(std::vector<std::string> const& topics) : topics_(topics) { }
bool TopicQuery::operator()(ConnectionInfo const* info) const {
foreach(string const& topic, topics_)
if (topic == info->topic)
return true;
return false;
}
// TypeQuery
TypeQuery::TypeQuery(std::string const& type) {
types_.push_back(type);
}
TypeQuery::TypeQuery(std::vector<std::string> const& types) : types_(types) { }
bool TypeQuery::operator()(ConnectionInfo const* info) const {
foreach(string const& type, types_)
if (type == info->datatype)
return true;
return false;
}
// BagQuery
BagQuery::BagQuery(Bag const* _bag, Query const& _query, uint32_t _bag_revision) : bag(_bag), query(_query), bag_revision(_bag_revision) {
}
// MessageRange
MessageRange::MessageRange(std::multiset<IndexEntry>::const_iterator const& _begin,
std::multiset<IndexEntry>::const_iterator const& _end,
ConnectionInfo const* _connection_info,
BagQuery const* _bag_query)
: begin(_begin), end(_end), connection_info(_connection_info), bag_query(_bag_query)
{
}
// ViewIterHelper
ViewIterHelper::ViewIterHelper(std::multiset<IndexEntry>::const_iterator _iter, MessageRange const* _range)
: iter(_iter), range(_range)
{
}
bool ViewIterHelperCompare::operator()(ViewIterHelper const& a, ViewIterHelper const& b) {
return (a.iter)->time > (b.iter)->time;
}
} // namespace rosbag

83
thirdparty/ros/ros_comm/tools/rosbag_storage/src/stream.cpp vendored Normal file
View File

@@ -0,0 +1,83 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "rosbag/stream.h"
#include "rosbag/chunked_file.h"
//#include <ros/ros.h>
using boost::shared_ptr;
namespace rosbag {
// StreamFactory
StreamFactory::StreamFactory(ChunkedFile* file) :
uncompressed_stream_(new UncompressedStream(file)),
bz2_stream_ (new BZ2Stream(file)),
lz4_stream_ (new LZ4Stream(file))
{
}
shared_ptr<Stream> StreamFactory::getStream(CompressionType type) const {
switch (type) {
case compression::Uncompressed: return uncompressed_stream_;
case compression::BZ2: return bz2_stream_;
case compression::LZ4: return lz4_stream_;
default: return shared_ptr<Stream>();
}
}
// Stream
Stream::Stream(ChunkedFile* file) : file_(file) { }
Stream::~Stream() { }
void Stream::startWrite() { }
void Stream::stopWrite() { }
void Stream::startRead() { }
void Stream::stopRead() { }
FILE* Stream::getFilePointer() { return file_->file_; }
uint64_t Stream::getCompressedIn() { return file_->compressed_in_; }
void Stream::setCompressedIn(uint64_t nbytes) { file_->compressed_in_ = nbytes; }
void Stream::advanceOffset(uint64_t nbytes) { file_->offset_ += nbytes; }
char* Stream::getUnused() { return file_->unused_; }
int Stream::getUnusedLength() { return file_->nUnused_; }
void Stream::setUnused(char* unused) { file_->unused_ = unused; }
void Stream::setUnusedLength(int nUnused) { file_->nUnused_ = nUnused; }
void Stream::clearUnused() { file_->clearUnused(); }
} // namespace rosbag

114
thirdparty/ros/ros_comm/tools/rosbag_storage/src/uncompressed_stream.cpp vendored Normal file
View File

@@ -0,0 +1,114 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "rosbag/chunked_file.h"
#include <iostream>
#include <cstring>
#include <boost/format.hpp>
using std::string;
using boost::format;
using ros::Exception;
namespace rosbag {
UncompressedStream::UncompressedStream(ChunkedFile* file) : Stream(file) { }
CompressionType UncompressedStream::getCompressionType() const {
return compression::Uncompressed;
}
void UncompressedStream::write(void* ptr, size_t size) {
size_t result = fwrite(ptr, 1, size, getFilePointer());
if (result != size)
throw BagIOException((format("Error writing to file: writing %1% bytes, wrote %2% bytes") % size % result).str());
advanceOffset(size);
}
void UncompressedStream::read(void* ptr, size_t size) {
size_t nUnused = (size_t) getUnusedLength();
char* unused = getUnused();
if (nUnused > 0) {
// We have unused data from the last compressed read
if (nUnused == size) {
// Copy the unused data into the buffer
memcpy(ptr, unused, nUnused);
clearUnused();
}
else if (nUnused < size) {
// Copy the unused data into the buffer
memcpy(ptr, unused, nUnused);
// Still have data to read
size -= nUnused;
// Read the remaining data from the file
int result = fread((char*) ptr + nUnused, 1, size, getFilePointer());
if ((size_t) result != size)
throw BagIOException((format("Error reading from file + unused: wanted %1% bytes, read %2% bytes") % size % result).str());
advanceOffset(size);
clearUnused();
}
else {
// nUnused_ > size
memcpy(ptr, unused, size);
setUnused(unused + size);
setUnusedLength(nUnused - size);
}
}
// No unused data - read from stream
int result = fread(ptr, 1, size, getFilePointer());
if ((size_t) result != size)
throw BagIOException((format("Error reading from file: wanted %1% bytes, read %2% bytes") % size % result).str());
advanceOffset(size);
}
void UncompressedStream::decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
if (dest_len < source_len)
throw BagException("dest_len not large enough");
memcpy(dest, source, source_len);
}
} // namespace rosbag

350
thirdparty/ros/ros_comm/tools/rosbag_storage/src/view.cpp vendored Normal file
View File

@@ -0,0 +1,350 @@
// Copyright (c) 2009, Willow Garage, Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of Willow Garage, Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#include "rosbag/view.h"
#include "rosbag/bag.h"
#include "rosbag/message_instance.h"
#include <boost/foreach.hpp>
#include <set>
#include <assert.h>
#define foreach BOOST_FOREACH
using std::map;
using std::string;
using std::vector;
using std::multiset;
namespace rosbag {
// View::iterator
View::iterator::iterator() : view_(NULL), view_revision_(0), message_instance_(NULL) { }
View::iterator::~iterator()
{
if (message_instance_ != NULL)
delete message_instance_;
}
View::iterator::iterator(View* view, bool end) : view_(view), view_revision_(0), message_instance_(NULL) {
if (view != NULL && !end)
populate();
}
View::iterator::iterator(const iterator& i) : view_(i.view_), iters_(i.iters_), view_revision_(i.view_revision_), message_instance_(NULL) { }
View::iterator &View::iterator::operator=(iterator const& i) {
if (this != &i) {
view_ = i.view_;
iters_ = i.iters_;
view_revision_ = i.view_revision_;
if (message_instance_ != NULL) {
delete message_instance_;
message_instance_ = NULL;
}
}
return *this;
}
void View::iterator::populate() {
assert(view_ != NULL);
iters_.clear();
foreach(MessageRange const* range, view_->ranges_)
if (range->begin != range->end)
iters_.push_back(ViewIterHelper(range->begin, range));
std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
view_revision_ = view_->view_revision_;
}
void View::iterator::populateSeek(multiset<IndexEntry>::const_iterator iter) {
assert(view_ != NULL);
iters_.clear();
foreach(MessageRange const* range, view_->ranges_) {
multiset<IndexEntry>::const_iterator start = std::lower_bound(range->begin, range->end, iter->time, IndexEntryCompare());
if (start != range->end)
iters_.push_back(ViewIterHelper(start, range));
}
std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
while (iter != iters_.back().iter)
increment();
view_revision_ = view_->view_revision_;
}
bool View::iterator::equal(View::iterator const& other) const {
// We need some way of verifying these are actually talking about
// the same merge_queue data since we shouldn't be able to compare
// iterators from different Views.
if (iters_.empty())
return other.iters_.empty();
if (other.iters_.empty())
return false;
return iters_.back().iter == other.iters_.back().iter;
}
void View::iterator::increment() {
assert(view_ != NULL);
// Our message instance is no longer valid
if (message_instance_ != NULL)
{
delete message_instance_;
message_instance_ = NULL;
}
view_->update();
// Note, updating may have blown away our message-ranges and
// replaced them in general the ViewIterHelpers are no longer
// valid, but the iterator it stores should still be good.
if (view_revision_ != view_->view_revision_)
populateSeek(iters_.back().iter);
if (view_->reduce_overlap_)
{
std::multiset<IndexEntry>::const_iterator last_iter = iters_.back().iter;
while (!iters_.empty() && iters_.back().iter == last_iter)
{
iters_.back().iter++;
if (iters_.back().iter == iters_.back().range->end)
iters_.pop_back();
std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
}
} else {
iters_.back().iter++;
if (iters_.back().iter == iters_.back().range->end)
iters_.pop_back();
std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
}
}
MessageInstance& View::iterator::dereference() const {
ViewIterHelper const& i = iters_.back();
if (message_instance_ == NULL)
message_instance_ = view_->newMessageInstance(i.range->connection_info, *(i.iter), *(i.range->bag_query->bag));
return *message_instance_;
}
// View
View::View(bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) { }
View::View(Bag const& bag, ros::Time const& start_time, ros::Time const& end_time, bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) {
addQuery(bag, start_time, end_time);
}
View::View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, ros::Time const& start_time, ros::Time const& end_time, bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) {
addQuery(bag, query, start_time, end_time);
}
View::~View() {
foreach(MessageRange* range, ranges_)
delete range;
foreach(BagQuery* query, queries_)
delete query;
}
ros::Time View::getBeginTime()
{
update();
ros::Time begin = ros::TIME_MAX;
foreach (rosbag::MessageRange* range, ranges_)
{
if (range->begin->time < begin)
begin = range->begin->time;
}
return begin;
}
ros::Time View::getEndTime()
{
update();
ros::Time end = ros::TIME_MIN;
foreach (rosbag::MessageRange* range, ranges_)
{
std::multiset<IndexEntry>::const_iterator e = range->end;
e--;
if (e->time > end)
end = e->time;
}
return end;
}
//! Simply copy the merge_queue state into the iterator
View::iterator View::begin() {
update();
return iterator(this);
}
//! Default constructed iterator signifies end
View::iterator View::end() { return iterator(this, true); }
uint32_t View::size() {
update();
if (size_revision_ != view_revision_)
{
size_cache_ = 0;
foreach (MessageRange* range, ranges_)
{
size_cache_ += std::distance(range->begin, range->end);
}
size_revision_ = view_revision_;
}
return size_cache_;
}
void View::addQuery(Bag const& bag, ros::Time const& start_time, ros::Time const& end_time) {
if ((bag.getMode() & bagmode::Read) != bagmode::Read)
throw BagException("Bag not opened for reading");
boost::function<bool(ConnectionInfo const*)> query = TrueQuery();
queries_.push_back(new BagQuery(&bag, Query(query, start_time, end_time), bag.bag_revision_));
updateQueries(queries_.back());
}
void View::addQuery(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, ros::Time const& start_time, ros::Time const& end_time) {
if ((bag.getMode() & bagmode::Read) != bagmode::Read)
throw BagException("Bag not opened for reading");
queries_.push_back(new BagQuery(&bag, Query(query, start_time, end_time), bag.bag_revision_));
updateQueries(queries_.back());
}
void View::updateQueries(BagQuery* q) {
for (map<uint32_t, ConnectionInfo*>::const_iterator i = q->bag->connections_.begin(); i != q->bag->connections_.end(); i++) {
ConnectionInfo const* connection = i->second;
// Skip if the query doesn't evaluate to true
if (!q->query.getQuery()(connection))
continue;
map<uint32_t, multiset<IndexEntry> >::const_iterator j = q->bag->connection_indexes_.find(connection->id);
// Skip if the bag doesn't have the corresponding index
if (j == q->bag->connection_indexes_.end())
continue;
multiset<IndexEntry> const& index = j->second;
// lower_bound/upper_bound do a binary search to find the appropriate range of Index Entries given our time range
IndexEntry start_time_lookup_entry = { q->query.getStartTime(), 0, 0 };
IndexEntry end_time_lookup_entry = { q->query.getEndTime() , 0, 0 };
std::multiset<IndexEntry>::const_iterator begin = index.lower_bound(start_time_lookup_entry);
std::multiset<IndexEntry>::const_iterator end = index.upper_bound(end_time_lookup_entry);
// Make sure we are at the right beginning
while (begin != index.begin() && begin->time >= q->query.getStartTime())
{
begin--;
if (begin->time < q->query.getStartTime())
{
begin++;
break;
}
}
if (begin != end)
{
// todo: make faster with a map of maps
bool found = false;
for (vector<MessageRange*>::iterator k = ranges_.begin(); k != ranges_.end(); k++) {
MessageRange* r = *k;
// If the topic and query are already in our ranges, we update
if (r->bag_query == q && r->connection_info->id == connection->id) {
r->begin = begin;
r->end = end;
found = true;
break;
}
}
if (!found)
ranges_.push_back(new MessageRange(begin, end, connection, q));
}
}
view_revision_++;
}
void View::update() {
foreach(BagQuery* query, queries_) {
if (query->bag->bag_revision_ != query->bag_revision) {
updateQueries(query);
query->bag_revision = query->bag->bag_revision_;
}
}
}
std::vector<const ConnectionInfo*> View::getConnections()
{
std::vector<const ConnectionInfo*> connections;
foreach(MessageRange* range, ranges_)
{
connections.push_back(range->connection_info);
}
return connections;
}
MessageInstance* View::newMessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index, Bag const& bag)
{
return new MessageInstance(connection_info, index, bag);
}
} // namespace rosbag