tum_to_bag.py

#!/usr/bin/env python3
"""
python3 tum_to_bag.py --src test.tum --dst map.db
"""
import argparse
import math
import numpy as np
from scipy.spatial.transform import Rotation as R

from rosbags.rosbag1 import Writer
from rosbags.serde import serialize_ros1
from rosbags.typesys.types import builtin_interfaces__msg__Time as Time
from rosbags.typesys.types import geometry_msgs__msg__Quaternion as Quaternion
from rosbags.typesys.types import geometry_msgs__msg__Point as Point
from rosbags.typesys.types import geometry_msgs__msg__Pose as Pose
from rosbags.typesys.types import geometry_msgs__msg__PoseStamped as PoseStamped
from rosbags.typesys.types import std_msgs__msg__Header as Header

def time_to_ros_time(timestamp):
    f, i = math.modf(timestamp)
    return Time(int(i), int(f * 10e+9))

def main(input_filename, writer):
    topic = "pose"
    frame_id = "map"
    conn_bag = writer.add_connection(topic, PoseStamped.__msgtype__)

    for line in open(input_filename):
        d = line.split()
        t = int(d[0])
        x, y, z, qx, qy, qz, qw  = [float(s) for s in d[1:]]
        msg = PoseStamped(
            Header(time_to_ros_time(t), frame_id),
            Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw)))
        msg_ros = serialize_ros1(msg, PoseStamped.__msgtype__)
        writer.write(conn_bag, t, msg_ros)


if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--src", type=str)
    parser.add_argument("--dst", type=str)
    args = parser.parse_args()

    input_filename = args.src
    output_filename = args.dst
    with Writer(output_filename) as writer:
        main(input_filename, writer)