From 640402a4184e2783bc63392dcdc0a9e3d193d569 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 10 Aug 2026 02:00:15 +0200 Subject: [PATCH 1/2] Add MCAP/ROS2-bag exporter for LAZ + IMU recordings Adds a laz_to_mcap console tool (apps/lidar_odometry_step_1) that reads a LAZ scan and optional IMU csv and writes a self-contained .mcap file (embedded ros2msg schemas, no ROS install needed to produce or inspect it) with /lidar_points (sensor_msgs/PointCloud2), /imu (sensor_msgs/Imu) and /lidar_sn (std_msgs/String) topics. Reuses load_point_cloud()/load_imu() from lidar_odometry_utils.cpp rather than load_data(), which additionally drops the first LAZ file's points as part of the odometry pipeline's own preprocessing -- wrong for a lossless exporter. The PointCloud2 field layout is selectable via --lidar-type (generic|velodyne|ouster|hesai) to match what different downstream LIO consumers expect, and topic names/frame_id are configurable. rosbags/McapWriter.{h,cpp} and cdr_serializer.hpp started from files copied out of the mandeye firmware/recorder project; rewrote them against plain McapPoint/McapImuSample structs instead of that project's LidarPoint/LidarIMU/lidars-BaseLidarClient.h types, which don't exist here. Also fixes 3rdparty/mcap's CMakeLists.txt: the INTERFACE target never linked liblz4/libzstd, which mcap's reader/writer .inl files need unconditionally even when compression is never used. Tests: rosbags/tests/test_mcap_writer.cpp (doctest, wired into BUILD_TESTING) round-trips PointCloud2 (all four layouts) and IMU messages through the real mcap::McapReader. Also validated end-to-end against a real mandeye recording with ros2 bag info/play (ROS2 jazzy). Co-Authored-By: Claude Sonnet 5 laz_to_mcap: support exporting a whole mandeye session directory The single-file positional arg now also accepts a directory: laz_to_mcap scans it for lidarNNNN.laz/.las + imuNNNN.csv chunk pairs (matched by the filename's last 4 digits, same convention load_data() uses), writes one /lidar_points message per chunk (in filename order), and merges + sorts every matched chunk's IMU samples into a single stream. Chunks without a matching csv still get their points exported, with a warning. --imu stays single-file-only since IMU now comes from the directory itself. Sequential, no threading, to keep the loop straightforward. Also gives McapFileWriter real per-channel sequence numbers instead of always writing 0 -- previously harmless (one message per channel), but directory mode now regularly writes many /lidar_points messages per file. Co-Authored-By: Claude Sonnet 5 Move laz_to_mcap from lidar_odometry_step_1 into console_tools It's a standalone LAZ/IMU-csv -> MCAP converter with no GUI and no real tie to the odometry app beyond reusing its load_point_cloud()/load_imu() parsing -- apps/console_tools is where every other converter like this (laz_to_pcd, laz_to_ply, pcd_to_laz, laz_to_txt) already lives, and matching that pattern means it's now gated by BUILD_WITH_CLI_TOOLS like its siblings instead of always building. Renamed mcap_export_main.cpp -> laz_to_mcap.cpp to match every sibling file in that directory being named after its target. It still pulls lidar_odometry_step_1/lidar_odometry_utils.{h,cpp} in by relative path for load_point_cloud()/load_imu() rather than duplicating them. Co-Authored-By: Claude Sonnet 5 Fix macOS-safe text and label formatting Co-authored-by: michalpelka <3209244+michalpelka@users.noreply.github.com> laz_to_mcap: add MLvxCalib calibration support, drop single-file mode Session directories now auto-detect calibration.json/.mjc + .sn (or accept --calibration/--sn overrides) and apply per-sensor extrinsics plus the configured IMU serial when exporting, matching the convention used by lidar_odometry.cpp and concatenate_multi_livox. Single-file (.laz + --imu) input is removed since multi-lidar calibration only makes sense for a full session directory. --- 3rdparty/mcap/CMakeLists.txt | 35 +++ CMakeLists.txt | 1 + apps/console_tools/CMakeLists.txt | 44 ++- apps/console_tools/laz_to_mcap.cpp | 481 +++++++++++++++++++++++++++++ cmake/dependencies.cmake | 5 + core/src/utils.cpp | 10 +- rosbags/McapWriter.cpp | 455 +++++++++++++++++++++++++++ rosbags/McapWriter.h | 138 +++++++++ rosbags/cdr_serializer.hpp | 386 +++++++++++++++++++++++ rosbags/tests/CMakeLists.txt | 25 ++ rosbags/tests/test_mcap_writer.cpp | 290 +++++++++++++++++ 11 files changed, 1864 insertions(+), 6 deletions(-) create mode 100644 3rdparty/mcap/CMakeLists.txt create mode 100644 apps/console_tools/laz_to_mcap.cpp create mode 100644 rosbags/McapWriter.cpp create mode 100644 rosbags/McapWriter.h create mode 100644 rosbags/cdr_serializer.hpp create mode 100644 rosbags/tests/CMakeLists.txt create mode 100644 rosbags/tests/test_mcap_writer.cpp diff --git a/3rdparty/mcap/CMakeLists.txt b/3rdparty/mcap/CMakeLists.txt new file mode 100644 index 00000000..e350c224 --- /dev/null +++ b/3rdparty/mcap/CMakeLists.txt @@ -0,0 +1,35 @@ +include(FetchContent) +FetchContent_Declare(mcap + GIT_REPOSITORY https://github.com/foxglove/mcap.git + GIT_TAG releases/cpp/v1.4.0 # pin a real tag +) +FetchContent_MakeAvailable(mcap) + +add_library(mcap INTERFACE) +target_include_directories(mcap INTERFACE ${mcap_SOURCE_DIR}/cpp/mcap/include) + +# mcap's reader/writer .inl always compile in LZ4/ZSTD (de)compression paths, +# even for callers that only ever write Compression::None -- so the symbols +# must be linked unconditionally, not just when compression is actually used. +# +# On Windows there's no system package manager providing liblz4/libzstd dev +# packages (unlike apt on Linux / Homebrew on macOS), and no vendored/prebuilt +# copy of them here yet -- see 3rdpartyBinary/Proj for that pattern if this +# ever needs revisiting. Until then, disable mcap's LZ4/ZSTD code paths on +# Windows via its own escape hatch: reading uncompressed/other-codec mcaps +# still works, only Compression::Lz4/Zstd at write time become unavailable -- +# fine since this repo's writer (rosbags/McapWriter) only ever writes +# Compression::None. +if(WIN32) + target_compile_definitions(mcap INTERFACE MCAP_COMPRESSION_NO_LZ4 MCAP_COMPRESSION_NO_ZSTD) +else() + find_library(MCAP_LZ4_LIBRARY NAMES lz4) + find_library(MCAP_ZSTD_LIBRARY NAMES zstd) + find_path(MCAP_LZ4_INCLUDE_DIR NAMES lz4frame.h) + find_path(MCAP_ZSTD_INCLUDE_DIR NAMES zstd.h) + if(NOT MCAP_LZ4_LIBRARY OR NOT MCAP_ZSTD_LIBRARY OR NOT MCAP_LZ4_INCLUDE_DIR OR NOT MCAP_ZSTD_INCLUDE_DIR) + message(FATAL_ERROR "mcap requires liblz4 and libzstd (dev packages) to build against") + endif() + target_link_libraries(mcap INTERFACE ${MCAP_LZ4_LIBRARY} ${MCAP_ZSTD_LIBRARY}) + target_include_directories(mcap INTERFACE ${MCAP_LZ4_INCLUDE_DIR} ${MCAP_ZSTD_INCLUDE_DIR}) +endif() diff --git a/CMakeLists.txt b/CMakeLists.txt index 381a21a8..914e91c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,6 +112,7 @@ if(BUILD_TESTING) enable_testing() add_subdirectory(shared/tests) add_subdirectory(apps/lidar_odometry_step_1/tests) + add_subdirectory(rosbags/tests) endif() set(CORE_LIBRARIES core) diff --git a/apps/console_tools/CMakeLists.txt b/apps/console_tools/CMakeLists.txt index 0db11a6e..d4d8cb52 100644 --- a/apps/console_tools/CMakeLists.txt +++ b/apps/console_tools/CMakeLists.txt @@ -42,4 +42,46 @@ target_include_directories(pcd_to_laz PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/inclu add_executable(laz_to_txt laz_to_txt.cpp) target_link_libraries(laz_to_txt PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog) -target_include_directories(laz_to_txt PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include) \ No newline at end of file +target_include_directories(laz_to_txt PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include) + +# laz_to_mcap: LAZ (+ optional IMU csv, or a whole mandeye session directory) +# -> MCAP/ROS2 bag exporter. Reuses load_point_cloud()/load_imu() from +# lidar_odometry_step_1/lidar_odometry_utils.cpp (not load_data(), which +# additionally drops the first LAZ file's points as part of that app's own +# odometry preprocessing -- wrong for a lossless exporter), so it needs +# TBB/spdlog/laszip/core plus that file's own header-only dependencies +# (glm/toml++/json/observation_equations/vqf), but none of the GUI libs. +add_executable( + laz_to_mcap laz_to_mcap.cpp + ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1/lidar_odometry_utils.h + ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp + ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.h ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.cpp + ) + +target_include_directories( + laz_to_mcap + PRIVATE ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1 + ${REPOSITORY_DIRECTORY}/core/include + ${REPOSITORY_DIRECTORY}/rosbags + ${THIRDPARTY_DIRECTORY} # csv.hpp (used by load_imu) + ${THIRDPARTY_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/tomlplusplus/include + ${THIRDPARTY_DIRECTORY}/json/include + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) + +target_link_libraries( + laz_to_mcap + PRIVATE + mcap + unordered_dense::unordered_dense + spdlog::spdlog + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS} + ${CORE_LIBRARIES}) + +if (MSVC) + target_compile_options(laz_to_mcap PRIVATE /bigobj) +endif() \ No newline at end of file diff --git a/apps/console_tools/laz_to_mcap.cpp b/apps/console_tools/laz_to_mcap.cpp new file mode 100644 index 00000000..723ce0b0 --- /dev/null +++ b/apps/console_tools/laz_to_mcap.cpp @@ -0,0 +1,481 @@ +// Standalone mandeye session directory -> MCAP/ROS2-bag exporter. +// +// Reuses load_point_cloud()/load_imu() (lidar_odometry_utils.cpp) -- the +// same LAZ/IMU-csv parsing lidar_odometry_step_1 itself uses -- rather than +// load_data(), which additionally applies odometry-pipeline-specific +// post-processing (e.g. it discards the first loaded LAZ file's points +// entirely) that would silently drop data for a lossless exporter. +#include "McapWriter.h" +#include "lidar_odometry_utils.h" + +#include +#include +#include +#include +#include + +#include + +namespace fs = std::filesystem; + +// load_imu()'s return type spelled out (rather than the `Imu` alias, which +// lives in lidar_odometry.h -- not included here, see the comment above). +using ImuData = std::vector, Eigen::Vector3f, Eigen::Vector3f>>; + +namespace +{ + +bool check_path_ext(const std::string& path, const char* ext) +{ + return fs::path(path).extension() == ext; +} + +std::string to_lower(std::string s) +{ + std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c) { return std::tolower(c); }); + return s; +} + +std::vector to_mcap_points(const std::vector& points) +{ + std::vector out; + out.reserve(points.size()); + for (const auto& p : points) + { + rosbags::McapPoint mp{}; + mp.x = static_cast(p.point.x()); + mp.y = static_cast(p.point.y()); + mp.z = static_cast(p.point.z()); + mp.intensity = p.intensity; + mp.laser_id = p.lidarid; + mp.timestamp = p.timestamp; + out.push_back(mp); + } + return out; +} + +std::vector to_mcap_imu(const ImuData& imu_data) +{ + std::vector out; + out.reserve(imu_data.size()); + for (const auto& [ts, gyr, acc] : imu_data) + { + rosbags::McapImuSample s{}; + s.timestamp = ts.first; + s.gyro_x = gyr.x(); + s.gyro_y = gyr.y(); + s.gyro_z = gyr.z(); + s.acc_x = acc.x(); + s.acc_y = acc.y(); + s.acc_z = acc.z(); + out.push_back(s); + } + return out; +} + +void sort_points_by_timestamp(std::vector& points) +{ + std::sort( + points.begin(), points.end(), [](const Point3Di& a, const Point3Di& b) + { + return a.timestamp < b.timestamp; + }); +} + +void sort_imu_by_timestamp(ImuData& imu) +{ + std::sort( + imu.begin(), imu.end(), [](const auto& a, const auto& b) + { + return std::get<0>(a).first < std::get<0>(b).first; + }); +} + +// Cuts the incoming points into one PointCloud2 message per 1/msg_hz seconds, +// so the bag replays at a lidar-like rate instead of one huge message per LAZ +// file. A point with timestamp t lands in bin floor(t * msg_hz): the bin grid +// is absolute, so it is identical for every chunk and a bin straddling a +// chunk boundary still yields a single message (pending points carry over to +// the next add()). msg_hz == 0 disables splitting -- one message per add(). +class MessageSplitter +{ +public: + MessageSplitter(rosbags::McapFileWriter& writer, double msg_hz) + : writer_(writer), msg_hz_(msg_hz) + { + } + + // `points` must be sorted by timestamp, and successive calls must be in + // timestamp order too (session chunks are processed oldest-first). + void add(const std::vector& points) + { + if (msg_hz_ <= 0.0) + { + write(points); + return; + } + for (const auto& p : points) + { + const int64_t bin = static_cast(std::floor(p.timestamp * msg_hz_)); + if (!pending_.empty() && bin != current_bin_) + flush(); + current_bin_ = bin; + pending_.push_back(p); + } + } + + // Writes whatever is still buffered; call once after the last add(). + void flush() + { + write(pending_); + pending_.clear(); + } + + size_t messages_written() const + { + return messages_; + } + +private: + void write(const std::vector& points) + { + if (points.empty()) + return; + const auto mcap_points = to_mcap_points(points); + const uint64_t stamp_ns = static_cast(mcap_points.front().timestamp * 1e9); + writer_.writePointCloud(stamp_ns, mcap_points); + ++messages_; + } + + rosbags::McapFileWriter& writer_; + double msg_hz_; + std::vector pending_; + int64_t current_bin_ = 0; + size_t messages_ = 0; +}; + +// Last 4 characters of the filename stem, matching the chunk-index convention +// mandeye recordings use (lidar0001.laz <-> imu0001.csv), mirroring +// lidar_odometry.cpp's load_data()/get4index. +std::string chunk_index(const std::string& path) +{ + const std::string stem = fs::path(path).stem().string(); + return stem.size() > 4 ? stem.substr(stem.size() - 4) : stem; +} + +// Per-sensor extrinsics (sensor id -> transform into the shared frame) plus +// which sensor id's IMU stream to use, resolved from an MLvxCalib +// calibration.json/.mjc + .sn file pair. Empty `per_sensor` means "apply no +// calibration", matching load_point_cloud()'s own convention. +struct Calibration +{ + std::unordered_map per_sensor; + int imu_id_to_use = 0; +}; + +// Mirrors lidar_odometry.cpp's load_data(): a session directory carries one +// calibration file (calibration.json/.mjc, skipping camera-calibration jsons +// named status*/cam0*/cam1*) and one .sn file describing every chunk. +void find_calibration_files(const fs::path& dir, std::string& calibration_file, std::string& sn_file) +{ + std::vector jsons, mjcs, sns; + for (const auto& entry : fs::directory_iterator(dir)) + { + if (!entry.is_regular_file()) + continue; + const std::string ext = to_lower(entry.path().extension().string()); + if (ext == ".json") + { + const std::string stem = to_lower(entry.path().stem().string()); + if (stem.starts_with("status") || stem.starts_with("cam0") || stem.starts_with("cam1")) + continue; + jsons.push_back(entry.path().string()); + } + else if (ext == ".mjc") + mjcs.push_back(entry.path().string()); + else if (ext == ".sn") + sns.push_back(entry.path().string()); + } + std::sort(jsons.begin(), jsons.end()); + std::sort(mjcs.begin(), mjcs.end()); + std::sort(sns.begin(), sns.end()); + + if (!jsons.empty()) + calibration_file = jsons.front(); + else if (!mjcs.empty()) + calibration_file = mjcs.front(); + + if (!sns.empty()) + sn_file = sns.front(); +} + +// Loads an MLvxCalib calibration.json/.mjc + .sn pair (see +// lidar_odometry_utils.h's MLvxCalib namespace doc comment for the file +// format) and combines them into a sensor-id-keyed extrinsics map, ready to +// pass to load_point_cloud(). Returns a default (no-op) Calibration if either +// path is empty. +Calibration load_calibration(const std::string& calibration_file, const std::string& sn_file) +{ + Calibration result; + if (calibration_file.empty() || sn_file.empty()) + return result; + + const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibration_file); + if (preloadedCalibration.empty()) + { + spdlog::warn("No calibration data found in {} - exporting without per-sensor calibration", calibration_file); + return result; + } + const auto idToSn = MLvxCalib::GetIdToSnMapping(sn_file); + const std::string imuSnToUse = MLvxCalib::GetImuSnToUse(calibration_file); + + spdlog::info("Loaded calibration for {} sensor(s) from {}", preloadedCalibration.size(), calibration_file); + for (const auto& [sn, _] : preloadedCalibration) + spdlog::info(" -> {}", sn); + + for (const auto& [id, sn] : idToSn) + { + const auto it = preloadedCalibration.find(sn); + if (it == preloadedCalibration.end()) + { + spdlog::warn("Sensor id {} (serial '{}') from {} has no entry in {} - its points will be dropped", id, sn, sn_file, calibration_file); + continue; + } + result.per_sensor[id] = it->second; + } + + result.imu_id_to_use = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); + spdlog::info("Using IMU id {} (serial '{}')", result.imu_id_to_use, imuSnToUse); + return result; +} + +struct Chunk +{ + std::string laz; + std::string imu; // empty if no matching csv was found +}; + +// Scans `dir` for lidarNNNN.laz/.las + imuNNNN.csv chunk pairs (a real +// mandeye recording folder) and matches them by chunk_index(). LAZ files +// without a matching csv are still exported (points only, no IMU for that +// chunk). +std::vector scan_session_directory(const fs::path& dir) +{ + std::vector laz_files, csv_files; + for (const auto& entry : fs::directory_iterator(dir)) + { + if (!entry.is_regular_file()) + continue; + const std::string ext = to_lower(entry.path().extension().string()); + if (ext == ".laz" || ext == ".las") + laz_files.push_back(entry.path().string()); + else if (ext == ".csv") + csv_files.push_back(entry.path().string()); + } + std::sort(laz_files.begin(), laz_files.end()); + std::sort(csv_files.begin(), csv_files.end()); + + std::vector chunks; + chunks.reserve(laz_files.size()); + for (const auto& laz : laz_files) + { + const std::string idx = chunk_index(laz); + Chunk chunk{laz, {}}; + for (const auto& csv : csv_files) + { + if (chunk_index(csv) == idx) + { + chunk.imu = csv; + break; + } + } + if (chunk.imu.empty()) + spdlog::warn("No matching IMU csv for {} (chunk index '{}')", laz, idx); + chunks.push_back(std::move(chunk)); + } + return chunks; +} + +void print_usage(const char* argv0) +{ + spdlog::error("Usage: {} [options]", argv0); + spdlog::error(" session_dir a mandeye recording folder of lidarNNNN.laz + imuNNNN.csv chunk"); + spdlog::error(" pairs (matched by filename's last 4 digits); every chunk becomes"); + spdlog::error(" its own /lidar_points message, IMU samples are merged into one stream"); + spdlog::error("Options:"); + spdlog::error(" --lidar-topic lidar PointCloud2 topic (default: /lidar_points)"); + spdlog::error(" --imu-topic IMU topic (default: /imu)"); + spdlog::error(" --sn-topic serial-number string topic (default: /lidar_sn)"); + spdlog::error(" --frame-id frame_id written into message headers (default: lidar)"); + spdlog::error(" --lidar-type PointCloud2 field layout: generic|velodyne|ouster|hesai (default: generic)"); + spdlog::error(" --msg_hz message rate: points are split into one PointCloud2 per"); + spdlog::error(" 1/hz seconds (default: 10; 0 = one message per input file/chunk)"); + spdlog::error(" --calibration MLvxCalib calibration.json/.mjc (multi-LiVoX extrinsics + IMU"); + spdlog::error(" selection); session_dir auto-detects this if omitted"); + spdlog::error(" --sn MLvxCalib .sn file (sensor id -> serial number); session_dir"); + spdlog::error(" auto-detects this if omitted. Must be given together with --calibration"); +} + +int run_session_directory( + const std::string& dir_path, const std::string& mcap_path, const rosbags::McapWriterOptions& options, double msg_hz, + std::string calibration_path, std::string sn_path) +{ + auto chunks = scan_session_directory(dir_path); + if (chunks.empty()) + { + spdlog::error("No .laz/.las files found in {}", dir_path); + return EXIT_FAILURE; + } + spdlog::info("Found {} chunk(s) in {}", chunks.size(), dir_path); + + if (calibration_path.empty() || sn_path.empty()) + { + std::string found_calibration, found_sn; + find_calibration_files(dir_path, found_calibration, found_sn); + if (calibration_path.empty()) + calibration_path = found_calibration; + if (sn_path.empty()) + sn_path = found_sn; + } + const Calibration calib = load_calibration(calibration_path, sn_path); + + rosbags::McapFileWriter writer(mcap_path, options); + if (!writer.isOpen()) + { + spdlog::error("Failed to open output mcap file {}", mcap_path); + return EXIT_FAILURE; + } + + size_t total_points = 0; + ImuData all_imu; + MessageSplitter splitter(writer, msg_hz); + for (size_t i = 0; i < chunks.size(); ++i) + { + auto points = load_point_cloud( + chunks[i].laz, /*ommit_points_with_timestamp_equals_zero=*/false, /*filter_threshold_xy_inner=*/0.0, + /*filter_threshold_xy_outer=*/std::numeric_limits::max(), /*calibrations=*/calib.per_sensor); + sort_points_by_timestamp(points); + total_points += points.size(); + splitter.add(points); + spdlog::info("[{}/{}] {}: {} points", i + 1, chunks.size(), chunks[i].laz, points.size()); + + if (!chunks[i].imu.empty()) + { + auto imu_data = load_imu(chunks[i].imu, calib.imu_id_to_use); + all_imu.insert(all_imu.end(), std::make_move_iterator(imu_data.begin()), std::make_move_iterator(imu_data.end())); + } + } + splitter.flush(); + spdlog::info("Loaded {} points across {} chunk(s), wrote {} point cloud message(s)", total_points, chunks.size(), splitter.messages_written()); + + if (!all_imu.empty()) + { + sort_imu_by_timestamp(all_imu); + writer.writeImu(to_mcap_imu(all_imu)); + spdlog::info("Loaded {} IMU samples across matched chunk(s)", all_imu.size()); + } + + spdlog::info("Wrote {}", mcap_path); + return EXIT_SUCCESS; +} + +} // namespace + +int main(const int argc, const char** argv) +{ + if (argc < 3) + { + print_usage(argv[0]); + return EXIT_FAILURE; + } + + const std::string input_path = argv[1]; + const std::string mcap_path = argv[2]; + std::string calibration_path; + std::string sn_path; + rosbags::McapWriterOptions options; + double msg_hz = 10.0; + + for (int i = 3; i < argc; ++i) + { + const std::string arg = argv[i]; + const bool hasValue = i + 1 < argc; + + if (arg == "--calibration" && hasValue) + calibration_path = argv[++i]; + else if (arg == "--sn" && hasValue) + sn_path = argv[++i]; + else if (arg == "--lidar-topic" && hasValue) + options.lidar_topic = argv[++i]; + else if (arg == "--imu-topic" && hasValue) + options.imu_topic = argv[++i]; + else if (arg == "--sn-topic" && hasValue) + options.sn_topic = argv[++i]; + else if (arg == "--frame-id" && hasValue) + options.frame_id = argv[++i]; + else if (arg == "--msg_hz" && hasValue) + { + const std::string value = argv[++i]; + try + { + msg_hz = std::stod(value); + } + catch (const std::exception&) + { + spdlog::error("Invalid --msg_hz '{}' (expected a number)", value); + return EXIT_FAILURE; + } + if (!std::isfinite(msg_hz) || msg_hz < 0.0) + { + spdlog::error("Invalid --msg_hz '{}' (expected >= 0; 0 = one message per input file/chunk)", value); + return EXIT_FAILURE; + } + } + else if (arg == "--lidar-type" && hasValue) + { + const std::string type = argv[++i]; + if (type == "generic") + options.lidar_layout = rosbags::PointCloudLayout::Generic; + else if (type == "velodyne") + options.lidar_layout = rosbags::PointCloudLayout::Velodyne; + else if (type == "ouster") + options.lidar_layout = rosbags::PointCloudLayout::Ouster; + else if (type == "hesai") + options.lidar_layout = rosbags::PointCloudLayout::Hesai; + else + { + spdlog::error("Unknown --lidar-type '{}' (expected generic|velodyne|ouster|hesai)", type); + return EXIT_FAILURE; + } + } + else + { + spdlog::error("Unrecognized argument '{}'", arg); + print_usage(argv[0]); + return EXIT_FAILURE; + } + } + + if (!check_path_ext(mcap_path, ".mcap")) + { + spdlog::error("Invalid extension for output file {} - expected .mcap", mcap_path); + return EXIT_FAILURE; + } + if (!fs::exists(input_path)) + { + spdlog::error("Input path {} does not exist", input_path); + return EXIT_FAILURE; + } + if (!fs::is_directory(input_path)) + { + spdlog::error("Input path {} is not a directory - expected a mandeye session directory", input_path); + return EXIT_FAILURE; + } + if (calibration_path.empty() != sn_path.empty()) + { + spdlog::error("--calibration and --sn must be given together"); + return EXIT_FAILURE; + } + + return run_session_directory(input_path, mcap_path, options, msg_hz, calibration_path, sn_path); +} \ No newline at end of file diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 172be2da..4baa3346 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -201,4 +201,9 @@ message(STATUS "Using external library: wgs84_do_puwg92") add_subdirectory(${EXTERNAL_LIBRARIES_DIRECTORY}/WGS84toCartesian) message(STATUS "Using external library: WGS84toCartesian") + +# mcap - support for ROS 2 bag export +add_subdirectory(${THIRDPARTY_DIRECTORY}/mcap) + message(STATUS "==== Dependencies Configuration Complete ====") + diff --git a/core/src/utils.cpp b/core/src/utils.cpp index de2c1986..8490769e 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -1096,7 +1096,7 @@ void info_window(const std::vector& infoLines, const std::vector= 1000.0f) - sprintf(label, "%.0f [km]", worldLength / 1000.0f); + snprintf(label, sizeof(label), "%.0f [km]", worldLength / 1000.0f); else if (worldLength >= 1.0f) - sprintf(label, "%.0f [m]", worldLength); + snprintf(label, sizeof(label), "%.0f [m]", worldLength); else if (worldLength >= 0.01f) - sprintf(label, "%.0f [cm]", worldLength * 100.0f); + snprintf(label, sizeof(label), "%.0f [cm]", worldLength * 100.0f); else - sprintf(label, "<1 [cm]"); + snprintf(label, sizeof(label), "<1 [cm]"); // drawLabel(axes[2].x() + 0.2f, axes[2].y() + 0.4f, axes[2].z() - 0.2f, label, // colors[2][0], colors[2][1], colors[2][2]); diff --git a/rosbags/McapWriter.cpp b/rosbags/McapWriter.cpp new file mode 100644 index 00000000..93b05e76 --- /dev/null +++ b/rosbags/McapWriter.cpp @@ -0,0 +1,455 @@ +#define MCAP_IMPLEMENTATION +#include "McapWriter.h" +#include "cdr_serializer.hpp" +#include +#include +#include +#include +#include // writer + reader implementations compiled here once + +namespace rosbags +{ + +// --------------------------------------------------------------------------- +// ros2msg schema strings (full definitions including nested types) +// --------------------------------------------------------------------------- + +static constexpr const char* kPointCloud2Schema = R"(std_msgs/Header header +uint32 height +uint32 width +sensor_msgs/PointField[] fields +bool is_bigendian +uint32 point_step +uint32 row_step +uint8[] data +bool is_dense +================================================================================ +MSG: std_msgs/Header +builtin_interfaces/Time stamp +string frame_id +================================================================================ +MSG: builtin_interfaces/Time +int32 sec +uint32 nanosec +================================================================================ +MSG: sensor_msgs/PointField +string name +uint32 offset +uint8 datatype +uint32 count +uint8 INT8=1 +uint8 UINT8=2 +uint8 INT16=3 +uint8 UINT16=4 +uint8 INT32=5 +uint8 UINT32=6 +uint8 FLOAT32=7 +uint8 FLOAT64=8 +)"; + +static constexpr const char* kStringSchema = R"(string data +)"; + +static constexpr const char* kImuSchema = R"(std_msgs/Header header +geometry_msgs/Quaternion orientation +float64[9] orientation_covariance +geometry_msgs/Vector3 angular_velocity +float64[9] angular_velocity_covariance +geometry_msgs/Vector3 linear_acceleration +float64[9] linear_acceleration_covariance +================================================================================ +MSG: std_msgs/Header +builtin_interfaces/Time stamp +string frame_id +================================================================================ +MSG: builtin_interfaces/Time +int32 sec +uint32 nanosec +================================================================================ +MSG: geometry_msgs/Quaternion +float64 x +float64 y +float64 z +float64 w +================================================================================ +MSG: geometry_msgs/Vector3 +float64 x +float64 y +float64 z +)"; + +// --------------------------------------------------------------------------- +// PointCloud2 binary layouts, little-endian (see PointCloudLayout in McapWriter.h +// for the full per-layout offset table). +// --------------------------------------------------------------------------- + +static constexpr uint32_t kPointStepGeneric = 28; +static constexpr uint32_t kPointStepVelodyne = 26; +static constexpr uint32_t kPointStepOuster = 26; +static constexpr uint32_t kPointStepHesai = 26; + +static uint32_t pointStepFor(PointCloudLayout layout) +{ + switch(layout) + { + case PointCloudLayout::Velodyne: return kPointStepVelodyne; + case PointCloudLayout::Ouster: return kPointStepOuster; + case PointCloudLayout::Hesai: return kPointStepHesai; + case PointCloudLayout::Generic: + default: return kPointStepGeneric; + } +} + +// Write one point as raw bytes per `layout`. Done field-by-field to avoid +// C++ struct padding surprises. +static void writePointBytes(CdrWriter& w, const McapPoint& p, double t0, PointCloudLayout layout) +{ + w.write_raw(&p.x, 4); + w.write_raw(&p.y, 4); + w.write_raw(&p.z, 4); + w.write_raw(&p.intensity, 4); + + switch(layout) + { + case PointCloudLayout::Velodyne: + { + w.write_raw(&p.ring, 2); + const double t = p.timestamp - t0; + w.write_raw(&t, 8); + return; + } + case PointCloudLayout::Ouster: + { + // t: ns relative to the message stamp, matching ouster-ros' per-point offset. + const uint32_t t = static_cast((p.timestamp - t0) * 1e9); + // LAZ carries no native reflectivity/ambient channel: reflectivity is + // approximated from intensity, ambient is always 0. + const uint16_t reflectivity = static_cast(std::clamp(p.intensity, 0.0f, 65535.0f)); + const uint16_t ambient = 0; + w.write_raw(&t, 4); + w.write_raw(&reflectivity, 2); + w.write_raw(&p.ring, 2); + w.write_raw(&ambient, 2); + return; + } + case PointCloudLayout::Hesai: + { + // timestamp: ABSOLUTE seconds (not relative to t0), per Hesai driver convention. + w.write_raw(&p.timestamp, 8); + w.write_raw(&p.ring, 2); + return; + } + case PointCloudLayout::Generic: + default: + { + w.write_raw(&p.ring, 2); + w.write_raw(&p.laser_id, 1); + const uint8_t pad = 0; + w.write_raw(&pad, 1); + const double t = p.timestamp - t0; + w.write_raw(&t, 8); + return; + } + } +} + +// --------------------------------------------------------------------------- +// CDR helpers +// --------------------------------------------------------------------------- + +static void writeHeader(CdrWriter& w, uint64_t timestamp_ns, const std::string& frame_id) +{ + w.write_i32(static_cast(timestamp_ns / 1'000'000'000ULL)); + w.write_u32(static_cast(timestamp_ns % 1'000'000'000ULL)); + w.write_string(frame_id); +} + +static std::vector serializePointCloud2( + uint64_t timestamp_ns, const std::vector& pts, const std::string& frame_id, PointCloudLayout layout) +{ + CdrWriter w; + + // Header + writeHeader(w, timestamp_ns, frame_id); + + // height / width (unordered: height=1, width=N) + w.write_u32(1); + w.write_u32(static_cast(pts.size())); + + // PointField[] + struct FieldDef + { + const char* name; + uint32_t offset; + uint8_t datatype; // 6=UINT32, 7=FLOAT32, 4=UINT16, 2=UINT8, 8=FLOAT64 + }; + static constexpr FieldDef kFieldsGeneric[] = { + {"x", 0, 7}, // FLOAT32 + {"y", 4, 7}, // FLOAT32 + {"z", 8, 7}, // FLOAT32 + {"intensity", 12, 7}, // FLOAT32 + {"ring", 16, 4}, // UINT16 + {"laser_id", 18, 2}, // UINT8 + {"time", 20, 8}, // FLOAT64 + }; + static constexpr FieldDef kFieldsVelodyne[] = { + {"x", 0, 7}, // FLOAT32 + {"y", 4, 7}, // FLOAT32 + {"z", 8, 7}, // FLOAT32 + {"intensity", 12, 7}, // FLOAT32 + {"ring", 16, 4}, // UINT16 + {"time", 18, 8}, // FLOAT64 + }; + static constexpr FieldDef kFieldsOuster[] = { + {"x", 0, 7}, // FLOAT32 + {"y", 4, 7}, // FLOAT32 + {"z", 8, 7}, // FLOAT32 + {"intensity", 12, 7}, // FLOAT32 + {"t", 16, 6}, // UINT32 + {"reflectivity", 20, 4}, // UINT16 + {"ring", 22, 4}, // UINT16 + {"ambient", 24, 4}, // UINT16 + }; + static constexpr FieldDef kFieldsHesai[] = { + {"x", 0, 7}, // FLOAT32 + {"y", 4, 7}, // FLOAT32 + {"z", 8, 7}, // FLOAT32 + {"intensity", 12, 7}, // FLOAT32 + {"timestamp", 16, 8}, // FLOAT64 + {"ring", 24, 4}, // UINT16 + }; + + const FieldDef* fields = kFieldsGeneric; + uint32_t nFields = std::size(kFieldsGeneric); + switch(layout) + { + case PointCloudLayout::Velodyne: + fields = kFieldsVelodyne; + nFields = std::size(kFieldsVelodyne); + break; + case PointCloudLayout::Ouster: + fields = kFieldsOuster; + nFields = std::size(kFieldsOuster); + break; + case PointCloudLayout::Hesai: + fields = kFieldsHesai; + nFields = std::size(kFieldsHesai); + break; + case PointCloudLayout::Generic: + default: break; + } + const uint32_t pointStep = pointStepFor(layout); + + w.write_u32(nFields); // sequence length + for(uint32_t i = 0; i < nFields; ++i) + { + w.write_string(fields[i].name); + w.write_u32(fields[i].offset); + w.write_u8(fields[i].datatype); + w.write_u32(1); // count + } + + w.write_bool(false); // is_bigendian + w.write_u32(pointStep); // point_step + w.write_u32(static_cast(pts.size()) * pointStep); // row_step + + // data[] — uint32 length + raw bytes (no internal CDR padding) + const uint32_t dataBytes = static_cast(pts.size()) * pointStep; + w.write_u32(dataBytes); + + const double t0 = pts.empty() ? 0.0 : pts.front().timestamp; + for(const auto& p : pts) + writePointBytes(w, p, t0, layout); + + w.write_bool(true); // is_dense + return w.data(); +} + +static std::vector serializeImu(uint64_t timestamp_ns, const McapImuSample& imu, const std::string& frame_id) +{ + CdrWriter w; + writeHeader(w, timestamp_ns, frame_id); + + // orientation quaternion — identity (no orientation data from lidar IMU) + w.write_f64(0.0); // x + w.write_f64(0.0); // y + w.write_f64(0.0); // z + w.write_f64(1.0); // w + + // orientation_covariance[9] — -1 in [0] signals "unknown" + w.write_f64(-1.0); + for(int i = 1; i < 9; ++i) + w.write_f64(0.0); + + // angular_velocity (rad/s) + w.write_f64(static_cast(imu.gyro_x)); + w.write_f64(static_cast(imu.gyro_y)); + w.write_f64(static_cast(imu.gyro_z)); + + // angular_velocity_covariance[9] + for(int i = 0; i < 9; ++i) + w.write_f64(0.0); + + // linear_acceleration (m/s²) + w.write_f64(static_cast(imu.acc_x)); + w.write_f64(static_cast(imu.acc_y)); + w.write_f64(static_cast(imu.acc_z)); + + // linear_acceleration_covariance[9] + for(int i = 0; i < 9; ++i) + w.write_f64(0.0); + + return w.data(); +} + +// --------------------------------------------------------------------------- +// Impl +// --------------------------------------------------------------------------- + +struct McapFileWriter::Impl +{ + mcap::McapWriter writer; + mcap::ChannelId lidarChannelId{0}; + mcap::ChannelId imuChannelId{0}; + mcap::ChannelId snChannelId{0}; + uint32_t lidarSequence{0}; + uint32_t imuSequence{0}; + uint32_t snSequence{0}; + McapWriterOptions options; + bool open{false}; +}; + +McapFileWriter::McapFileWriter(const std::filesystem::path& path, const McapWriterOptions& options) + : impl_(std::make_unique()) +{ + impl_->options = options; + + mcap::McapWriterOptions opts("ros2"); + opts.compression = mcap::Compression::None; + opts.chunkSize = 128ULL * 1024 * 1024; + + auto status = impl_->writer.open(path.string(), opts); + if(!status.ok()) + { + std::cerr << "McapWriter: failed to open " << path << ": " << status.message << "\n"; + return; + } + + // Register PointCloud2 schema (shared by both PointCloudLayout variants -- + // only the runtime `fields`/point_step differ, not the message schema itself) + mcap::Schema pc2Schema("sensor_msgs/msg/PointCloud2", + "ros2msg", + {reinterpret_cast(kPointCloud2Schema), + reinterpret_cast(kPointCloud2Schema) + std::strlen(kPointCloud2Schema)}); + impl_->writer.addSchema(pc2Schema); + + // Register Imu schema + mcap::Schema imuSchema( + "sensor_msgs/msg/Imu", + "ros2msg", + {reinterpret_cast(kImuSchema), reinterpret_cast(kImuSchema) + std::strlen(kImuSchema)}); + impl_->writer.addSchema(imuSchema); + + // Register channels + mcap::Channel lidarChannel(impl_->options.lidar_topic, "cdr", pc2Schema.id); + impl_->writer.addChannel(lidarChannel); + impl_->lidarChannelId = lidarChannel.id; + + mcap::Channel imuChannel(impl_->options.imu_topic, "cdr", imuSchema.id); + impl_->writer.addChannel(imuChannel); + impl_->imuChannelId = imuChannel.id; + + // Register std_msgs/msg/String schema + /lidar_sn channel + mcap::Schema strSchema("std_msgs/msg/String", "ros2msg", + {reinterpret_cast(kStringSchema), + reinterpret_cast(kStringSchema) + std::strlen(kStringSchema)}); + impl_->writer.addSchema(strSchema); + + mcap::Channel snChannel(impl_->options.sn_topic, "cdr", strSchema.id); + impl_->writer.addChannel(snChannel); + impl_->snChannelId = snChannel.id; + + impl_->open = true; +} + +McapFileWriter::~McapFileWriter() +{ + if(impl_ && impl_->open) + impl_->writer.close(); +} + +bool McapFileWriter::isOpen() const +{ + return impl_ && impl_->open; +} + +void McapFileWriter::writeSn(uint64_t timestamp_ns, const std::string& data) +{ + if(!isOpen()) + return; + + CdrWriter w; + w.write_string(data); + + const auto& payload = w.data(); + mcap::Message msg; + msg.channelId = impl_->snChannelId; + msg.sequence = impl_->snSequence++; + msg.publishTime = timestamp_ns; + msg.logTime = timestamp_ns; + msg.data = reinterpret_cast(payload.data()); + msg.dataSize = payload.size(); + + auto status = impl_->writer.write(msg); + if(!status.ok()) + std::cerr << "McapWriter: writeSn error: " << status.message << "\n"; +} + +void McapFileWriter::writePointCloud(uint64_t timestamp_ns, const std::vector& points) +{ + if(!isOpen() || points.empty()) + return; + std::cerr << "McapWriter: " << points.size() << " points\n"; + auto payload = serializePointCloud2(timestamp_ns, points, impl_->options.frame_id, impl_->options.lidar_layout); + + mcap::Message msg; + msg.channelId = impl_->lidarChannelId; + msg.sequence = impl_->lidarSequence++; + msg.publishTime = timestamp_ns; + msg.logTime = timestamp_ns; + msg.data = reinterpret_cast(payload.data()); + msg.dataSize = payload.size(); + + auto status = impl_->writer.write(msg); + if(!status.ok()) + std::cerr << "McapWriter: write error: " << status.message << "\n"; +} + +void McapFileWriter::writeImuSample(const McapImuSample& sample) +{ + if(!isOpen()) + return; + + const uint64_t ts = static_cast(sample.timestamp * 1e9); + auto payload = serializeImu(ts, sample, impl_->options.frame_id); + + mcap::Message msg; + msg.channelId = impl_->imuChannelId; + msg.sequence = impl_->imuSequence++; + msg.publishTime = ts; + msg.logTime = ts; + msg.data = reinterpret_cast(payload.data()); + msg.dataSize = payload.size(); + + auto s = impl_->writer.write(msg); + if(!s.ok()) + std::cerr << "McapWriter: IMU write error: " << s.message << "\n"; +} + +void McapFileWriter::writeImu(const std::vector& imu) +{ + for(const auto& sample : imu) + writeImuSample(sample); +} + +} // namespace rosbags \ No newline at end of file diff --git a/rosbags/McapWriter.h b/rosbags/McapWriter.h new file mode 100644 index 00000000..cd09c8e9 --- /dev/null +++ b/rosbags/McapWriter.h @@ -0,0 +1,138 @@ +#pragma once +#include +#include +#include +#include +#include + +// Forward-declare mcap types so this header stays light. +namespace mcap +{ +class McapWriter; +} + +namespace rosbags +{ + +// One lidar point, laid out for direct copy into the ros2msg PointCloud2 +// wire format (see McapFileWriter's class comment below). `timestamp` is +// an absolute timestamp in seconds (e.g. LAS/LAZ gps_time), matching the +// unit HDMapping already stores on Core::Point3Di::timestamp. +struct McapPoint +{ + float x{}; + float y{}; + float z{}; + float intensity{}; + uint16_t ring{}; + uint8_t laser_id{}; + double timestamp{}; +}; + +// One IMU sample. `timestamp` is an absolute timestamp in seconds. +// gyro is rad/s, acc is m/s^2 (sensor_msgs/Imu convention) -- values are +// written through as given, no unit conversion is performed. +struct McapImuSample +{ + double timestamp{}; + float gyro_x{}; + float gyro_y{}; + float gyro_z{}; + float acc_x{}; + float acc_y{}; + float acc_z{}; +}; + +// Selects the sensor_msgs/msg/PointCloud2 field layout the lidar channel is +// written with. The message type is always PointCloud2 -- only the `fields` +// array/point_step (and thus which McapPoint members get written) changes, +// to match what a given downstream consumer expects. +enum class PointCloudLayout +{ + Generic, // x,y,z,intensity,ring,laser_id,time -- point_step = 28 (default) + Velodyne, // x,y,z,intensity,ring,time -- point_step = 26 (no laser_id) + Ouster, // x,y,z,intensity,t,reflectivity,ring,ambient -- point_step = 26 + Hesai, // x,y,z,intensity,timestamp,ring -- point_step = 26 +}; + +struct McapWriterOptions +{ + std::string frame_id = "lidar"; + std::string lidar_topic = "/lidar_points"; + std::string imu_topic = "/imu"; + std::string sn_topic = "/lidar_sn"; + PointCloudLayout lidar_layout = PointCloudLayout::Generic; +}; + +// Writes lidar points and IMU samples to an MCAP file using ros2msg schema +// encoding + CDR serialization. The resulting file is playable with +// `ros2 bag play` and renderable in Foxglove Studio without any ROS2 runtime. +// +// Default topics: +// /lidar_points — sensor_msgs/msg/PointCloud2 (field layout per options().lidar_layout) +// /imu — sensor_msgs/msg/Imu +// /lidar_sn — std_msgs/msg/String +// +// PointCloud2 field layouts (see PointCloudLayout): +// Generic (point_step = 28): +// x float32 offset 0 +// y float32 offset 4 +// z float32 offset 8 +// intensity float32 offset 12 +// ring uint16 offset 16 +// laser_id uint8 offset 18 +// time float64 offset 20 (seconds, relative to the message stamp) +// Velodyne (point_step = 26, no laser_id): +// x float32 offset 0 +// y float32 offset 4 +// z float32 offset 8 +// intensity float32 offset 12 +// ring uint16 offset 16 +// time float64 offset 18 (seconds, relative to the message stamp) +// Ouster (point_step = 26, matches ouster-ros' field names/types): +// x float32 offset 0 +// y float32 offset 4 +// z float32 offset 8 +// intensity float32 offset 12 +// t uint32 offset 16 (nanoseconds, relative to the message stamp) +// reflectivity uint16 offset 20 (McapPoint::intensity clamped to [0,65535]; LAZ has no native reflectivity) +// ring uint16 offset 22 +// ambient uint16 offset 24 (always 0; LAZ has no native ambient-light channel) +// Hesai (point_step = 26, matches common Hesai driver field names/types): +// x float32 offset 0 +// y float32 offset 4 +// z float32 offset 8 +// intensity float32 offset 12 +// timestamp float64 offset 16 (seconds, ABSOLUTE -- not relative to the message stamp, per Hesai convention) +// ring uint16 offset 24 +class McapFileWriter +{ +public: + explicit McapFileWriter(const std::filesystem::path& path, const McapWriterOptions& options = {}); + ~McapFileWriter(); + + // Non-copyable, movable + McapFileWriter(const McapFileWriter&) = delete; + McapFileWriter& operator=(const McapFileWriter&) = delete; + + // Write a batch of points as a single PointCloud2 message (layout per options().lidar_layout). + // timestamp_ns is the message publish time (nanoseconds since epoch). + void writePointCloud(uint64_t timestamp_ns, const std::vector& points); + + // Write a single IMU sample as its own /imu message. + void writeImuSample(const McapImuSample& imu); + + // Write a batch of IMU samples, one /imu message per sample. + void writeImu(const std::vector& imu); + + // Write a string to /lidar_sn (std_msgs/msg/String). + void writeSn(uint64_t timestamp_ns, const std::string& data); + + bool isOpen() const; + +private: + struct Impl; + std::unique_ptr impl_; +}; + +} // namespace rosbags \ No newline at end of file diff --git a/rosbags/cdr_serializer.hpp b/rosbags/cdr_serializer.hpp new file mode 100644 index 00000000..bdfdf622 --- /dev/null +++ b/rosbags/cdr_serializer.hpp @@ -0,0 +1,386 @@ +#pragma once +#include "McapWriter.h" +#include +#include +#include +#include +#include +#include + +// Minimal CDR (OMG CDR little-endian) serializer for ros2msg-encoded MCAP messages. +// The 4-byte RTPS representation header {0x00,0x01,0x00,0x00} is written in the +// constructor so callers just append fields directly. +class CdrWriter +{ +public: + CdrWriter() + { + buf_ = {0x00, 0x01, 0x00, 0x00}; + } + + void write_bool(bool v) + { + buf_.push_back(v ? 1u : 0u); + } + void write_u8(uint8_t v) + { + buf_.push_back(v); + } + + void write_u16(uint16_t v) + { + pad_to(2); + append(&v, 2); + } + void write_i32(int32_t v) + { + pad_to(4); + append(&v, 4); + } + void write_u32(uint32_t v) + { + pad_to(4); + append(&v, 4); + } + void write_f32(float v) + { + pad_to(4); + append(&v, 4); + } + void write_f64(double v) + { + pad_to(8); + append(&v, 8); + } + + void write_string(const std::string& s) + { + write_u32(static_cast(s.size() + 1)); + buf_.insert(buf_.end(), s.begin(), s.end()); + buf_.push_back(0); + } + void write_string(const char* s) + { + const size_t len = std::strlen(s); + write_u32(static_cast(len + 1)); + buf_.insert(buf_.end(), s, s + len); + buf_.push_back(0); + } + + // Raw bytes — no alignment, for uint8[] sequence payloads. + void write_raw(const void* src, size_t n) + { + const auto* p = static_cast(src); + buf_.insert(buf_.end(), p, p + n); + } + void write_raw(const std::string& s) + { + buf_.insert(buf_.end(), s.begin(), s.end()); + } + + const std::vector& data() const + { + return buf_; + } + +private: + std::vector buf_; + + // Alignment is counted from byte 4 (the CDR data section start). + void pad_to(size_t n) + { + size_t mod = (buf_.size() - 4) % n; + if(mod) + buf_.resize(buf_.size() + (n - mod), 0); + } + + void append(const void* src, size_t n) + { + const auto* p = static_cast(src); + buf_.insert(buf_.end(), p, p + n); + } +}; + +// --------------------------------------------------------------------------- +// CDR reader — mirrors CdrWriter alignment rules exactly. +// Constructed from a raw message buffer; skips the 4-byte RTPS header. +// --------------------------------------------------------------------------- +class CdrReader +{ +public: + CdrReader(const void* data, size_t size) + : data_(static_cast(data)) + , size_(size) + , pos_(4) + { } + + bool ok() const + { + return pos_ <= size_; + } + + bool read_bool() + { + return pos_ < size_ ? data_[pos_++] != 0 : false; + } + uint8_t read_u8() + { + return pos_ < size_ ? data_[pos_++] : 0; + } + + uint16_t read_u16() + { + align(2); + uint16_t v = 0; + safe_copy(&v, 2); + return v; + } + int32_t read_i32() + { + align(4); + int32_t v = 0; + safe_copy(&v, 4); + return v; + } + uint32_t read_u32() + { + align(4); + uint32_t v = 0; + safe_copy(&v, 4); + return v; + } + float read_f32() + { + align(4); + float v = 0; + safe_copy(&v, 4); + return v; + } + double read_f64() + { + align(8); + double v = 0; + safe_copy(&v, 8); + return v; + } + + std::string read_string() + { + uint32_t len = read_u32(); + if(len == 0 || pos_ + len > size_) + { + pos_ = std::min(pos_ + len, size_); + return {}; + } + std::string s(reinterpret_cast(data_ + pos_), len - 1); + pos_ += len; + return s; + } + + // Returns raw pointer at current pos and advances by n; nullptr if out of bounds. + const uint8_t* read_raw(size_t n) + { + if(pos_ + n > size_) + return nullptr; + const uint8_t* p = data_ + pos_; + pos_ += n; + return p; + } + + void skip(size_t n) + { + pos_ = std::min(pos_ + n, size_); + } + +private: + const uint8_t* data_; + size_t size_; + size_t pos_; + + // Alignment is counted from byte 4 (the CDR data section start). + void align(size_t n) + { + size_t mod = (pos_ - 4) % n; + if(mod) + pos_ += n - mod; + } + + void safe_copy(void* dst, size_t n) + { + if(pos_ + n <= size_) + { + std::memcpy(dst, data_ + pos_, n); + pos_ += n; + } + } +}; + +// --------------------------------------------------------------------------- +// ROS2 CDR message decoders +// --------------------------------------------------------------------------- + +namespace rosbags +{ + +// sensor_msgs/msg/PointCloud2 → std::vector +// Decodes by field name/offset (as parsed from the message's own `fields` +// array) rather than a fixed per-layout struct, since several +// PointCloudLayout variants share the same point_step (26 bytes) but place +// different fields at different offsets. Recognizes the field names +// McapWriter emits for any PointCloudLayout: x,y,z,intensity (always), +// ring, laser_id, time (relative f64), t (relative u32 ns, Ouster), +// timestamp (absolute f64, Hesai). Unrecognized fields are ignored. +inline std::vector decodePc2(const uint8_t* data, size_t size) +{ + std::vector points; + CdrReader r(data, size); + + const int32_t stamp_sec = r.read_i32(); + const uint32_t stamp_nsec = r.read_u32(); + r.read_string(); // frame_id + + const double stamp_s = static_cast(stamp_sec) + static_cast(stamp_nsec) * 1e-9; + + r.read_u32(); // height + const uint32_t width = r.read_u32(); + + struct FieldInfo + { + uint32_t offset; + bool present = false; + }; + FieldInfo xF, yF, zF, intensityF, ringF, laserIdF, timeF, tF, timestampF; + + const uint32_t nFields = r.read_u32(); + for(uint32_t i = 0; i < nFields; ++i) + { + const std::string name = r.read_string(); + const uint32_t offset = r.read_u32(); + r.read_u8(); // datatype (implied by field name for our own writer's output) + r.read_u32(); // count + + FieldInfo info{offset, true}; + if(name == "x") + xF = info; + else if(name == "y") + yF = info; + else if(name == "z") + zF = info; + else if(name == "intensity") + intensityF = info; + else if(name == "ring") + ringF = info; + else if(name == "laser_id") + laserIdF = info; + else if(name == "time") + timeF = info; + else if(name == "t") + tF = info; + else if(name == "timestamp") + timestampF = info; + } + + r.read_bool(); + const uint32_t point_step = r.read_u32(); + r.read_u32(); // row_step + + const uint32_t dataLen = r.read_u32(); + const uint8_t* rawData = r.read_raw(dataLen); + + if(!rawData || point_step == 0 || !xF.present || !yF.present || !zF.present || !intensityF.present) + return points; + + const uint32_t nPts = dataLen / point_step; + (void)width; + points.reserve(nPts); + + for(uint32_t i = 0; i < nPts; ++i) + { + const uint8_t* p = rawData + i * point_step; + + McapPoint pt{}; + std::memcpy(&pt.x, p + xF.offset, 4); + std::memcpy(&pt.y, p + yF.offset, 4); + std::memcpy(&pt.z, p + zF.offset, 4); + std::memcpy(&pt.intensity, p + intensityF.offset, 4); + + if(ringF.present) + std::memcpy(&pt.ring, p + ringF.offset, 2); + if(laserIdF.present) + std::memcpy(&pt.laser_id, p + laserIdF.offset, 1); + + if(timestampF.present) + { + // Absolute timestamp (Hesai) -- no offset from the message stamp. + std::memcpy(&pt.timestamp, p + timestampF.offset, 8); + } + else if(timeF.present) + { + double rel_time = 0.0; + std::memcpy(&rel_time, p + timeF.offset, 8); + pt.timestamp = stamp_s + rel_time; + } + else if(tF.present) + { + uint32_t rel_ns = 0; + std::memcpy(&rel_ns, p + tF.offset, 4); + pt.timestamp = stamp_s + static_cast(rel_ns) * 1e-9; + } + else + { + pt.timestamp = stamp_s; + } + + points.push_back(pt); + } + + return points; +} + +// sensor_msgs/msg/Imu → McapImuSample (one sample per CDR message) +inline std::optional decodeImu(const uint8_t* data, size_t size) +{ + CdrReader r(data, size); + + const int32_t stamp_sec = r.read_i32(); + const uint32_t stamp_nsec = r.read_u32(); + r.read_string(); // frame_id + + const double stamp_s = static_cast(stamp_sec) + static_cast(stamp_nsec) * 1e-9; + + // orientation quaternion (skip) + r.read_f64(); + r.read_f64(); + r.read_f64(); + r.read_f64(); + // orientation_covariance[9] (skip) + for(int i = 0; i < 9; ++i) + r.read_f64(); + + McapImuSample imu{}; + imu.gyro_x = static_cast(r.read_f64()); + imu.gyro_y = static_cast(r.read_f64()); + imu.gyro_z = static_cast(r.read_f64()); + // angular_velocity_covariance[9] (skip) + for(int i = 0; i < 9; ++i) + r.read_f64(); + + imu.acc_x = static_cast(r.read_f64()); + imu.acc_y = static_cast(r.read_f64()); + imu.acc_z = static_cast(r.read_f64()); + + imu.timestamp = stamp_s; + if(!r.ok()) + return std::nullopt; + return imu; +} + +// std_msgs/msg/String → std::string (empty on parse error) +inline std::string decodeSn(const uint8_t* data, size_t size) +{ + CdrReader r(data, size); + std::string s = r.read_string(); + return r.ok() ? s : std::string{}; +} + +} // namespace rosbags \ No newline at end of file diff --git a/rosbags/tests/CMakeLists.txt b/rosbags/tests/CMakeLists.txt new file mode 100644 index 00000000..ebfee54e --- /dev/null +++ b/rosbags/tests/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(hdmapping_rosbags_tests) + +# Unit tests for rosbags/ (LAZ/IMU -> MCAP/ROS2-bag exporter support code). +# Uses doctest, same as shared/tests -- see that CMakeLists.txt for why. +add_executable(hdmapping_rosbags_tests + test_mcap_writer.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../McapWriter.h + ${CMAKE_CURRENT_SOURCE_DIR}/../McapWriter.cpp +) + +target_include_directories(hdmapping_rosbags_tests PRIVATE + ${THIRDPARTY_DIRECTORY}/doctest + ${CMAKE_CURRENT_SOURCE_DIR}/.. +) + +target_link_libraries(hdmapping_rosbags_tests PRIVATE mcap) + +if (MSVC) + target_compile_definitions(hdmapping_rosbags_tests PRIVATE _USE_MATH_DEFINES) +endif() + +include(CTest) +add_test(NAME hdmapping_rosbags_tests COMMAND hdmapping_rosbags_tests) \ No newline at end of file diff --git a/rosbags/tests/test_mcap_writer.cpp b/rosbags/tests/test_mcap_writer.cpp new file mode 100644 index 00000000..63a9071c --- /dev/null +++ b/rosbags/tests/test_mcap_writer.cpp @@ -0,0 +1,290 @@ +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include + +#include "McapWriter.h" +#include "cdr_serializer.hpp" + +// No MCAP_IMPLEMENTATION define here: McapWriter.cpp (linked into this test +// binary) already provides the mcap library's implementation once. +#include + +#include +#include +#include +#include + +namespace fs = std::filesystem; + +namespace +{ + +fs::path tempMcapPath(const char* name) +{ + return fs::temp_directory_path() / name; +} + +std::vector makePoints(size_t n, double t0) +{ + std::mt19937 rng(42); + std::uniform_real_distribution coord(-50.0f, 50.0f); + + std::vector points; + points.reserve(n); + for (size_t i = 0; i < n; ++i) + { + rosbags::McapPoint p{}; + p.x = coord(rng); + p.y = coord(rng); + p.z = coord(rng); + p.intensity = static_cast(i % 256); + p.ring = static_cast(i % 16); + p.laser_id = static_cast(i % 4); + p.timestamp = t0 + static_cast(i) * 1e-6; + points.push_back(p); + } + return points; +} + +// Reads every message on `topic` back out of an mcap file and decodes it with `decode`. +template +std::vector readTopic(const fs::path& path, const std::string& topic, Decode decode) +{ + std::vector out; + mcap::McapReader reader; + auto status = reader.open(path.string()); + REQUIRE(status.ok()); + + auto messages = reader.readMessages(); + for (const auto& view : messages) + { + if (view.channel->topic != topic) + continue; + auto decoded = decode(reinterpret_cast(view.message.data), view.message.dataSize); + out.push_back(std::move(decoded)); + } + reader.close(); + return out; +} + +} // namespace + +TEST_CASE("McapFileWriter: PointCloud2 round-trip (Generic layout)") +{ + const auto path = tempMcapPath("hdmapping_test_generic.mcap"); + const auto points = makePoints(500, 1000.0); + + { + rosbags::McapWriterOptions options; + rosbags::McapFileWriter writer(path, options); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(points.front().timestamp * 1e9), points); + } + + const auto clouds = readTopic>( + path, "/lidar_points", [](const uint8_t* d, size_t n) + { + return rosbags::decodePc2(d, n); + }); + + REQUIRE(clouds.size() == 1); + const auto& decoded = clouds.front(); + REQUIRE(decoded.size() == points.size()); + + for (size_t i = 0; i < points.size(); ++i) + { + CHECK(decoded[i].x == doctest::Approx(points[i].x)); + CHECK(decoded[i].y == doctest::Approx(points[i].y)); + CHECK(decoded[i].z == doctest::Approx(points[i].z)); + CHECK(decoded[i].intensity == doctest::Approx(points[i].intensity)); + CHECK(decoded[i].ring == points[i].ring); + CHECK(decoded[i].laser_id == points[i].laser_id); + CHECK(decoded[i].timestamp == doctest::Approx(points[i].timestamp).epsilon(1e-6)); + } + + fs::remove(path); +} + +TEST_CASE("McapFileWriter: PointCloud2 round-trip (Velodyne layout drops laser_id)") +{ + const auto path = tempMcapPath("hdmapping_test_velodyne.mcap"); + const auto points = makePoints(200, 2000.0); + + { + rosbags::McapWriterOptions options; + options.lidar_layout = rosbags::PointCloudLayout::Velodyne; + rosbags::McapFileWriter writer(path, options); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(points.front().timestamp * 1e9), points); + } + + const auto clouds = readTopic>( + path, "/lidar_points", [](const uint8_t* d, size_t n) + { + return rosbags::decodePc2(d, n); + }); + + REQUIRE(clouds.size() == 1); + const auto& decoded = clouds.front(); + REQUIRE(decoded.size() == points.size()); + + for (size_t i = 0; i < points.size(); ++i) + { + CHECK(decoded[i].x == doctest::Approx(points[i].x)); + CHECK(decoded[i].ring == points[i].ring); + CHECK(decoded[i].laser_id == 0); // Velodyne layout has no laser_id field + CHECK(decoded[i].timestamp == doctest::Approx(points[i].timestamp).epsilon(1e-6)); + } + + fs::remove(path); +} + +TEST_CASE("McapFileWriter: PointCloud2 round-trip (Ouster layout, relative uint32 ns time)") +{ + const auto path = tempMcapPath("hdmapping_test_ouster.mcap"); + const auto points = makePoints(200, 3000.0); + + { + rosbags::McapWriterOptions options; + options.lidar_layout = rosbags::PointCloudLayout::Ouster; + rosbags::McapFileWriter writer(path, options); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(points.front().timestamp * 1e9), points); + } + + const auto clouds = readTopic>( + path, "/lidar_points", [](const uint8_t* d, size_t n) + { + return rosbags::decodePc2(d, n); + }); + + REQUIRE(clouds.size() == 1); + const auto& decoded = clouds.front(); + REQUIRE(decoded.size() == points.size()); + + for (size_t i = 0; i < points.size(); ++i) + { + CHECK(decoded[i].x == doctest::Approx(points[i].x)); + CHECK(decoded[i].ring == points[i].ring); + CHECK(decoded[i].laser_id == 0); // Ouster layout has no laser_id field + // "t" is uint32 ns, so round-trip precision is ~1ns rather than exact double equality. + CHECK(decoded[i].timestamp == doctest::Approx(points[i].timestamp).epsilon(1e-8)); + } + + fs::remove(path); +} + +TEST_CASE("McapFileWriter: PointCloud2 round-trip (Hesai layout, absolute timestamp)") +{ + const auto path = tempMcapPath("hdmapping_test_hesai.mcap"); + const auto points = makePoints(200, 4000.0); + + { + rosbags::McapWriterOptions options; + options.lidar_layout = rosbags::PointCloudLayout::Hesai; + rosbags::McapFileWriter writer(path, options); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(points.front().timestamp * 1e9), points); + } + + const auto clouds = readTopic>( + path, "/lidar_points", [](const uint8_t* d, size_t n) + { + return rosbags::decodePc2(d, n); + }); + + REQUIRE(clouds.size() == 1); + const auto& decoded = clouds.front(); + REQUIRE(decoded.size() == points.size()); + + for (size_t i = 0; i < points.size(); ++i) + { + CHECK(decoded[i].x == doctest::Approx(points[i].x)); + CHECK(decoded[i].ring == points[i].ring); + CHECK(decoded[i].laser_id == 0); // Hesai layout has no laser_id field + // timestamp is written as a raw absolute double -- exact round-trip. + CHECK(decoded[i].timestamp == points[i].timestamp); + } + + fs::remove(path); +} + +TEST_CASE("McapFileWriter: IMU round-trip") +{ + const auto path = tempMcapPath("hdmapping_test_imu.mcap"); + + std::vector samples; + for (int i = 0; i < 10; ++i) + { + rosbags::McapImuSample s{}; + s.timestamp = 5000.0 + i * 0.005; + s.gyro_x = 0.01f * i; + s.gyro_y = -0.02f * i; + s.gyro_z = 0.03f * i; + s.acc_x = 0.1f; + s.acc_y = 0.2f; + s.acc_z = 9.81f; + samples.push_back(s); + } + + { + rosbags::McapFileWriter writer(path); + REQUIRE(writer.isOpen()); + writer.writeImu(samples); + } + + const auto decoded = readTopic>( + path, "/imu", [](const uint8_t* d, size_t n) + { + return rosbags::decodeImu(d, n); + }); + + REQUIRE(decoded.size() == samples.size()); + for (size_t i = 0; i < samples.size(); ++i) + { + REQUIRE(decoded[i].has_value()); + CHECK(decoded[i]->gyro_x == doctest::Approx(samples[i].gyro_x)); + CHECK(decoded[i]->gyro_y == doctest::Approx(samples[i].gyro_y)); + CHECK(decoded[i]->gyro_z == doctest::Approx(samples[i].gyro_z)); + CHECK(decoded[i]->acc_x == doctest::Approx(samples[i].acc_x)); + CHECK(decoded[i]->acc_y == doctest::Approx(samples[i].acc_y)); + CHECK(decoded[i]->acc_z == doctest::Approx(samples[i].acc_z)); + CHECK(decoded[i]->timestamp == doctest::Approx(samples[i].timestamp).epsilon(1e-6)); + } + + fs::remove(path); +} + +TEST_CASE("McapFileWriter: custom topic names are honored") +{ + const auto path = tempMcapPath("hdmapping_test_topics.mcap"); + + rosbags::McapWriterOptions options; + options.lidar_topic = "/custom/points"; + options.imu_topic = "/custom/imu"; + options.sn_topic = "/custom/sn"; + + const auto points = makePoints(10, 42.0); + { + rosbags::McapFileWriter writer(path, options); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(points.front().timestamp * 1e9), points); + writer.writeSn(0, "SN123"); + } + + mcap::McapReader reader; + auto status = reader.open(path.string()); + REQUIRE(status.ok()); + status = reader.readSummary(mcap::ReadSummaryMethod::AllowFallbackScan); + REQUIRE(status.ok()); + + std::vector topics; + for (const auto& [id, channel] : reader.channels()) + topics.push_back(channel->topic); + reader.close(); + + CHECK(std::find(topics.begin(), topics.end(), "/custom/points") != topics.end()); + CHECK(std::find(topics.begin(), topics.end(), "/custom/imu") != topics.end()); + CHECK(std::find(topics.begin(), topics.end(), "/custom/sn") != topics.end()); + + fs::remove(path); +} \ No newline at end of file From 0f09ebd1edfc5a41578a9184dcd17a52e2c93ebc Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Thu, 20 Aug 2026 14:44:09 +0200 Subject: [PATCH 2/2] Fix std::byte for mcap headers under MSVC's project-wide _HAS_STD_BYTE=0 cmake/cpu_optimizations.cmake forces -D_HAS_STD_BYTE=0 for all MSVC targets (a leftover pybind11/Windows SDK workaround), which silently disables std::byte project-wide. mcap's headers are the first thing in this codebase to actually need it, so Windows CI failed with "'byte' is not a member of 'std'" in McapWriter.cpp and test_mcap_writer.cpp. Re-enable std::byte scoped to just those two translation units, ahead of any standard header that would otherwise lock the disabled value in for the rest of the TU, rather than touching the global define. --- apps/console_tools/laz_to_mcap.cpp | 627 +++++++++++++++-------------- rosbags/McapWriter.h | 12 + rosbags/tests/test_mcap_writer.cpp | 8 + 3 files changed, 347 insertions(+), 300 deletions(-) diff --git a/apps/console_tools/laz_to_mcap.cpp b/apps/console_tools/laz_to_mcap.cpp index 723ce0b0..9ed666e3 100644 --- a/apps/console_tools/laz_to_mcap.cpp +++ b/apps/console_tools/laz_to_mcap.cpp @@ -25,359 +25,387 @@ using ImuData = std::vector, Eigen::Vector3 namespace { -bool check_path_ext(const std::string& path, const char* ext) -{ - return fs::path(path).extension() == ext; -} - -std::string to_lower(std::string s) -{ - std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c) { return std::tolower(c); }); - return s; -} - -std::vector to_mcap_points(const std::vector& points) -{ - std::vector out; - out.reserve(points.size()); - for (const auto& p : points) - { - rosbags::McapPoint mp{}; - mp.x = static_cast(p.point.x()); - mp.y = static_cast(p.point.y()); - mp.z = static_cast(p.point.z()); - mp.intensity = p.intensity; - mp.laser_id = p.lidarid; - mp.timestamp = p.timestamp; - out.push_back(mp); - } - return out; -} - -std::vector to_mcap_imu(const ImuData& imu_data) -{ - std::vector out; - out.reserve(imu_data.size()); - for (const auto& [ts, gyr, acc] : imu_data) + bool check_path_ext(const std::string& path, const char* ext) { - rosbags::McapImuSample s{}; - s.timestamp = ts.first; - s.gyro_x = gyr.x(); - s.gyro_y = gyr.y(); - s.gyro_z = gyr.z(); - s.acc_x = acc.x(); - s.acc_y = acc.y(); - s.acc_z = acc.z(); - out.push_back(s); + return fs::path(path).extension() == ext; } - return out; -} - -void sort_points_by_timestamp(std::vector& points) -{ - std::sort( - points.begin(), points.end(), [](const Point3Di& a, const Point3Di& b) - { - return a.timestamp < b.timestamp; - }); -} -void sort_imu_by_timestamp(ImuData& imu) -{ - std::sort( - imu.begin(), imu.end(), [](const auto& a, const auto& b) - { - return std::get<0>(a).first < std::get<0>(b).first; - }); -} - -// Cuts the incoming points into one PointCloud2 message per 1/msg_hz seconds, -// so the bag replays at a lidar-like rate instead of one huge message per LAZ -// file. A point with timestamp t lands in bin floor(t * msg_hz): the bin grid -// is absolute, so it is identical for every chunk and a bin straddling a -// chunk boundary still yields a single message (pending points carry over to -// the next add()). msg_hz == 0 disables splitting -- one message per add(). -class MessageSplitter -{ -public: - MessageSplitter(rosbags::McapFileWriter& writer, double msg_hz) - : writer_(writer), msg_hz_(msg_hz) + std::string to_lower(std::string s) { + std::transform( + s.begin(), + s.end(), + s.begin(), + [](unsigned char c) + { + return std::tolower(c); + }); + return s; } - // `points` must be sorted by timestamp, and successive calls must be in - // timestamp order too (session chunks are processed oldest-first). - void add(const std::vector& points) + std::vector to_mcap_points(const std::vector& points) { - if (msg_hz_ <= 0.0) - { - write(points); - return; - } + std::vector out; + out.reserve(points.size()); for (const auto& p : points) { - const int64_t bin = static_cast(std::floor(p.timestamp * msg_hz_)); - if (!pending_.empty() && bin != current_bin_) - flush(); - current_bin_ = bin; - pending_.push_back(p); + rosbags::McapPoint mp{}; + mp.x = static_cast(p.point.x()); + mp.y = static_cast(p.point.y()); + mp.z = static_cast(p.point.z()); + mp.intensity = p.intensity; + mp.laser_id = p.lidarid; + mp.timestamp = p.timestamp; + out.push_back(mp); } + return out; } - // Writes whatever is still buffered; call once after the last add(). - void flush() + std::vector to_mcap_imu(const ImuData& imu_data) { - write(pending_); - pending_.clear(); + std::vector out; + out.reserve(imu_data.size()); + for (const auto& [ts, gyr, acc] : imu_data) + { + rosbags::McapImuSample s{}; + s.timestamp = ts.first; + s.gyro_x = gyr.x(); + s.gyro_y = gyr.y(); + s.gyro_z = gyr.z(); + s.acc_x = acc.x(); + s.acc_y = acc.y(); + s.acc_z = acc.z(); + out.push_back(s); + } + return out; } - size_t messages_written() const + void sort_points_by_timestamp(std::vector& points) { - return messages_; + std::sort( + points.begin(), + points.end(), + [](const Point3Di& a, const Point3Di& b) + { + return a.timestamp < b.timestamp; + }); } -private: - void write(const std::vector& points) + void sort_imu_by_timestamp(ImuData& imu) { - if (points.empty()) - return; - const auto mcap_points = to_mcap_points(points); - const uint64_t stamp_ns = static_cast(mcap_points.front().timestamp * 1e9); - writer_.writePointCloud(stamp_ns, mcap_points); - ++messages_; + std::sort( + imu.begin(), + imu.end(), + [](const auto& a, const auto& b) + { + return std::get<0>(a).first < std::get<0>(b).first; + }); } - rosbags::McapFileWriter& writer_; - double msg_hz_; - std::vector pending_; - int64_t current_bin_ = 0; - size_t messages_ = 0; -}; - -// Last 4 characters of the filename stem, matching the chunk-index convention -// mandeye recordings use (lidar0001.laz <-> imu0001.csv), mirroring -// lidar_odometry.cpp's load_data()/get4index. -std::string chunk_index(const std::string& path) -{ - const std::string stem = fs::path(path).stem().string(); - return stem.size() > 4 ? stem.substr(stem.size() - 4) : stem; -} - -// Per-sensor extrinsics (sensor id -> transform into the shared frame) plus -// which sensor id's IMU stream to use, resolved from an MLvxCalib -// calibration.json/.mjc + .sn file pair. Empty `per_sensor` means "apply no -// calibration", matching load_point_cloud()'s own convention. -struct Calibration -{ - std::unordered_map per_sensor; - int imu_id_to_use = 0; -}; - -// Mirrors lidar_odometry.cpp's load_data(): a session directory carries one -// calibration file (calibration.json/.mjc, skipping camera-calibration jsons -// named status*/cam0*/cam1*) and one .sn file describing every chunk. -void find_calibration_files(const fs::path& dir, std::string& calibration_file, std::string& sn_file) -{ - std::vector jsons, mjcs, sns; - for (const auto& entry : fs::directory_iterator(dir)) + // Cuts the incoming points into one PointCloud2 message per 1/msg_hz seconds, + // so the bag replays at a lidar-like rate instead of one huge message per LAZ + // file. A point with timestamp t lands in bin floor(t * msg_hz): the bin grid + // is absolute, so it is identical for every chunk and a bin straddling a + // chunk boundary still yields a single message (pending points carry over to + // the next add()). msg_hz == 0 disables splitting -- one message per add(). + class MessageSplitter { - if (!entry.is_regular_file()) - continue; - const std::string ext = to_lower(entry.path().extension().string()); - if (ext == ".json") + public: + MessageSplitter(rosbags::McapFileWriter& writer, double msg_hz) + : writer_(writer) + , msg_hz_(msg_hz) { - const std::string stem = to_lower(entry.path().stem().string()); - if (stem.starts_with("status") || stem.starts_with("cam0") || stem.starts_with("cam1")) - continue; - jsons.push_back(entry.path().string()); } - else if (ext == ".mjc") - mjcs.push_back(entry.path().string()); - else if (ext == ".sn") - sns.push_back(entry.path().string()); - } - std::sort(jsons.begin(), jsons.end()); - std::sort(mjcs.begin(), mjcs.end()); - std::sort(sns.begin(), sns.end()); - - if (!jsons.empty()) - calibration_file = jsons.front(); - else if (!mjcs.empty()) - calibration_file = mjcs.front(); - - if (!sns.empty()) - sn_file = sns.front(); -} - -// Loads an MLvxCalib calibration.json/.mjc + .sn pair (see -// lidar_odometry_utils.h's MLvxCalib namespace doc comment for the file -// format) and combines them into a sensor-id-keyed extrinsics map, ready to -// pass to load_point_cloud(). Returns a default (no-op) Calibration if either -// path is empty. -Calibration load_calibration(const std::string& calibration_file, const std::string& sn_file) -{ - Calibration result; - if (calibration_file.empty() || sn_file.empty()) - return result; - const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibration_file); - if (preloadedCalibration.empty()) - { - spdlog::warn("No calibration data found in {} - exporting without per-sensor calibration", calibration_file); - return result; - } - const auto idToSn = MLvxCalib::GetIdToSnMapping(sn_file); - const std::string imuSnToUse = MLvxCalib::GetImuSnToUse(calibration_file); + // `points` must be sorted by timestamp, and successive calls must be in + // timestamp order too (session chunks are processed oldest-first). + void add(const std::vector& points) + { + if (msg_hz_ <= 0.0) + { + write(points); + return; + } + for (const auto& p : points) + { + const int64_t bin = static_cast(std::floor(p.timestamp * msg_hz_)); + if (!pending_.empty() && bin != current_bin_) + flush(); + current_bin_ = bin; + pending_.push_back(p); + } + } - spdlog::info("Loaded calibration for {} sensor(s) from {}", preloadedCalibration.size(), calibration_file); - for (const auto& [sn, _] : preloadedCalibration) - spdlog::info(" -> {}", sn); + // Writes whatever is still buffered; call once after the last add(). + void flush() + { + write(pending_); + pending_.clear(); + } - for (const auto& [id, sn] : idToSn) - { - const auto it = preloadedCalibration.find(sn); - if (it == preloadedCalibration.end()) + size_t messages_written() const { - spdlog::warn("Sensor id {} (serial '{}') from {} has no entry in {} - its points will be dropped", id, sn, sn_file, calibration_file); - continue; + return messages_; } - result.per_sensor[id] = it->second; - } - result.imu_id_to_use = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); - spdlog::info("Using IMU id {} (serial '{}')", result.imu_id_to_use, imuSnToUse); - return result; -} + private: + void write(const std::vector& points) + { + if (points.empty()) + return; + const auto mcap_points = to_mcap_points(points); + const uint64_t stamp_ns = static_cast(mcap_points.front().timestamp * 1e9); + writer_.writePointCloud(stamp_ns, mcap_points); + ++messages_; + } -struct Chunk -{ - std::string laz; - std::string imu; // empty if no matching csv was found -}; - -// Scans `dir` for lidarNNNN.laz/.las + imuNNNN.csv chunk pairs (a real -// mandeye recording folder) and matches them by chunk_index(). LAZ files -// without a matching csv are still exported (points only, no IMU for that -// chunk). -std::vector scan_session_directory(const fs::path& dir) -{ - std::vector laz_files, csv_files; - for (const auto& entry : fs::directory_iterator(dir)) + rosbags::McapFileWriter& writer_; + double msg_hz_; + std::vector pending_; + int64_t current_bin_ = 0; + size_t messages_ = 0; + }; + + // Last 4 characters of the filename stem, matching the chunk-index convention + // mandeye recordings use (lidar0001.laz <-> imu0001.csv), mirroring + // lidar_odometry.cpp's load_data()/get4index. + std::string chunk_index(const std::string& path) { - if (!entry.is_regular_file()) - continue; - const std::string ext = to_lower(entry.path().extension().string()); - if (ext == ".laz" || ext == ".las") - laz_files.push_back(entry.path().string()); - else if (ext == ".csv") - csv_files.push_back(entry.path().string()); + const std::string stem = fs::path(path).stem().string(); + return stem.size() > 4 ? stem.substr(stem.size() - 4) : stem; } - std::sort(laz_files.begin(), laz_files.end()); - std::sort(csv_files.begin(), csv_files.end()); - std::vector chunks; - chunks.reserve(laz_files.size()); - for (const auto& laz : laz_files) + // Per-sensor extrinsics (sensor id -> transform into the shared frame) plus + // which sensor id's IMU stream to use, resolved from an MLvxCalib + // calibration.json/.mjc + .sn file pair. Empty `per_sensor` means "apply no + // calibration", matching load_point_cloud()'s own convention. + struct Calibration + { + std::unordered_map per_sensor; + int imu_id_to_use = 0; + }; + + // Mirrors lidar_odometry.cpp's load_data(): a session directory carries one + // calibration file (calibration.json/.mjc, skipping camera-calibration jsons + // named status*/cam0*/cam1*) and one .sn file describing every chunk. + void find_calibration_files(const fs::path& dir, std::string& calibration_file, std::string& sn_file) { - const std::string idx = chunk_index(laz); - Chunk chunk{laz, {}}; - for (const auto& csv : csv_files) + std::vector jsons, mjcs, sns; + for (const auto& entry : fs::directory_iterator(dir)) { - if (chunk_index(csv) == idx) + if (!entry.is_regular_file()) + continue; + const std::string ext = to_lower(entry.path().extension().string()); + if (ext == ".json") { - chunk.imu = csv; - break; + const std::string stem = to_lower(entry.path().stem().string()); + if (stem.starts_with("status") || stem.starts_with("cam0") || stem.starts_with("cam1")) + continue; + jsons.push_back(entry.path().string()); } + else if (ext == ".mjc") + mjcs.push_back(entry.path().string()); + else if (ext == ".sn") + sns.push_back(entry.path().string()); } - if (chunk.imu.empty()) - spdlog::warn("No matching IMU csv for {} (chunk index '{}')", laz, idx); - chunks.push_back(std::move(chunk)); + std::sort(jsons.begin(), jsons.end()); + std::sort(mjcs.begin(), mjcs.end()); + std::sort(sns.begin(), sns.end()); + + if (!jsons.empty()) + calibration_file = jsons.front(); + else if (!mjcs.empty()) + calibration_file = mjcs.front(); + + if (!sns.empty()) + sn_file = sns.front(); } - return chunks; -} -void print_usage(const char* argv0) -{ - spdlog::error("Usage: {} [options]", argv0); - spdlog::error(" session_dir a mandeye recording folder of lidarNNNN.laz + imuNNNN.csv chunk"); - spdlog::error(" pairs (matched by filename's last 4 digits); every chunk becomes"); - spdlog::error(" its own /lidar_points message, IMU samples are merged into one stream"); - spdlog::error("Options:"); - spdlog::error(" --lidar-topic lidar PointCloud2 topic (default: /lidar_points)"); - spdlog::error(" --imu-topic IMU topic (default: /imu)"); - spdlog::error(" --sn-topic serial-number string topic (default: /lidar_sn)"); - spdlog::error(" --frame-id frame_id written into message headers (default: lidar)"); - spdlog::error(" --lidar-type PointCloud2 field layout: generic|velodyne|ouster|hesai (default: generic)"); - spdlog::error(" --msg_hz message rate: points are split into one PointCloud2 per"); - spdlog::error(" 1/hz seconds (default: 10; 0 = one message per input file/chunk)"); - spdlog::error(" --calibration MLvxCalib calibration.json/.mjc (multi-LiVoX extrinsics + IMU"); - spdlog::error(" selection); session_dir auto-detects this if omitted"); - spdlog::error(" --sn MLvxCalib .sn file (sensor id -> serial number); session_dir"); - spdlog::error(" auto-detects this if omitted. Must be given together with --calibration"); -} - -int run_session_directory( - const std::string& dir_path, const std::string& mcap_path, const rosbags::McapWriterOptions& options, double msg_hz, - std::string calibration_path, std::string sn_path) -{ - auto chunks = scan_session_directory(dir_path); - if (chunks.empty()) + // Loads an MLvxCalib calibration.json/.mjc + .sn pair (see + // lidar_odometry_utils.h's MLvxCalib namespace doc comment for the file + // format) and combines them into a sensor-id-keyed extrinsics map, ready to + // pass to load_point_cloud(). Returns a default (no-op) Calibration if either + // path is empty. + Calibration load_calibration(const std::string& calibration_file, const std::string& sn_file) { - spdlog::error("No .laz/.las files found in {}", dir_path); - return EXIT_FAILURE; + Calibration result; + if (calibration_file.empty() || sn_file.empty()) + return result; + + const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibration_file); + if (preloadedCalibration.empty()) + { + spdlog::warn("No calibration data found in {} - exporting without per-sensor calibration", calibration_file); + return result; + } + const auto idToSn = MLvxCalib::GetIdToSnMapping(sn_file); + const std::string imuSnToUse = MLvxCalib::GetImuSnToUse(calibration_file); + + spdlog::info("Loaded calibration for {} sensor(s) from {}", preloadedCalibration.size(), calibration_file); + for (const auto& [sn, _] : preloadedCalibration) + spdlog::info(" -> {}", sn); + + for (const auto& [id, sn] : idToSn) + { + const auto it = preloadedCalibration.find(sn); + if (it == preloadedCalibration.end()) + { + spdlog::warn( + "Sensor id {} (serial '{}') from {} has no entry in {} - its points will be dropped", + id, + sn, + sn_file, + calibration_file); + continue; + } + result.per_sensor[id] = it->second; + } + + result.imu_id_to_use = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); + spdlog::info("Using IMU id {} (serial '{}')", result.imu_id_to_use, imuSnToUse); + return result; } - spdlog::info("Found {} chunk(s) in {}", chunks.size(), dir_path); - if (calibration_path.empty() || sn_path.empty()) + struct Chunk { - std::string found_calibration, found_sn; - find_calibration_files(dir_path, found_calibration, found_sn); - if (calibration_path.empty()) - calibration_path = found_calibration; - if (sn_path.empty()) - sn_path = found_sn; + std::string laz; + std::string imu; // empty if no matching csv was found + }; + + // Scans `dir` for lidarNNNN.laz/.las + imuNNNN.csv chunk pairs (a real + // mandeye recording folder) and matches them by chunk_index(). LAZ files + // without a matching csv are still exported (points only, no IMU for that + // chunk). + std::vector scan_session_directory(const fs::path& dir) + { + std::vector laz_files, csv_files; + for (const auto& entry : fs::directory_iterator(dir)) + { + if (!entry.is_regular_file()) + continue; + const std::string ext = to_lower(entry.path().extension().string()); + if (ext == ".laz" || ext == ".las") + laz_files.push_back(entry.path().string()); + else if (ext == ".csv") + csv_files.push_back(entry.path().string()); + } + std::sort(laz_files.begin(), laz_files.end()); + std::sort(csv_files.begin(), csv_files.end()); + + std::vector chunks; + chunks.reserve(laz_files.size()); + for (const auto& laz : laz_files) + { + const std::string idx = chunk_index(laz); + Chunk chunk{ laz, {} }; + for (const auto& csv : csv_files) + { + if (chunk_index(csv) == idx) + { + chunk.imu = csv; + break; + } + } + if (chunk.imu.empty()) + spdlog::warn("No matching IMU csv for {} (chunk index '{}')", laz, idx); + chunks.push_back(std::move(chunk)); + } + return chunks; } - const Calibration calib = load_calibration(calibration_path, sn_path); - rosbags::McapFileWriter writer(mcap_path, options); - if (!writer.isOpen()) + void print_usage(const char* argv0) { - spdlog::error("Failed to open output mcap file {}", mcap_path); - return EXIT_FAILURE; + spdlog::error("Usage: {} [options]", argv0); + spdlog::error(" session_dir a mandeye recording folder of lidarNNNN.laz + imuNNNN.csv chunk"); + spdlog::error(" pairs (matched by filename's last 4 digits); every chunk becomes"); + spdlog::error(" its own /lidar_points message, IMU samples are merged into one stream"); + spdlog::error("Options:"); + spdlog::error(" --lidar-topic lidar PointCloud2 topic (default: /lidar_points)"); + spdlog::error(" --imu-topic IMU topic (default: /imu)"); + spdlog::error(" --sn-topic serial-number string topic (default: /lidar_sn)"); + spdlog::error(" --frame-id frame_id written into message headers (default: lidar)"); + spdlog::error(" --lidar-type PointCloud2 field layout: generic|velodyne|ouster|hesai (default: generic)"); + spdlog::error(" --msg_hz message rate: points are split into one PointCloud2 per"); + spdlog::error(" 1/hz seconds (default: 10; 0 = one message per input file/chunk)"); + spdlog::error(" --calibration MLvxCalib calibration.json/.mjc (multi-LiVoX extrinsics + IMU"); + spdlog::error(" selection); session_dir auto-detects this if omitted"); + spdlog::error(" --sn MLvxCalib .sn file (sensor id -> serial number); session_dir"); + spdlog::error(" auto-detects this if omitted. Must be given together with --calibration"); } - size_t total_points = 0; - ImuData all_imu; - MessageSplitter splitter(writer, msg_hz); - for (size_t i = 0; i < chunks.size(); ++i) + int run_session_directory( + const std::string& dir_path, + const std::string& mcap_path, + const rosbags::McapWriterOptions& options, + double msg_hz, + std::string calibration_path, + std::string sn_path) { - auto points = load_point_cloud( - chunks[i].laz, /*ommit_points_with_timestamp_equals_zero=*/false, /*filter_threshold_xy_inner=*/0.0, - /*filter_threshold_xy_outer=*/std::numeric_limits::max(), /*calibrations=*/calib.per_sensor); - sort_points_by_timestamp(points); - total_points += points.size(); - splitter.add(points); - spdlog::info("[{}/{}] {}: {} points", i + 1, chunks.size(), chunks[i].laz, points.size()); - - if (!chunks[i].imu.empty()) + auto chunks = scan_session_directory(dir_path); + if (chunks.empty()) { - auto imu_data = load_imu(chunks[i].imu, calib.imu_id_to_use); - all_imu.insert(all_imu.end(), std::make_move_iterator(imu_data.begin()), std::make_move_iterator(imu_data.end())); + spdlog::error("No .laz/.las files found in {}", dir_path); + return EXIT_FAILURE; } - } - splitter.flush(); - spdlog::info("Loaded {} points across {} chunk(s), wrote {} point cloud message(s)", total_points, chunks.size(), splitter.messages_written()); + spdlog::info("Found {} chunk(s) in {}", chunks.size(), dir_path); - if (!all_imu.empty()) - { - sort_imu_by_timestamp(all_imu); - writer.writeImu(to_mcap_imu(all_imu)); - spdlog::info("Loaded {} IMU samples across matched chunk(s)", all_imu.size()); - } + if (calibration_path.empty() || sn_path.empty()) + { + std::string found_calibration, found_sn; + find_calibration_files(dir_path, found_calibration, found_sn); + if (calibration_path.empty()) + calibration_path = found_calibration; + if (sn_path.empty()) + sn_path = found_sn; + } + const Calibration calib = load_calibration(calibration_path, sn_path); - spdlog::info("Wrote {}", mcap_path); - return EXIT_SUCCESS; -} + rosbags::McapFileWriter writer(mcap_path, options); + if (!writer.isOpen()) + { + spdlog::error("Failed to open output mcap file {}", mcap_path); + return EXIT_FAILURE; + } + + size_t total_points = 0; + ImuData all_imu; + MessageSplitter splitter(writer, msg_hz); + for (size_t i = 0; i < chunks.size(); ++i) + { + auto points = load_point_cloud( + chunks[i].laz, + /*ommit_points_with_timestamp_equals_zero=*/false, + /*filter_threshold_xy_inner=*/0.0, + /*filter_threshold_xy_outer=*/std::numeric_limits::max(), + /*calibrations=*/calib.per_sensor); + sort_points_by_timestamp(points); + total_points += points.size(); + splitter.add(points); + spdlog::info("[{}/{}] {}: {} points", i + 1, chunks.size(), chunks[i].laz, points.size()); + + if (!chunks[i].imu.empty()) + { + auto imu_data = load_imu(chunks[i].imu, calib.imu_id_to_use); + all_imu.insert(all_imu.end(), std::make_move_iterator(imu_data.begin()), std::make_move_iterator(imu_data.end())); + } + } + splitter.flush(); + spdlog::info( + "Loaded {} points across {} chunk(s), wrote {} point cloud message(s)", + total_points, + chunks.size(), + splitter.messages_written()); + + if (!all_imu.empty()) + { + sort_imu_by_timestamp(all_imu); + writer.writeImu(to_mcap_imu(all_imu)); + spdlog::info("Loaded {} IMU samples across matched chunk(s)", all_imu.size()); + } + + spdlog::info("Wrote {}", mcap_path); + return EXIT_SUCCESS; + } } // namespace @@ -419,8 +447,7 @@ int main(const int argc, const char** argv) try { msg_hz = std::stod(value); - } - catch (const std::exception&) + } catch (const std::exception&) { spdlog::error("Invalid --msg_hz '{}' (expected a number)", value); return EXIT_FAILURE; diff --git a/rosbags/McapWriter.h b/rosbags/McapWriter.h index cd09c8e9..85fd4873 100644 --- a/rosbags/McapWriter.h +++ b/rosbags/McapWriter.h @@ -1,4 +1,16 @@ #pragma once + +// cmake/cpu_optimizations.cmake defines _HAS_STD_BYTE=0 project-wide for +// MSVC (worked around a std::byte/pybind11 conflict on Windows -- see commit +// 2af71be); mcap's headers (pulled in transitively by McapWriter.cpp and +// rosbags/tests/test_mcap_writer.cpp) genuinely need real std::byte, so +// re-enable it here before any standard header can lock the disabled value +// in for these translation units. +#if defined(_MSC_VER) +# undef _HAS_STD_BYTE +# define _HAS_STD_BYTE 1 +#endif + #include #include #include diff --git a/rosbags/tests/test_mcap_writer.cpp b/rosbags/tests/test_mcap_writer.cpp index 63a9071c..428c54d8 100644 --- a/rosbags/tests/test_mcap_writer.cpp +++ b/rosbags/tests/test_mcap_writer.cpp @@ -1,3 +1,11 @@ +// See rosbags/McapWriter.h for why this needs to come before any standard +// header (including , which pulls in plenty of its own) gets a +// chance to lock in the project-wide _HAS_STD_BYTE=0 MSVC workaround. +#if defined(_MSC_VER) +# undef _HAS_STD_BYTE +# define _HAS_STD_BYTE 1 +#endif + #define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN #include