diff --git a/README.md b/README.md index 280d19a..0cf7024 100644 --- a/README.md +++ b/README.md @@ -197,7 +197,7 @@ We use nosetests to run the unit tests: OK -The packages nose and pexpect have to be installed with pip to run the tests. +The packages `nose` and `pexpect` have to be installed with pip to run the tests. ## Contributing diff --git a/bin/rock2infuse b/bin/rock2infuse new file mode 100644 index 0000000..e6a55cc --- /dev/null +++ b/bin/rock2infuse @@ -0,0 +1,26 @@ +#!/usr/bin/env python +import argparse +import pocolog2msgpack + + +def main(): + args = parse_args() + pocolog2msgpack.rock2infuse_logfile( + args.rock_logfile, args.infuse_logfile, args.verbose) + + +def parse_args(): + argparser = argparse.ArgumentParser( + description="Convert MsgPack with Rock's types to InFuse's types.") + argparser.add_argument( + "rock_logfile", type=str, help="MsgPack with Rock's types") + argparser.add_argument( + "infuse_logfile", type=str, help="Output file") + argparser.add_argument( + "--verbose", "-v", action="count", default=0, help="Verbosity level") + args = argparser.parse_args() + return args + + +if __name__ == "__main__": + main() diff --git a/pocolog2msgpack.py b/pocolog2msgpack.py index efb9b49..4710d56 100644 --- a/pocolog2msgpack.py +++ b/pocolog2msgpack.py @@ -1,4 +1,6 @@ import msgpack +import mmap +from math import sqrt def object2relational(input_filename, output_filename, whitelist=()): @@ -77,3 +79,317 @@ def _convert_metadata(converted_log, log, port_name): converted_log[port_name]["timestamp"] = metadata["timestamps"] n_rows = len(metadata["timestamps"]) converted_log[port_name]["type"] = [metadata["type"]] * n_rows + + +def rock2infuse_logfile(input_filename, output_filename, verbose=0): + """Convert a MsgPack logfile from rock log format to infuse format. + + Parameters + ---------- + input_filename : str + Name of the original logfile + + output_filename : str + Name of the converted logfile + + verbose : int, optional (default: 0) + Verbosity level + """ + with open(input_filename, "rb") as inf: + with mmap.mmap(inf.fileno(), 0, access=mmap.ACCESS_READ) as m: + with open(output_filename, "wb") as outf: + u = msgpack.Unpacker(inf, encoding="utf8") + p = msgpack.Packer(encoding="utf8") + _convert(u, outf, p, verbose) + + +def _convert(u, outf, p, verbose): + n_keys = u.read_map_header() + if verbose: + print("Logfile [%d streams]" % n_keys) + outf.write(p.pack_map_header(n_keys)) + for _ in range(n_keys): + key = u.unpack() + if verbose: + print(" Stream [%s]" % key) + outf.write(p.pack(key)) + if key.endswith(".meta"): + _convert_meta_stream(u, outf, p, verbose) + else: + _convert_stream(u, outf, p, verbose) + + +def _convert_meta_stream(u, outf, p, verbose): + n_keys = u.read_map_header() + outf.write(p.pack_map_header(n_keys)) + for _ in range(n_keys): + key = u.unpack() + if verbose > 1: + print(" Meta [%s]" % key) + outf.write(p.pack(key)) + if key == "timestamps": + _convert_array(u, outf, p, verbose=0) + elif key == "type": + typename = u.unpack() + infuse_typename = _map_typename(typename) + if verbose > 1: + print(" [%s -> %s]" % (typename, infuse_typename)) + outf.write(p.pack(infuse_typename)) + else: + entry = u.unpack() + outf.write(p.pack(entry)) + + +def _convert_stream(u, outf, p, verbose): + _convert_array( + u, outf, p, entry_converter=_translate_sample, verbose=verbose) + + +def _convert_array(u, outf, p, entry_converter=None, verbose=0): + n_entries = u.read_array_header() + outf.write(p.pack_array_header(n_entries)) + for i in range(n_entries): + if verbose > 1: + print(" Converting [%d/%d]" % (i + 1, n_entries)) + entry = u.unpack() + if entry_converter is not None: + entry = entry_converter(entry) + outf.write(p.pack(entry)) + del entry + + +def rock2infuse(data): + """WARNING: modifies input data!""" + data = _translate_typenames(data) + data = _translate_types(data) + return data + + +def _translate_typenames(data): + for k in data.keys(): + if not k.endswith(".meta"): + continue + + if "type" in data[k]: + data[k]["type"] = _map_typename(data[k]["type"]) + return data + + +def _map_typename(typename): + if typename.endswith("_m"): + typename = typename[:-2] + if typename.startswith("/base"): + typename = typename.split("/")[-1] + if typename == "Frame": + typename = "Image" + return typename + + +def _translate_types(data): + for k in data.keys(): + if k.endswith(".meta"): + continue + + samples = data[k] + for t in range(len(samples)): + samples[t] = _translate_sample(samples[t]) + return data + + +def _translate_sample(sample): + if isinstance(sample, dict): + sample = _translate_dict(sample) + elif isinstance(sample, list): + sample = _translate_list(sample) + return sample + + +def _translate_dict(sample): + # TODO fix timestamp / ref_time inconsistencies when we switch to new + # version of ESROCOS' types + converted, new_sample = _convert_time(sample) + if converted: + return new_sample + converted, new_sample = _convert_vector(sample) + if converted: + return new_sample + # TODO don't know how to handle matrices correctly... + converted, new_sample = _convert_quaternion(sample) + if converted: + return new_sample + converted, new_sample = _convert_pointcloud(sample) + if converted: + return new_sample + converted, new_sample = _convert_depth_map(sample) + if converted: + return new_sample + converted, new_sample = _convert_frame(sample) + if converted: + return new_sample + converted, new_sample = _convert_rigid_body_state(sample) + if converted: + return new_sample + converted, new_sample = _convert_joints(sample) + if converted: + return new_sample + converted, new_sample = _convert_imusensors(sample) + if converted: + return new_sample + sample = _translate_time_to_ref_time(sample) + + for k in sample.keys(): + if isinstance(sample[k], dict): + sample[k] = _translate_sample(sample[k]) + elif isinstance(sample[k], list): + sample[k] = [_translate_sample(el) for el in sample[k]] + return sample + + +def _convert_time(sample): + if "usecPerSec" in sample: + return True, {"microseconds": sample["microseconds"]} + else: + return False, None + + +def _convert_vector(sample): + if "data" in sample and len(sample) == 1: + return True, sample["data"] + else: + return False, None + + +def _convert_quaternion(sample): + if "re" in sample and "im" in sample: + return True, sample["im"] + [sample["re"]] # quaternion x, y, z, w + else: + return False, None + + +def _convert_depth_map(sample): + if "vertical_projection" in sample: + new_sample = { + "ref_time": sample["time"], + "timestamps": sample["timestamps"], + "vertical_projection": sample["vertical_projection"].lower(), + "horizontal_projection": sample["horizontal_projection"].lower(), + "vertical_interval": sample["vertical_interval"], + "horizontal_interval": sample["horizontal_interval"], + "vertical_size": sample["vertical_size"], + "horizontal_size": sample["horizontal_size"], + "distances": sample["distances"], + "remissions": sample["remissions"], + } + return True, new_sample + else: + return False, None + + +def _convert_frame(sample): + if "frame_mode" in sample: + new_sample = { + "frame_time": sample["time"], + "received_time": sample["received_time"], + "attributes": _convert_frame_attributes(sample["attributes"]), + "image": sample["image"], + "datasize": sample["size"], + "data_depth": sample["data_depth"], + "pixel_size": sample["pixel_size"], + "row_size": sample["row_size"], + "frame_mode": sample["frame_mode"].lower(), + "frame_status": sample["frame_status"].lower(), + } + return True, new_sample + else: + return False, None + + +def _convert_frame_attributes(frame_attributes): + result = [] + for fa in frame_attributes: + result.append({ + "att_name": fa["name_"], + "data": fa["data_"] + }) + return result + + +def _convert_rigid_body_state(sample): + if "time" in sample and "sourceFrame" in sample and "position" in sample: + new_sample = { + "timestamp": _translate_sample(sample["time"]), + "sourceFrame": sample["sourceFrame"], + "targetFrame": sample["targetFrame"], + "pos": _translate_sample(sample["position"]), + "cov_position": _convert_square_matrix(sample["cov_position"])[1], + "orient": _convert_quaternion(sample["orientation"])[1], + "cov_orientation": _convert_square_matrix(sample["cov_orientation"])[1], + "velocity": _translate_sample(sample["velocity"]), + "cov_velocity": _convert_square_matrix(sample["cov_velocity"])[1], + "angular_velocity": _translate_sample(sample["angular_velocity"]), + "cov_angular_velocity": _convert_square_matrix(sample["cov_angular_velocity"])[1], + } + return True, new_sample + else: + return False, None + + +def _convert_joints(sample): + if "time" in sample and "names" in sample and "elements" in sample: + new_sample = { + "timestamp": _translate_sample(sample["time"]), + "names": sample["names"], + "elements": sample["elements"] + } + return True, new_sample + else: + return False, None + + +def _convert_imusensors(sample): + if "gyro" in sample: + new_sample = { + "timestamp": _translate_sample(sample["time"]), + "acc": _translate_sample(sample["acc"]), + "gyro": _translate_sample(sample["gyro"]), + "mag": _translate_sample(sample["mag"]), + } + return True, new_sample + else: + return False, None + + +def _convert_pointcloud(sample): + if "points" in sample: + new_sample = { + "metadata": + {"timeStamp": _translate_sample(sample["time"])}, + "data": + {"points": _translate_sample(sample["points"]), + "colors": [p[:3] for p in + _translate_sample(sample["colors"])] + } + } + return True, new_sample + else: + return False, None + + +def _convert_square_matrix(sample): + content = sample["data"] + n_rows = int(sqrt(len(content))) + assert n_rows * n_rows == len(content) + matrix = [[content[i * n_rows + j] + for j in range(n_rows)] for i in range(n_rows)] + return True, matrix + + +def _translate_time_to_ref_time(sample): + if "time" in sample and "latitude" not in sample: # Exception: /gps/Solution + sample["ref_time"] = sample["time"] + del sample["time"] + return sample + + +def _translate_list(sample): + return [_translate_sample(el) for el in sample] diff --git a/setup.py b/setup.py index f9a7e41..2d6fb39 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,7 @@ def setup_package(): license="unknown", py_modules=["pocolog2msgpack"], requires=["msgpack"], + scripts=["bin/rock2infuse"], ) diff --git a/src/Converter.cpp b/src/Converter.cpp index 00910c7..4dfa93d 100644 --- a/src/Converter.cpp +++ b/src/Converter.cpp @@ -16,7 +16,7 @@ void addValidInputDataStreams( const std::vector& streams, std::vector& dataStreams, - const std::string& only); + const std::vector& exclude, const std::string& only); int convertStreams( msgpack_packer& packer, std::vector& dataStreams, const int size, const int containerLimit, const int start, const int end, @@ -29,14 +29,16 @@ int convertMetaData( int convert(const std::vector& logfiles, const std::string& output, - const int size, const int containerLimit, const std::string& only, - const int start, const int end, const int verbose) + const int size, const int containerLimit, + const std::vector& exclude, + const std::string& only, const int start, const int end, + const int verbose) { pocolog_cpp::MultiFileIndex* multiIndex = new pocolog_cpp::MultiFileIndex(); multiIndex->createIndex(logfiles); std::vector streams = multiIndex->getAllStreams(); std::vector dataStreams; - addValidInputDataStreams(streams, dataStreams, only); + addValidInputDataStreams(streams, dataStreams, exclude, only); if(verbose >= 1) std::cout << "[pocolog2msgpack] " << dataStreams.size() << " streams" << std::endl; @@ -63,13 +65,16 @@ int convert(const std::vector& logfiles, const std::string& output, void addValidInputDataStreams( const std::vector& streams, std::vector& dataStreams, - const std::string& only) + const std::vector& exclude, const std::string& only) { dataStreams.reserve(streams.size()); for(size_t i = 0; i < streams.size(); i++) { if(only != "" && only != streams[i]->getName()) continue; + if(std::find(exclude.begin(), exclude.end(), streams[i]->getName()) != + exclude.end()) + continue; pocolog_cpp::InputDataStream* dataStream = dynamic_cast(streams[i]); @@ -523,9 +528,9 @@ bool Converter::visit_(Typelib::Container const& type) } else { - numElements = containerLimit; std::cerr << "truncating " << type.kind() << "! (" << numElements << " > " << containerLimit << ")" << std::endl; + numElements = containerLimit; } } diff --git a/src/Converter.hpp b/src/Converter.hpp index 337a8a0..2911b91 100644 --- a/src/Converter.hpp +++ b/src/Converter.hpp @@ -11,6 +11,7 @@ * @param output name of the MsgPack logfile, will be created * @param size size of the size type for containers in the logfile * @param containerLimit maximum lenght of a container that will be converted + * @param exclude ports excluded from conversion * @param only only convert the port given by this argument * @param start index of the first sample that will be exported * @param end index after the last sample that will be exported @@ -18,7 +19,8 @@ * @return exit status of the program */ int convert(const std::vector& logfiles, const std::string& output, - const int size, const int containerLimit, const std::string& only, + const int size, const int containerLimit, + const std::vector& exclude, const std::string& only, const int start, const int end, const int verbose); diff --git a/src/pocolog2msgpack.cpp b/src/pocolog2msgpack.cpp index f363fed..c4d9d6b 100644 --- a/src/pocolog2msgpack.cpp +++ b/src/pocolog2msgpack.cpp @@ -20,10 +20,13 @@ int main(int argc, char *argv[]) "Logfiles") ("output,o", boost::program_options::value()->default_value("output.msg"), "Output file") + ("exclude,e", + boost::program_options::value >()->multitoken(), + "Exclude stream") ("size,s", boost::program_options::value()->default_value(8), "Length of the size type. This should be 8 for most machines, " "but it can be 1, e.g. on robots.") - ("container-limit,c", boost::program_options::value()->default_value(10000), + ("container-limit,c", boost::program_options::value()->default_value(300000), "Maximum length of a container that will be read and converted. " "This option should only be used if you have old logfiles from " "which we can't read the container size properly and have to limit " @@ -60,6 +63,9 @@ int main(int argc, char *argv[]) const std::string output = vm["output"].as(); const int size = vm["size"].as(); const int containerLimit = vm["container-limit"].as(); + std::vector exclude; + if(vm.count("exclude") > 0) + exclude = vm["exclude"].as >(); const std::string only = vm["only"].as(); const int start = vm["start"].as(); const int end = vm["end"].as(); @@ -81,6 +87,6 @@ int main(int argc, char *argv[]) return EXIT_FAILURE; } - return convert(logfiles, output, size, containerLimit, only, start, end, - verbose); + return convert(logfiles, output, size, containerLimit, exclude, only, + start, end, verbose); } diff --git a/test/test_infuse.py b/test/test_infuse.py new file mode 100644 index 0000000..132c8be --- /dev/null +++ b/test/test_infuse.py @@ -0,0 +1,176 @@ +from pocolog2msgpack import rock2infuse +from nose.tools import assert_equal, assert_in + + +def test_convert_nothing(): + data = rock2infuse({}) + assert_equal(data, {}) + + +def test_convert_time(): + data = { + "/component.port": + [ + { + "microseconds": 5, + } + ], + "/component.port.meta": + { + "type": "/base/Time", + "timestamps": [5] + } + } + data = rock2infuse(data) + assert_in("/component.port", data) + samples = data["/component.port"] + assert_equal(len(samples), 1) + assert_equal(len(samples[0]), 1) + assert_equal(samples[0]["microseconds"], 5) + assert_in("microseconds", samples[0]) + assert_in("/component.port.meta", data) + meta = data["/component.port.meta"] + assert_in("type", meta) + assert_equal(meta["type"], "Time") + assert_in("timestamps", meta) + assert_equal(len(meta["timestamps"]), 1) + + +def test_convert_vector2d(): + data = { + "/component.port": + [ + { + "data": [0, 1] + } + ], + "/component.port.meta": + { + "type": "/base/Vector2d", + "timestamps": [5] + } + } + data = rock2infuse(data) + samples = data["/component.port"] + assert_equal(samples[0], [0, 1]) + + +def test_convert_quaternion(): + data = { + "/component.port": + [ + { + "re": 0, + "im": [1, 2, 3] + } + ], + "/component.port.meta": + { + "type": "/base/Quaterniond", + "timestamps": [5] + } + } + data = rock2infuse(data) + samples = data["/component.port"] + assert_equal(samples[0], [1, 2, 3, 0]) + + +def test_convert_rigid_body_state(): + data = { + "/component.port": + [ + { + "time": { + "microseconds": 5, + }, + "sourceFrame": "A", + "targetFrame": "B", + "position": {"data": [0, 1, 2]}, + "cov_position": {"data": [0, 1, 2, 3, 4, 5, 6, 7, 8]}, + "orientation": {"re": 0, "im": [1, 2, 3]}, + "cov_orientation": {"data": [0, 1, 2, 3, 4, 5, 6, 7, 8]}, + "velocity": {"data": [2, 3, 4]}, + "cov_velocity": {"data": [0, 1, 2, 3, 4, 5, 6, 7, 8]}, + "angular_velocity": {"data": [3, 4, 5]}, + "cov_angular_velocity": {"data": [0, 1, 2, 3, 4, 5, 6, 7, 8]} + } + ], + "/component.port.meta": + { + "type": "/base/samples/RigidBodyState", + "timestamps": [5], + } + } + data = rock2infuse(data) + samples = data["/component.port"] + assert_equal(samples[0]["timestamp"]["microseconds"], 5) + assert_equal(samples[0]["sourceFrame"], "A") + assert_equal(samples[0]["targetFrame"], "B") + assert_equal(samples[0]["pos"], [0, 1, 2]) + assert_equal(samples[0]["cov_position"], [[0, 1, 2], [3, 4, 5], [6, 7, 8]]) + assert_equal(samples[0]["orient"], [1, 2, 3, 0]) + assert_equal(samples[0]["cov_orientation"], [[0, 1, 2], [3, 4, 5], [6, 7, 8]]) + assert_equal(samples[0]["velocity"], [2, 3, 4]) + assert_equal(samples[0]["cov_velocity"], [[0, 1, 2], [3, 4, 5], [6, 7, 8]]) + assert_equal(samples[0]["angular_velocity"], [3, 4, 5]) + assert_equal(samples[0]["cov_angular_velocity"], [[0, 1, 2], [3, 4, 5], [6, 7, 8]]) + + +def test_convert_laser_scan(): + data = { + "/component.port": + [ + { + "time": {"microseconds": 3}, + "start_angle": 0.0, + "angular_resolution": 0.1, + "speed": 0.1, + "ranges": [0, 1, 2, 3], + "minRange": 0, + "maxRange": 3, + "remission": [0, 1, 2, 3] + } + ], + "/component.port.meta": + { + "type": "/base/samples/LaserScan", + "timestamps": [5] + } + } + data = rock2infuse(data) + samples = data["/component.port"] + assert_equal(samples[0]["ref_time"]["microseconds"], 3) + assert_equal(samples[0]["start_angle"], 0.0) + assert_equal(samples[0]["angular_resolution"], 0.1) + assert_equal(samples[0]["speed"], 0.1) + assert_equal(samples[0]["ranges"], [0, 1, 2, 3]) + assert_equal(samples[0]["minRange"], 0) + assert_equal(samples[0]["maxRange"], 3) + assert_equal(samples[0]["remission"], [0, 1, 2, 3]) + + +def test_convert_pointcloud(): + data = { + "/component.port": + [ + { + "time": {"microseconds": 3}, + "points": [{"data": [0, 1, 2]}, {"data": [2, 3, 4]}], + "colors": [{"data": [255, 255, 255, 255]}, + {"data": [255, 255, 255, 255]}] + } + ], + "/component.port.meta": + { + "type": "/base/samples/Pointcloud", + "timestamps": [5] + } + } + data = rock2infuse(data) + samples = data["/component.port"] + assert_equal( + samples[0]["metadata"]["timeStamp"]["microseconds"], 3) + assert_equal(samples[0]["data"]["points"], + [[0, 1, 2], [2, 3, 4]]) + assert_equal(samples[0]["data"]["colors"], + [[255, 255, 255], [255, 255, 255]])