Skip to content

Commit

Permalink
Implement Bag encryption/decryption.
Browse files Browse the repository at this point in the history
  • Loading branch information
jwon02 committed Nov 3, 2017
1 parent 72c1bfe commit a2c0545
Show file tree
Hide file tree
Showing 13 changed files with 1,154 additions and 33 deletions.
20 changes: 16 additions & 4 deletions tools/rosbag/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -38,17 +38,29 @@ 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")
install(TARGETS rosbag
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(TARGETS record play
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
if(WIN32)
install(TARGETS record play
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
else()
install(TARGETS record play 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
Expand Down
189 changes: 189 additions & 0 deletions tools/rosbag/src/encrypt.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,189 @@
/*********************************************************************
* 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 <iostream>

#include <boost/scoped_ptr.hpp>
#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <boost/regex.hpp>

#include <ros/ros.h>

#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<std::string>()->default_value("rosbag/AesCbcEncryptor"), "encryptor name")
("param,r", po::value<std::string>()->default_value("*"), "encryptor parameter")
("bz2,j", "use BZ2 compression")
("lz4", "use lz4 compression")
("inbag", po::value<std::string>(), "bag file to encrypt")
("outbag,o", po::value<std::string>(), "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<std::string>();
if (vm.count("param"))
opts.param = vm["param"].as<std::string>();
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<std::string>();
} else {
throw ros::Exception("You must specify bag to encrypt.");
}
if (vm.count("outbag"))
opts.outbag = vm["outbag"].as<std::string>();
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<boost::progress_display> 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);
}
66 changes: 66 additions & 0 deletions tools/rosbag/src/rosbag/rosbag_main.py
Original file line number Diff line number Diff line change
Expand Up @@ -591,6 +591,42 @@ def reindex_cmd(argv):

bag_op(args, 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, lambda b: False, op, options.output_dir, options.force, options.quiet)

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, 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):
for inbag_filename in inbag_filenames:
# Check we can read the file
Expand Down Expand Up @@ -742,6 +778,33 @@ def reindex_op(inbag, outbag, quiet):
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]]
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)
Expand Down Expand Up @@ -878,6 +941,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
Expand Down
65 changes: 49 additions & 16 deletions tools/rosbag_storage/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ 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)

Expand All @@ -24,21 +24,40 @@ 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})
if(WIN32)
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/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})
else()
add_library(rosbag_storage
src/aes_encryptor.cpp
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} crypto gpgme)
endif()

install(TARGETS rosbag_storage
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
Expand All @@ -50,3 +69,17 @@ 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)
target_link_libraries(test_aes_encryptor rosbag_storage ${catkin_LIBRARIES} ${Boost_LIBRARIES} crypto gpgme)
endif()
endif()
Loading

0 comments on commit a2c0545

Please sign in to comment.