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..9ed666e3 --- /dev/null +++ b/apps/console_tools/laz_to_mcap.cpp @@ -0,0 +1,508 @@ +// 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..85fd4873 --- /dev/null +++ b/rosbags/McapWriter.h @@ -0,0 +1,150 @@ +#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 +#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..428c54d8 --- /dev/null +++ b/rosbags/tests/test_mcap_writer.cpp @@ -0,0 +1,298 @@ +// 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 + +#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