Create ROS bag from Spectacular Log (#347)

experimental/overhead_matching/BUILD

@@ -1,4 +1,11 @@
 
+
+package(features=["warning_compile_flags"])
+
+load("@pybind11_bazel//:build_defs.bzl", "pybind_extension")
+load("@rules_python//python:defs.bzl", "py_binary")
+load("@pip//:requirements.bzl", "requirement")
+
 cc_library(
     name = "spectacular_log",
     hdrs = ["spectacular_log.hh"],
@@ -25,3 +32,20 @@ cc_test(
 	"@fmt",
     ]
 )
+
+pybind_extension(
+    name = "spectacular_log_python",
+    srcs = ["spectacular_log_python.cc"],
+    data = ["//common/time:robot_time_python.so"],
+    deps = [":spectacular_log"],
+)
+
+py_binary(
+    name = "spectacular_log_to_rosbag",
+    srcs = ["spectacular_log_to_rosbag.py"],
+    data = [":spectacular_log_python.so"],
+    deps = [
+	requirement("rosbags"),
+	requirement("numpy"),
+    ],
+)

experimental/overhead_matching/spectacular_log.cc

@@ -154,20 +154,20 @@ std::optional<ImuSample> SpectacularLog::get_imu_sample(const time::RobotTimesta
 }
 
 std::optional<FrameGroup> SpectacularLog::get_frame(const int frame_id) const {
-    if (frame_id < 0 || frame_id >= frame_info_.size()) {
+    if (frame_id < 0 || frame_id >= static_cast<int>(frame_info_.size())) {
         return std::nullopt;
     }
 
     const auto &frame_info = frame_info_.at(frame_id);
 
     // Read the desired rgb frame
     if (video_ == nullptr) {
-        const fs::path rgb_path = log_path_ / "data.mov";
-        video_ = std::make_unique<cv::VideoCapture>(rgb_path.string(), cv::CAP_FFMPEG);
+        const fs::path color_path = log_path_ / "data.mov";
+        video_ = std::make_unique<cv::VideoCapture>(color_path.string(), cv::CAP_FFMPEG);
     }
-    cv::Mat rgb_frame;
+    cv::Mat bgr_frame;
     seek_to_frame(frame_info.frame_number, make_in_out(*video_));
-    video_->read(rgb_frame);
+    video_->read(bgr_frame);
 
     // Read the desired depth frame
     const fs::path depth_frame_path =
@@ -177,9 +177,9 @@ std::optional<FrameGroup> SpectacularLog::get_frame(const int frame_id) const {
     return {{
         .time_of_validity =
             time::as_duration(frame_info.time_of_validity_s) + time::RobotTimestamp(),
-        .rgb_frame = std::move(rgb_frame),
+        .bgr_frame = std::move(bgr_frame),
         .depth_frame = std::move(depth_frame),
-        .rgb_calibration = frame_info.calibration.at(0),
+        .color_calibration = frame_info.calibration.at(0),
         .depth_calibration = frame_info.calibration.at(1),
     }};
 }

experimental/overhead_matching/spectacular_log.hh

@@ -22,10 +22,10 @@ struct FrameCalibration {
 
 struct FrameGroup {
     time::RobotTimestamp time_of_validity;
-    cv::Mat rgb_frame;
+    cv::Mat bgr_frame;
     cv::Mat depth_frame;
 
-    FrameCalibration rgb_calibration;
+    FrameCalibration color_calibration;
     FrameCalibration depth_calibration;
 };
 

experimental/overhead_matching/spectacular_log_python.cc

@@ -0,0 +1,62 @@
+
+#include <iostream>
+
+#include "experimental/overhead_matching/spectacular_log.hh"
+#include "opencv2/core/eigen.hpp"
+#include "pybind11/eigen.h"
+#include "pybind11/eigen/tensor.h"
+#include "pybind11/pybind11.h"
+#include "pybind11/stl.h"
+#include "pybind11/stl/filesystem.h"
+#include "unsupported/Eigen/CXX11/Tensor"
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace robot::experimental::overhead_matching {
+namespace {
+Eigen::Tensor<uint8_t, 3> tensor_map_from_cv_mat(const cv::Mat &mat) {
+    Eigen::Tensor<uint8_t, 3> tensor;
+    cv::cv2eigen(mat, tensor);
+    return tensor;
+}
+}  // namespace
+PYBIND11_MODULE(spectacular_log_python, m) {
+    m.doc() = "spectacular log";
+
+    py::module_::import("common.time.robot_time_python");
+
+    py::class_<FrameCalibration>(m, "FrameCalibration")
+        .def(py::init<>())
+        .def_readwrite("focal_length", &FrameCalibration::focal_length)
+        .def_readwrite("principal_point", &FrameCalibration::principal_point)
+        .def_readwrite("exposure_time", &FrameCalibration::exposure_time_s)
+        .def_readwrite("depth_scale", &FrameCalibration::depth_scale);
+
+    py::class_<FrameGroup>(m, "FrameGroup")
+        .def(py::init<>())
+        .def_readwrite("time_of_validity", &FrameGroup::time_of_validity)
+        .def("bgr_frame",
+             [](const FrameGroup &self) { return tensor_map_from_cv_mat(self.bgr_frame); })
+        .def("depth_frame",
+             [](const FrameGroup &self) { return tensor_map_from_cv_mat(self.depth_frame); })
+        .def_readwrite("color_calibration", &FrameGroup::color_calibration)
+        .def_readwrite("depth_calibration", &FrameGroup::depth_calibration);
+
+    py::class_<ImuSample>(m, "ImuSample")
+        .def(py::init<>())
+        .def_readwrite("time_of_validity", &ImuSample::time_of_validity)
+        .def_readwrite("accel_mpss", &ImuSample::accel_mpss)
+        .def_readwrite("gyro_radps", &ImuSample::gyro_radps);
+
+    py::class_<SpectacularLog>(m, "SpectacularLog")
+        .def(py::init<std::filesystem::path>())
+        .def("get_imu_sample", &SpectacularLog::get_imu_sample)
+        .def("get_frame", &SpectacularLog::get_frame)
+        .def("min_imu_time", &SpectacularLog::min_imu_time)
+        .def("max_imu_time", &SpectacularLog::max_imu_time)
+        .def("min_frame_time", &SpectacularLog::min_frame_time)
+        .def("max_frame_time", &SpectacularLog::max_frame_time)
+        .def("num_frames", &SpectacularLog::num_frames);
+}
+}  // namespace robot::experimental::overhead_matching

experimental/overhead_matching/spectacular_log_test.cc

@@ -63,7 +63,7 @@ TEST(SpectacularLogTest, happy_case) {
                                                fmt::format("frames2/{:08d}.png", frame_id));
         const cv::Mat depth_frame = cv::imread(depth_path, cv::IMREAD_GRAYSCALE);
 
-        EXPECT_TRUE(images_equal(expected_frame, frame.rgb_frame));
+        EXPECT_TRUE(images_equal(expected_frame, frame.bgr_frame));
         EXPECT_TRUE(images_equal(depth_frame, frame.depth_frame));
 
         for (int i = 0; i < FRAME_SKIP - 1; i++) {

experimental/overhead_matching/spectacular_log_to_rosbag.py

@@ -0,0 +1,125 @@
+
+from rosbags.rosbag1 import Writer
+from rosbags.typesys import Stores, get_typestore
+import argparse
+import numpy as np
+
+from experimental.overhead_matching import spectacular_log_python as slp
+import common.time.robot_time_python as rtp
+
+from pathlib import Path
+
+
+def main(spectacular_log_path: Path, rosbag_out: Path):
+    log = slp.SpectacularLog(spectacular_log_path)
+
+    typestore = get_typestore(Stores.ROS1_NOETIC)
+    Header = typestore.types["std_msgs/msg/Header"]
+    Imu = typestore.types["sensor_msgs/msg/Imu"]
+    Image = typestore.types["sensor_msgs/msg/Image"]
+    Quaternion = typestore.types["geometry_msgs/msg/Quaternion"]
+    Vector3 = typestore.types["geometry_msgs/msg/Vector3"]
+    Time = typestore.types["builtin_interfaces/msg/Time"]
+
+    with Writer(rosbag_out) as writer:
+        imu_conn = writer.add_connection('/imu0', Imu.__msgtype__, typestore=typestore)
+        bgr_conn = writer.add_connection(
+            '/cam0/image_raw', Image.__msgtype__, typestore=typestore)
+        depth_conn = writer.add_connection(
+            '/cam0/depth_raw', Image.__msgtype__, typestore=typestore)
+
+        # Write out the IMU Data and Camera Data
+        t = log.min_imu_time()
+        i = 0
+
+        curr_frame_id = 0
+        frame = log.get_frame(curr_frame_id)
+
+        while t < log.max_imu_time():
+
+            sample = log.get_imu_sample(t)
+            imu_time_since_epoch = t.time_since_epoch()
+
+            while (frame is not None) and (frame.time_of_validity < t):
+                frame_time_since_epoch = frame.time_of_validity.time_since_epoch()
+                header = Header(
+                    seq=curr_frame_id,
+                    stamp=Time(
+                        sec=int(frame_time_since_epoch.total_seconds()),
+                        nanosec=1000 * frame_time_since_epoch.microseconds),
+                    frame_id='cam0'
+                )
+
+                bgr_frame = frame.bgr_frame()
+                # Foxglove expects mono images to have 16 bit depth, we we convert
+                # to np.uint16 here. Note that this doubles the size of the images
+                depth_frame = frame.depth_frame().astype(np.uint16)
+
+                bgr_ros = Image(
+                    header=header,
+                    height=bgr_frame.shape[0],
+                    width=bgr_frame.shape[1],
+                    encoding='bgr8',
+                    is_bigendian=False,
+                    # This is the size of each row in bytes
+                    step=bgr_frame.shape[1] * 3,
+                    data=bgr_frame.flatten()
+                )
+
+                depth_ros = Image(
+                    header=header,
+                    height=depth_frame.shape[0],
+                    width=depth_frame.shape[1],
+                    encoding='mono16',
+                    is_bigendian=False,
+                    # This is the size of each row in bytes
+                    step=depth_frame.shape[1] * 2,
+                    # the rosbags library wants a flat buffer, so we view the 16 bit pixels as uint8
+                    data=depth_frame.flatten().view(np.uint8)
+                )
+
+                timestamp_ns = (int(frame_time_since_epoch.total_seconds()) * 1_000_000_000
+                                + frame_time_since_epoch.microseconds * 1000)
+
+                writer.write(
+                    bgr_conn, timestamp_ns, typestore.serialize_ros1(bgr_ros, Image.__msgtype__))
+                writer.write(
+                    depth_conn, timestamp_ns, typestore.serialize_ros1(depth_ros, Image.__msgtype__))
+
+                curr_frame_id += 1
+                frame = log.get_frame(curr_frame_id)
+
+            imu_ros = Imu(
+                header=Header(
+                    seq=i,
+                    stamp=Time(
+                        sec=int(imu_time_since_epoch.total_seconds()),
+                        nanosec=1000 * imu_time_since_epoch.microseconds),
+                    frame_id='imu0'),
+                orientation=Quaternion(x=0, y=0, z=0, w=1),
+                orientation_covariance=-np.ones((3, 3)).flatten(),
+                angular_velocity=Vector3(
+                    x=sample.gyro_radps[0], y=sample.gyro_radps[1], z=sample.gyro_radps[2]),
+                angular_velocity_covariance=np.eye(3).flatten(),
+                linear_acceleration=Vector3(
+                    x=sample.accel_mpss[0], y=sample.accel_mpss[1], z=sample.accel_mpss[2]),
+                linear_acceleration_covariance=np.eye(3).flatten()
+            )
+
+            timestamp_ns = (int(imu_time_since_epoch.total_seconds()) * 1_000_000_000
+                            + imu_time_since_epoch.microseconds * 1000)
+
+            writer.write(imu_conn, timestamp_ns, typestore.serialize_ros1(imu_ros, Imu.__msgtype__))
+
+            t += rtp.as_duration(0.01)
+            i += 1
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--input", required=True, help="path to unzipped spectacular_log")
+    parser.add_argument("--output", required=True, help="path to output rosbag")
+
+    args = parser.parse_args()
+
+    main(Path(args.input), Path(args.output))

third_party/python/requirements.in

@@ -21,3 +21,4 @@ spatialmath-python==1.1.8
 reportlab==4.1.0
 seaborn==0.13.2
 depthai==2.28.0.0
+rosbags==0.9.23