diff --git a/tools/rosbag/CMakeLists.txt b/tools/rosbag/CMakeLists.txt
index 286b14f2b4..4fa8ff90d4 100644
--- a/tools/rosbag/CMakeLists.txt
+++ b/tools/rosbag/CMakeLists.txt
@@ -38,6 +38,11 @@ target_link_libraries(record rosbag)
add_executable(play src/play.cpp)
target_link_libraries(play rosbag)
+if(NOT WIN32)
+ add_executable(encrypt src/encrypt.cpp)
+ target_link_libraries(encrypt ${catkin_LIBRARIES})
+endif()
+
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h")
@@ -49,6 +54,12 @@ install(TARGETS record play
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+if(NOT WIN32)
+ install(TARGETS encrypt
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+endif()
catkin_install_python(PROGRAMS
scripts/bag2png.py
scripts/bagsort.py
diff --git a/tools/rosbag/package.xml b/tools/rosbag/package.xml
index d6234dc5fd..d2591ac8d0 100644
--- a/tools/rosbag/package.xml
+++ b/tools/rosbag/package.xml
@@ -30,6 +30,8 @@
boost
genmsg
genpy
+ python-crypto
+ python-gnupg
python-rospkg
rosbag_storage
rosconsole
diff --git a/tools/rosbag/src/encrypt.cpp b/tools/rosbag/src/encrypt.cpp
new file mode 100644
index 0000000000..33265c84a0
--- /dev/null
+++ b/tools/rosbag/src/encrypt.cpp
@@ -0,0 +1,198 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2017, 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.
+*********************************************************************/
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+
+namespace po = boost::program_options;
+
+struct EncryptorOptions
+{
+ EncryptorOptions() : quiet(false), compression(rosbag::compression::Uncompressed) { }
+
+ void buildOutbagName();
+
+ bool quiet;
+ std::string plugin;
+ std::string param;
+ rosbag::CompressionType compression;
+ std::string inbag;
+ std::string outbag;
+};
+
+void EncryptorOptions::buildOutbagName()
+{
+ if (!outbag.empty())
+ return;
+ if (inbag.empty())
+ throw ros::Exception("Input bag is not specified.");
+ std::string::size_type pos = inbag.find_last_of('.');
+ if (pos == std::string::npos)
+ throw ros::Exception("Input bag name has no extension.");
+ outbag = inbag.substr(0, pos) + std::string(".out") + inbag.substr(pos);
+}
+
+//! Parse the command-line arguments for encrypt options
+EncryptorOptions parseOptions(int argc, char** argv)
+{
+ EncryptorOptions opts;
+
+ po::options_description desc("Allowed options");
+
+ desc.add_options()
+ ("help,h", "produce help message")
+ ("quiet,q", "suppress console output")
+ ("plugin,p", po::value()->default_value("rosbag/AesCbcEncryptor"), "encryptor name")
+ ("param,r", po::value()->default_value("*"), "encryptor parameter")
+ ("bz2,j", "use BZ2 compression")
+ ("lz4", "use lz4 compression")
+ ("inbag", po::value(), "bag file to encrypt")
+ ("outbag,o", po::value(), "bag file encrypted")
+ ;
+
+ po::positional_options_description p;
+ p.add("inbag", -1);
+
+ po::variables_map vm;
+
+ try
+ {
+ po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
+ }
+ catch (boost::program_options::invalid_command_line_syntax& e)
+ {
+ throw ros::Exception(e.what());
+ }
+ catch (boost::program_options::unknown_option& e)
+ {
+ throw ros::Exception(e.what());
+ }
+
+ if (vm.count("help"))
+ {
+ std::cout << desc << std::endl;
+ exit(0);
+ }
+
+ if (vm.count("quiet"))
+ opts.quiet = true;
+ if (vm.count("plugin"))
+ opts.plugin = vm["plugin"].as();
+ if (vm.count("param"))
+ opts.param = vm["param"].as();
+ if (vm.count("bz2"))
+ opts.compression = rosbag::compression::BZ2;
+ if (vm.count("lz4"))
+ opts.compression = rosbag::compression::LZ4;
+ if (vm.count("inbag"))
+ opts.inbag = vm["inbag"].as();
+ else
+ throw ros::Exception("You must specify bag to encrypt.");
+ if (vm.count("outbag"))
+ opts.outbag = vm["outbag"].as();
+ opts.buildOutbagName();
+
+ return opts;
+}
+
+std::string getStringCompressionType(rosbag::CompressionType compression)
+{
+ switch(compression)
+ {
+ case rosbag::compression::Uncompressed: return "none";
+ case rosbag::compression::BZ2: return "bz2";
+ case rosbag::compression::LZ4: return "lz4";
+ default: return "Unknown";
+ }
+}
+
+int encrypt(EncryptorOptions const& options)
+{
+ if (!options.quiet)
+ {
+ std::cout << "Output bag: " << options.outbag << "\n";
+ std::cout << "Encryption: " << options.plugin << ":" << options.param << "\n";
+ std::cout << "Compression: " << getStringCompressionType(options.compression) << "\n";
+ }
+ rosbag::Bag inbag(options.inbag, rosbag::bagmode::Read);
+ rosbag::Bag outbag(options.outbag, rosbag::bagmode::Write);
+ // Compression type is per chunk, and cannot be retained.
+ // If chunk-by-chunk encryption is implemented, compression type could be honored.
+ outbag.setEncryptorPlugin(options.plugin, options.param);
+ outbag.setCompression(options.compression);
+ rosbag::View view(inbag);
+ boost::scoped_ptr progress;
+ if (!options.quiet)
+ progress.reset(new boost::progress_display(view.size(), std::cout, "Progress:\n ", " ", " "));
+ for (rosbag::View::const_iterator it = view.begin(); it != view.end(); ++it)
+ {
+ outbag.write(it->getTopic(), it->getTime(), *it, it->getConnectionHeader());
+ if (progress)
+ ++(*progress);
+ }
+ outbag.close();
+ inbag.close();
+ return 0;
+}
+
+int main(int argc, char** argv)
+{
+ // Parse the command-line options
+ EncryptorOptions opts;
+ try
+ {
+ opts = parseOptions(argc, argv);
+ }
+ catch (ros::Exception const& ex)
+ {
+ ROS_ERROR("Error reading options: %s", ex.what());
+ return 1;
+ }
+ catch(boost::regex_error const& ex)
+ {
+ ROS_ERROR("Error reading options: %s\n", ex.what());
+ return 1;
+ }
+
+ return encrypt(opts);
+}
diff --git a/tools/rosbag/src/rosbag/bag.py b/tools/rosbag/src/rosbag/bag.py
index 3130b43b73..dfa18499cb 100644
--- a/tools/rosbag/src/rosbag/bag.py
+++ b/tools/rosbag/src/rosbag/bag.py
@@ -50,6 +50,10 @@
import time
import yaml
+from Crypto import Random
+from Crypto.Cipher import AES
+import gnupg
+
try:
from cStringIO import StringIO # Python 2.x
except ImportError:
@@ -95,6 +99,20 @@ class ROSBagUnindexedException(ROSBagException):
def __init__(self):
ROSBagException.__init__(self, 'Unindexed bag')
+class ROSBagEncryptNotSupportedException(ROSBagException):
+ """
+ Exception raised when encryption is not supported.
+ """
+ def __init__(self, value):
+ ROSBagException.__init__(self, value)
+
+class ROSBagEncryptException(ROSBagException):
+ """
+ Exception raised when encryption or decryption failed.
+ """
+ def __init__(self, value):
+ ROSBagException.__init__(self, value)
+
class Compression:
"""
Allowable compression types
@@ -105,6 +123,270 @@ class Compression:
BagMessage = collections.namedtuple('BagMessage', 'topic message timestamp')
+class _ROSBagEncryptor(object):
+ """
+ Base class for bag encryptor.
+ """
+ _ENCRYPTOR_FIELD_NAME = 'encryptor'
+
+ def __init__(self):
+ pass
+
+class _ROSBagNoEncryptor(_ROSBagEncryptor):
+ """
+ Class for unencrypted bags.
+ """
+ def __init__(self):
+ super(_ROSBagNoEncryptor, self).__init__()
+
+ def initialize(self, _, __):
+ pass
+
+ def encrypt_chunk(self, chunk_size, _, __):
+ return chunk_size
+
+ def decrypt_chunk(self, chunk):
+ return chunk
+
+ def add_fields_to_file_header(self, _):
+ pass
+
+ def read_fields_from_file_header(self, _):
+ pass
+
+ def write_encrypted_header(self, write_header, f, header):
+ return write_header(f, header)
+
+ def read_encrypted_header(self, read_header, f, req_op=None):
+ return read_header(f, req_op)
+
+ def add_info_rows(self, rows):
+ pass
+
+ def get_info_str(self):
+ return ''
+
+class _ROSBagAesCbcEncryptor(_ROSBagEncryptor):
+ """
+ Class for AES-CBC-encrypted bags.
+ """
+ NAME = 'rosbag/AesCbcEncryptor'
+ _GPG_USER_FIELD_NAME = 'gpg_user'
+ _ENCRYPTED_KEY_FIELD_NAME = 'encrypted_key'
+
+ def __init__(self):
+ """
+ Create AES encryptor.
+ """
+ super(_ROSBagAesCbcEncryptor, self).__init__()
+ # User name of GPG key used for symmetric key encryption
+ self._gpg_key_user = None
+ # Symmetric key for encryption/decryption
+ self._symmetric_key = None
+ # Encrypted symmetric key
+ self._encrypted_symmetric_key = None
+
+ def initialize(self, bag, gpg_key_user):
+ """
+ Initialize encryptor by composing AES symmetric key.
+ @param bag: bag to be encrypted/decrypted
+ @type bag: Bag
+ @param gpg_key_user: user name of GPG key used for symmetric key encryption
+ @type gpg_key_user: str
+ @raise ROSBagException: if GPG key user has already been set
+ """
+ if bag._mode != 'w':
+ return
+ if self._gpg_key_user == gpg_key_user:
+ return
+ if not self._gpg_key_user:
+ self._gpg_key_user = gpg_key_user
+ self._build_symmetric_key()
+ else:
+ raise ROSBagException('Encryption user has already been set to {}'.format(self._gpg_key_user))
+
+ def encrypt_chunk(self, chunk_size, chunk_data_pos, f):
+ """
+ Read chunk from file, encrypt it, and write back to file.
+ @param chunk_size: size of chunk
+ @type chunk_size: int
+ @param chunk_data_pos: position of chunk data portion
+ @type chunk_data_pos: int
+ @param f: file stream
+ @type f: file
+ @return: size of initialization vector and encrypted chunk
+ @rtype: int
+ """
+ f.seek(chunk_data_pos)
+ chunk = _read(f, chunk_size)
+ # Encrypt chunk
+ iv = Random.new().read(AES.block_size)
+ f.seek(chunk_data_pos)
+ f.write(iv)
+ cipher = AES.new(self._symmetric_key, AES.MODE_CBC, iv)
+ encrypted_chunk = cipher.encrypt(_add_padding(chunk))
+ # Write encrypted chunk
+ f.write(encrypted_chunk)
+ f.truncate(f.tell())
+ return AES.block_size + len(encrypted_chunk)
+
+ def decrypt_chunk(self, encrypted_chunk):
+ """
+ Decrypt chunk.
+ @param encrypted_chunk: chunk to decrypt
+ @type encrypted_chunk: str
+ @return: decrypted chunk
+ @rtype: str
+ @raise ROSBagFormatException: if size of input chunk is not multiple of AES block size
+ """
+ if len(encrypted_chunk) % AES.block_size != 0:
+ raise ROSBagFormatException('Error in encrypted chunk size: {}'.format(len(encrypted_chunk)))
+ if len(encrypted_chunk) < AES.block_size:
+ raise ROSBagFormatException('No initialization vector in encrypted chunk: {}'.format(len(encrypted_chunk)))
+
+ iv = encrypted_chunk[:AES.block_size]
+ cipher = AES.new(self._symmetric_key, AES.MODE_CBC, iv)
+ decrypted_chunk = cipher.decrypt(encrypted_chunk[AES.block_size:])
+ return _remove_padding(decrypted_chunk)
+
+ def add_fields_to_file_header(self, header):
+ """
+ Add encryptor information to bag file header.
+ @param header: bag file header
+ @type header: dict
+ """
+ header[self._ENCRYPTOR_FIELD_NAME] = self.NAME
+ header[self._GPG_USER_FIELD_NAME] = self._gpg_key_user
+ header[self._ENCRYPTED_KEY_FIELD_NAME] = self._encrypted_symmetric_key
+
+ def read_fields_from_file_header(self, header):
+ """
+ Read encryptor information from bag file header.
+ @param header: bag file header
+ @type header: dict
+ @raise ROSBagFormatException: if GPG key user is not found in header
+ """
+ try:
+ self._encrypted_symmetric_key = _read_str_field(header, self._ENCRYPTED_KEY_FIELD_NAME)
+ except ROSBagFormatException:
+ raise ROSBagFormatException('Encrypted symmetric key is not found in header')
+ try:
+ self._gpg_key_user = _read_str_field(header, self._GPG_USER_FIELD_NAME)
+ except ROSBagFormatException:
+ raise ROSBagFormatException('GPG key user is not found in header')
+ try:
+ self._symmetric_key = _decrypt_string_gpg(self._encrypted_symmetric_key)
+ except ROSBagFormatException:
+ raise
+
+ def write_encrypted_header(self, _, f, header):
+ """
+ Write encrypted header to bag file.
+ @param f: file stream
+ @type f: file
+ @param header: unencrypted header
+ @type header: dict
+ @return: encrypted string representing header
+ @rtype: str
+ """
+ header_str = b''
+ equal = b'='
+ for k, v in header.items():
+ if not isinstance(k, bytes):
+ k = k.encode()
+ if not isinstance(v, bytes):
+ v = v.encode()
+ header_str += _pack_uint32(len(k) + 1 + len(v)) + k + equal + v
+
+ iv = Random.new().read(AES.block_size)
+ enc_str = iv
+ cipher = AES.new(self._symmetric_key, AES.MODE_CBC, iv)
+ enc_str += cipher.encrypt(_add_padding(header_str))
+ _write_sized(f, enc_str)
+ return enc_str
+
+ def read_encrypted_header(self, _, f, req_op=None):
+ """
+ Read encrypted header from bag file.
+ @param f: file stream
+ @type f: file
+ @param req_op: expected header op code
+ @type req_op: int
+ @return: decrypted header
+ @rtype: dict
+ @raise ROSBagFormatException: if error occurs while decrypting/reading header
+ """
+ # Read header
+ try:
+ header = self._decrypt_encrypted_header(f)
+ except ROSBagException as ex:
+ raise ROSBagFormatException('Error reading header: %s' % str(ex))
+
+ return _build_header_from_str(header, req_op)
+
+ def add_info_rows(self, rows):
+ """
+ Add rows for rosbag info.
+ @param rows: information on bag encryption
+ @type rows: list of tuples
+ """
+ rows.append(('encryption', self.NAME))
+ rows.append(('GPG key user', self._gpg_key_user))
+
+ def get_info_str(self):
+ """
+ Return string for rosbag info.
+ @return: information on bag encryption
+ @rtype: str
+ """
+ return 'encryption: %s\nGPG key user: %s\n' % (self.NAME, self._gpg_key_user)
+
+ def _build_symmetric_key(self):
+ if not self._gpg_key_user:
+ return
+ self._symmetric_key = Random.new().read(AES.block_size)
+ self._encrypted_symmetric_key = _encrypt_string_gpg(self._gpg_key_user, self._symmetric_key)
+
+ def _decrypt_encrypted_header(self, f):
+ try:
+ size = _read_uint32(f)
+ except struct.error as ex:
+ raise ROSBagFormatException('error unpacking uint32: %s' % str(ex))
+
+ if size % AES.block_size != 0:
+ raise ROSBagFormatException('Error in encrypted header size: {}'.format(size))
+ if size < AES.block_size:
+ raise ROSBagFormatException('No initialization vector in encrypted header: {}'.format(size))
+
+ iv = _read(f, AES.block_size)
+ size -= AES.block_size
+ encrypted_header = _read(f, size)
+ cipher = AES.new(self._symmetric_key, AES.MODE_CBC, iv)
+ header = cipher.decrypt(encrypted_header)
+ return _remove_padding(header)
+
+def _add_padding(input_str):
+ # Add PKCS#7 padding to input string
+ return input_str + (AES.block_size - len(input_str) % AES.block_size) * chr(AES.block_size - len(input_str) % AES.block_size)
+
+def _remove_padding(input_str):
+ # Remove PKCS#7 padding from input string
+ return input_str[:-ord(input_str[len(input_str) - 1:])]
+
+def _encrypt_string_gpg(key_user, input):
+ gpg = gnupg.GPG()
+ enc_data = gpg.encrypt(input, [key_user], always_trust=True)
+ if not enc_data.ok:
+ raise ROSBagEncryptException('Failed to encrypt bag: {}. Have you installed a required public key?'.format(enc_data.status))
+ return str(enc_data)
+
+def _decrypt_string_gpg(input):
+ gpg = gnupg.GPG()
+ dec_data = gpg.decrypt(input, passphrase='clearpath')
+ if not dec_data.ok:
+ raise ROSBagEncryptException('Failed to decrypt bag: {}. Have you installed a required private key?'.format(dec_data.status))
+ return str(dec_data)
+
class Bag(object):
"""
Bag serialize messages to and from a single file on disk using the bag format.
@@ -170,6 +452,8 @@ def __init__(self, f, mode='r', compression=Compression.NONE, chunk_threshold=76
self._curr_compression = Compression.NONE
+ self._encryptor = _ROSBagNoEncryptor()
+
self._open(f, mode, allow_unindexed)
self._output_file = self._file
@@ -349,8 +633,8 @@ def write(self, topic, msg, t=None, raw=False):
header = { 'topic' : topic, 'type' : msg.__class__._type, 'md5sum' : msg.__class__._md5sum, 'message_definition' : msg._full_text }
connection_info = _ConnectionInfo(conn_id, topic, header)
-
- self._write_connection_record(connection_info)
+ # No need to encrypt connection records in chunk (encrypt=False)
+ self._write_connection_record(connection_info, False)
self._connections[conn_id] = connection_info
self._topic_connections[topic] = connection_info
@@ -607,7 +891,21 @@ def get_type_and_topic_info(self, topic_filters=None):
frequency=frequency)
return collections.namedtuple("TypesAndTopicsTuple", ["msg_types", "topics"])(msg_types=types, topics=topics_t)
-
+
+ def set_encryptor(self, encryptor=None, param=None):
+ if self._chunks:
+ raise ROSBagException('Cannot set encryptor after chunks are written')
+ if encryptor is None:
+ self._encryptor = _ROSBagNoEncryptor()
+ elif encryptor == _ROSBagAesCbcEncryptor.NAME:
+ if sys.platform == 'win32':
+ raise ROSBagEncryptNotSupportedException('AES CBC encryptor is not supported for Windows')
+ else:
+ self._encryptor = _ROSBagAesCbcEncryptor()
+ else:
+ self._encryptor = _ROSBagNoEncryptor()
+ self._encryptor.initialize(self, param)
+
def __str__(self):
rows = []
@@ -712,6 +1010,8 @@ def __str__(self):
rows.append(('uncompressed', '%*s' % (total_size_str_length, total_uncompressed_size_str)))
rows.append(('compressed', '%*s' % (total_size_str_length, total_compressed_size_str)))
+ self._encryptor.add_info_rows(rows)
+
datatypes = set()
datatype_infos = []
for connection in self._connections.values():
@@ -859,6 +1159,8 @@ def _get_yaml_info(self, key=None):
s += 'uncompressed: %d\n' % sum((h.uncompressed_size for h in self._chunk_headers.values()))
s += 'compressed: %d\n' % sum((h.compressed_size for h in self._chunk_headers.values()))
+ s += self._encryptor.get_info_str()
+
datatypes = set()
datatype_infos = []
for connection in self._connections.values():
@@ -1265,7 +1567,8 @@ def _stop_writing(self):
# Write connection infos
for connection_info in self._connections.values():
- self._write_connection_record(connection_info)
+ # Encrypt connection records in index data (encrypt: True)
+ self._write_connection_record(connection_info, True)
# Write chunk infos
for chunk_info in self._chunks:
@@ -1314,6 +1617,10 @@ def _stop_writing_chunk(self):
self._set_compression_mode(Compression.NONE)
compressed_size = self._file.tell() - self._curr_chunk_data_pos
+ # When encryption is on, compressed_size represents encrypted chunk size;
+ # When decrypting, the actual compressed size can be deduced from the decrypted chunk
+ compressed_size = self._encryptor.encrypt_chunk(compressed_size, self._curr_chunk_data_pos, self._file)
+
# Rewrite the chunk header with the size of the chunk (remembering current offset)
end_of_chunk_pos = self._file.tell()
self._file.seek(self._curr_chunk_info.pos)
@@ -1354,18 +1661,24 @@ def _write_file_header_record(self, index_pos, connection_count, chunk_count):
'conn_count': _pack_uint32(connection_count),
'chunk_count': _pack_uint32(chunk_count)
}
+ self._encryptor.add_fields_to_file_header(header)
_write_record(self._file, header, padded_size=_FILE_HEADER_LENGTH)
- def _write_connection_record(self, connection_info):
+ def _write_connection_record(self, connection_info, encrypt):
header = {
'op': _pack_uint8(_OP_CONNECTION),
'topic': connection_info.topic,
'conn': _pack_uint32(connection_info.id)
}
-
- _write_header(self._output_file, header)
-
- _write_header(self._output_file, connection_info.header)
+ if encrypt:
+ self._encryptor.write_encrypted_header(_write_header, self._output_file, header)
+ else:
+ _write_header(self._output_file, header)
+
+ if encrypt:
+ self._encryptor.write_encrypted_header(_write_header, self._output_file, connection_info.header)
+ else:
+ _write_header(self._output_file, connection_info.header)
def _write_message_data_record(self, connection_id, t, serialized_bytes):
header = {
@@ -1674,6 +1987,9 @@ def _read_header(f, req_op=None):
except ROSBagException as ex:
raise ROSBagFormatException('Error reading header: %s' % str(ex))
+ return _build_header_from_str(header, req_op)
+
+def _build_header_from_str(header, req_op):
# Parse header into a dict
header_dict = {}
while header != b'':
@@ -2107,7 +2423,6 @@ def reindex(self):
chunk_pos = f.tell()
if chunk_pos >= total_bytes:
break
-
yield chunk_pos
try:
@@ -2129,10 +2444,21 @@ def _reindex_read_chunk(self, f, chunk_pos, total_bytes):
raise ROSBagException('unterminated chunk at %d' % chunk_pos)
if chunk_header.compression == Compression.NONE:
- chunk_file = f
+ encrypted_chunk = _read(f, chunk_header.compressed_size)
+
+ chunk = self.bag._encryptor.decrypt_chunk(encrypted_chunk)
+
+ if self.decompressed_chunk_io:
+ self.decompressed_chunk_io.close()
+ self.decompressed_chunk_io = StringIO(chunk)
+
+ chunk_file = self.decompressed_chunk_io
+
else:
- # Read the compressed chunk
- compressed_chunk = _read(f, chunk_header.compressed_size)
+ # Read the chunk, and decrypt/decompress it
+ encrypted_chunk = _read(f, chunk_header.compressed_size)
+
+ compressed_chunk = self.bag._encryptor.decrypt_chunk(encrypted_chunk)
# Decompress it
if chunk_header.compression == Compression.BZ2:
@@ -2151,18 +2477,15 @@ def _reindex_read_chunk(self, f, chunk_pos, total_bytes):
# Read chunk connection and message records
self.bag._curr_chunk_info = None
- if chunk_header.compression == Compression.NONE:
- offset = chunk_file.tell() - chunk_pos
- else:
- offset = chunk_file.tell()
+ offset = chunk_file.tell()
expected_index_length = 0
while offset < chunk_header.uncompressed_size:
op = _peek_next_header_op(chunk_file)
-
if op == _OP_CONNECTION:
- connection_info = self.read_connection_record(chunk_file)
+ # Connection records in chunk are not encrypted (encrypt: False)
+ connection_info = self.read_connection_record(chunk_file, False)
if connection_info.id not in self.bag._connections:
self.bag._connections[connection_info.id] = connection_info
@@ -2203,10 +2526,7 @@ def _reindex_read_chunk(self, f, chunk_pos, total_bytes):
# Unknown record type so skip
_skip_record(chunk_file)
- if chunk_header.compression == Compression.NONE:
- offset = chunk_file.tell() - chunk_pos
- else:
- offset = chunk_file.tell()
+ offset = chunk_file.tell()
# Skip over index records, connection records and chunk info records
next_op = _peek_next_header_op(f)
@@ -2243,7 +2563,7 @@ def _read_terminal_connection_records(self):
if self._advance_to_next_record(_OP_CONNECTION):
# Read the CONNECTION records
while True:
- connection_info = r.read_connection_record(f)
+ connection_info = r.read_connection_record(f, False)
b._connections[connection_info.id] = connection_info
b._connection_indexes[connection_info.id] = []
@@ -2292,7 +2612,8 @@ def start_reading(self):
# Read the connection records
self.bag._connection_indexes = {}
for i in range(self.bag._connection_count):
- connection_info = self.read_connection_record(self.bag._file)
+ # Connection records in index data are encrypted (encrypt: True)
+ connection_info = self.read_connection_record(self.bag._file, True)
self.bag._connections[connection_info.id] = connection_info
self.bag._connection_indexes[connection_info.id] = []
@@ -2308,6 +2629,10 @@ def start_reading(self):
if not self.bag._skip_index:
self._read_connection_index_records()
+ except ROSBagEncryptNotSupportedException:
+ raise
+ except ROSBagEncryptException:
+ raise
except Exception as ex:
raise ROSBagUnindexedException()
@@ -2346,16 +2671,29 @@ def read_file_header_record(self):
self.bag._index_data_pos = _read_uint64_field(header, 'index_pos')
self.bag._chunk_count = _read_uint32_field(header, 'chunk_count')
self.bag._connection_count = _read_uint32_field(header, 'conn_count')
+ try:
+ encryptor = _read_str_field(header, 'encryptor')
+ self.bag.set_encryptor(encryptor)
+ self.bag._encryptor.read_fields_from_file_header(header)
+ except ROSBagFormatException:
+ # If encryptor header is not found, keep going
+ pass
_skip_sized(self.bag._file) # skip over the record data, i.e. padding
- def read_connection_record(self, f):
- header = _read_header(f, _OP_CONNECTION)
+ def read_connection_record(self, f, encrypt):
+ if encrypt:
+ header = self.bag._encryptor.read_encrypted_header(_read_header, f, _OP_CONNECTION)
+ else:
+ header = _read_header(f, _OP_CONNECTION)
conn_id = _read_uint32_field(header, 'conn')
topic = _read_str_field (header, 'topic')
- connection_header = _read_header(f)
+ if encrypt:
+ connection_header = self.bag._encryptor.read_encrypted_header(_read_header, f)
+ else:
+ connection_header = _read_header(f)
return _ConnectionInfo(conn_id, topic, connection_header)
@@ -2430,13 +2768,25 @@ def seek_and_read_message_data_record(self, position, raw):
raise ROSBagException('no chunk at position %d' % chunk_pos)
if chunk_header.compression == Compression.NONE:
- f = self.bag._file
- f.seek(chunk_header.data_pos + offset)
+ if self.decompressed_chunk_pos != chunk_pos:
+ f = self.bag._file
+ f.seek(chunk_header.data_pos)
+ encrypted_chunk = _read(f, chunk_header.compressed_size)
+
+ chunk = self.bag._encryptor.decrypt_chunk(encrypted_chunk)
+
+ self.decompressed_chunk_pos = chunk_pos
+
+ if self.decompressed_chunk_io:
+ self.decompressed_chunk_io.close()
+ self.decompressed_chunk_io = StringIO(chunk)
else:
if self.decompressed_chunk_pos != chunk_pos:
# Seek to the chunk data, read and decompress
self.bag._file.seek(chunk_header.data_pos)
- compressed_chunk = _read(self.bag._file, chunk_header.compressed_size)
+ encrypted_chunk = _read(self.bag._file, chunk_header.compressed_size)
+
+ compressed_chunk = self.bag._encryptor.decrypt_chunk(encrypted_chunk)
if chunk_header.compression == Compression.BZ2:
self.decompressed_chunk = bz2.decompress(compressed_chunk)
@@ -2451,8 +2801,8 @@ def seek_and_read_message_data_record(self, position, raw):
self.decompressed_chunk_io.close()
self.decompressed_chunk_io = StringIO(self.decompressed_chunk)
- f = self.decompressed_chunk_io
- f.seek(offset)
+ f = self.decompressed_chunk_io
+ f.seek(offset)
# Skip any CONNECTION records
while True:
diff --git a/tools/rosbag/src/rosbag/rosbag_main.py b/tools/rosbag/src/rosbag/rosbag_main.py
index 1203880ff8..6996df085e 100644
--- a/tools/rosbag/src/rosbag/rosbag_main.py
+++ b/tools/rosbag/src/rosbag/rosbag_main.py
@@ -47,7 +47,7 @@
import roslib.message
import roslib.packages
-from .bag import Bag, Compression, ROSBagException, ROSBagFormatException, ROSBagUnindexedException
+from .bag import Bag, Compression, ROSBagException, ROSBagFormatException, ROSBagUnindexedException, ROSBagEncryptNotSupportedException, ROSBagEncryptException
from .migration import MessageMigrator, fixbag2, checkbag
def print_trans(old, new, indent):
@@ -167,6 +167,8 @@ def info_cmd(argv):
if i < len(args) - 1:
print('---')
+ except (ROSBagEncryptNotSupportedException, ROSBagEncryptException) as ex:
+ print('ERROR: %s' % str(ex), file=sys.stderr)
except ROSBagUnindexedException as ex:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % arg,
file=sys.stderr)
@@ -332,6 +334,9 @@ def eval_fn(topic, m, t):
try:
inbag = Bag(inbag_filename)
+ except (ROSBagEncryptNotSupportedException, ROSBagEncryptException) as ex:
+ print('ERROR: %s' % str(ex), file=sys.stderr)
+ return
except ROSBagUnindexedException as ex:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % inbag_filename, file=sys.stderr)
return
@@ -429,6 +434,9 @@ def fix_cmd(argv):
try:
migrations = fixbag2(migrator, inbag_filename, outname, options.force)
+ except (ROSBagEncryptNotSupportedException, ROSBagEncryptException) as ex:
+ print('ERROR: %s' % str(ex), file=sys.stderr)
+ return
except ROSBagUnindexedException as ex:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % inbag_filename,
file=sys.stderr)
@@ -476,6 +484,9 @@ def check_cmd(argv):
# First check that the bag is not unindexed
try:
Bag(args[0])
+ except (ROSBagEncryptNotSupportedException, ROSBagEncryptException) as ex:
+ print('ERROR: %s' % str(ex), file=sys.stderr)
+ return
except ROSBagUnindexedException as ex:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % args[0], file=sys.stderr)
return
@@ -557,7 +568,7 @@ def compress_cmd(argv):
op = lambda inbag, outbag, quiet: change_compression_op(inbag, outbag, options.compression, options.quiet)
- bag_op(args, False, lambda b: False, op, options.output_dir, options.force, options.quiet)
+ bag_op(args, False, True, lambda b: False, op, options.output_dir, options.force, options.quiet)
def decompress_cmd(argv):
parser = optparse.OptionParser(usage='rosbag decompress [options] BAGFILE1 [BAGFILE2 ...]',
@@ -573,7 +584,7 @@ def decompress_cmd(argv):
op = lambda inbag, outbag, quiet: change_compression_op(inbag, outbag, Compression.NONE, options.quiet)
- bag_op(args, False, lambda b: False, op, options.output_dir, options.force, options.quiet)
+ bag_op(args, False, True, lambda b: False, op, options.output_dir, options.force, options.quiet)
def reindex_cmd(argv):
parser = optparse.OptionParser(usage='rosbag reindex [options] BAGFILE1 [BAGFILE2 ...]',
@@ -589,24 +600,63 @@ def reindex_cmd(argv):
op = lambda inbag, outbag, quiet: reindex_op(inbag, outbag, options.quiet)
- bag_op(args, True, lambda b: b.version > 102, op, options.output_dir, options.force, options.quiet)
+ bag_op(args, True, True, lambda b: b.version > 102, op, options.output_dir, options.force, options.quiet)
+
+def encrypt_cmd(argv):
+ parser = optparse.OptionParser(usage='rosbag encrypt [options] BAGFILE1 [BAGFILE2 ...]',
+ description='Encrypt one or more bag files.')
+ parser.add_option( '--output-dir', action='store', dest='output_dir', help='write to directory DIR', metavar='DIR')
+ parser.add_option('-f', '--force', action='store_true', dest='force', help='force overwriting of backup file if it exists')
+ parser.add_option('-q', '--quiet', action='store_true', dest='quiet', help='suppress noncritical messages')
+ parser.add_option("-p", "--plugin", action='store', dest="plugin", default='rosbag/AesCbcEncryptor', help='encryptor plugin name')
+ parser.add_option("-r", "--param", action='store', dest="param", default='*', help='encryptor plugin parameter')
+ parser.add_option('-j', '--bz2', action='store_const', dest='compression', help='use BZ2 compression', const=Compression.BZ2, default=Compression.NONE)
+ parser.add_option( '--lz4', action='store_const', dest='compression', help='use lz4 compression', const=Compression.LZ4)
+ (options, args) = parser.parse_args(argv)
+
+ if len(args) < 1:
+ parser.error('You must specify at least one bag file.')
+
+ op = lambda inbag, outbag, quiet: change_encryption_op(inbag, outbag, options.plugin, options.param, options.compression, options.quiet)
+
+ bag_op(args, False, True, lambda b: False, op, options.output_dir, options.force, options.quiet)
-def bag_op(inbag_filenames, allow_unindexed, copy_fn, op, output_dir=None, force=False, quiet=False):
+def decrypt_cmd(argv):
+ parser = optparse.OptionParser(usage='rosbag decrypt [options] BAGFILE1 [BAGFILE2 ...]',
+ description='Decrypt one or more bag files.')
+ parser.add_option( '--output-dir', action='store', dest='output_dir', help='write to directory DIR', metavar='DIR')
+ parser.add_option('-f', '--force', action='store_true', dest='force', help='force overwriting of backup file if it exists')
+ parser.add_option('-q', '--quiet', action='store_true', dest='quiet', help='suppress noncritical messages')
+ parser.add_option('-j', '--bz2', action='store_const', dest='compression', help='use BZ2 compression', const=Compression.BZ2, default=Compression.NONE)
+ parser.add_option( '--lz4', action='store_const', dest='compression', help='use lz4 compression', const=Compression.LZ4)
+ (options, args) = parser.parse_args(argv)
+
+ if len(args) < 1:
+ parser.error('You must specify at least one bag file.')
+
+ op = lambda inbag, outbag, quiet: change_encryption_op(inbag, outbag, 'rosbag/NoEncryptor', '*', options.compression, options.quiet)
+ # Note the second paramater is True: Python Bag class cannot read index information from encrypted bag files
+ bag_op(args, True, False, lambda b: False, op, options.output_dir, options.force, options.quiet)
+
+def bag_op(inbag_filenames, allow_unindexed, open_inbag, copy_fn, op, output_dir=None, force=False, quiet=False):
for inbag_filename in inbag_filenames:
- # Check we can read the file
- try:
- inbag = Bag(inbag_filename, 'r', allow_unindexed=allow_unindexed)
- except ROSBagUnindexedException:
- print('ERROR bag unindexed: %s. Run rosbag reindex.' % inbag_filename, file=sys.stderr)
- continue
- except (ROSBagException, IOError) as ex:
- print('ERROR reading %s: %s' % (inbag_filename, str(ex)), file=sys.stderr)
- continue
+ if open_inbag:
+ # Check we can read the file
+ try:
+ inbag = Bag(inbag_filename, 'r', allow_unindexed=allow_unindexed)
+ except ROSBagUnindexedException:
+ print('ERROR bag unindexed: %s. Run rosbag reindex.' % inbag_filename, file=sys.stderr)
+ continue
+ except (ROSBagException, IOError) as ex:
+ print('ERROR reading %s: %s' % (inbag_filename, str(ex)), file=sys.stderr)
+ continue
- # Determine whether we should copy the bag
- copy = copy_fn(inbag)
-
- inbag.close()
+ # Determine whether we should copy the bag
+ copy = copy_fn(inbag)
+
+ inbag.close()
+ else:
+ copy = False
# Determine filename for output bag
if output_dir is None:
@@ -642,30 +692,51 @@ def bag_op(inbag_filenames, allow_unindexed, copy_fn, op, output_dir=None, force
source_filename = inbag_filename
try:
- inbag = Bag(source_filename, 'r', allow_unindexed=allow_unindexed)
+ if open_inbag:
+ inbag = Bag(source_filename, 'r', allow_unindexed=allow_unindexed)
- # Open the output bag file for writing
- try:
- if copy:
- outbag = Bag(outbag_filename, 'a', allow_unindexed=allow_unindexed)
- else:
- outbag = Bag(outbag_filename, 'w')
- except (ROSBagException, IOError) as ex:
- print('ERROR writing to %s: %s' % (outbag_filename, str(ex)), file=sys.stderr)
- inbag.close()
- continue
+ # Open the output bag file for writing
+ try:
+ if copy:
+ outbag = Bag(outbag_filename, 'a', allow_unindexed=allow_unindexed)
+ else:
+ outbag = Bag(outbag_filename, 'w')
+ except (ROSBagException, IOError) as ex:
+ print('ERROR writing to %s: %s' % (outbag_filename, str(ex)), file=sys.stderr)
+ inbag.close()
+ continue
- # Perform the operation
- try:
- op(inbag, outbag, quiet=quiet)
- except ROSBagException as ex:
- print('\nERROR operating on %s: %s' % (source_filename, str(ex)), file=sys.stderr)
+ # Perform the operation
+ try:
+ op(inbag, outbag, quiet=quiet)
+ except ROSBagException as ex:
+ print('\nERROR operating on %s: %s' % (source_filename, str(ex)), file=sys.stderr)
+ inbag.close()
+ outbag.close()
+ continue
+
+ outbag.close()
inbag.close()
+ else:
+ # Open the output bag file for writing
+ try:
+ if copy:
+ outbag = Bag(outbag_filename, 'a', allow_unindexed=allow_unindexed)
+ else:
+ outbag = Bag(outbag_filename, 'w')
+ except (ROSBagException, IOError) as ex:
+ print('ERROR writing to %s: %s' % (outbag_filename, str(ex)), file=sys.stderr)
+ continue
+
+ # Perform the operation
+ try:
+ op(source_filename, outbag, quiet=quiet)
+ except ROSBagException as ex:
+ print('\nERROR operating on %s: %s' % (source_filename, str(ex)), file=sys.stderr)
+ outbag.close()
+ continue
+
outbag.close()
- continue
-
- outbag.close()
- inbag.close()
except KeyboardInterrupt:
if backup_filename is not None:
@@ -731,6 +802,8 @@ def reindex_op(inbag, outbag, quiet):
try:
for offset in outbag.reindex():
pass
+ except (ROSBagEncryptNotSupportedException, ROSBagEncryptException) as ex:
+ raise
except:
pass
else:
@@ -738,10 +811,42 @@ def reindex_op(inbag, outbag, quiet):
try:
for offset in outbag.reindex():
meter.step(offset)
+ except (ROSBagEncryptNotSupportedException, ROSBagEncryptException) as ex:
+ raise
except:
pass
meter.finish()
+def change_encryption_op(inbag, outbag, plugin, param, compression, quiet):
+ # Output file must be closed before written by the encrypt process
+ outbag.close()
+
+ encryptpath = roslib.packages.find_node('rosbag', 'encrypt')
+ if not encryptpath:
+ parser.error("Cannot find rosbag/encrypt executable")
+ cmd = [encryptpath[0]]
+ if type(inbag) is str:
+ cmd.extend([inbag])
+ else:
+ cmd.extend([inbag.filename])
+ cmd.extend(['-o', outbag.filename])
+ cmd.extend(['-p', plugin])
+ cmd.extend(['-r', param])
+ if compression == 'bz2':
+ cmd.extend(['-j'])
+ elif compression == 'lz4':
+ cmd.extend(['--lz4'])
+ if quiet:
+ cmd.extend(['-q'])
+
+ old_handler = signal.signal(
+ signal.SIGTERM,
+ lambda signum, frame: _stop_process(signum, frame, old_handler, process)
+ )
+
+ process = subprocess.Popen(cmd)
+ process.wait()
+
class RosbagCmds(UserDict):
def __init__(self):
UserDict.__init__(self)
@@ -878,6 +983,9 @@ def rosbagmain(argv=None):
cmds.add_cmd('compress', compress_cmd, 'Compress one or more bag files.')
cmds.add_cmd('decompress', decompress_cmd, 'Decompress one or more bag files.')
cmds.add_cmd('reindex', reindex_cmd, 'Reindexes one or more bag files.')
+ if sys.platform != 'win32':
+ cmds.add_cmd('encrypt', encrypt_cmd, 'Encrypt one or more bag files.')
+ cmds.add_cmd('decrypt', decrypt_cmd, 'Decrypt one or more bag files.')
if argv is None:
argv = sys.argv
diff --git a/tools/rosbag_storage/CMakeLists.txt b/tools/rosbag_storage/CMakeLists.txt
index a951ddb93f..75da667da0 100644
--- a/tools/rosbag_storage/CMakeLists.txt
+++ b/tools/rosbag_storage/CMakeLists.txt
@@ -7,14 +7,14 @@ if(NOT WIN32)
endif()
find_package(console_bridge REQUIRED)
-find_package(catkin REQUIRED COMPONENTS cpp_common roscpp_serialization roscpp_traits rostime roslz4)
+find_package(catkin REQUIRED COMPONENTS cpp_common pluginlib 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
+ CATKIN_DEPENDS pluginlib roslz4
DEPENDS console_bridge Boost
)
@@ -24,21 +24,29 @@ 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})
+set(AES_ENCRYPT_SOURCE "")
+set(AES_ENCRYPT_LIBRARIES "")
+if(NOT WIN32)
+ set(AES_ENCRYPT_SOURCE "src/aes_encryptor.cpp")
+ set(AES_ENCRYPT_LIBRARIES "crypto" "gpgme")
+endif()
+
add_library(rosbag_storage
+ ${AES_ENCRYPT_SOURCE}
src/bag.cpp
src/bag_player.cpp
src/buffer.cpp
src/bz2_stream.cpp
src/lz4_stream.cpp
src/chunked_file.cpp
+ src/encryptor.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})
+target_link_libraries(rosbag_storage ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BZIP2_LIBRARIES} ${console_bridge_LIBRARIES} ${AES_ENCRYPT_LIBRARIES})
install(TARGETS rosbag_storage
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -50,3 +58,19 @@ install(DIRECTORY include/
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
+
+if(NOT WIN32)
+ install(FILES encryptor_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ )
+
+ if(CATKIN_ENABLE_TESTING)
+ find_package(rostest)
+
+ catkin_add_gtest(test_aes_encryptor test/test_aes_encryptor.cpp
+ WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
+ if(TARGET test_aes_encryptor)
+ target_link_libraries(test_aes_encryptor rosbag_storage ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+ endif()
+ endif()
+endif()
diff --git a/tools/rosbag_storage/encryptor_plugins.xml b/tools/rosbag_storage/encryptor_plugins.xml
new file mode 100644
index 0000000000..91c0d31dc4
--- /dev/null
+++ b/tools/rosbag_storage/encryptor_plugins.xml
@@ -0,0 +1,8 @@
+
+
+ This is a plugin for no encryption.
+
+
+ This is a plugin for AES-128 CBC encryption using a GPG key.
+
+
diff --git a/tools/rosbag_storage/include/rosbag/bag.h b/tools/rosbag_storage/include/rosbag/bag.h
index 1c3ceb93ae..ebb2ae263f 100644
--- a/tools/rosbag_storage/include/rosbag/bag.h
+++ b/tools/rosbag_storage/include/rosbag/bag.h
@@ -40,6 +40,7 @@
#include "rosbag/buffer.h"
#include "rosbag/chunked_file.h"
#include "rosbag/constants.h"
+#include "rosbag/encryptor.h"
#include "rosbag/exceptions.h"
#include "rosbag/structures.h"
@@ -61,6 +62,8 @@
#include
#include
+#include
+
#include "console_bridge/console.h"
#if defined logDebug
# undef logDebug
@@ -141,6 +144,16 @@ class ROSBAG_DECL Bag
void setChunkThreshold(uint32_t chunk_threshold); //!< Set the threshold for creating new chunks
uint32_t getChunkThreshold() const; //!< Get the threshold for creating new chunks
+ //! Set encryptor of the bag file
+ /*!
+ * \param plugin_name The name of the encryptor plugin
+ * \param plugin_param The string parameter to be passed to the plugin initialization method
+ *
+ * Call this method to specify an encryptor for writing bag contents. This method need not be called when
+ * reading or appending a bag file: The encryptor is read from the bag file header.
+ */
+ void setEncryptorPlugin(const std::string& plugin_name, const std::string& plugin_param = std::string());
+
//! Write a message into the bag file
/*!
* \param topic The topic name
@@ -224,7 +237,7 @@ class ROSBAG_DECL Bag
void writeVersion();
void writeFileHeaderRecord();
- void writeConnectionRecord(ConnectionInfo const* connection_info);
+ void writeConnectionRecord(ConnectionInfo const* connection_info, const bool encrypt);
void appendConnectionRecordToBuffer(Buffer& buf, ConnectionInfo const* connection_info);
template
void writeMessageDataRecord(uint32_t conn_id, ros::Time const& time, T const& msg);
@@ -337,6 +350,11 @@ class ROSBAG_DECL Bag
mutable Buffer* current_buffer_;
mutable uint64_t decompressed_chunk_; //!< position of decompressed chunk
+
+ // Encryptor plugin loader
+ pluginlib::ClassLoader encryptor_loader_;
+ // Active encryptor
+ boost::shared_ptr encryptor_;
};
} // namespace rosbag
@@ -569,8 +587,8 @@ void Bag::doWrite(std::string const& topic, ros::Time const& time, T const& msg,
(*connection_info->header)["message_definition"] = connection_info->msg_def;
}
connections_[conn_id] = connection_info;
-
- writeConnectionRecord(connection_info);
+ // No need to encrypt connection records in chunks
+ writeConnectionRecord(connection_info, false);
appendConnectionRecordToBuffer(outgoing_chunk_buffer_, connection_info);
}
diff --git a/tools/rosbag_storage/include/rosbag/constants.h b/tools/rosbag_storage/include/rosbag/constants.h
index 60f4998f61..e1ba0866b4 100644
--- a/tools/rosbag_storage/include/rosbag/constants.h
+++ b/tools/rosbag_storage/include/rosbag/constants.h
@@ -61,6 +61,7 @@ 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+
+static const std::string ENCRYPTOR_FIELD_NAME = "encryptor"; // 2.0+
// Legacy header fields
static const std::string MD5_FIELD_NAME = "md5"; // <2.0
diff --git a/tools/rosbag_storage/include/rosbag/encryptor.h b/tools/rosbag_storage/include/rosbag/encryptor.h
new file mode 100644
index 0000000000..2c3f8adab4
--- /dev/null
+++ b/tools/rosbag_storage/include/rosbag/encryptor.h
@@ -0,0 +1,204 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2017, 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_ENCRYPTION_H
+#define ROSBAG_ENCRYPTION_H
+
+#include "rosbag/buffer.h"
+#include "rosbag/chunked_file.h"
+#include "rosbag/structures.h"
+
+#include "ros/header.h"
+
+#include
+#include
+
+#include
+
+#ifndef _WIN32
+ #include
+ #include
+#endif
+
+namespace rosbag {
+
+class Bag;
+
+class EncryptorBase
+{
+protected:
+ EncryptorBase() { }
+
+public:
+ virtual ~EncryptorBase() { }
+
+ //! Initialize encryptor
+ /*!
+ * \param bag The Bag instance
+ * \param plugin_param The string parameter used while initializing the encryptor
+ *
+ * This method is called by setEncryptorPlugin, which loads an encryptor plugin.
+ */
+ virtual void initialize(Bag const& bag, std::string const& plugin_param) = 0;
+
+ //! Encrypt chunk
+ /*!
+ * \return The byte size of the encrypted chunk
+ * \param chunk_size The byte size of the original chunk
+ * \param chunk_data_pos The start position of the chunk data in bag file stream
+ * \param file The bag file stream
+ *
+ * This method reads the original chunk from [chunk_data_pos, chunk_data_pos+chunk_size), encrypts it, and
+ * writes back to the file stream starting at chunk_data_pos.
+ */
+ virtual uint32_t encryptChunk(const uint32_t chunk_size, const uint64_t chunk_data_pos, ChunkedFile& file) = 0;
+
+ //! Decrypt chunk
+ /*!
+ * \param chunk_header The header of the encrypted chunk
+ * \param decrypted_chunk The buffer where decrypted chunk is written to
+ * \param file The bag file stream from which the encrypted chunk is read
+ *
+ * This method reads the encrypted chunk from file stream, decrypts, and writes it to decrypted_chunk.
+ */
+ virtual void decryptChunk(ChunkHeader const& chunk_header, Buffer& decrypted_chunk, ChunkedFile& file) const = 0;
+
+ //! Add encryptor information to bag file header
+ /*!
+ * \param header_fields The header fields of the bag
+ *
+ * Called for a bag being written, this method adds encryptor-specific fields to the bag file header. Those fields
+ * are used when decrypting the bag. ENCRYPTOR_FIELD_NAME must be specified in the header except for NoEncryptor.
+ */
+ virtual void addFieldsToFileHeader(ros::M_string& header_fields) const = 0;
+
+ //! Read encryptor information from bag file header
+ /*!
+ * \param header_fields The header fields of the bag
+ *
+ * Called for a bag being read, this method reads encryptor-specific fields from the bag file header.
+ */
+ virtual void readFieldsFromFileHeader(ros::M_string const& header_fields) = 0;
+
+ //! Write encrypted header to bag file
+ /*!
+ * \param write_header The functor writing unencrypted header
+ * \param header_fields The header fields to be written
+ * \param file The bag file stream
+ *
+ * This method encrypts given header fields, and writes them to the bag file.
+ */
+ virtual void writeEncryptedHeader(boost::function write_header, ros::M_string const& header_fields, ChunkedFile& file) = 0;
+
+ //! Read encrypted header from bag file
+ /*!
+ * \param read_header The functor reading unencrypted header
+ * \param header The header object read
+ * \param header_buffer The header buffer read
+ * \param file The bag file stream
+ *
+ * This method reads and decrypts encrypted header to output header object (header) and buffer (header_buffer).
+ */
+ virtual bool readEncryptedHeader(boost::function read_header, ros::Header& header, Buffer& header_buffer, ChunkedFile& file) = 0;
+};
+
+class NoEncryptor : public EncryptorBase
+{
+public:
+ NoEncryptor() { }
+ ~NoEncryptor() { }
+
+ void initialize(Bag const&, std::string const&) { }
+ uint32_t encryptChunk(const uint32_t, const uint64_t, ChunkedFile&);
+ void decryptChunk(ChunkHeader const&, Buffer&, ChunkedFile&) const;
+ void addFieldsToFileHeader(ros::M_string&) const { }
+ void readFieldsFromFileHeader(ros::M_string const&) { }
+ void writeEncryptedHeader(boost::function, ros::M_string const&, ChunkedFile&);
+ bool readEncryptedHeader(boost::function, ros::Header&, Buffer&, ChunkedFile&);
+};
+
+#ifndef _WIN32
+//! Initialize GPGME library
+/*!
+ * This method initializes GPGME library, and set locale.
+ */
+void initGpgme();
+
+//! Get GPG key
+/*!
+ * \param ctx GPGME context
+ * \param user User name of the GPG key
+ * \param key GPG key found
+ *
+ * This method outputs a GPG key in the system keyring corresponding to the given user name.
+ * This method throws BagException if the key is not found or error occurred.
+ */
+void getGpgKey(gpgme_ctx_t& ctx, std::string const& user, gpgme_key_t& key);
+
+class AesCbcEncryptor : public EncryptorBase
+{
+public:
+ static const std::string GPG_USER_FIELD_NAME;
+ static const std::string ENCRYPTED_KEY_FIELD_NAME;
+
+public:
+ AesCbcEncryptor() { }
+ ~AesCbcEncryptor() { }
+
+ void initialize(Bag const& bag, std::string const& gpg_key_user);
+ uint32_t encryptChunk(const uint32_t chunk_size, const uint64_t chunk_data_pos, ChunkedFile& file);
+ void decryptChunk(ChunkHeader const& chunk_header, Buffer& decrypted_chunk, ChunkedFile& file) const;
+ void addFieldsToFileHeader(ros::M_string& header_fields) const;
+ void readFieldsFromFileHeader(ros::M_string const& header_fields);
+ void writeEncryptedHeader(boost::function, ros::M_string const& header_fields, ChunkedFile&);
+ bool readEncryptedHeader(boost::function, ros::Header& header, Buffer& header_buffer, ChunkedFile&);
+
+private:
+ void buildSymmetricKey();
+
+private:
+ // User name of GPG key used for symmetric key encryption
+ std::string gpg_key_user_;
+ // Symmetric key for encryption/decryption
+ std::basic_string symmetric_key_;
+ // Encrypted symmetric key
+ std::string encrypted_symmetric_key_;
+ // AES keys for encryption/decryption
+ AES_KEY aes_encrypt_key_;
+ AES_KEY aes_decrypt_key_;
+};
+#endif
+}
+
+#endif
diff --git a/tools/rosbag_storage/package.xml b/tools/rosbag_storage/package.xml
index eadc8287e6..471e05960b 100644
--- a/tools/rosbag_storage/package.xml
+++ b/tools/rosbag_storage/package.xml
@@ -14,8 +14,12 @@
bzip2
cpp_common
libconsole-bridge-dev
+ libgpgme-dev
+ libssl-dev
+ pluginlib
roscpp_serialization
roscpp_traits
+ rostest
rostime
roslz4
@@ -23,6 +27,9 @@
bzip2
cpp_common
libconsole-bridge-dev
+ libgpgme-dev
+ libssl-dev
+ pluginlib
roscpp_serialization
roscpp_traits
rostime
@@ -30,5 +37,6 @@
+
diff --git a/tools/rosbag_storage/src/aes_encryptor.cpp b/tools/rosbag_storage/src/aes_encryptor.cpp
new file mode 100644
index 0000000000..e0fcb807e5
--- /dev/null
+++ b/tools/rosbag_storage/src/aes_encryptor.cpp
@@ -0,0 +1,372 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2017, 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.
+*********************************************************************/
+
+#include "rosbag/bag.h"
+#include "rosbag/encryptor.h"
+
+#include
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(rosbag::AesCbcEncryptor, rosbag::EncryptorBase)
+
+namespace rosbag
+{
+
+const std::string AesCbcEncryptor::GPG_USER_FIELD_NAME = "gpg_user";
+const std::string AesCbcEncryptor::ENCRYPTED_KEY_FIELD_NAME = "encrypted_key";
+
+void initGpgme() {
+ // Check version method must be called before en/decryption
+ gpgme_check_version(0);
+ // Set locale
+ setlocale(LC_ALL, "");
+ gpgme_set_locale(NULL, LC_CTYPE, setlocale(LC_CTYPE, NULL));
+#ifdef LC_MESSAGES
+ gpgme_set_locale(NULL, LC_MESSAGES, setlocale(LC_MESSAGES, NULL));
+#endif
+}
+
+void getGpgKey(gpgme_ctx_t& ctx, std::string const& user, gpgme_key_t& key) {
+ gpgme_error_t err;
+ // Asterisk means an arbitrary user.
+ if (user == std::string("*")) {
+ err = gpgme_op_keylist_start(ctx, 0, 0);
+ } else {
+ err = gpgme_op_keylist_start(ctx, user.c_str(), 0);
+ }
+ if (err) {
+ throw BagException((boost::format("gpgme_op_keylist_start returned %1%") % gpgme_strerror(err)).str());
+ }
+ while (true) {
+ err = gpgme_op_keylist_next(ctx, &key);
+ if (!err) {
+ if (user == std::string("*") || strcmp(key->uids->name, user.c_str()) == 0) {
+ break;
+ }
+ gpgme_key_release(key);
+ } else if (gpg_err_code(err) == GPG_ERR_EOF) {
+ if (user == std::string("*")) {
+ // A method throws an exception (instead of returning a specific value) if the key is not found
+ // This allows rosbag client applications to work without modifying their source code
+ throw BagException("GPG key not found");
+ } else {
+ throw BagException((boost::format("GPG key not found for a user %1%") % user.c_str()).str());
+ }
+ } else {
+ throw BagException((boost::format("gpgme_op_keylist_next returned %1%") % err).str());
+ }
+ }
+ err = gpgme_op_keylist_end(ctx);
+ if (err) {
+ throw BagException((boost::format("gpgme_op_keylist_end returned %1%") % gpgme_strerror(err)).str());
+ }
+}
+
+//! Encrypt string using GPGME
+/*!
+ * \return Encrypted string
+ * \param user User name of the GPG key to be used for encryption
+ * \param input Input string to be encrypted
+ *
+ * This method encrypts the given string using the GPG key owned by the specified user.
+ * This method throws BagException in case of errors.
+ */
+static std::string encryptStringGpg(std::string& user, std::basic_string const& input) {
+ gpgme_ctx_t ctx;
+ gpgme_error_t err = gpgme_new(&ctx);
+ if (err) {
+ throw BagException((boost::format("Failed to create a GPG context: %1%") % gpgme_strerror(err)).str());
+ }
+
+ gpgme_key_t keys[2] = {NULL, NULL};
+ getGpgKey(ctx, user, keys[0]);
+ if (user == std::string("*")) {
+ user = std::string(keys[0]->uids->name);
+ }
+
+ gpgme_data_t input_data;
+ err = gpgme_data_new_from_mem(&input_data, reinterpret_cast(input.c_str()), input.length(), 1);
+ if (err) {
+ gpgme_release(ctx);
+ throw BagException(
+ (boost::format("Failed to encrypt string: gpgme_data_new_from_mem returned %1%") % gpgme_strerror(err)).str());
+ }
+ gpgme_data_t output_data;
+ err = gpgme_data_new(&output_data);
+ if (err) {
+ gpgme_data_release(input_data);
+ gpgme_release(ctx);
+ throw BagException(
+ (boost::format("Failed to encrypt string: gpgme_data_new returned %1%") % gpgme_strerror(err)).str());
+ }
+ err = gpgme_op_encrypt(ctx, keys, static_cast(GPGME_ENCRYPT_ALWAYS_TRUST), input_data, output_data);
+ if (err) {
+ gpgme_data_release(output_data);
+ gpgme_data_release(input_data);
+ gpgme_release(ctx);
+ throw BagException((boost::format("Failed to encrypt: %1%. Have you installed a public key %2%?") % gpgme_strerror(err) % user).str());
+ }
+ gpgme_key_release(keys[0]);
+ std::size_t output_length = gpgme_data_seek(output_data, 0, SEEK_END);
+ std::string output(output_length, 0);
+ gpgme_data_seek(output_data, 0, SEEK_SET);
+ ssize_t bytes_read = gpgme_data_read(output_data, &output[0], output_length);
+ // Release resources and return
+ gpgme_data_release(output_data);
+ gpgme_data_release(input_data);
+ gpgme_release(ctx);
+ if (-1 == bytes_read) {
+ throw BagException("Failed to read encrypted string");
+ }
+ return output;
+}
+
+//! Decrypt string using GPGME
+/*!
+ * \return Decrypted string
+ * \param user User name of the GPG key to be used for decryption
+ * \param input Encrypted string
+ *
+ * This method decrypts the given encrypted string. This method throws BagException in case of errors.
+ */
+static std::basic_string decryptStringGpg(std::string const& user, std::string const& input) {
+ gpgme_ctx_t ctx;
+ gpgme_error_t err = gpgme_new(&ctx);
+ if (err) {
+ throw BagException((boost::format("Failed to create a GPG context: %1%") % gpgme_strerror(err)).str());
+ }
+
+ gpgme_data_t input_data;
+ err = gpgme_data_new_from_mem(&input_data, input.c_str(), input.length(), 1);
+ if (err) {
+ gpgme_release(ctx);
+ throw BagException(
+ (boost::format("Failed to decrypt bag: gpgme_data_new_from_mem returned %1%") % gpgme_strerror(err)).str());
+ }
+ gpgme_data_t output_data;
+ err = gpgme_data_new(&output_data);
+ if (err) {
+ gpgme_data_release(input_data);
+ gpgme_release(ctx);
+ throw BagException(
+ (boost::format("Failed to decrypt bag: gpgme_data_new returned %1%") % gpgme_strerror(err)).str());
+ }
+ err = gpgme_op_decrypt(ctx, input_data, output_data);
+ if (err) {
+ gpgme_data_release(output_data);
+ gpgme_data_release(input_data);
+ gpgme_release(ctx);
+ throw BagException((boost::format("Failed to decrypt bag: %1%. Have you installed a private key %2%?") % gpgme_strerror(err) % user).str());
+ }
+ std::size_t output_length = gpgme_data_seek(output_data, 0, SEEK_END);
+ if (output_length != AES_BLOCK_SIZE) {
+ gpgme_data_release(output_data);
+ gpgme_data_release(input_data);
+ gpgme_release(ctx);
+ throw BagException("Decrypted string length mismatches");
+ }
+ std::basic_string output(output_length, 0);
+ gpgme_data_seek(output_data, 0, SEEK_SET);
+ ssize_t bytes_read = gpgme_data_read(output_data, reinterpret_cast(&output[0]), output_length);
+ // Release resources and return
+ gpgme_data_release(output_data);
+ gpgme_data_release(input_data);
+ gpgme_release(ctx);
+ if (-1 == bytes_read) {
+ throw BagException("Failed to read decrypted symmetric key");
+ }
+ return output;
+}
+
+static std::string readHeaderField(ros::M_string const& header_fields, std::string const& field_name) {
+ ros::M_string::const_iterator it = header_fields.find(field_name);
+ if (it == header_fields.end()) {
+ return std::string();
+ }
+ return it->second;
+}
+
+void AesCbcEncryptor::initialize(Bag const& bag, std::string const& gpg_key_user) {
+ // GPGME must be initialized even when reading
+ initGpgme();
+ // Encryption user can be set only when writing a bag file
+ if (bag.getMode() != bagmode::Write) {
+ return;
+ }
+ if (gpg_key_user_ == gpg_key_user) {
+ return;
+ }
+ if (gpg_key_user_.empty()) {
+ gpg_key_user_ = gpg_key_user;
+ buildSymmetricKey();
+ AES_set_encrypt_key(&symmetric_key_[0], AES_BLOCK_SIZE*8, &aes_encrypt_key_);
+ } else {
+ // Encryption user cannot change once set
+ throw BagException(
+ (boost::format("Encryption user has already been set to %s") % gpg_key_user_.c_str()).str());
+ }
+}
+
+uint32_t AesCbcEncryptor::encryptChunk(const uint32_t chunk_size, const uint64_t chunk_data_pos, ChunkedFile& file) {
+ // Read existing (compressed) chunk
+ std::basic_string compressed_chunk(chunk_size, 0);
+ file.seek(chunk_data_pos);
+ file.read((char*) &compressed_chunk[0], chunk_size);
+ // Apply PKCS#7 padding to the chunk
+ std::size_t pad_size = AES_BLOCK_SIZE - chunk_size % AES_BLOCK_SIZE;
+ compressed_chunk.resize(compressed_chunk.length() + pad_size, pad_size);
+ // Encrypt chunk
+ std::basic_string encrypted_chunk(compressed_chunk.length(), 0);
+ std::basic_string iv(AES_BLOCK_SIZE, 0);
+ if (!RAND_bytes(&iv[0], AES_BLOCK_SIZE)) {
+ throw BagException("Failed to build initialization vector");
+ }
+ file.seek(chunk_data_pos);
+ file.write((char*) &iv[0], AES_BLOCK_SIZE);
+ AES_cbc_encrypt(&compressed_chunk[0], &encrypted_chunk[0], encrypted_chunk.length(), &aes_encrypt_key_, &iv[0], AES_ENCRYPT);
+ // Write encrypted chunk
+ file.write((char*) &encrypted_chunk[0], encrypted_chunk.length());
+ file.truncate(chunk_data_pos + AES_BLOCK_SIZE + encrypted_chunk.length());
+ return AES_BLOCK_SIZE + encrypted_chunk.length();
+}
+
+void AesCbcEncryptor::decryptChunk(ChunkHeader const& chunk_header, Buffer& decrypted_chunk, ChunkedFile& file) const {
+ // Test encrypted chunk size
+ if (chunk_header.compressed_size % AES_BLOCK_SIZE != 0) {
+ throw BagFormatException((boost::format("Error in encrypted chunk size: %d") % chunk_header.compressed_size).str());
+ }
+ // Read encrypted chunk
+ if (chunk_header.compressed_size < AES_BLOCK_SIZE) {
+ throw BagFormatException((boost::format("No initialization vector in encrypted chunk: %d") % chunk_header.compressed_size).str());
+ }
+ std::basic_string iv(AES_BLOCK_SIZE, 0);
+ file.read((char*) &iv[0], AES_BLOCK_SIZE);
+ std::basic_string encrypted_chunk(chunk_header.compressed_size - AES_BLOCK_SIZE, 0);
+ file.read((char*) &encrypted_chunk[0], chunk_header.compressed_size - AES_BLOCK_SIZE);
+ // Decrypt chunk
+ decrypted_chunk.setSize(chunk_header.compressed_size - AES_BLOCK_SIZE);
+ AES_cbc_encrypt(&encrypted_chunk[0], (unsigned char*) decrypted_chunk.getData(), chunk_header.compressed_size - AES_BLOCK_SIZE,
+ &aes_decrypt_key_, &iv[0], AES_DECRYPT);
+ if (decrypted_chunk.getSize() == 0) {
+ throw BagFormatException("Decrypted chunk is empty");
+ }
+ decrypted_chunk.setSize(decrypted_chunk.getSize() - *(decrypted_chunk.getData()+decrypted_chunk.getSize()-1));
+}
+
+void AesCbcEncryptor::addFieldsToFileHeader(ros::M_string &header_fields) const {
+ header_fields[ENCRYPTOR_FIELD_NAME] = "rosbag/AesCbcEncryptor";
+ header_fields[GPG_USER_FIELD_NAME] = gpg_key_user_;
+ header_fields[ENCRYPTED_KEY_FIELD_NAME] = encrypted_symmetric_key_;
+}
+
+void AesCbcEncryptor::readFieldsFromFileHeader(ros::M_string const& header_fields) {
+ encrypted_symmetric_key_ = readHeaderField(header_fields, ENCRYPTED_KEY_FIELD_NAME);
+ if (encrypted_symmetric_key_.empty()) {
+ throw BagFormatException("Encrypted symmetric key is not found in header");
+ }
+ gpg_key_user_ = readHeaderField(header_fields, GPG_USER_FIELD_NAME);
+ if (gpg_key_user_.empty()) {
+ throw BagFormatException("GPG key user is not found in header");
+ }
+ symmetric_key_ = decryptStringGpg(gpg_key_user_, encrypted_symmetric_key_);
+ AES_set_decrypt_key(&symmetric_key_[0], AES_BLOCK_SIZE*8, &aes_decrypt_key_);
+}
+
+void AesCbcEncryptor::writeEncryptedHeader(boost::function, ros::M_string const& header_fields, ChunkedFile& file) {
+ boost::shared_array header_buffer;
+ uint32_t header_len;
+ ros::Header::write(header_fields, header_buffer, header_len);
+ // Apply PKCS#7 padding to the header
+ std::size_t pad_size = AES_BLOCK_SIZE - header_len % AES_BLOCK_SIZE;
+ uint32_t encrypted_buffer_size = header_len + pad_size;
+ std::basic_string header_buffer_with_pad(encrypted_buffer_size, pad_size);
+ memcpy(&header_buffer_with_pad[0], header_buffer.get(), header_len);
+ // Encrypt chunk
+ std::basic_string encrypted_buffer(encrypted_buffer_size, 0);
+ std::basic_string iv(AES_BLOCK_SIZE, 0);
+ if (!RAND_bytes(&iv[0], AES_BLOCK_SIZE)) {
+ throw BagException("Failed to build initialization vector");
+ }
+ encrypted_buffer_size += AES_BLOCK_SIZE;
+ file.write((char*) &encrypted_buffer_size, 4);
+ encrypted_buffer_size -= AES_BLOCK_SIZE;
+ file.write((char*) &iv[0], AES_BLOCK_SIZE);
+ AES_cbc_encrypt(&header_buffer_with_pad[0], &encrypted_buffer[0], encrypted_buffer_size, &aes_encrypt_key_, &iv[0], AES_ENCRYPT);
+ // Write
+ file.write((char*) &encrypted_buffer[0], encrypted_buffer_size);
+}
+
+bool AesCbcEncryptor::readEncryptedHeader(boost::function, ros::Header& header, Buffer& header_buffer, ChunkedFile& file) {
+ // Read the encrypted header length
+ uint32_t encrypted_header_len;
+ file.read((char*) &encrypted_header_len, 4);
+ if (encrypted_header_len % AES_BLOCK_SIZE != 0) {
+ throw BagFormatException((boost::format("Error in encrypted header length: %d") % encrypted_header_len).str());
+ }
+ if (encrypted_header_len < AES_BLOCK_SIZE) {
+ throw BagFormatException((boost::format("No initialization vector in encrypted header: %d") % encrypted_header_len).str());
+ }
+ // Read encrypted header
+ std::basic_string iv(AES_BLOCK_SIZE, 0);
+ file.read((char*) &iv[0], AES_BLOCK_SIZE);
+ encrypted_header_len -= AES_BLOCK_SIZE;
+ std::basic_string encrypted_header(encrypted_header_len, 0);
+ file.read((char*) &encrypted_header[0], encrypted_header_len);
+ // Decrypt header
+ header_buffer.setSize(encrypted_header_len);
+ AES_cbc_encrypt(&encrypted_header[0], (unsigned char*) header_buffer.getData(), encrypted_header_len, &aes_decrypt_key_, &iv[0], AES_DECRYPT);
+ if (header_buffer.getSize() == 0) {
+ throw BagFormatException("Decrypted header is empty");
+ }
+ header_buffer.setSize(header_buffer.getSize() - *(header_buffer.getData()+header_buffer.getSize()-1));
+ // Parse the header
+ std::string error_msg;
+ return header.parse(header_buffer.getData(), header_buffer.getSize(), error_msg);
+}
+
+void AesCbcEncryptor::buildSymmetricKey() {
+ // Compose a new symmetric key for a bag file to be written
+ if (gpg_key_user_.empty()) {
+ return;
+ }
+ symmetric_key_.resize(AES_BLOCK_SIZE);
+ if (!RAND_bytes(&symmetric_key_[0], AES_BLOCK_SIZE)) {
+ throw BagException("Failed to build symmetric key");
+ }
+ // Encrypted session key is written in bag file header
+ encrypted_symmetric_key_ = encryptStringGpg(gpg_key_user_, symmetric_key_);
+}
+
+} // namespace rosbag
diff --git a/tools/rosbag_storage/src/bag.cpp b/tools/rosbag_storage/src/bag.cpp
index 9655e8541c..7e2e67171a 100644
--- a/tools/rosbag_storage/src/bag.cpp
+++ b/tools/rosbag_storage/src/bag.cpp
@@ -57,12 +57,12 @@ using ros::Time;
namespace rosbag {
-Bag::Bag()
+Bag::Bag() : encryptor_loader_("rosbag_storage", "rosbag::EncryptorBase")
{
init();
}
-Bag::Bag(string const& filename, uint32_t mode)
+Bag::Bag(string const& filename, uint32_t mode) : encryptor_loader_("rosbag_storage", "rosbag::EncryptorBase")
{
init();
open(filename, mode);
@@ -70,7 +70,7 @@ Bag::Bag(string const& filename, uint32_t mode)
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
-Bag::Bag(Bag&& other) {
+Bag::Bag(Bag&& other) : encryptor_loader_("rosbag_storage", "rosbag::EncryptorBase") {
init();
swap(other);
}
@@ -101,6 +101,7 @@ void Bag::init() {
curr_chunk_data_pos_ = 0;
current_buffer_ = 0;
decompressed_chunk_ = 0;
+ setEncryptorPlugin(std::string("rosbag/NoEncryptor"));
}
void Bag::open(string const& filename, uint32_t mode) {
@@ -217,6 +218,14 @@ void Bag::setCompression(CompressionType compression) {
compression_ = compression;
}
+void Bag::setEncryptorPlugin(std::string const& plugin_name, std::string const& plugin_param) {
+ if (!chunks_.empty()) {
+ throw BagException("Cannot set encryption plugin after chunks are written");
+ }
+ encryptor_ = encryptor_loader_.createInstance(plugin_name);
+ encryptor_->initialize(*this, plugin_param);
+}
+
// Version
void Bag::writeVersion() {
@@ -357,6 +366,7 @@ void Bag::writeFileHeaderRecord() {
header[INDEX_POS_FIELD_NAME] = toHeaderString(&index_data_pos_);
header[CONNECTION_COUNT_FIELD_NAME] = toHeaderString(&connection_count_);
header[CHUNK_COUNT_FIELD_NAME] = toHeaderString(&chunk_count_);
+ encryptor_->addFieldsToFileHeader(header);
boost::shared_array header_buffer;
uint32_t header_len;
@@ -397,6 +407,12 @@ void Bag::readFileHeaderRecord() {
if (version_ >= 200) {
readField(fields, CONNECTION_COUNT_FIELD_NAME, true, &connection_count_);
readField(fields, CHUNK_COUNT_FIELD_NAME, true, &chunk_count_);
+ std::string encryptor_plugin_name;
+ readField(fields, ENCRYPTOR_FIELD_NAME, 0, UINT_MAX, false, encryptor_plugin_name);
+ if (!encryptor_plugin_name.empty()) {
+ setEncryptorPlugin(encryptor_plugin_name);
+ encryptor_->readFieldsFromFileHeader(fields);
+ }
}
CONSOLE_BRIDGE_logDebug("Read FILE_HEADER: index_pos=%llu connection_count=%d chunk_count=%d",
@@ -440,6 +456,10 @@ void Bag::stopWritingChunk() {
file_.setWriteMode(compression::Uncompressed);
uint32_t compressed_size = file_.getOffset() - curr_chunk_data_pos_;
+ // When encryption is on, compressed_size represents encrypted chunk size;
+ // When decrypting, the actual compressed size can be deduced from the decrypted chunk
+ compressed_size = encryptor_->encryptChunk(compressed_size, curr_chunk_data_pos_, file_);
+
// Rewrite the chunk header with the size of the chunk (remembering current offset)
uint64_t end_of_chunk_pos = file_.getOffset();
@@ -643,11 +663,11 @@ void Bag::readConnectionIndexRecord200() {
void Bag::writeConnectionRecords() {
for (map::const_iterator i = connections_.begin(); i != connections_.end(); i++) {
ConnectionInfo const* connection_info = i->second;
- writeConnectionRecord(connection_info);
+ writeConnectionRecord(connection_info, true);
}
}
-void Bag::writeConnectionRecord(ConnectionInfo const* connection_info) {
+void Bag::writeConnectionRecord(ConnectionInfo const* connection_info, const bool encrypt) {
CONSOLE_BRIDGE_logDebug("Writing CONNECTION [%llu:%d]: topic=%s id=%d",
(unsigned long long) file_.getOffset(), getChunkOffset(), connection_info->topic.c_str(), connection_info->id);
@@ -655,9 +675,16 @@ void Bag::writeConnectionRecord(ConnectionInfo const* connection_info) {
header[OP_FIELD_NAME] = toHeaderString(&OP_CONNECTION);
header[TOPIC_FIELD_NAME] = connection_info->topic;
header[CONNECTION_FIELD_NAME] = toHeaderString(&connection_info->id);
- writeHeader(header);
- writeHeader(*connection_info->header);
+ if (encrypt)
+ encryptor_->writeEncryptedHeader(boost::bind(&Bag::writeHeader, this, _1), header, file_);
+ else
+ writeHeader(header);
+
+ if (encrypt)
+ encryptor_->writeEncryptedHeader(boost::bind(&Bag::writeHeader, this, _1), *connection_info->header, file_);
+ else
+ writeHeader(*connection_info->header);
}
void Bag::appendConnectionRecordToBuffer(Buffer& buf, ConnectionInfo const* connection_info) {
@@ -672,7 +699,7 @@ void Bag::appendConnectionRecordToBuffer(Buffer& buf, ConnectionInfo const* conn
void Bag::readConnectionRecord() {
ros::Header header;
- if (!readHeader(header))
+ if (!encryptor_->readEncryptedHeader(boost::bind(&Bag::readHeader, this, _1), header, header_buffer_, file_))
throw BagFormatException("Error reading CONNECTION header");
M_string& fields = *header.getValues();
@@ -685,7 +712,7 @@ void Bag::readConnectionRecord() {
readField(fields, TOPIC_FIELD_NAME, true, topic);
ros::Header connection_header;
- if (!readHeader(connection_header))
+ if (!encryptor_->readEncryptedHeader(boost::bind(&Bag::readHeader, this, _1), connection_header, header_buffer_, file_))
throw BagFormatException("Error reading connection header");
// If this is a new connection, update connections
@@ -806,12 +833,10 @@ void Bag::readMessageDataRecord102(uint64_t offset, ros::Header& header) const {
// Reading this into a buffer isn't completely necessary, but we do it anyways for now
void Bag::decompressRawChunk(ChunkHeader const& chunk_header) const {
assert(chunk_header.compression == COMPRESSION_NONE);
- assert(chunk_header.compressed_size == chunk_header.uncompressed_size);
CONSOLE_BRIDGE_logDebug("compressed_size: %d uncompressed_size: %d", chunk_header.compressed_size, chunk_header.uncompressed_size);
- decompress_buffer_.setSize(chunk_header.compressed_size);
- file_.read((char*) decompress_buffer_.getData(), chunk_header.compressed_size);
+ encryptor_->decryptChunk(chunk_header, decompress_buffer_, file_);
// todo check read was successful
}
@@ -823,8 +848,7 @@ void Bag::decompressBz2Chunk(ChunkHeader const& chunk_header) const {
CONSOLE_BRIDGE_logDebug("compressed_size: %d uncompressed_size: %d", chunk_header.compressed_size, chunk_header.uncompressed_size);
- chunk_buffer_.setSize(chunk_header.compressed_size);
- file_.read((char*) chunk_buffer_.getData(), chunk_header.compressed_size);
+ encryptor_->decryptChunk(chunk_header, chunk_buffer_, file_);
decompress_buffer_.setSize(chunk_header.uncompressed_size);
file_.decompress(compression, decompress_buffer_.getData(), decompress_buffer_.getSize(), chunk_buffer_.getData(), chunk_buffer_.getSize());
@@ -840,8 +864,7 @@ void Bag::decompressLz4Chunk(ChunkHeader const& chunk_header) const {
CONSOLE_BRIDGE_logDebug("lz4 compressed_size: %d uncompressed_size: %d",
chunk_header.compressed_size, chunk_header.uncompressed_size);
- chunk_buffer_.setSize(chunk_header.compressed_size);
- file_.read((char*) chunk_buffer_.getData(), chunk_header.compressed_size);
+ encryptor_->decryptChunk(chunk_header, chunk_buffer_, file_);
decompress_buffer_.setSize(chunk_header.uncompressed_size);
file_.decompress(compression, decompress_buffer_.getData(), decompress_buffer_.getSize(), chunk_buffer_.getData(), chunk_buffer_.getSize());
@@ -1155,6 +1178,7 @@ void Bag::swap(Bag& other) {
swap(outgoing_chunk_buffer_, other.outgoing_chunk_buffer_);
swap(current_buffer_, other.current_buffer_);
swap(decompressed_chunk_, other.decompressed_chunk_);
+ swap(encryptor_, other.encryptor_);
}
bool Bag::isOpen() const { return file_.isOpen(); }
diff --git a/tools/rosbag_storage/src/encryptor.cpp b/tools/rosbag_storage/src/encryptor.cpp
new file mode 100644
index 0000000000..17c62815d6
--- /dev/null
+++ b/tools/rosbag_storage/src/encryptor.cpp
@@ -0,0 +1,59 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2017, 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.
+*********************************************************************/
+
+#include "rosbag/encryptor.h"
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(rosbag::NoEncryptor, rosbag::EncryptorBase)
+
+namespace rosbag
+{
+
+uint32_t NoEncryptor::encryptChunk(const uint32_t chunk_size, const uint64_t, ChunkedFile&) { return chunk_size; }
+
+void NoEncryptor::decryptChunk(ChunkHeader const& chunk_header, Buffer& decrypted_chunk, ChunkedFile& file) const {
+ decrypted_chunk.setSize(chunk_header.compressed_size);
+ file.read((char*) decrypted_chunk.getData(), chunk_header.compressed_size);
+}
+
+void NoEncryptor::writeEncryptedHeader(boost::function write_header, ros::M_string const& header_fields, ChunkedFile&) {
+ write_header(header_fields);
+}
+
+bool NoEncryptor::readEncryptedHeader(boost::function read_header, ros::Header& header, Buffer&, ChunkedFile&) {
+ return read_header(header);
+}
+
+} // namespace rosbag
diff --git a/tools/rosbag_storage/test/test_aes_encryptor.cpp b/tools/rosbag_storage/test/test_aes_encryptor.cpp
new file mode 100644
index 0000000000..a9936c9684
--- /dev/null
+++ b/tools/rosbag_storage/test/test_aes_encryptor.cpp
@@ -0,0 +1,162 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2017, 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.
+*********************************************************************/
+
+#include
+
+#include
+
+#include
+
+#include "std_msgs/String.h"
+
+#include "rosbag/bag.h"
+#include "rosbag/encryptor.h"
+#include "rosbag/view.h"
+
+const char *GPG_KEY_USER = "Foo00";
+const char *GPG_PRIVATE_SUBKEY = "-----BEGIN PGP PRIVATE KEY BLOCK-----\n"
+ "Version: GnuPG v1\n\n"
+ "lQEVBFn3acoBCACwhq4iofk2V3/4yQy9++pHa3D4SPJdt1G/h83D+t9m95FoTHZl"
+ "zIY5bjKAKQ+NI6u5eQKNndAA7QLg8UGML6VqO7wmlxYSMOqRc4i0QMuUTA87hK4u"
+ "ozcHnjwaRhQzapwhnAMUu4058DIUyTus7ugD81C6y0nNT2PPwQzKifMmMcIgBvKm"
+ "vio7IK1A2tOsyQJD3jo99ZQhxq/eOQIwCs/BZfxu0OWnSJkx98Rsf7w3GC8tqhWK"
+ "avt/rBaFVoS3eZMkgQfT/ep92dYSLi/3/1pMtAt3kEVw3ZrvrCi47KDhpuaIV0kI"
+ "WlfHJF/YBNFbe4rRNwmQ5PkZYW1IwR14A+QFABEBAAH+B2UCR05VAbQqRm9vMDAg"
+ "KEEgR1BHIGtleSBmb3IgdGVzdCBjYXNlcykgPGJhckBiYXo+iQE4BBMBAgAiBQJZ"
+ "92nKAhsDBgsJCAcDAgYVCAIJCgsEFgIDAQIeAQIXgAAKCRCvPh8IgO8IzWTJB/9d"
+ "PO8BrpXtm/tosaGJFHT8FDt4PerCoKD6SqKj6HDrSU8tgUh+qLd+S1SkZ9Zg+yLq"
+ "ccCTeUg40XEvcKeTob8hWbTMKdl1cijM1jJCAUTe9zZd7URHoEkNWdEgA/saCZUj"
+ "TDeImYNcvv93SisKldf4gd67vkBETB4kEY7v7EE2YTge12S1vd9/9Ra6ZQn/qJSh"
+ "aPfZ5RfL1FEexQW+h2+bfpxW+ej/s2uM05AdEMmmGDaSVpimpPDkfF2YX4ESM7cl"
+ "fQzn1sCyVeHQWYvvEFTChdPlIR+gMZUhW+KicEqKsCDleh/jnSG/OoVZQkzec6Kt"
+ "zls7NO+fqIqFDLrNBWMTnQOXBFn3acoBCAClwRyFmeIkocrnodiqViqoGaBPOrwh"
+ "2NTaPvoiinwYwqUb9TynsrosEZTfjxfzDBawFG/nuBB7y0LhQYJKIEW6dKBT3HRe"
+ "hdhzfhZEaGGLwWxCqq5yywDt0JWrT92BX8hzwxKquRs3fynFGuW9YG5pdQ/wC3Na"
+ "j4uXg5Qy5wJZ0tqi/AiMIZTZGXBc/nP7rP7sryu3BYAXSPndx1mZYbSU1K5vBL3f"
+ "FkDAkon++cvF1+5D9Aoy1ukVmYn5fEhB4GoZwJEYCtgYofY5Fwbb7NsGHqrLBUgv"
+ "PwD5khZQ092rXEYPqkYrJ7vRX3/LdO4gmJupD1e1U2sRBOgzD/Pfw15FABEBAAEA"
+ "B/jMvmgLEV+bbnffNZpszcocE8Rjbw1mT/7Vl2bxCsmUr73uIFSTQxXRIMoZlRmO"
+ "dLWRrleo/3tc/UT0+fZoRJFrTjK88j5ag933PR2Zm1X/S9DgT7wQSOrc13Ts0mUD"
+ "aff4lMTr5J5kmZLGHx4beazpCM0Y0tM40TPVu10bg1srJUyCJgmqPJD0sbBfCBcT"
+ "jY99eWdT7tMr60G1Xw487RMXr/5eT7V2jcTe6JRivzQwvcsx1TuPWJajXY/l8BUJ"
+ "C7DitQP9x5DrA3s7qDxVbqiD0xrzCFc5FuOpcG2yV7LCuvT1144a1teCBh/gIoxh"
+ "EAnethtCVlmEufH74guq9ikEAMiZTkgsMNNHGtl7zNDc5ZEW22ESbrq81sbW9Z2p"
+ "CEvFU2HUsUQRkun6BcGpewTD+IOv2nSLlZQHEue4Ny9k7d4dWSew1ab3FMk9klpX"
+ "C7SEPYYDnZ8Ar+3nFozxKI3D1BvR374jSf2akMuIITteCnMXhvMLLFsk1Dz5zuWX"
+ "ARGfBADTiDyw387tZoR0Mhsiw1FwDJRISNYnC5YxWWzpDPfrZ3HZD44PqdwcxgBd"
+ "Uvtr8BtWHxs3skzn3oeNFT0yhVJi2Bn0QvGMIuGWBnhUvASv/rM3Zt22G5dCUVyz"
+ "RVGr1yx2xSZnVOeXQRSfp9yevAixcC8ATa8GUx9K8OSP/0NtmwP/Z5nW5s0WoG1H"
+ "Yna4s5bhNJ1y6Jue7bhw0gKXhx/9RNilTmq7NU6vTc1vqj0nw5e45WpojqZvSt0f"
+ "+bEL5eW+YnFb0MLJ9gIey4QZbOJeHB1xOaqz54Fnf8t9MmV/efLKvTHdj/WFM3fL"
+ "Z2iesAjK4hCyB2raeD9SOCP2ofztcmpIGYkBHwQYAQIACQUCWfdpygIbDAAKCRCv"
+ "Ph8IgO8Izd2IB/sFNTx/3l4mX9NUvyTpEhXKseti7JncjxZblUTV5MJ154nrfb2w"
+ "Xc81OmPmEDtE0PxTjYVasBD3lZNyLglw9kPpS5qZXU8kO4D6kNjRhdgKd/GqNHSo"
+ "U1LqutU/nbJc5H3AtGMLhg4Afa9xsYIRcszgtyWBbYCJ1MhkBaROAY42VzOyOi6z"
+ "pHOsnQH3xGlOdBYLeIvStCVP4XmHw/Mcx7LVnIfx48MFxVDw1iDiZej0K3cO24Ne"
+ "dSCgzJUVZQ5K4fTSu2xq1jD5zgWZ/Iu8PvAl5skKzgB2HpDVxIMVAELvxaMgKh3S"
+ "LYxADpgShsNtJiU/Dbn8jA+BlK/77fGnkvnc"
+ "=YcIw"
+ "-----END PGP PRIVATE KEY BLOCK-----";
+const char *MESSAGE = "message foo";
+const char *TOPIC_NAME = "topic_bar";
+
+void importGpgKey(gpgme_ctx_t &ctx) {
+ gpgme_data_t key_data;
+ gpgme_error_t err = gpgme_data_new_from_mem(&key_data, GPG_PRIVATE_SUBKEY, std::strlen(GPG_PRIVATE_SUBKEY), 1);
+ if (err) {
+ gpgme_release(ctx);
+ FAIL() << "gpgme_data_new_from_mem returned " << gpgme_strerror(err);
+ }
+ err = gpgme_op_import(ctx, key_data);
+ if (err) {
+ gpgme_data_release(key_data);
+ gpgme_release(ctx);
+ FAIL() << "gpgme_op_import returned " << gpgme_strerror(err);
+ }
+ gpgme_import_result_t res = gpgme_op_import_result(ctx);
+ EXPECT_TRUE(res->imports);
+ gpgme_data_release(key_data);
+}
+
+TEST(AesCbcEncryptor, EncryptAndDecryptBag) {
+ // Import key
+ rosbag::initGpgme();
+ gpgme_ctx_t ctx;
+ gpgme_error_t err = gpgme_new(&ctx);
+ if (err) {
+ FAIL() << "Failed to create a GPG context: " << gpgme_strerror(err);
+ }
+ importGpgKey(ctx);
+
+ // Test if the key has been imported
+ gpgme_key_t key;
+ try {
+ rosbag::getGpgKey(ctx, GPG_KEY_USER, key);
+ }
+ catch (rosbag::BagException const& e) {
+ gpgme_release(ctx);
+ FAIL() << "Failed to get the imported GPG key: " << e.what();
+ }
+
+ // Write a message to an encrypted bag file
+ char temp_dir_templ[] = "/tmp/bagXXXXXX";
+ char *temp_dir = mkdtemp(temp_dir_templ);
+ std::string bag_file_name = std::string(temp_dir) + "/foo.bag";
+ rosbag::Bag bag(bag_file_name, rosbag::bagmode::Write);
+ bag.setEncryptorPlugin("rosbag/AesCbcEncryptor", GPG_KEY_USER);
+ std_msgs::String msg;
+ msg.data = MESSAGE;
+ bag.write(TOPIC_NAME, ros::TIME_MIN, msg);
+ bag.close();
+
+ // Test the message decrypted from the bag file
+ bag.open(bag_file_name, rosbag::bagmode::Read);
+ rosbag::View view(bag);
+ EXPECT_EQ(view.size(), 1);
+ EXPECT_EQ(TOPIC_NAME, view.begin()->getTopic());
+ EXPECT_EQ(MESSAGE, view.begin()->instantiate()->data);
+ bag.close();
+
+ // Remove the bag file
+ boost::filesystem::remove(bag_file_name);
+ // Delete the key
+ gpgme_op_delete(ctx, key, 1);
+ // Release GPG context
+ gpgme_release(ctx);
+}
+
+int main(int argc, char **argv) {
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}