diff --git a/apps/console_tools/CMakeLists.txt b/apps/console_tools/CMakeLists.txt index af51b84d..caca0970 100644 --- a/apps/console_tools/CMakeLists.txt +++ b/apps/console_tools/CMakeLists.txt @@ -115,6 +115,72 @@ if (MSVC) target_compile_options(mcap_to_laz PRIVATE /bigobj) endif() +# bag_to_laz / laz_to_bag: the ROS1 bag twins of mcap_to_laz / laz_to_mcap, +# built from the same sources with a define that swaps the reader (writer) for +# rosbags/Ros1Bag -- no ROS installation needed. Reading bz2 chunks needs +# libbz2 (liblz4 comes with the mcap target), so both are skipped without it. +find_package(BZip2) +if (BZip2_FOUND) + add_executable( + bag_to_laz mcap_to_laz.cpp + ${REPOSITORY_DIRECTORY}/rosbags/Ros1Bag.h ${REPOSITORY_DIRECTORY}/rosbags/Ros1Bag.cpp + ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.h ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.cpp + ${REPOSITORY_DIRECTORY}/rosbags/MandeyeSessionWriter.h ${REPOSITORY_DIRECTORY}/rosbags/MandeyeSessionWriter.cpp + ) + target_compile_definitions(bag_to_laz PRIVATE HDMAPPING_IMPORT_ROS1_BAG) + target_include_directories( + bag_to_laz + PRIVATE ${REPOSITORY_DIRECTORY}/rosbags + ${LASZIP_INCLUDE_DIR}/LASzip/include) + target_link_libraries( + bag_to_laz + PRIVATE + mcap + BZip2::BZip2 + spdlog::spdlog + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS}) + + add_executable( + laz_to_bag 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/Ros1Bag.h ${REPOSITORY_DIRECTORY}/rosbags/Ros1Bag.cpp + ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.h ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.cpp + ) + target_compile_definitions(laz_to_bag PRIVATE HDMAPPING_EXPORT_ROS1_BAG) + target_include_directories( + laz_to_bag + 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_bag + PRIVATE + mcap + BZip2::BZip2 + unordered_dense::unordered_dense + spdlog::spdlog + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS} + ${CORE_LIBRARIES}) + + if (MSVC) + target_compile_options(bag_to_laz PRIVATE /bigobj) + target_compile_options(laz_to_bag PRIVATE /bigobj) + endif() +else() + message(STATUS "bag_to_laz and laz_to_bag skipped: libbz2 not found") +endif() + # ouster_pcap_to_laz: Ouster pcap + metadata JSON -> mandeye session directory. # Same writer as mcap_to_laz; the pcap/packet decoding is rosbags/OusterPcapReader, # so laszip + nlohmann json + spdlog is the whole dependency set. diff --git a/apps/console_tools/laz_to_mcap.cpp b/apps/console_tools/laz_to_mcap.cpp index 9ed666e3..4f9cfe1f 100644 --- a/apps/console_tools/laz_to_mcap.cpp +++ b/apps/console_tools/laz_to_mcap.cpp @@ -1,4 +1,6 @@ -// Standalone mandeye session directory -> MCAP/ROS2-bag exporter. +// Standalone mandeye session directory -> MCAP/ROS2-bag exporter. Built a +// second time as laz_to_bag (HDMAPPING_EXPORT_ROS1_BAG), which writes a ROS1 bag +// through rosbags/Ros1Bag instead -- same layouts, topics and message split. // // Reuses load_point_cloud()/load_imu() (lidar_odometry_utils.cpp) -- the // same LAZ/IMU-csv parsing lidar_odometry_step_1 itself uses -- rather than @@ -7,6 +9,9 @@ // entirely) that would silently drop data for a lossless exporter. #include "McapWriter.h" #include "lidar_odometry_utils.h" +#if defined(HDMAPPING_EXPORT_ROS1_BAG) +#include "Ros1Bag.h" +#endif #include #include @@ -25,6 +30,17 @@ using ImuData = std::vector, Eigen::Vector3 namespace { +#if defined(HDMAPPING_EXPORT_ROS1_BAG) + using BagWriter = rosbags::Ros1BagFileWriter; + constexpr const char* kOutputExtension = ".bag"; +#else + using BagWriter = rosbags::McapFileWriter; + constexpr const char* kOutputExtension = ".mcap"; +#endif + + // Standard gravity: the session's IMU csv is in g, sensor_msgs/Imu wants m/s^2. + constexpr double kStandardGravity = 9.80665; + bool check_path_ext(const std::string& path, const char* ext) { return fs::path(path).extension() == ext; @@ -43,25 +59,38 @@ namespace return s; } - std::vector to_mcap_points(const std::vector& points) + // `rings` is parallel to `points` (load_point_cloud()'s ring output). The result + // is sorted by timestamp, ties by ring: the order in which a spinning lidar + // fires, and the order every PointCloud2 layout's relative time is taken in. + std::vector to_mcap_points(const std::vector& points, const std::vector& rings) { std::vector out; out.reserve(points.size()); - for (const auto& p : points) + for (size_t i = 0; i < points.size(); ++i) { + const auto& p = points[i]; 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.ring = i < rings.size() ? rings[i] : 0; mp.laser_id = p.lidarid; mp.timestamp = p.timestamp; out.push_back(mp); } + std::stable_sort( + out.begin(), + out.end(), + [](const rosbags::McapPoint& a, const rosbags::McapPoint& b) + { + return a.timestamp < b.timestamp || (a.timestamp == b.timestamp && a.ring < b.ring); + }); return out; } - std::vector to_mcap_imu(const ImuData& imu_data) + // `acc_scale` brings the csv's acceleration to the unit written (1 keeps g). + std::vector to_mcap_imu(const ImuData& imu_data, double acc_scale) { std::vector out; out.reserve(imu_data.size()); @@ -72,25 +101,14 @@ namespace 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(); + s.acc_x = static_cast(acc.x() * acc_scale); + s.acc_y = static_cast(acc.y() * acc_scale); + s.acc_z = static_cast(acc.z() * acc_scale); 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( @@ -111,7 +129,7 @@ namespace class MessageSplitter { public: - MessageSplitter(rosbags::McapFileWriter& writer, double msg_hz) + MessageSplitter(BagWriter& writer, double msg_hz) : writer_(writer) , msg_hz_(msg_hz) { @@ -119,7 +137,7 @@ namespace // `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) + void add(const std::vector& points) { if (msg_hz_ <= 0.0) { @@ -149,19 +167,18 @@ namespace } private: - void write(const std::vector& points) + 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); + const uint64_t stamp_ns = static_cast(points.front().timestamp * 1e9); + writer_.writePointCloud(stamp_ns, points); ++messages_; } - rosbags::McapFileWriter& writer_; + BagWriter& writer_; double msg_hz_; - std::vector pending_; + std::vector pending_; int64_t current_bin_ = 0; size_t messages_ = 0; }; @@ -315,7 +332,7 @@ namespace void print_usage(const char* argv0) { - spdlog::error("Usage: {} [options]", argv0); + spdlog::error("Usage: {} [options]", argv0, kOutputExtension); 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"); @@ -324,20 +341,43 @@ namespace 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(" --lidar-type PointCloud2 field layout: generic|velodyne|ouster|hesai|canonical"); + spdlog::error(" (default: generic); canonical = x y z intensity ring time(float32) t,"); + spdlog::error(" the benchmark's canonical spinning-lidar bag"); + spdlog::error(" --imu-acc-unit unit IMU acceleration is written in (default: g, the session's own,"); + spdlog::error(" copied as is; ms2 = m/s^2 as sensor_msgs/Imu defines it)"); 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"); + spdlog::error("The lidar ring is read from the LAS extra-bytes attribute 'ring' when the session has it"); + spdlog::error("(mcap_to_laz and bag_to_laz write it), otherwise every point's ring is 0."); + } + + // The ROS1 bag writer finishes its index explicitly so a write error is not lost + // in a destructor; MCAP finishes on destruction. + bool finish(rosbags::McapFileWriter&) + { + return true; + } +#if defined(HDMAPPING_EXPORT_ROS1_BAG) + bool finish(rosbags::Ros1BagFileWriter& writer) + { + if (writer.close()) + return true; + spdlog::error("Failed to finish the bag: {}", writer.error()); + return false; } +#endif int run_session_directory( const std::string& dir_path, const std::string& mcap_path, const rosbags::McapWriterOptions& options, double msg_hz, + double acc_scale, std::string calibration_path, std::string sn_path) { @@ -360,15 +400,17 @@ namespace } const Calibration calib = load_calibration(calibration_path, sn_path); - rosbags::McapFileWriter writer(mcap_path, options); + BagWriter writer(mcap_path, options); if (!writer.isOpen()) { - spdlog::error("Failed to open output mcap file {}", mcap_path); + spdlog::error("Failed to open output file {}", mcap_path); return EXIT_FAILURE; } size_t total_points = 0; + size_t points_with_ring = 0; ImuData all_imu; + std::vector rings; MessageSplitter splitter(writer, msg_hz); for (size_t i = 0; i < chunks.size(); ++i) { @@ -377,10 +419,11 @@ namespace /*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); + /*calibrations=*/calib.per_sensor, + /*rings=*/&rings); total_points += points.size(); - splitter.add(points); + points_with_ring += static_cast(std::count_if(rings.begin(), rings.end(), [](uint16_t r) { return r != 0; })); + splitter.add(to_mcap_points(points, rings)); spdlog::info("[{}/{}] {}: {} points", i + 1, chunks.size(), chunks[i].laz, points.size()); if (!chunks[i].imu.empty()) @@ -395,14 +438,18 @@ namespace total_points, chunks.size(), splitter.messages_written()); + if (points_with_ring == 0 && total_points > 0) + spdlog::info("No point has a ring (the session has no 'ring' attribute, or every ring is 0)"); if (!all_imu.empty()) { sort_imu_by_timestamp(all_imu); - writer.writeImu(to_mcap_imu(all_imu)); + writer.writeImu(to_mcap_imu(all_imu, acc_scale)); spdlog::info("Loaded {} IMU samples across matched chunk(s)", all_imu.size()); } + if (!finish(writer)) + return EXIT_FAILURE; spdlog::info("Wrote {}", mcap_path); return EXIT_SUCCESS; } @@ -423,6 +470,7 @@ int main(const int argc, const char** argv) std::string sn_path; rosbags::McapWriterOptions options; double msg_hz = 10.0; + double acc_scale = 1.0; for (int i = 3; i < argc; ++i) { @@ -469,9 +517,24 @@ int main(const int argc, const char** argv) options.lidar_layout = rosbags::PointCloudLayout::Ouster; else if (type == "hesai") options.lidar_layout = rosbags::PointCloudLayout::Hesai; + else if (type == "canonical") + options.lidar_layout = rosbags::PointCloudLayout::Canonical; + else + { + spdlog::error("Unknown --lidar-type '{}' (expected generic|velodyne|ouster|hesai|canonical)", type); + return EXIT_FAILURE; + } + } + else if (arg == "--imu-acc-unit" && hasValue) + { + const std::string unit = argv[++i]; + if (unit == "g") + acc_scale = 1.0; + else if (unit == "ms2") + acc_scale = kStandardGravity; else { - spdlog::error("Unknown --lidar-type '{}' (expected generic|velodyne|ouster|hesai)", type); + spdlog::error("Unknown --imu-acc-unit '{}' (expected g|ms2)", unit); return EXIT_FAILURE; } } @@ -483,9 +546,9 @@ int main(const int argc, const char** argv) } } - if (!check_path_ext(mcap_path, ".mcap")) + if (!check_path_ext(mcap_path, kOutputExtension)) { - spdlog::error("Invalid extension for output file {} - expected .mcap", mcap_path); + spdlog::error("Invalid extension for output file {} - expected {}", mcap_path, kOutputExtension); return EXIT_FAILURE; } if (!fs::exists(input_path)) @@ -504,5 +567,5 @@ int main(const int argc, const char** argv) return EXIT_FAILURE; } - return run_session_directory(input_path, mcap_path, options, msg_hz, calibration_path, sn_path); + return run_session_directory(input_path, mcap_path, options, msg_hz, acc_scale, calibration_path, sn_path); } \ No newline at end of file diff --git a/apps/console_tools/mcap_to_laz.cpp b/apps/console_tools/mcap_to_laz.cpp index ad7a06c4..52c2fea3 100644 --- a/apps/console_tools/mcap_to_laz.cpp +++ b/apps/console_tools/mcap_to_laz.cpp @@ -1,5 +1,7 @@ // MCAP/ROS2-bag -> mandeye session directory importer: the inverse of -// laz_to_mcap. +// laz_to_mcap. Built a second time as bag_to_laz (HDMAPPING_IMPORT_ROS1_BAG), +// which reads a ROS1 bag through rosbags/Ros1Bag instead -- the inverse of +// laz_to_bag. Everything after the reader is shared. // // Deliberately does not link lidar_odometry_utils.cpp. Nothing here needs // load_point_cloud()/load_imu(); reading is handled by rosbags/McapReader and @@ -7,12 +9,16 @@ // core/TBB/glm/toml++/vqf. #include "MandeyeSessionWriter.h" #include "McapReader.h" +#if defined(HDMAPPING_IMPORT_ROS1_BAG) +#include "Ros1Bag.h" +#endif #include #include #include #include #include +#include #include #include @@ -23,6 +29,50 @@ namespace fs = std::filesystem; namespace { +#if defined(HDMAPPING_IMPORT_ROS1_BAG) + using BagReader = rosbags::Ros1BagFileReader; + constexpr const char* kInputExtension = ".bag"; + constexpr const char* kInputDescription = "a ROS1 bag (format 2.0; uncompressed, bz2 or lz4 chunks)"; +#else + using BagReader = rosbags::McapFileReader; + constexpr const char* kInputExtension = ".mcap"; + constexpr const char* kInputDescription = "a ros2msg/CDR MCAP bag, e.g. one written by laz_to_mcap"; +#endif + + // Standard gravity, the g HDMapping's IMU csv is scaled in. + constexpr double kStandardGravity = 9.80665; + + // What the importer does to every IMU sample before it reaches the csv: + // rotate it into the lidar frame (the frame HDMapping assumes the IMU is in) + // and bring acceleration to g. + struct ImuTransform + { + double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0; // IMU frame -> lidar frame + double acc_scale = 1.0; // 1 for a bag already in g, 1/9.80665 for m/s^2 + }; + + void rotate(const ImuTransform& q, float& x, float& y, float& z) + { + // v' = v + 2w (u x v) + 2 u x (u x v), u = (qx, qy, qz) + const double vx = x, vy = y, vz = z; + const double cx = q.qy * vz - q.qz * vy, cy = q.qz * vx - q.qx * vz, cz = q.qx * vy - q.qy * vx; + x = static_cast(vx + 2.0 * (q.qw * cx + q.qy * cz - q.qz * cy)); + y = static_cast(vy + 2.0 * (q.qw * cy + q.qz * cx - q.qx * cz)); + z = static_cast(vz + 2.0 * (q.qw * cz + q.qx * cy - q.qy * cx)); + } + + void apply_imu_transform(const ImuTransform& t, std::vector& samples) + { + for (auto& s : samples) + { + rotate(t, s.gyro_x, s.gyro_y, s.gyro_z); + rotate(t, s.acc_x, s.acc_y, s.acc_z); + s.acc_x = static_cast(s.acc_x * t.acc_scale); + s.acc_y = static_cast(s.acc_y * t.acc_scale); + s.acc_z = static_cast(s.acc_z * t.acc_scale); + } + } + // Absolute time grid, matching MessageSplitter in laz_to_mcap.cpp: a timestamp t // belongs to bin floor(t / chunk_seconds). Because the grid is absolute rather // than relative to the first message, lidar and IMU bin identically without the @@ -37,8 +87,9 @@ namespace void print_usage(const char* argv0) { - spdlog::error("Usage: {} [options]", argv0); - spdlog::error(" input.mcap a ros2msg/CDR MCAP bag, e.g. one written by laz_to_mcap"); + spdlog::error("Usage: {} [ ...] [options]", argv0, kInputExtension, kInputExtension); + spdlog::error(" input{} {}; several inputs (e.g. the parts of a split", kInputExtension, kInputDescription); + spdlog::error(" recording) are read in the order given into one session"); spdlog::error(" output_session_dir written as mandeye lidarNNNN.laz + imuNNNN.csv chunk pairs,"); spdlog::error(" ready to open in lidar_odometry_step_1"); spdlog::error("Options:"); @@ -52,14 +103,19 @@ namespace spdlog::error(" correct for any bag that fills them in, including laz_to_mcap's."); spdlog::error(" A preset forces that driver's layout for bags whose field array is"); spdlog::error(" missing or wrong, and warns if it disagrees with what the file says."); + spdlog::error(" --imu-rotation qx qy qz qw rotates IMU samples into the lidar frame (R_lidar_imu as a"); + spdlog::error(" quaternion; default: identity, i.e. the IMU is in the lidar frame)"); + spdlog::error(" --imu-acc-unit unit of the bag's IMU acceleration (default: g, copied as is;"); + spdlog::error(" ms2 = m/s^2 as sensor_msgs/Imu defines it, divided by 9.80665)"); spdlog::error(" --list print the file's channels and exit"); spdlog::error(""); + spdlog::error("The lidar ring, when the bag has one, is kept as the LAS extra-bytes attribute 'ring'."); spdlog::error("Single-lidar bags only: one PointCloud2 topic in, one point cloud stream out. No"); spdlog::error("calibration.json or .sn file is written, so lidar_odometry_step_1 applies no"); spdlog::error("extrinsics to the result."); } - int list_channels(const rosbags::McapFileReader& reader) + int list_channels(const BagReader& reader) { const auto& channels = reader.channels(); if (channels.empty()) @@ -94,9 +150,9 @@ namespace // full stream is small -- 200 Hz for an hour is ~700k samples -- and buffering it // lets each chunk's csv be written in one shot after the lidar pass has decided // where the chunk boundaries fall. - bool collect_imu(rosbags::McapFileReader& reader, std::vector& out, std::string& serial_number) + bool collect_imu(BagReader& reader, std::vector& out, std::string& serial_number) { - rosbags::McapFileReader::Callbacks callbacks; + BagReader::Callbacks callbacks; callbacks.onImu = [&](const rosbags::McapImuSample& sample) { out.push_back(sample); @@ -123,8 +179,10 @@ namespace // Streams point clouds into lidarNNNN.laz, starting a new chunk whenever the // message crosses a time-bin boundary. Returns the bin of each chunk written, in // chunk-index order, so the IMU pass can route samples to the same chunks. + // Several readers are one continuous stream: the bin grid is absolute, so a bin + // that straddles two input files stays a single chunk. bool write_lidar_chunks( - rosbags::McapFileReader& reader, + std::vector>& readers, rosbags::MandeyeSessionWriter& writer, double chunk_seconds, std::vector& chunk_bins, @@ -134,7 +192,7 @@ namespace int64_t current_bin = 0; double last_stamp = -std::numeric_limits::infinity(); - rosbags::McapFileReader::Callbacks callbacks; + BagReader::Callbacks callbacks; callbacks.onPointCloud = [&](uint64_t /*log_time_ns*/, std::vector&& points) { if (!ok || points.empty()) @@ -170,8 +228,13 @@ namespace ++stats.lidar_messages; }; - if (!reader.read(callbacks)) - return false; + for (auto& reader : readers) + { + if (!reader->read(callbacks)) + return false; + if (!ok) + return false; + } if (!writer.endChunk()) return false; return ok; @@ -233,13 +296,13 @@ int main(const int argc, const char** argv) return EXIT_FAILURE; } - const std::string mcap_path = argv[1]; - std::string session_dir; + std::vector positionals; rosbags::McapReaderOptions options; double chunk_seconds = 20.0; bool list_only = false; + ImuTransform imu_transform; - for (int i = 2; i < argc; ++i) + for (int i = 1; i < argc; ++i) { const std::string arg = argv[i]; const bool hasValue = i + 1 < argc; @@ -284,8 +347,47 @@ int main(const int argc, const char** argv) return EXIT_FAILURE; } } - else if (!arg.starts_with("--") && session_dir.empty()) - session_dir = arg; + else if (arg == "--imu-rotation" && i + 4 < argc) + { + try + { + imu_transform.qx = std::stod(argv[++i]); + imu_transform.qy = std::stod(argv[++i]); + imu_transform.qz = std::stod(argv[++i]); + imu_transform.qw = std::stod(argv[++i]); + } catch (const std::exception&) + { + spdlog::error("Invalid --imu-rotation (expected four numbers qx qy qz qw)"); + return EXIT_FAILURE; + } + const double n = std::sqrt( + imu_transform.qx * imu_transform.qx + imu_transform.qy * imu_transform.qy + imu_transform.qz * imu_transform.qz + + imu_transform.qw * imu_transform.qw); + if (!std::isfinite(n) || std::abs(n - 1.0) > 1e-3) + { + spdlog::error("Invalid --imu-rotation (not a unit quaternion, norm {:.6f})", n); + return EXIT_FAILURE; + } + imu_transform.qx /= n; + imu_transform.qy /= n; + imu_transform.qz /= n; + imu_transform.qw /= n; + } + else if (arg == "--imu-acc-unit" && hasValue) + { + const std::string unit = argv[++i]; + if (unit == "g") + imu_transform.acc_scale = 1.0; + else if (unit == "ms2") + imu_transform.acc_scale = 1.0 / kStandardGravity; + else + { + spdlog::error("Unknown --imu-acc-unit '{}' (expected g|ms2)", unit); + return EXIT_FAILURE; + } + } + else if (!arg.starts_with("--")) + positionals.push_back(arg); else { spdlog::error("Unrecognized argument '{}'", arg); @@ -294,61 +396,113 @@ int main(const int argc, const char** argv) } } - if (fs::path(mcap_path).extension() != ".mcap") + // Every positional is an input except the last, the session dir (with --list, + // all of them are inputs). + std::string session_dir; + if (!list_only) { - spdlog::error("Invalid extension for input file {} - expected .mcap", mcap_path); - return EXIT_FAILURE; + if (positionals.size() < 2) + { + spdlog::error("Missing "); + print_usage(argv[0]); + return EXIT_FAILURE; + } + session_dir = positionals.back(); + positionals.pop_back(); } - if (!fs::exists(mcap_path)) + if (positionals.empty()) { - spdlog::error("Input file {} does not exist", mcap_path); + print_usage(argv[0]); return EXIT_FAILURE; } - if (!list_only && session_dir.empty()) + for (const auto& input : positionals) { - spdlog::error("Missing "); - print_usage(argv[0]); - return EXIT_FAILURE; + if (fs::path(input).extension() != kInputExtension) + { + spdlog::error("Invalid extension for input file {} - expected {}", input, kInputExtension); + return EXIT_FAILURE; + } + if (!fs::exists(input)) + { + spdlog::error("Input file {} does not exist", input); + return EXIT_FAILURE; + } } - rosbags::McapFileReader reader(mcap_path, options); + std::vector> readers; + for (const auto& input : positionals) + readers.push_back(std::make_unique(input, options)); if (list_only) - return list_channels(reader); - - if (!reader.isOpen()) { - spdlog::error("Cannot read {}: {}", mcap_path, reader.error()); - spdlog::error("Run with --list to see the file's channels."); - return EXIT_FAILURE; + int rc = EXIT_SUCCESS; + for (size_t k = 0; k < readers.size(); ++k) + { + spdlog::info("{}:", positionals[k]); + if (list_channels(*readers[k]) != EXIT_SUCCESS) + rc = EXIT_FAILURE; + } + return rc; } - const auto& topics = reader.topics(); - if (topics.lidar.empty()) + for (size_t k = 0; k < readers.size(); ++k) { - spdlog::error("No sensor_msgs/msg/PointCloud2 topic found in {}", mcap_path); - return EXIT_FAILURE; + if (!readers[k]->isOpen()) + { + spdlog::error("Cannot read {}: {}", positionals[k], readers[k]->error()); + spdlog::error("Run with --list to see the file's channels."); + return EXIT_FAILURE; + } + if (readers[k]->topics().lidar.empty()) + { + spdlog::error("No sensor_msgs/msg/PointCloud2 topic found in {}", positionals[k]); + return EXIT_FAILURE; + } + // One session is one sensor: auto-detection must land on the same topics in every input. + const auto& t = readers[k]->topics(); + const auto& first = readers.front()->topics(); + if (t.lidar != first.lidar || t.imu != first.imu) + { + spdlog::error( + "{} resolves to lidar='{}' imu='{}' but {} to lidar='{}' imu='{}' - pass --lidar-topic/--imu-topic", + positionals[k], + t.lidar, + t.imu, + positionals.front(), + first.lidar, + first.imu); + return EXIT_FAILURE; + } } + if (readers.size() > 1) + spdlog::info("Reading {} inputs in the order given", readers.size()); + + const auto& topics = readers.front()->topics(); spdlog::info( "Reading lidar='{}' (layout: {}) imu='{}' sn='{}'", topics.lidar, rosbags::pc2PresetName(options.lidar_preset), topics.imu.empty() ? "" : topics.imu, topics.sn.empty() ? "" : topics.sn); - if (!reader.timeOrdered()) - spdlog::warn("{} has no message indexes - reading in file order instead of log-time order", mcap_path); + for (size_t k = 0; k < readers.size(); ++k) + if (!readers[k]->timeOrdered()) + spdlog::warn("{} has no message indexes - reading in file order instead of log-time order", positionals[k]); std::vector imu; std::string serial_number; - if (!topics.imu.empty() && !collect_imu(reader, imu, serial_number)) + for (size_t k = 0; k < readers.size() && !topics.imu.empty(); ++k) { - spdlog::error("Failed reading IMU stream: {}", reader.error()); - return EXIT_FAILURE; + if (!collect_imu(*readers[k], imu, serial_number)) + { + spdlog::error("Failed reading IMU stream of {}: {}", positionals[k], readers[k]->error()); + return EXIT_FAILURE; + } } if (!serial_number.empty()) spdlog::info("Bag serial number: {}", serial_number); + apply_imu_transform(imu_transform, imu); spdlog::info("Read {} IMU sample(s)", imu.size()); - rosbags::MandeyeSessionWriter writer(session_dir); + rosbags::MandeyeSessionWriter writer(session_dir, /*with_rgb=*/false, /*with_ring=*/true); if (!writer.isOpen()) { spdlog::error("Cannot write session directory: {}", writer.error()); @@ -357,16 +511,21 @@ int main(const int argc, const char** argv) std::vector chunk_bins; ImportStats stats; - if (!write_lidar_chunks(reader, writer, chunk_seconds, chunk_bins, stats)) + if (!write_lidar_chunks(readers, writer, chunk_seconds, chunk_bins, stats)) { - spdlog::error("Failed writing lidar chunks: {}", writer.error().empty() ? reader.error() : writer.error()); + std::string why = writer.error(); + for (const auto& reader : readers) + if (why.empty()) + why = reader->error(); + spdlog::error("Failed writing lidar chunks: {}", why); return EXIT_FAILURE; } // Emitted before the empty check on purpose: a forced --lidar-type that the // file disagrees with is the likeliest reason for decoding nothing at all, so // the explanation has to come out even on the failure path. - if (!reader.lidarLayoutWarning().empty()) - spdlog::warn("PointCloud2 decoding: {}", reader.lidarLayoutWarning()); + for (size_t k = 0; k < readers.size(); ++k) + if (!readers[k]->lidarLayoutWarning().empty()) + spdlog::warn("PointCloud2 decoding ({}): {}", positionals[k], readers[k]->lidarLayoutWarning()); if (chunk_bins.empty()) { @@ -390,7 +549,9 @@ int main(const int argc, const char** argv) writer.imuSamplesWritten()); if (stats.out_of_order) - spdlog::warn("Point cloud messages were not in ascending time order - chunk boundaries may not be monotonic"); + spdlog::warn( + "Point cloud messages were not in ascending time order - chunk boundaries may not be monotonic{}", + readers.size() > 1 ? " (are the inputs given in recording order?)" : ""); // lidar_odometry_step_1's load_data() needs more than two input files and then // discards the first chunk's points outright, so a session of one or two diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 42359ddf..a339b087 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -1,6 +1,7 @@ #include "lidar_odometry_utils.h" #include "csv.hpp" #include +#include #include #include #include @@ -497,12 +498,44 @@ std::vector, Eigen::Vector3f, Eigen::Vector return all_data; } +namespace +{ + // Byte offset of the uint16 LAS extra-bytes attribute `name` within a point's + // extra bytes, or -1 when the file does not describe one by that name and type. + int find_u16_extra_bytes_attribute(const laszip_header* header, const char* name) + { + // LAS 1.4 extra-bytes descriptor: 192 bytes each, data_type 1-based (3 = uint16, + // 0 = undocumented with `options` holding the byte count). + constexpr size_t kDescriptorSize = 192; + constexpr int kTypeSize[] = { 0, 1, 1, 2, 2, 4, 4, 8, 8, 4, 8 }; + for (laszip_U32 i = 0; i < header->number_of_variable_length_records; ++i) + { + const laszip_vlr_struct& vlr = header->vlrs[i]; + if (vlr.record_id != 4 || std::strncmp(vlr.user_id, "LASF_Spec", 16) != 0 || vlr.data == nullptr) + continue; + int start = 0; + for (size_t a = 0; a + kDescriptorSize <= vlr.record_length_after_header; a += kDescriptorSize) + { + const laszip_U8* d = vlr.data + a; + const int data_type = d[2]; + char attribute[33] = {}; + std::memcpy(attribute, d + 4, 32); + if (std::strcmp(attribute, name) == 0) + return data_type == 3 ? start : -1; + start += data_type == 0 ? d[3] : (data_type <= 10 ? kTypeSize[data_type] : 0); + } + } + return -1; + } +} // namespace + std::vector load_point_cloud( const std::string& lazFile, bool ommit_points_with_timestamp_equals_zero, double filter_threshold_xy_inner, double filter_threshold_xy_outer, - const std::unordered_map& calibrations) + const std::unordered_map& calibrations, + std::vector* rings) { std::vector points; laszip_POINTER laszip_reader; @@ -540,6 +573,10 @@ std::vector load_point_cloud( const int fmt = header->point_data_format; const bool has_rgb = fmt == 2 || fmt == 3 || fmt == 5 || fmt == 7 || fmt == 8 || fmt == 10; + if (rings) + rings->clear(); + const int ring_offset = rings ? find_u16_extra_bytes_attribute(header, "ring") : -1; + for (laszip_U32 j = 0; j < header->number_of_point_records; j++) { if (laszip_read_point(laszip_reader)) @@ -590,6 +627,13 @@ std::vector load_point_cloud( sqrt(pf.x() * pf.x() + pf.y() * pf.y()) < filter_threshold_xy_outer) { points.emplace_back(p); + if (rings) + { + uint16_t ring = 0; + if (ring_offset >= 0 && ring_offset + 2 <= point->num_extra_bytes) + std::memcpy(&ring, point->extra_bytes + ring_offset, 2); + rings->push_back(ring); + } } else { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 027971e9..39c0219b 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -344,13 +344,16 @@ std::vector, Eigen::Vector3f, Eigen::Vector //! @param ommit_points_with_timestamp_equals_zero - if true, points with timestamp == 0 will be omited //! @param filter_threshold_xy - threshold for filtering points in xy plane //! @param calibrations - map of calibrations for each scanner key is scanner id. +//! @param rings - if not null, receives each returned point's laser ring, read from the +//! uint16 LAS extra-bytes attribute "ring" (0 for every point when the file has none) //! @return vector of points of @ref Point3Di type std::vector load_point_cloud( const std::string& lazFile, bool ommit_points_with_timestamp_equals_zero, double filter_threshold_xy_inner, double filter_threshold_xy_outer, - const std::unordered_map& calibrations); + const std::unordered_map& calibrations, + std::vector* rings = nullptr); bool save_poses(const std::string& file_name, const std::vector& m_poses, const std::vector& filenames); diff --git a/rosbags/MandeyeSessionWriter.cpp b/rosbags/MandeyeSessionWriter.cpp index a3f696d6..2770a107 100644 --- a/rosbags/MandeyeSessionWriter.cpp +++ b/rosbags/MandeyeSessionWriter.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -35,6 +36,7 @@ struct MandeyeSessionWriter::Impl std::string error; bool open = false; bool withRgb = false; + bool withRing = false; laszip_POINTER writer = nullptr; laszip_point* point = nullptr; @@ -51,11 +53,12 @@ struct MandeyeSessionWriter::Impl } }; -MandeyeSessionWriter::MandeyeSessionWriter(const std::filesystem::path& dir, bool with_rgb) +MandeyeSessionWriter::MandeyeSessionWriter(const std::filesystem::path& dir, bool with_rgb, bool with_ring) : impl_(std::make_unique()) { impl_->dir = dir; impl_->withRgb = with_rgb; + impl_->withRing = with_ring; std::error_code ec; std::filesystem::create_directories(dir, ec); @@ -126,6 +129,15 @@ bool MandeyeSessionWriter::beginChunk(int index) header->y_offset = 0.0; header->z_offset = 0.0; + if(impl_->withRing) + { + // LAS_ATTRIBUTE_U16 = 2 in laszip's 0-based attribute type codes. The + // attribute adds the extra-bytes VLR; the record grows by its 2 bytes. + if(laszip_add_attribute(impl_->writer, 2, kSessionRingAttribute, "laser ring index", 1.0, 0.0)) + return impl_->fail("failed to add the ring attribute for " + path.string()); + header->point_data_record_length += 2; + } + if(laszip_open_writer(impl_->writer, path.string().c_str(), /*compress=*/1)) return impl_->fail("failed to open laszip writer for " + path.string()); @@ -171,6 +183,12 @@ bool MandeyeSessionWriter::addPoints(const std::vector& points, const // user_data is always 0 -- and it must be written, because a session with a // non-zero id but no matching calibration entry has its points dropped. impl_->point->user_data = 0; + if(impl_->withRing) + { + if(impl_->point->num_extra_bytes < 2) + return impl_->fail("laszip point has no room for the ring attribute"); + std::memcpy(impl_->point->extra_bytes, &p.ring, 2); + } laszip_F64 coordinates[3] = {p.x, p.y, p.z}; if(laszip_set_coordinates(impl_->writer, coordinates)) diff --git a/rosbags/MandeyeSessionWriter.h b/rosbags/MandeyeSessionWriter.h index 14157106..03dc9cda 100644 --- a/rosbags/MandeyeSessionWriter.h +++ b/rosbags/MandeyeSessionWriter.h @@ -10,6 +10,9 @@ namespace rosbags { +// Name of the LAS extra-bytes attribute that carries each point's laser ring. +inline constexpr const char* kSessionRingAttribute = "ring"; + // Writes a single-lidar mandeye-style recording directory: lidarNNNN.laz / // imuNNNN.csv chunk pairs, indexed by the same trailing-4-digit convention // load_data() in apps/lidar_odometry_step_1 matches them by. @@ -26,6 +29,12 @@ namespace rosbags // red/green/blue (with_rgb only) LAS 16-bit colour, point format 3 // instead of 1; load_point_cloud() reads // neither the colour nor the format +// ring (with_ring only) McapPoint::ring as a uint16 LAS "extra +// bytes" attribute named "ring" (see +// kSessionRingAttribute). user_data stays the +// lidar id and point_source_ID stays free; +// load_point_cloud() returns it only when +// asked for rings // // The IMU csv uses the modern named-column format load_imu() prefers, in the // column order concatenate_multi_livox already writes, with both timestamps in @@ -37,8 +46,9 @@ class MandeyeSessionWriter using Rgb = std::array; // Creates `dir` if it does not exist. `with_rgb` writes every chunk as LAS - // point format 3 (format 1 + RGB) instead of format 1. - explicit MandeyeSessionWriter(const std::filesystem::path& dir, bool with_rgb = false); + // point format 3 (format 1 + RGB) instead of format 1; `with_ring` adds the + // "ring" extra-bytes attribute. + explicit MandeyeSessionWriter(const std::filesystem::path& dir, bool with_rgb = false, bool with_ring = false); ~MandeyeSessionWriter(); // Non-copyable diff --git a/rosbags/McapWriter.cpp b/rosbags/McapWriter.cpp index 64fdaa1f..b668f515 100644 --- a/rosbags/McapWriter.cpp +++ b/rosbags/McapWriter.cpp @@ -2,6 +2,7 @@ #include "McapWriter.h" #include "cdr_serializer.hpp" #include +#include #include #include #include @@ -111,42 +112,101 @@ float64 z // --------------------------------------------------------------------------- // PointCloud2 binary layouts, little-endian (see PointCloudLayout in McapWriter.h -// for the full per-layout offset table). +// for the full per-layout offset table). Public so the ROS1 bag writer packs +// exactly the same records. // --------------------------------------------------------------------------- -static constexpr uint32_t kPointStepGeneric = 28; -static constexpr uint32_t kPointStepVelodyne = 26; -static constexpr uint32_t kPointStepOuster = 26; -static constexpr uint32_t kPointStepHesai = 26; +std::vector pointCloudFields(PointCloudLayout layout) +{ + switch(layout) + { + case PointCloudLayout::Velodyne: + return { + {"x", 0, 7}, // FLOAT32 + {"y", 4, 7}, // FLOAT32 + {"z", 8, 7}, // FLOAT32 + {"intensity", 12, 7}, // FLOAT32 + {"ring", 16, 4}, // UINT16 + {"time", 18, 8}, // FLOAT64 + }; + case PointCloudLayout::Ouster: + return { + {"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 + }; + case PointCloudLayout::Hesai: + return { + {"x", 0, 7}, // FLOAT32 + {"y", 4, 7}, // FLOAT32 + {"z", 8, 7}, // FLOAT32 + {"intensity", 12, 7}, // FLOAT32 + {"timestamp", 16, 8}, // FLOAT64 + {"ring", 24, 4}, // UINT16 + }; + case PointCloudLayout::Canonical: + return { + {"x", 0, 7}, // FLOAT32 + {"y", 4, 7}, // FLOAT32 + {"z", 8, 7}, // FLOAT32 + {"intensity", 12, 7}, // FLOAT32 + {"ring", 16, 4}, // UINT16 + {"time", 20, 7}, // FLOAT32 + {"t", 24, 6}, // UINT32 + }; + case PointCloudLayout::Generic: + default: + return { + {"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 uint32_t pointStepFor(PointCloudLayout layout) +uint32_t pointCloudPointStep(PointCloudLayout layout) { switch(layout) { - case PointCloudLayout::Velodyne: return kPointStepVelodyne; - case PointCloudLayout::Ouster: return kPointStepOuster; - case PointCloudLayout::Hesai: return kPointStepHesai; + case PointCloudLayout::Velodyne: + case PointCloudLayout::Ouster: + case PointCloudLayout::Hesai: return 26; + case PointCloudLayout::Canonical: case PointCloudLayout::Generic: - default: return kPointStepGeneric; + default: return 28; } } -// 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) +// Packs one point at `d` per `layout`. Done field-by-field to avoid C++ struct +// padding surprises; padding bytes are left as the caller zeroed them. +static void packPoint(uint8_t* d, 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); + const auto put = [&d](const void* v, size_t n) + { + std::memcpy(d, v, n); + d += n; + }; + put(&p.x, 4); + put(&p.y, 4); + put(&p.z, 4); + put(&p.intensity, 4); switch(layout) { case PointCloudLayout::Velodyne: { - w.write_raw(&p.ring, 2); + put(&p.ring, 2); const double t = p.timestamp - t0; - w.write_raw(&t, 8); + put(&t, 8); return; } case PointCloudLayout::Ouster: @@ -157,33 +217,53 @@ static void writePointBytes(CdrWriter& w, const McapPoint& p, double t0, PointCl // 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); + put(&t, 4); + put(&reflectivity, 2); + put(&p.ring, 2); + put(&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); + put(&p.timestamp, 8); + put(&p.ring, 2); + return; + } + case PointCloudLayout::Canonical: + { + put(&p.ring, 2); + d += 2; // padding + const double dt = p.timestamp - t0; + const float time = static_cast(dt); + const uint32_t t = static_cast(std::llround(dt * 1e9)); + put(&time, 4); + put(&t, 4); 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); + put(&p.ring, 2); + put(&p.laser_id, 1); + d += 1; // padding const double t = p.timestamp - t0; - w.write_raw(&t, 8); + put(&t, 8); return; } } } +std::vector packPointCloud(const std::vector& points, PointCloudLayout layout) +{ + const uint32_t step = pointCloudPointStep(layout); + std::vector data(points.size() * step, 0); + const double t0 = points.empty() ? 0.0 : points.front().timestamp; + for(size_t i = 0; i < points.size(); ++i) + packPoint(data.data() + i * step, points[i], t0, layout); + return data; +} + // --------------------------------------------------------------------------- // CDR helpers // --------------------------------------------------------------------------- @@ -208,75 +288,15 @@ static std::vector serializePointCloud2( 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 auto fields = pointCloudFields(layout); + const uint32_t pointStep = pointCloudPointStep(layout); - const FieldDef* fields = kFieldsGeneric; - uint32_t nFields = std::size(kFieldsGeneric); - switch(layout) + w.write_u32(static_cast(fields.size())); // sequence length + for(const auto& f : fields) { - 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_string(f.name); + w.write_u32(f.offset); + w.write_u8(f.datatype); w.write_u32(1); // count } @@ -285,12 +305,9 @@ static std::vector serializePointCloud2( 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); + const auto data = packPointCloud(pts, layout); + w.write_u32(static_cast(data.size())); + w.write_raw(data.data(), data.size()); w.write_bool(true); // is_dense return w.data(); diff --git a/rosbags/McapWriter.h b/rosbags/McapWriter.h index d083b63e..c7053b79 100644 --- a/rosbags/McapWriter.h +++ b/rosbags/McapWriter.h @@ -82,8 +82,27 @@ enum class PointCloudLayout 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 + Canonical, // x,y,z,intensity,ring,time(float32),t -- point_step = 28 (benchmark canonical bag) }; +// One PointCloud2 field of a layout; datatype as in sensor_msgs/PointField +// (2 UINT8, 4 UINT16, 6 UINT32, 7 FLOAT32, 8 FLOAT64). +struct PointCloudField +{ + const char* name; + uint32_t offset; + uint8_t datatype; +}; + +// The field table and point_step of `layout` (the tables below), and the +// PointCloud2 `data` bytes for `points` in it. Relative per-point times are +// taken from points.front(), so `points` should be sorted by timestamp. +// McapFileWriter uses these, and so does the ROS1 bag writer, so both formats +// carry identical records. +std::vector pointCloudFields(PointCloudLayout layout); +uint32_t pointCloudPointStep(PointCloudLayout layout); +std::vector packPointCloud(const std::vector& points, PointCloudLayout layout); + struct McapWriterOptions { std::string frame_id = "lidar"; @@ -144,6 +163,16 @@ struct McapWriterOptions // intensity float32 offset 12 // timestamp float64 offset 16 (seconds, ABSOLUTE -- not relative to the message stamp, per Hesai convention) // ring uint16 offset 24 +// Canonical (point_step = 28, the benchmark's canonical spinning-lidar bag; its float32 +// `time` is what FAST-LIO/LIO-SAM Velodyne point types read -- PCL matches fields by +// name and datatype, so the float64 `time` of the Velodyne layout above reads as 0 there): +// x float32 offset 0 +// y float32 offset 4 +// z float32 offset 8 +// intensity float32 offset 12 +// ring uint16 offset 16 (2 bytes padding follow) +// time float32 offset 20 (seconds, relative to the message stamp) +// t uint32 offset 24 (nanoseconds, relative to the message stamp) class McapFileWriter { public: diff --git a/rosbags/Ros1Bag.cpp b/rosbags/Ros1Bag.cpp new file mode 100644 index 00000000..43073e94 --- /dev/null +++ b/rosbags/Ros1Bag.cpp @@ -0,0 +1,1135 @@ +#include "Ros1Bag.h" + +#include +#if !defined(MCAP_COMPRESSION_NO_LZ4) +#include +#endif + +#include +#include +#include +#include + +namespace rosbags +{ + +// --------------------------------------------------------------------------- +// Message definitions as `rosbag record` stores them (sensor_msgs 1.13). +// --------------------------------------------------------------------------- + +const char* const kRos1PointCloud2Type = "sensor_msgs/PointCloud2"; +const char* const kRos1PointCloud2Md5 = "1158d486dd51d683ce2f1be655c3c181"; +const char* const kRos1PointCloud2Definition = 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 +uint32 seq +time stamp +string frame_id +================================================================================ +MSG: sensor_msgs/PointField +uint8 INT8=1 +uint8 UINT8=2 +uint8 INT16=3 +uint8 UINT16=4 +uint8 INT32=5 +uint8 UINT32=6 +uint8 FLOAT32=7 +uint8 FLOAT64=8 +string name +uint32 offset +uint8 datatype +uint32 count +)"; + +const char* const kRos1ImuType = "sensor_msgs/Imu"; +const char* const kRos1ImuMd5 = "6a62c6daae103f4ff57a132d6f95cec2"; +const char* const kRos1ImuDefinition = 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 +uint32 seq +time stamp +string frame_id +================================================================================ +MSG: geometry_msgs/Quaternion +float64 x +float64 y +float64 z +float64 w +================================================================================ +MSG: geometry_msgs/Vector3 +float64 x +float64 y +float64 z +)"; + +namespace +{ + +constexpr char kMagic[] = "#ROSBAG V2.0\n"; +constexpr size_t kMagicSize = sizeof(kMagic) - 1; +constexpr size_t kBagHeaderRecordSize = 4096; + +constexpr uint8_t kOpMessageData = 0x02; +constexpr uint8_t kOpBagHeader = 0x03; +constexpr uint8_t kOpIndexData = 0x04; +constexpr uint8_t kOpChunk = 0x05; +constexpr uint8_t kOpChunkInfo = 0x06; +constexpr uint8_t kOpConnection = 0x07; + +// Little-endian cursor over a byte buffer; any read past the end clears `ok`. +struct ByteReader +{ + const uint8_t* p; + size_t n; + size_t i = 0; + bool ok = true; + + template + T get() + { + T v{}; + if(i + sizeof(T) > n) + { + ok = false; + return v; + } + std::memcpy(&v, p + i, sizeof(T)); + i += sizeof(T); + return v; + } + + const uint8_t* bytes(size_t k) + { + if(i + k > n) + { + ok = false; + return nullptr; + } + const uint8_t* out = p + i; + i += k; + return out; + } + + std::string str() + { + const uint32_t len = get(); + const uint8_t* b = ok ? bytes(len) : nullptr; + return b ? std::string(reinterpret_cast(b), len) : std::string(); + } +}; + +struct ByteWriter +{ + std::vector& out; + + template + void put(T v) + { + const auto* b = reinterpret_cast(&v); + out.insert(out.end(), b, b + sizeof(T)); + } + + void bytes(const void* data, size_t k) + { + const auto* b = static_cast(data); + out.insert(out.end(), b, b + k); + } + + void str(const std::string& s) + { + put(static_cast(s.size())); + bytes(s.data(), s.size()); + } +}; + +using HeaderFields = std::map; + +bool parseHeaderFields(const uint8_t* p, size_t n, HeaderFields& fields) +{ + ByteReader r{p, n}; + while(r.ok && r.i < n) + { + const std::string field = r.str(); + if(!r.ok) + return false; + const auto eq = field.find('='); + if(eq == std::string::npos) + return false; + fields[field.substr(0, eq)] = field.substr(eq + 1); + } + return r.ok; +} + +template +bool fieldAs(const HeaderFields& fields, const char* name, T& value) +{ + const auto it = fields.find(name); + if(it == fields.end() || it->second.size() != sizeof(T)) + return false; + std::memcpy(&value, it->second.data(), sizeof(T)); + return true; +} + +// ROS time: uint32 sec, uint32 nsec. +uint64_t timeFromField(const std::string& v) +{ + uint32_t sec = 0, nsec = 0; + if(v.size() == 8) + { + std::memcpy(&sec, v.data(), 4); + std::memcpy(&nsec, v.data() + 4, 4); + } + return uint64_t(sec) * 1'000'000'000ULL + nsec; +} + +template +std::string asField(T v) +{ + return std::string(reinterpret_cast(&v), sizeof(T)); +} + +std::string timeField(uint64_t ns) +{ + const uint32_t sec = static_cast(ns / 1'000'000'000ULL); + const uint32_t nsec = static_cast(ns % 1'000'000'000ULL); + return asField(sec) + asField(nsec); +} + +using FieldList = std::vector>; + +std::vector headerBytes(const FieldList& fields) +{ + std::vector out; + ByteWriter w{out}; + for(const auto& [name, value] : fields) + { + w.put(static_cast(name.size() + 1 + value.size())); + w.bytes(name.data(), name.size()); + w.put('='); + w.bytes(value.data(), value.size()); + } + return out; +} + +void appendRecord(std::vector& out, const FieldList& header, const uint8_t* data, size_t size) +{ + const auto h = headerBytes(header); + ByteWriter w{out}; + w.put(static_cast(h.size())); + w.bytes(h.data(), h.size()); + w.put(static_cast(size)); + w.bytes(data, size); +} + +std::vector connectionData(const Ros1Connection& c) +{ + return headerBytes( + {{"topic", c.topic}, {"type", c.type}, {"md5sum", c.md5sum}, {"message_definition", c.message_definition}}); +} + +void appendConnectionRecord(std::vector& out, const Ros1Connection& c) +{ + const auto data = connectionData(c); + appendRecord( + out, {{"op", asField(kOpConnection)}, {"conn", asField(c.id)}, {"topic", c.topic}}, data.data(), data.size()); +} + +bool decompress(const std::string& compression, const std::vector& in, size_t out_size, std::vector& out, std::string& error) +{ + out.resize(out_size); + if(compression == "none") + { + if(in.size() != out_size) + { + error = "uncompressed chunk size mismatch"; + return false; + } + out = in; + return true; + } + if(compression == "bz2") + { + unsigned int dst = static_cast(out_size); + const int rc = BZ2_bzBuffToBuffDecompress( + reinterpret_cast(out.data()), + &dst, + const_cast(reinterpret_cast(in.data())), + static_cast(in.size()), + 0, + 0); + if(rc != BZ_OK || dst != out_size) + { + error = "bz2 chunk decompression failed (code " + std::to_string(rc) + ")"; + return false; + } + return true; + } +#if !defined(MCAP_COMPRESSION_NO_LZ4) // same switch as the mcap target: no liblz4 on Windows + if(compression == "lz4") + { + LZ4F_dctx* ctx = nullptr; + if(LZ4F_isError(LZ4F_createDecompressionContext(&ctx, LZ4F_VERSION))) + { + error = "cannot create an LZ4 decompression context"; + return false; + } + size_t src_pos = 0, dst_pos = 0; + size_t rc = 1; + while(rc != 0 && src_pos < in.size() && dst_pos < out_size) + { + size_t src_len = in.size() - src_pos, dst_len = out_size - dst_pos; + rc = LZ4F_decompress(ctx, out.data() + dst_pos, &dst_len, in.data() + src_pos, &src_len, nullptr); + if(LZ4F_isError(rc)) + break; + src_pos += src_len; + dst_pos += dst_len; + } + LZ4F_freeDecompressionContext(ctx); + if(LZ4F_isError(rc) || dst_pos != out_size) + { + error = "lz4 chunk decompression failed"; + return false; + } + return true; + } +#endif + error = "unsupported chunk compression '" + compression + "'"; + return false; +} + +// One record read from the file: header fields plus its data. +bool readRecord(std::ifstream& f, HeaderFields& fields, std::vector& data, std::string& error) +{ + uint32_t header_len = 0; + if(!f.read(reinterpret_cast(&header_len), 4)) + return false; // clean end of file + std::vector header(header_len); + uint32_t data_len = 0; + if(!f.read(reinterpret_cast(header.data()), header_len) || !f.read(reinterpret_cast(&data_len), 4)) + { + error = "truncated record header"; + return false; + } + data.resize(data_len); + if(!f.read(reinterpret_cast(data.data()), data_len)) + { + error = "truncated record data"; + return false; + } + fields.clear(); + if(!parseHeaderFields(header.data(), header.size(), fields)) + { + error = "malformed record header"; + return false; + } + return true; +} + +Ros1Connection connectionFromRecord(const HeaderFields& header, const std::vector& data, bool& ok) +{ + Ros1Connection c; + ok = fieldAs(header, "conn", c.id); + HeaderFields info; + ok = ok && parseHeaderFields(data.data(), data.size(), info); + const auto topic = header.find("topic"); + c.topic = topic != header.end() ? topic->second : info["topic"]; + c.type = info["type"]; + c.md5sum = info["md5sum"]; + c.message_definition = info["message_definition"]; + return c; +} + +} // namespace + +// --------------------------------------------------------------------------- +// Ros1BagReader +// --------------------------------------------------------------------------- + +Ros1BagReader::Ros1BagReader(const std::filesystem::path& path) + : file_(path, std::ios::binary) +{ + if(!file_) + { + error_ = "cannot open " + path.string(); + return; + } + char magic[kMagicSize]; + if(!file_.read(magic, kMagicSize) || std::memcmp(magic, kMagic, kMagicSize) != 0) + { + error_ = path.string() + " is not a ROS1 bag (format 2.0)"; + return; + } + HeaderFields header; + std::vector data; + uint8_t op = 0; + if(!readRecord(file_, header, data, error_) || !fieldAs(header, "op", op) || op != kOpBagHeader || + !fieldAs(header, "index_pos", index_pos_)) + { + if(error_.empty()) + error_ = "missing bag header record"; + return; + } + readIndex(); +} + +bool Ros1BagReader::isOpen() const +{ + return error_.empty(); +} + +const std::string& Ros1BagReader::error() const +{ + return error_; +} + +const std::map& Ros1BagReader::connections() const +{ + return connections_; +} + +bool Ros1BagReader::readIndex() +{ + if(index_pos_ == 0) + return true; // unindexed (still recording or not reindexed): connections come from the chunks + const auto resume = file_.tellg(); + file_.seekg(static_cast(index_pos_)); + HeaderFields header; + std::vector data; + std::string err; + while(readRecord(file_, header, data, err)) + { + uint8_t op = 0; + if(fieldAs(header, "op", op) && op == kOpConnection) + { + bool ok = false; + const auto c = connectionFromRecord(header, data, ok); + if(ok) + connections_[c.id] = c; + } + } + file_.clear(); + file_.seekg(resume); + return err.empty(); +} + +bool Ros1BagReader::parseRecords(const uint8_t* data, size_t size, const MessageFn& fn, bool& stop) +{ + ByteReader r{data, size}; + while(r.i < size) + { + const uint32_t header_len = r.get(); + const uint8_t* header = r.bytes(header_len); + const uint32_t data_len = r.get(); + const uint8_t* body = r.bytes(data_len); + HeaderFields fields; + uint8_t op = 0; + if(!r.ok || !parseHeaderFields(header, header_len, fields) || !fieldAs(fields, "op", op)) + { + error_ = "malformed record inside a chunk"; + return false; + } + if(op == kOpConnection) + { + bool ok = false; + const auto c = connectionFromRecord(fields, std::vector(body, body + data_len), ok); + if(!ok) + { + error_ = "malformed connection record"; + return false; + } + connections_.try_emplace(c.id, c); + } + else if(op == kOpMessageData) + { + uint32_t conn = 0; + const auto time = fields.find("time"); + if(!fieldAs(fields, "conn", conn) || time == fields.end()) + { + error_ = "malformed message record"; + return false; + } + const auto it = connections_.find(conn); + if(it == connections_.end()) + { + error_ = "message on unknown connection " + std::to_string(conn); + return false; + } + if(!fn(it->second, timeFromField(time->second), body, data_len)) + { + stop = true; + return true; + } + } + } + return true; +} + +bool Ros1BagReader::forEachMessage(const MessageFn& fn) +{ + if(!isOpen()) + return false; + file_.clear(); + file_.seekg(static_cast(kMagicSize)); + HeaderFields header; + std::vector data, chunk; + if(!readRecord(file_, header, data, error_)) + return false; // bag header record + + bool stop = false; + while(!stop) + { + const auto pos = static_cast(file_.tellg()); + if(index_pos_ != 0 && pos >= index_pos_) + break; + if(!readRecord(file_, header, data, error_)) + return error_.empty(); + uint8_t op = 0; + fieldAs(header, "op", op); + if(op == kOpChunk) + { + uint32_t size = 0; + const auto compression = header.find("compression"); + if(compression == header.end() || !fieldAs(header, "size", size)) + { + error_ = "malformed chunk record"; + return false; + } + if(!decompress(compression->second, data, size, chunk, error_)) + return false; + if(!parseRecords(chunk.data(), chunk.size(), fn, stop)) + return false; + } + else if(op == kOpConnection || op == kOpMessageData) + { + // Records outside chunks: rebuild the record and reuse the chunk parser. + std::vector record; + FieldList list(header.begin(), header.end()); + appendRecord(record, list, data.data(), data.size()); + if(!parseRecords(record.data(), record.size(), fn, stop)) + return false; + } + // Index data and chunk info records carry nothing a sequential reader needs. + } + return true; +} + +// --------------------------------------------------------------------------- +// Ros1BagWriter +// --------------------------------------------------------------------------- + +namespace +{ + +std::vector bagHeaderRecord(uint64_t index_pos, uint32_t conn_count, uint32_t chunk_count) +{ + const FieldList header = { + {"op", asField(kOpBagHeader)}, + {"index_pos", asField(index_pos)}, + {"conn_count", asField(conn_count)}, + {"chunk_count", asField(chunk_count)}}; + const size_t header_len = headerBytes(header).size(); + // The bag header record is padded to 4096 bytes so it can be rewritten in place. + const std::vector padding(kBagHeaderRecordSize - 8 - header_len, ' '); + std::vector out; + appendRecord(out, header, padding.data(), padding.size()); + return out; +} + +} // namespace + +Ros1BagWriter::Ros1BagWriter(const std::filesystem::path& path, size_t chunk_threshold) + : file_(path, std::ios::binary | std::ios::trunc) + , chunk_threshold_(chunk_threshold) +{ + if(!file_) + { + error_ = "cannot create " + path.string(); + return; + } + file_.write(kMagic, kMagicSize); + const auto header = bagHeaderRecord(0, 0, 0); + file_.write(reinterpret_cast(header.data()), header.size()); +} + +Ros1BagWriter::~Ros1BagWriter() +{ + close(); +} + +bool Ros1BagWriter::isOpen() const +{ + return error_.empty() && file_.good(); +} + +const std::string& Ros1BagWriter::error() const +{ + return error_; +} + +uint32_t Ros1BagWriter::addConnection( + const std::string& topic, const std::string& type, const std::string& md5sum, const std::string& message_definition) +{ + const auto id = static_cast(connections_.size()); + connections_.push_back({id, topic, type, md5sum, message_definition}); + return id; +} + +void Ros1BagWriter::write(uint32_t connection, uint64_t time_ns, const std::vector& data) +{ + if(closed_ || connection >= connections_.size()) + return; + if(chunk_.empty()) + { + chunk_start_ns_ = time_ns; + chunk_end_ns_ = time_ns; + } + chunk_start_ns_ = std::min(chunk_start_ns_, time_ns); + chunk_end_ns_ = std::max(chunk_end_ns_, time_ns); + // A chunk must carry the connection record of every connection it uses. + if(chunk_connections_.insert(connection).second) + appendConnectionRecord(chunk_, connections_[connection]); + const auto offset = static_cast(chunk_.size()); + appendRecord( + chunk_, + {{"op", asField(kOpMessageData)}, {"conn", asField(connection)}, {"time", timeField(time_ns)}}, + data.data(), + data.size()); + chunk_index_[connection].push_back({time_ns, offset}); + if(chunk_.size() >= chunk_threshold_) + flushChunk(); +} + +void Ros1BagWriter::flushChunk() +{ + if(chunk_.empty()) + return; + ChunkInfo info{static_cast(file_.tellp()), chunk_start_ns_, chunk_end_ns_, {}}; + std::vector out; + appendRecord( + out, + {{"op", asField(kOpChunk)}, {"compression", "none"}, {"size", asField(static_cast(chunk_.size()))}}, + chunk_.data(), + chunk_.size()); + for(const auto& [conn, entries] : chunk_index_) + { + std::vector index; + ByteWriter w{index}; + for(const auto& e : entries) + { + const auto t = timeField(e.time_ns); + w.bytes(t.data(), t.size()); + w.put(e.offset); + } + appendRecord( + out, + {{"op", asField(kOpIndexData)}, + {"ver", asField(uint32_t{1})}, + {"conn", asField(conn)}, + {"count", asField(static_cast(entries.size()))}}, + index.data(), + index.size()); + info.counts[conn] = static_cast(entries.size()); + } + file_.write(reinterpret_cast(out.data()), out.size()); + chunk_infos_.push_back(std::move(info)); + chunk_.clear(); + chunk_connections_.clear(); + chunk_index_.clear(); +} + +bool Ros1BagWriter::close() +{ + if(closed_) + return isOpen(); + closed_ = true; + if(!file_.is_open()) + return false; + flushChunk(); + const auto index_pos = static_cast(file_.tellp()); + std::vector out; + for(const auto& c : connections_) + appendConnectionRecord(out, c); + for(const auto& info : chunk_infos_) + { + std::vector data; + ByteWriter w{data}; + for(const auto& [conn, count] : info.counts) + { + w.put(conn); + w.put(count); + } + appendRecord( + out, + {{"op", asField(kOpChunkInfo)}, + {"ver", asField(uint32_t{1})}, + {"chunk_pos", asField(info.pos)}, + {"start_time", timeField(info.start_ns)}, + {"end_time", timeField(info.end_ns)}, + {"count", asField(static_cast(info.counts.size()))}}, + data.data(), + data.size()); + } + file_.write(reinterpret_cast(out.data()), out.size()); + const auto header = bagHeaderRecord( + index_pos, static_cast(connections_.size()), static_cast(chunk_infos_.size())); + file_.seekp(static_cast(kMagicSize)); + file_.write(reinterpret_cast(header.data()), header.size()); + file_.close(); + if(file_.fail() && error_.empty()) + error_ = "write failed"; + return error_.empty(); +} + +// --------------------------------------------------------------------------- +// sensor_msgs +// --------------------------------------------------------------------------- + +namespace +{ + +void readHeader(ByteReader& r, uint32_t& seq, uint64_t& stamp_ns, std::string& frame_id) +{ + seq = r.get(); + const uint32_t sec = r.get(); + const uint32_t nsec = r.get(); + stamp_ns = uint64_t(sec) * 1'000'000'000ULL + nsec; + frame_id = r.str(); +} + +void writeHeader(ByteWriter& w, uint32_t seq, uint64_t stamp_ns, const std::string& frame_id) +{ + w.put(seq); + w.put(static_cast(stamp_ns / 1'000'000'000ULL)); + w.put(static_cast(stamp_ns % 1'000'000'000ULL)); + w.str(frame_id); +} + +} // namespace + +bool decodeRos1PointCloud2(const uint8_t* data, size_t size, Ros1PointCloud2& msg) +{ + ByteReader r{data, size}; + readHeader(r, msg.seq, msg.stamp_ns, msg.frame_id); + msg.height = r.get(); + msg.width = r.get(); + const uint32_t n_fields = r.get(); + msg.fields.clear(); + for(uint32_t i = 0; i < n_fields && r.ok; ++i) + { + Ros1PointField f; + f.name = r.str(); + f.offset = r.get(); + f.datatype = r.get(); + f.count = r.get(); + msg.fields.push_back(f); + } + msg.is_bigendian = r.get() != 0; + msg.point_step = r.get(); + msg.row_step = r.get(); + const uint32_t n_data = r.get(); + const uint8_t* bytes = r.ok ? r.bytes(n_data) : nullptr; + if(bytes) + msg.data.assign(bytes, bytes + n_data); + msg.is_dense = r.get() != 0; + return r.ok; +} + +std::vector encodeRos1PointCloud2(const Ros1PointCloud2& msg) +{ + std::vector out; + out.reserve(msg.data.size() + 256); + ByteWriter w{out}; + writeHeader(w, msg.seq, msg.stamp_ns, msg.frame_id); + w.put(msg.height); + w.put(msg.width); + w.put(static_cast(msg.fields.size())); + for(const auto& f : msg.fields) + { + w.str(f.name); + w.put(f.offset); + w.put(f.datatype); + w.put(f.count); + } + w.put(msg.is_bigendian ? 1 : 0); + w.put(msg.point_step); + w.put(msg.row_step); + w.put(static_cast(msg.data.size())); + w.bytes(msg.data.data(), msg.data.size()); + w.put(msg.is_dense ? 1 : 0); + return out; +} + +bool decodeRos1Imu(const uint8_t* data, size_t size, Ros1Imu& msg) +{ + ByteReader r{data, size}; + readHeader(r, msg.seq, msg.stamp_ns, msg.frame_id); + for(double& v : msg.orientation) + v = r.get(); + for(double& v : msg.orientation_covariance) + v = r.get(); + for(double& v : msg.angular_velocity) + v = r.get(); + for(double& v : msg.angular_velocity_covariance) + v = r.get(); + for(double& v : msg.linear_acceleration) + v = r.get(); + for(double& v : msg.linear_acceleration_covariance) + v = r.get(); + return r.ok; +} + +std::vector encodeRos1Imu(const Ros1Imu& msg) +{ + std::vector out; + ByteWriter w{out}; + writeHeader(w, msg.seq, msg.stamp_ns, msg.frame_id); + for(double v : msg.orientation) + w.put(v); + for(double v : msg.orientation_covariance) + w.put(v); + for(double v : msg.angular_velocity) + w.put(v); + for(double v : msg.angular_velocity_covariance) + w.put(v); + for(double v : msg.linear_acceleration) + w.put(v); + for(double v : msg.linear_acceleration_covariance) + w.put(v); + return out; +} + +// --------------------------------------------------------------------------- +// Ros1BagFileReader +// --------------------------------------------------------------------------- + +namespace +{ + +const char* const kRos1StringType = "std_msgs/String"; +const char* const kRos1StringMd5 = "992ce8a1687cec8c8bd883ec73ca41d1"; +const char* const kRos1StringDefinition = "string data\n"; + +// "sensor_msgs/PointCloud2" -> "PointCloud2", as McapReader compares schema names. +std::string_view typeName(std::string_view type) +{ + const auto slash = type.rfind('/'); + return slash == std::string_view::npos ? type : type.substr(slash + 1); +} + +// livox_ros_driver/CustomMsg, the same mapping as decodeLivoxCustomMsg(): ring = +// line, laser_id = tag, intensity = reflectivity, time = timebase + offset_time. +std::vector decodeRos1LivoxCustomMsg(const uint8_t* data, size_t size, std::string* warning) +{ + ByteReader r{data, size}; + uint32_t seq = 0; + uint64_t stamp_ns = 0; + std::string frame_id; + readHeader(r, seq, stamp_ns, frame_id); + const uint64_t timebase = r.get(); + r.get(); // point_num + r.get(); // lidar_id + r.bytes(3); // rsvd + const uint32_t n = r.get(); + std::vector points; + if(!r.ok) + { + if(warning && warning->empty()) + *warning = "truncated CustomMsg header"; + return points; + } + points.reserve(n); + const double timebase_s = static_cast(timebase) * 1e-9; + for(uint32_t i = 0; i < n; ++i) + { + const uint32_t offset_time = r.get(); + McapPoint p{}; + p.x = r.get(); + p.y = r.get(); + p.z = r.get(); + p.intensity = static_cast(r.get()); + p.laser_id = r.get(); + p.ring = r.get(); + if(!r.ok) + { + if(warning && warning->empty()) + *warning = "truncated CustomMsg point data at index " + std::to_string(i); + break; + } + p.timestamp = timebase_s + static_cast(offset_time) * 1e-9; + points.push_back(p); + } + return points; +} + +} // namespace + +Ros1BagFileReader::Ros1BagFileReader(const std::filesystem::path& path, const McapReaderOptions& options) + : reader_(path) + , options_(options) +{ + if(!reader_.isOpen()) + { + error_ = reader_.error(); + return; + } + for(const auto& [id, c] : reader_.connections()) + { + // One topic can be recorded through several connections (publishers). + const bool seen = std::any_of(channels_.begin(), channels_.end(), [&](const ChannelInfo& ch) { return ch.topic == c.topic; }); + if(!seen) + channels_.push_back({c.topic, c.type, "ros1", 0}); + } + if(channels_.empty()) + { + error_ = "the bag has no index section (still recording or not reindexed: run `rosbag reindex`)"; + return; + } + std::sort(channels_.begin(), channels_.end(), [](const ChannelInfo& a, const ChannelInfo& b) { return a.topic < b.topic; }); + + // Same rules as McapFileReader: a requested topic must exist; otherwise the + // single channel of a matching type is used, and ambiguity is an error for the + // streams that must be unique. + const auto resolve = [&](const std::string& requested, std::initializer_list types, bool required_unique, std::string& out) + { + if(!requested.empty()) + { + if(std::none_of(channels_.begin(), channels_.end(), [&](const ChannelInfo& c) { return c.topic == requested; })) + { + error_ = "topic '" + requested + "' is not present in the bag"; + return false; + } + out = requested; + return true; + } + std::vector candidates; + for(const auto& c : channels_) + if(std::find(types.begin(), types.end(), typeName(c.schema_name)) != types.end()) + candidates.push_back(&c); + if(candidates.empty()) + return true; + if(candidates.size() > 1) + { + if(!required_unique) + return true; + error_ = "found " + std::to_string(candidates.size()) + " candidate topics ("; + for(size_t i = 0; i < candidates.size(); ++i) + error_ += (i ? ", " : "") + candidates[i]->topic; + error_ += ") - pick one explicitly"; + return false; + } + out = candidates.front()->topic; + return true; + }; + if(!resolve(options.lidar_topic, {"PointCloud2", "CustomMsg"}, true, topics_.lidar) || + !resolve(options.imu_topic, {"Imu"}, true, topics_.imu) || !resolve(options.sn_topic, {"String"}, false, topics_.sn)) + return; + + if(!topics_.lidar.empty()) + { + const auto it = std::find_if(channels_.begin(), channels_.end(), [&](const ChannelInfo& c) { return c.topic == topics_.lidar; }); + const auto type = typeName(it->schema_name); + if(type == "PointCloud2") + topics_.lidar_schema = LidarSchema::PointCloud2; + else if(type == "CustomMsg") + topics_.lidar_schema = LidarSchema::LivoxCustomMsg; + else + { + error_ = "topic '" + topics_.lidar + "' carries " + it->schema_name + ", not a point cloud"; + return; + } + } + open_ = true; +} + +bool Ros1BagFileReader::isOpen() const +{ + return open_; +} + +const std::string& Ros1BagFileReader::error() const +{ + return error_; +} + +const ResolvedTopics& Ros1BagFileReader::topics() const +{ + return topics_; +} + +const std::vector& Ros1BagFileReader::channels() const +{ + return channels_; +} + +bool Ros1BagFileReader::timeOrdered() const +{ + return true; +} + +const std::string& Ros1BagFileReader::lidarLayoutWarning() const +{ + return warning_; +} + +bool Ros1BagFileReader::read(const Callbacks& callbacks) +{ + if(!open_) + return false; + Ros1PointCloud2 cloud; + Ros1Imu imu; + const bool ok = reader_.forEachMessage( + [&](const Ros1Connection& c, uint64_t time_ns, const uint8_t* data, size_t size) + { + if(c.topic == topics_.lidar && callbacks.onPointCloud) + { + std::vector points; + if(topics_.lidar_schema == LidarSchema::LivoxCustomMsg) + { + points = decodeRos1LivoxCustomMsg(data, size, &warning_); + } + else if(!decodeRos1PointCloud2(data, size, cloud)) + { + if(warning_.empty()) + warning_ = "truncated PointCloud2 message"; + } + else if(cloud.is_bigendian) + { + if(warning_.empty()) + warning_ = "PointCloud2 is big-endian, which is not supported"; + } + else + { + Pc2Layout msg; + for(const auto& f : cloud.fields) + addPc2Field(msg, f.name, f.offset, static_cast(f.datatype), f.count); + msg.point_step = cloud.point_step; + points = decodePc2Points( + msg, + static_cast(cloud.fields.size()), + static_cast(cloud.stamp_ns) * 1e-9, + cloud.data.data(), + cloud.data.size(), + options_.lidar_preset, + &warning_); + } + if(!points.empty()) + callbacks.onPointCloud(time_ns, std::move(points)); + } + else if(c.topic == topics_.imu && callbacks.onImu) + { + if(decodeRos1Imu(data, size, imu)) + { + McapImuSample s; + s.timestamp = static_cast(imu.stamp_ns) * 1e-9; + s.gyro_x = static_cast(imu.angular_velocity[0]); + s.gyro_y = static_cast(imu.angular_velocity[1]); + s.gyro_z = static_cast(imu.angular_velocity[2]); + s.acc_x = static_cast(imu.linear_acceleration[0]); + s.acc_y = static_cast(imu.linear_acceleration[1]); + s.acc_z = static_cast(imu.linear_acceleration[2]); + callbacks.onImu(s); + } + } + else if(c.topic == topics_.sn && callbacks.onSn) + { + ByteReader r{data, size}; + const std::string sn = r.str(); + if(r.ok) + callbacks.onSn(time_ns, sn); + } + return true; + }); + if(!ok) + error_ = reader_.error(); + return ok; +} + +// --------------------------------------------------------------------------- +// Ros1BagFileWriter +// --------------------------------------------------------------------------- + +Ros1BagFileWriter::Ros1BagFileWriter(const std::filesystem::path& path, const McapWriterOptions& options) + : bag_(path) + , options_(options) +{ + lidar_ = bag_.addConnection(options_.lidar_topic, kRos1PointCloud2Type, kRos1PointCloud2Md5, kRos1PointCloud2Definition); + imu_ = bag_.addConnection(options_.imu_topic, kRos1ImuType, kRos1ImuMd5, kRos1ImuDefinition); +} + +bool Ros1BagFileWriter::isOpen() const +{ + return bag_.isOpen(); +} + +const std::string& Ros1BagFileWriter::error() const +{ + return bag_.error(); +} + +void Ros1BagFileWriter::writePointCloud(uint64_t timestamp_ns, const std::vector& points) +{ + Ros1PointCloud2 msg; + msg.seq = lidar_seq_++; + msg.stamp_ns = timestamp_ns; + msg.frame_id = options_.pointcloud_frame_id.empty() ? options_.frame_id : options_.pointcloud_frame_id; + msg.width = static_cast(points.size()); + for(const auto& f : pointCloudFields(options_.lidar_layout)) + msg.fields.push_back({f.name, f.offset, f.datatype, 1}); + msg.point_step = pointCloudPointStep(options_.lidar_layout); + msg.row_step = msg.width * msg.point_step; + msg.data = packPointCloud(points, options_.lidar_layout); + bag_.write(lidar_, timestamp_ns, encodeRos1PointCloud2(msg)); +} + +void Ros1BagFileWriter::writeImuSample(const McapImuSample& sample) +{ + Ros1Imu msg; + msg.seq = imu_seq_++; + msg.stamp_ns = static_cast(sample.timestamp * 1e9); + msg.frame_id = options_.frame_id; + msg.angular_velocity[0] = sample.gyro_x; + msg.angular_velocity[1] = sample.gyro_y; + msg.angular_velocity[2] = sample.gyro_z; + msg.linear_acceleration[0] = sample.acc_x; + msg.linear_acceleration[1] = sample.acc_y; + msg.linear_acceleration[2] = sample.acc_z; + bag_.write(imu_, msg.stamp_ns, encodeRos1Imu(msg)); +} + +void Ros1BagFileWriter::writeImu(const std::vector& imu) +{ + for(const auto& s : imu) + writeImuSample(s); +} + +void Ros1BagFileWriter::writeSn(uint64_t timestamp_ns, const std::string& data) +{ + // Added on first use, so a bag without a serial number has no empty topic. + if(!sn_) + sn_ = bag_.addConnection(options_.sn_topic, kRos1StringType, kRos1StringMd5, kRos1StringDefinition); + std::vector out; + ByteWriter w{out}; + w.str(data); + bag_.write(*sn_, timestamp_ns, out); +} + +bool Ros1BagFileWriter::close() +{ + return bag_.close(); +} + +} // namespace rosbags diff --git a/rosbags/Ros1Bag.h b/rosbags/Ros1Bag.h new file mode 100644 index 00000000..faf1481c --- /dev/null +++ b/rosbags/Ros1Bag.h @@ -0,0 +1,234 @@ +#pragma once + +// ROS1 bag (format 2.0) reading and writing without a ROS installation, plus +// ROS1 serialization of sensor_msgs/PointCloud2 and sensor_msgs/Imu. +// +// Reader: chunk compression none, bz2 and lz4 (roslz4 writes LZ4 frames). +// Writer: uncompressed chunks with per-chunk index records and a full index +// section, so `rosbag info`, `rosbag play` and the Python rosbags package all +// read the result. +// +// Ros1BagFileReader / Ros1BagFileWriter at the end of this header are the ROS1 +// counterparts of McapFileReader / McapFileWriter: same options, same +// McapPoint/McapImuSample structs, same PointCloud2 layouts. + +#include "McapReader.h" // McapReaderOptions, ResolvedTopics, ChannelInfo, Callbacks +#include "McapWriter.h" // McapPoint, McapImuSample, McapWriterOptions, PointCloudLayout + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rosbags +{ + +struct Ros1Connection +{ + uint32_t id{}; + std::string topic; + std::string type; // e.g. sensor_msgs/PointCloud2 + std::string md5sum; + std::string message_definition; +}; + +class Ros1BagReader +{ +public: + explicit Ros1BagReader(const std::filesystem::path& path); + + bool isOpen() const; + const std::string& error() const; + + // Connections listed in the bag's index section (empty for an unindexed bag + // until forEachMessage() has seen them inside the chunks). + const std::map& connections() const; + + // Calls fn for every message in file order. fn returns false to stop early. + // Returns false on a read or decompression error (see error()). + using MessageFn = std::function; + bool forEachMessage(const MessageFn& fn); + +private: + bool readIndex(); + bool parseRecords(const uint8_t* data, size_t size, const MessageFn& fn, bool& stop); + + std::ifstream file_; + std::string error_; + uint64_t index_pos_{}; + std::map connections_; +}; + +class Ros1BagWriter +{ +public: + explicit Ros1BagWriter(const std::filesystem::path& path, size_t chunk_threshold = 768 * 1024); + ~Ros1BagWriter(); + + Ros1BagWriter(const Ros1BagWriter&) = delete; + Ros1BagWriter& operator=(const Ros1BagWriter&) = delete; + + bool isOpen() const; + const std::string& error() const; + + // Returns the connection id to pass to write(). + uint32_t addConnection( + const std::string& topic, const std::string& type, const std::string& md5sum, const std::string& message_definition); + + // Writes one ROS1-serialized message; time_ns is the bag record time. + void write(uint32_t connection, uint64_t time_ns, const std::vector& data); + + // Writes the last chunk and the index section. Called by the destructor too. + bool close(); + +private: + struct IndexEntry + { + uint64_t time_ns; + uint32_t offset; + }; + struct ChunkInfo + { + uint64_t pos; + uint64_t start_ns; + uint64_t end_ns; + std::map counts; + }; + + void flushChunk(); + + std::ofstream file_; + std::string error_; + size_t chunk_threshold_; + bool closed_{false}; + std::vector connections_; + std::vector chunk_; + std::set chunk_connections_; + std::map> chunk_index_; + uint64_t chunk_start_ns_{}; + uint64_t chunk_end_ns_{}; + std::vector chunk_infos_; +}; + +// --------------------------------------------------------------------------- +// sensor_msgs in ROS1 serialization +// --------------------------------------------------------------------------- + +struct Ros1PointField +{ + std::string name; + uint32_t offset{}; + uint8_t datatype{}; // 1 INT8, 2 UINT8, 3 INT16, 4 UINT16, 5 INT32, 6 UINT32, 7 FLOAT32, 8 FLOAT64 + uint32_t count{1}; +}; + +struct Ros1PointCloud2 +{ + uint32_t seq{}; + uint64_t stamp_ns{}; + std::string frame_id; + uint32_t height{1}; + uint32_t width{}; + std::vector fields; + bool is_bigendian{false}; + uint32_t point_step{}; + uint32_t row_step{}; + std::vector data; + bool is_dense{true}; +}; + +struct Ros1Imu +{ + uint32_t seq{}; + uint64_t stamp_ns{}; + std::string frame_id; + double orientation[4]{0.0, 0.0, 0.0, 1.0}; // x, y, z, w + double orientation_covariance[9]{-1.0}; // [0] = -1: orientation unknown + double angular_velocity[3]{}; + double angular_velocity_covariance[9]{}; + double linear_acceleration[3]{}; + double linear_acceleration_covariance[9]{}; +}; + +extern const char* const kRos1PointCloud2Type; +extern const char* const kRos1PointCloud2Md5; +extern const char* const kRos1PointCloud2Definition; +extern const char* const kRos1ImuType; +extern const char* const kRos1ImuMd5; +extern const char* const kRos1ImuDefinition; + +bool decodeRos1PointCloud2(const uint8_t* data, size_t size, Ros1PointCloud2& msg); +std::vector encodeRos1PointCloud2(const Ros1PointCloud2& msg); +bool decodeRos1Imu(const uint8_t* data, size_t size, Ros1Imu& msg); +std::vector encodeRos1Imu(const Ros1Imu& msg); + +// --------------------------------------------------------------------------- +// McapFileReader / McapFileWriter counterparts +// --------------------------------------------------------------------------- + +// Reads a ROS1 bag into McapPoint/McapImuSample, like McapFileReader does for +// MCAP: topics are auto-detected or forced the same way (sensor_msgs/PointCloud2 +// or livox_ros_driver CustomMsg for the lidar, sensor_msgs/Imu, std_msgs/String), +// and PointCloud2 records go through the same decodePc2Points() as MCAP ones. +class Ros1BagFileReader +{ +public: + using Callbacks = McapFileReader::Callbacks; + + explicit Ros1BagFileReader(const std::filesystem::path& path, const McapReaderOptions& options = {}); + + bool isOpen() const; + const std::string& error() const; + const ResolvedTopics& topics() const; + const std::vector& channels() const; + // `rosbag record` writes messages in the order it received them, so a bag is + // read in ascending record time. + bool timeOrdered() const; + const std::string& lidarLayoutWarning() const; + + // Streams the whole bag through `callbacks`; `log_time_ns` is the bag record + // time. Returns false on a read error (see error()). + bool read(const Callbacks& callbacks); + +private: + Ros1BagReader reader_; + McapReaderOptions options_; + ResolvedTopics topics_; + std::vector channels_; + std::string error_; + std::string warning_; + bool open_{false}; +}; + +// Writes McapPoint/McapImuSample to a ROS1 bag, like McapFileWriter does for +// MCAP: same topics, frame ids and PointCloud2 layouts (pointCloudFields()). +class Ros1BagFileWriter +{ +public: + explicit Ros1BagFileWriter(const std::filesystem::path& path, const McapWriterOptions& options = {}); + + bool isOpen() const; + const std::string& error() const; + + void writePointCloud(uint64_t timestamp_ns, const std::vector& points); + void writeImuSample(const McapImuSample& imu); + void writeImu(const std::vector& imu); + void writeSn(uint64_t timestamp_ns, const std::string& data); + + // Finishes the bag's index; the destructor calls it too. + bool close(); + +private: + Ros1BagWriter bag_; + McapWriterOptions options_; + uint32_t lidar_{}, imu_{}; + std::optional sn_; + uint32_t lidar_seq_{}, imu_seq_{}; +}; + +} // namespace rosbags diff --git a/rosbags/cdr_serializer.hpp b/rosbags/cdr_serializer.hpp index a4ab196d..b61173d5 100644 --- a/rosbags/cdr_serializer.hpp +++ b/rosbags/cdr_serializer.hpp @@ -592,8 +592,48 @@ inline std::vector decodeLivoxCustomMsg( return points; } -inline std::vector decodePc2( - const uint8_t* data, size_t size, Pc2Preset preset = Pc2Preset::Auto, std::string* warning = nullptr) +// Matches one PointCloud2 field description onto `msg`, as decodePc2() does for +// each entry of the `fields` array. Unrecognized names and array fields are ignored. +inline void addPc2Field(Pc2Layout& msg, const std::string& name, uint32_t offset, Pc2Type type, uint32_t count) +{ + // count > 1 is an array field (never used for the scalars below). + if(count != 1 || pc2TypeSize(type) == 0) + return; + + const Pc2Field field{offset, type}; + if(name == "x") + msg.x = field; + else if(name == "y") + msg.y = field; + else if(name == "z") + msg.z = field; + else if(name == "intensity") + msg.intensity = field; + else if(name == "ring") + msg.ring = field; + else if(name == "laser_id") + msg.laser_id = field; + else if(name == "time") + msg.time = field; + else if(name == "t") + msg.t = field; + else if(name == "timestamp") + msg.timestamp = field; +} + +// The point records of one PointCloud2, whatever its serialization: `msg` is the +// layout built from the message's fields array (point_step included), `n_fields` +// how many fields that array had, `stamp_s` the header stamp. Shared by +// decodePc2() (ROS2/CDR) and the ROS1 bag reader, so both apply the same presets, +// time-field rules and warnings. +inline std::vector decodePc2Points( + const Pc2Layout& msg, + uint32_t n_fields, + double stamp_s, + const uint8_t* rawData, + size_t dataLen, + Pc2Preset preset = Pc2Preset::Auto, + std::string* warning = nullptr) { const auto warn = [&](std::string message) { if(warning && warning->empty()) @@ -601,75 +641,11 @@ inline std::vector decodePc2( }; 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 - r.read_u32(); // width -- the record count is derived from data/point_step instead - - Pc2Layout msg; - 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(); - const auto type = static_cast(r.read_u8()); - const uint32_t count = r.read_u32(); - - // count > 1 is an array field (never used for the scalars below). - if(count != 1 || pc2TypeSize(type) == 0) - continue; - - const Pc2Field field{offset, type}; - if(name == "x") - msg.x = field; - else if(name == "y") - msg.y = field; - else if(name == "z") - msg.z = field; - else if(name == "intensity") - msg.intensity = field; - else if(name == "ring") - msg.ring = field; - else if(name == "laser_id") - msg.laser_id = field; - else if(name == "time") - msg.time = field; - else if(name == "t") - msg.t = field; - else if(name == "timestamp") - msg.timestamp = field; - } - - const bool is_bigendian = r.read_bool(); - msg.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) - { - warn("PointCloud2 data array is truncated"); - return points; - } - if(is_bigendian) - { - // No real ROS2 bag sets this; byte-swapping every field to support a - // hypothetical one would be untestable, so refuse rather than misread. - warn("PointCloud2 is big-endian, which is not supported"); - return points; - } Pc2Layout layout; if(pc2PresetLayout(preset, layout)) { - if(nFields > 0) + if(n_fields > 0) { const std::string diff = detail::comparePc2Layouts(layout, msg); if(!diff.empty()) @@ -698,12 +674,12 @@ inline std::vector decodePc2( return points; } - const uint32_t nPts = dataLen / layout.point_step; + const size_t nPts = dataLen / layout.point_step; points.reserve(nPts); - for(uint32_t i = 0; i < nPts; ++i) + for(size_t i = 0; i < nPts; ++i) { - const uint8_t* p = rawData + static_cast(i) * layout.point_step; + const uint8_t* p = rawData + i * layout.point_step; McapPoint pt{}; pt.x = static_cast(readPc2Field(p, layout.x)); @@ -742,6 +718,59 @@ inline std::vector decodePc2( return points; } +inline std::vector decodePc2( + const uint8_t* data, size_t size, Pc2Preset preset = Pc2Preset::Auto, std::string* warning = nullptr) +{ + const auto warn = [&](std::string message) { + if(warning && warning->empty()) + *warning = std::move(message); + }; + + 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 + r.read_u32(); // width -- the record count is derived from data/point_step instead + + Pc2Layout msg; + 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(); + const auto type = static_cast(r.read_u8()); + const uint32_t count = r.read_u32(); + addPc2Field(msg, name, offset, type, count); + } + + const bool is_bigendian = r.read_bool(); + msg.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) + { + warn("PointCloud2 data array is truncated"); + return {}; + } + if(is_bigendian) + { + // No real ROS2 bag sets this; byte-swapping every field to support a + // hypothetical one would be untestable, so refuse rather than misread. + warn("PointCloud2 is big-endian, which is not supported"); + return {}; + } + + return decodePc2Points(msg, nFields, stamp_s, rawData, dataLen, preset, warning); +} + // sensor_msgs/msg/Imu → McapImuSample (one sample per CDR message) inline std::optional decodeImu(const uint8_t* data, size_t size) { diff --git a/rosbags/tests/CMakeLists.txt b/rosbags/tests/CMakeLists.txt index cdf3a1c5..2b2e1942 100644 --- a/rosbags/tests/CMakeLists.txt +++ b/rosbags/tests/CMakeLists.txt @@ -3,13 +3,14 @@ cmake_minimum_required(VERSION 4.0.0) project(hdmapping_rosbags_tests) # Unit tests for rosbags/ (LAZ/IMU <-> MCAP/ROS2-bag support code, both -# directions, and the Ouster pcap reader). Uses doctest, same as shared/tests -- see that CMakeLists.txt for +# directions, the ROS1 bag reader/writer and the Ouster pcap reader). Uses doctest, same as shared/tests -- see that CMakeLists.txt for # why. Only McapWriter.cpp defines MCAP_IMPLEMENTATION; McapReader.cpp relies on # it being linked in alongside. add_executable(hdmapping_rosbags_tests test_mcap_writer.cpp test_mcap_reader.cpp test_ouster_pcap_reader.cpp + test_ros1_bag.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../McapWriter.h ${CMAKE_CURRENT_SOURCE_DIR}/../McapWriter.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../McapReader.h @@ -18,6 +19,8 @@ add_executable(hdmapping_rosbags_tests ${CMAKE_CURRENT_SOURCE_DIR}/../MandeyeSessionWriter.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../OusterPcapReader.h ${CMAKE_CURRENT_SOURCE_DIR}/../OusterPcapReader.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../Ros1Bag.h + ${CMAKE_CURRENT_SOURCE_DIR}/../Ros1Bag.cpp ) target_include_directories(hdmapping_rosbags_tests PRIVATE @@ -27,7 +30,8 @@ target_include_directories(hdmapping_rosbags_tests PRIVATE ${THIRDPARTY_DIRECTORY}/json/include ) -target_link_libraries(hdmapping_rosbags_tests PRIVATE mcap ${PLATFORM_LASZIP_LIB}) +find_package(BZip2 REQUIRED) +target_link_libraries(hdmapping_rosbags_tests PRIVATE mcap BZip2::BZip2 ${PLATFORM_LASZIP_LIB}) if (MSVC) target_compile_definitions(hdmapping_rosbags_tests PRIVATE _USE_MATH_DEFINES) diff --git a/rosbags/tests/test_mcap_reader.cpp b/rosbags/tests/test_mcap_reader.cpp index 773af5eb..c43f1cd3 100644 --- a/rosbags/tests/test_mcap_reader.cpp +++ b/rosbags/tests/test_mcap_reader.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -375,6 +376,67 @@ TEST_CASE("MandeyeSessionWriter: LAZ stores unscaled gps_time, zero lidar id and fs::remove_all(dir); } +TEST_CASE("MandeyeSessionWriter: with_ring stores the ring as a uint16 extra-bytes attribute") +{ + const auto dir = tempPath("hdmapping_session_ring"); + fs::remove_all(dir); + + auto points = makeReaderPoints(300, 2345.5); + for (size_t i = 0; i < points.size(); ++i) + points[i].ring = static_cast((i * 7) % 64); + { + rosbags::MandeyeSessionWriter writer(dir, /*with_rgb=*/false, /*with_ring=*/true); + REQUIRE_MESSAGE(writer.isOpen(), writer.error()); + REQUIRE(writer.beginChunk(0)); + REQUIRE_MESSAGE(writer.addPoints(points), writer.error()); + REQUIRE(writer.endChunk()); + } + + laszip_POINTER reader = nullptr; + REQUIRE(laszip_create(&reader) == 0); + laszip_BOOL is_compressed = 0; + REQUIRE(laszip_open_reader(reader, (dir / "lidar0000.laz").string().c_str(), &is_compressed) == 0); + laszip_header* header = nullptr; + REQUIRE(laszip_get_header_pointer(reader, &header) == 0); + laszip_point* point = nullptr; + REQUIRE(laszip_get_point_pointer(reader, &point) == 0); + + // The standard fields keep their meaning: format 1, lidar id in user_data. + CHECK(header->point_data_format == 1); + CHECK(header->point_data_record_length == 30); + + // The extra-bytes VLR names the attribute, so other tools see a "ring" field. + bool described = false; + for (laszip_U32 v = 0; v < header->number_of_variable_length_records; ++v) + { + const auto& vlr = header->vlrs[v]; + if (vlr.record_id == 4 && std::string(vlr.user_id, 9) == "LASF_Spec" && vlr.record_length_after_header >= 192) + { + CHECK(vlr.data[2] == 3); // LAS data_type 3 = unsigned short + CHECK(std::string(reinterpret_cast(vlr.data + 4)) == rosbags::kSessionRingAttribute); + described = true; + } + } + CHECK(described); + + REQUIRE(header->number_of_point_records == points.size()); + for (size_t i = 0; i < points.size(); ++i) + { + REQUIRE(laszip_read_point(reader) == 0); + REQUIRE(point->num_extra_bytes == 2); + uint16_t ring = 0; + std::memcpy(&ring, point->extra_bytes, 2); + CHECK(ring == points[i].ring); + CHECK(point->user_data == 0); + CHECK(point->point_source_ID == 0); + CHECK(point->gps_time == points[i].timestamp); + } + laszip_close_reader(reader); + laszip_destroy(reader); + + fs::remove_all(dir); +} + TEST_CASE("MandeyeSessionWriter: with_rgb writes point format 3 with 16-bit colour") { const auto dir = tempPath("hdmapping_session_laz_rgb"); diff --git a/rosbags/tests/test_mcap_writer.cpp b/rosbags/tests/test_mcap_writer.cpp index 0dcc798d..01dc044b 100644 --- a/rosbags/tests/test_mcap_writer.cpp +++ b/rosbags/tests/test_mcap_writer.cpp @@ -216,6 +216,52 @@ TEST_CASE("McapFileWriter: PointCloud2 round-trip (Hesai layout, absolute timest fs::remove(path); } +TEST_CASE("McapFileWriter: PointCloud2 round-trip (Canonical layout, float32 time and uint32 t)") +{ + const auto path = tempMcapPath("hdmapping_test_canonical.mcap"); + const auto points = makePoints(200, 5000.0); + + { + rosbags::McapWriterOptions options; + options.lidar_layout = rosbags::PointCloudLayout::Canonical; + 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); // Canonical layout has no laser_id field + CHECK(decoded[i].timestamp == doctest::Approx(points[i].timestamp).epsilon(1e-8)); + } + + // The field table is the point of this layout: float32 `time` (what FAST-LIO/LIO-SAM + // Velodyne point types match) and uint32 `t`, both relative to the first point. + const auto raw = readTopic>( + path, "/lidar_points", [](const uint8_t* d, size_t n) + { + return std::vector(d, d + n); + }); + REQUIRE(raw.size() == 1); + const std::string bytes(raw.front().begin(), raw.front().end()); + for (const char* name : {"intensity", "ring", "time", "t"}) + CHECK(bytes.find(name) != std::string::npos); + + fs::remove(path); +} + TEST_CASE("McapFileWriter: IMU round-trip") { const auto path = tempMcapPath("hdmapping_test_imu.mcap"); diff --git a/rosbags/tests/test_ros1_bag.cpp b/rosbags/tests/test_ros1_bag.cpp new file mode 100644 index 00000000..5bafbd40 --- /dev/null +++ b/rosbags/tests/test_ros1_bag.cpp @@ -0,0 +1,327 @@ +// See rosbags/McapWriter.h for why this needs to come before any standard +// header (including ) on MSVC. +#if defined(_MSC_VER) +# undef _HAS_STD_BYTE +# define _HAS_STD_BYTE 1 +#endif + +#include + +#include "Ros1Bag.h" + +#include +#include +#include +#include +#include + +namespace fs = std::filesystem; + +namespace +{ + +rosbags::Ros1PointCloud2 makeCloud(uint32_t seq, uint64_t stamp_ns, uint32_t n) +{ + rosbags::Ros1PointCloud2 c; + c.seq = seq; + c.stamp_ns = stamp_ns; + c.frame_id = "lidar"; + c.width = n; + c.fields = {{"x", 0, 7, 1}, {"y", 4, 7, 1}, {"z", 8, 7, 1}, {"ring", 12, 4, 1}}; + c.point_step = 16; + c.row_step = n * c.point_step; + c.data.resize(c.row_step); + for (size_t i = 0; i < c.data.size(); ++i) + c.data[i] = static_cast((i * 7 + seq) & 0xff); + return c; +} + +rosbags::Ros1Imu makeImu(uint32_t seq, uint64_t stamp_ns) +{ + rosbags::Ros1Imu m; + m.seq = seq; + m.stamp_ns = stamp_ns; + m.frame_id = "lidar"; + m.angular_velocity[0] = 0.1 * seq; + m.angular_velocity[2] = -0.5; + m.linear_acceleration[2] = 9.81; + return m; +} + +} // namespace + +TEST_CASE("Ros1Bag: PointCloud2 encode/decode round-trip") +{ + const auto in = makeCloud(7, 1'710'255'656'973'876'000ULL, 50); + const auto bytes = rosbags::encodeRos1PointCloud2(in); + rosbags::Ros1PointCloud2 out; + REQUIRE(rosbags::decodeRos1PointCloud2(bytes.data(), bytes.size(), out)); + CHECK(out.seq == in.seq); + CHECK(out.stamp_ns == in.stamp_ns); + CHECK(out.frame_id == in.frame_id); + CHECK(out.width == in.width); + REQUIRE(out.fields.size() == in.fields.size()); + CHECK(out.fields[3].name == "ring"); + CHECK(out.fields[3].datatype == 4); + CHECK(out.point_step == in.point_step); + CHECK(out.data == in.data); + CHECK(out.is_dense); + + // A truncated message must be rejected, not read past its end. + CHECK_FALSE(rosbags::decodeRos1PointCloud2(bytes.data(), bytes.size() - 10, out)); +} + +TEST_CASE("Ros1Bag: Imu encode/decode round-trip") +{ + const auto in = makeImu(3, 42'000'000'123ULL); + const auto bytes = rosbags::encodeRos1Imu(in); + // header (4 + 8 + 4 + 5) + 4 + 9 + 3 + 9 + 3 + 9 doubles + CHECK(bytes.size() == 21 + 37 * 8); + rosbags::Ros1Imu out; + REQUIRE(rosbags::decodeRos1Imu(bytes.data(), bytes.size(), out)); + CHECK(out.stamp_ns == in.stamp_ns); + CHECK(out.orientation[3] == 1.0); + CHECK(out.orientation_covariance[0] == -1.0); + CHECK(out.angular_velocity[0] == in.angular_velocity[0]); + CHECK(out.angular_velocity[2] == in.angular_velocity[2]); + CHECK(out.linear_acceleration[2] == in.linear_acceleration[2]); +} + +TEST_CASE("Ros1Bag: writer output reads back message for message across chunks") +{ + const auto path = fs::temp_directory_path() / "hdmapping_test_ros1.bag"; + const uint64_t t0 = 1'710'255'656'000'000'000ULL; + std::vector> clouds, imus; + + { + // Small chunks, so the bag has many chunks and index records to get right. + rosbags::Ros1BagWriter writer(path, 4096); + REQUIRE(writer.isOpen()); + const auto lidar = writer.addConnection( + "/lidar/pointcloud", + rosbags::kRos1PointCloud2Type, + rosbags::kRos1PointCloud2Md5, + rosbags::kRos1PointCloud2Definition); + const auto imu = writer.addConnection( + "/lidar/imu", rosbags::kRos1ImuType, rosbags::kRos1ImuMd5, rosbags::kRos1ImuDefinition); + for (uint32_t i = 0; i < 20; ++i) + { + const uint64_t t = t0 + i * 100'000'000ULL; + clouds.push_back(rosbags::encodeRos1PointCloud2(makeCloud(i, t, 100))); + writer.write(lidar, t, clouds.back()); + for (uint32_t k = 0; k < 40; ++k) + { + const uint64_t ti = t + k * 2'500'000ULL; + imus.push_back(rosbags::encodeRos1Imu(makeImu(i * 40 + k, ti))); + writer.write(imu, ti, imus.back()); + } + } + REQUIRE(writer.close()); + } + + rosbags::Ros1BagReader reader(path); + REQUIRE(reader.isOpen()); + // Connections come from the index section, before any message is read. + REQUIRE(reader.connections().size() == 2); + CHECK(reader.connections().at(0).topic == "/lidar/pointcloud"); + CHECK(reader.connections().at(0).md5sum == rosbags::kRos1PointCloud2Md5); + CHECK(reader.connections().at(1).type == rosbags::kRos1ImuType); + CHECK(reader.connections().at(1).message_definition == rosbags::kRos1ImuDefinition); + + size_t n_cloud = 0, n_imu = 0; + uint64_t last_cloud_time = 0; + const bool ok = reader.forEachMessage( + [&](const rosbags::Ros1Connection& c, uint64_t time_ns, const uint8_t* data, size_t size) + { + const std::vector bytes(data, data + size); + if (c.topic == "/lidar/pointcloud") + { + REQUIRE(n_cloud < clouds.size()); + CHECK(bytes == clouds[n_cloud]); + CHECK(time_ns == t0 + n_cloud * 100'000'000ULL); + last_cloud_time = time_ns; + ++n_cloud; + } + else + { + REQUIRE(n_imu < imus.size()); + CHECK(bytes == imus[n_imu]); + ++n_imu; + } + return true; + }); + CHECK(ok); + CHECK(n_cloud == clouds.size()); + CHECK(n_imu == imus.size()); + CHECK(last_cloud_time == t0 + 19 * 100'000'000ULL); + + // Stopping early is honoured. + size_t seen = 0; + CHECK(reader.forEachMessage( + [&](const rosbags::Ros1Connection&, uint64_t, const uint8_t*, size_t) + { + return ++seen < 5; + })); + CHECK(seen == 5); + + fs::remove(path); +} + +TEST_CASE("Ros1Bag: reader rejects a file that is not a ROS1 bag") +{ + const auto path = fs::temp_directory_path() / "hdmapping_test_not_a_bag.bag"; + { + std::ofstream f(path, std::ios::binary); + f << "#ROSBAG V1.2\nnot a bag"; + } + rosbags::Ros1BagReader reader(path); + CHECK_FALSE(reader.isOpen()); + CHECK_FALSE(reader.error().empty()); + fs::remove(path); +} + +TEST_CASE("Ros1BagFileWriter -> Ros1BagFileReader: canonical layout and IMU round-trip") +{ + const auto path = fs::temp_directory_path() / "hdmapping_test_ros1_canonical.bag"; + const double t0 = 1'710'255'656.5; + std::vector> clouds(3); + for (size_t c = 0; c < clouds.size(); ++c) + { + for (uint32_t i = 0; i < 640; ++i) + { + rosbags::McapPoint p{}; + p.x = 0.01f * static_cast(i); + p.y = -0.02f * static_cast(i); + p.z = 1.5f; + p.intensity = static_cast(i % 200); + p.ring = static_cast(i % 64); + p.timestamp = t0 + 0.1 * static_cast(c) + 1e-4 * static_cast(i / 64); + clouds[c].push_back(p); + } + } + std::vector imu; + for (int k = 0; k < 120; ++k) + imu.push_back({t0 + 0.0025 * k, 0.01f * k, -0.02f, 0.3f, 0.1f, -0.2f, 9.81f}); + + { + rosbags::McapWriterOptions options; + options.lidar_topic = "/lidar/pointcloud"; + options.imu_topic = "/lidar/imu"; + options.lidar_layout = rosbags::PointCloudLayout::Canonical; + rosbags::Ros1BagFileWriter writer(path, options); + REQUIRE_MESSAGE(writer.isOpen(), writer.error()); + for (const auto& c : clouds) + writer.writePointCloud(static_cast(std::llround(c.front().timestamp * 1e9)), c); + writer.writeImu(imu); + REQUIRE(writer.close()); + } + + // Topics are auto-detected by type, as for MCAP. + rosbags::Ros1BagFileReader reader(path); + REQUIRE_MESSAGE(reader.isOpen(), reader.error()); + CHECK(reader.topics().lidar == "/lidar/pointcloud"); + CHECK(reader.topics().imu == "/lidar/imu"); + CHECK(reader.topics().lidar_schema == rosbags::LidarSchema::PointCloud2); + CHECK(reader.channels().size() == 2); // no empty /lidar_sn topic + + std::vector> got; + std::vector got_imu; + rosbags::Ros1BagFileReader::Callbacks callbacks; + callbacks.onPointCloud = [&](uint64_t, std::vector&& points) + { + got.push_back(std::move(points)); + }; + callbacks.onImu = [&](const rosbags::McapImuSample& s) + { + got_imu.push_back(s); + }; + REQUIRE(reader.read(callbacks)); + CHECK(reader.lidarLayoutWarning().empty()); + + REQUIRE(got.size() == clouds.size()); + for (size_t c = 0; c < clouds.size(); ++c) + { + REQUIRE(got[c].size() == clouds[c].size()); + for (size_t i = 0; i < clouds[c].size(); ++i) + { + CHECK(got[c][i].x == clouds[c][i].x); + CHECK(got[c][i].intensity == clouds[c][i].intensity); + CHECK(got[c][i].ring == clouds[c][i].ring); + // decodePc2Points() takes the float32 `time` (seconds since the first point), which + // resolves well below a microsecond over a 0.1 ms spread. + CHECK(got[c][i].timestamp == doctest::Approx(clouds[c][i].timestamp).epsilon(1e-12)); + } + } + REQUIRE(got_imu.size() == imu.size()); + for (size_t k = 0; k < imu.size(); ++k) + { + CHECK(got_imu[k].gyro_x == imu[k].gyro_x); + CHECK(got_imu[k].acc_z == imu[k].acc_z); + CHECK(got_imu[k].timestamp == doctest::Approx(imu[k].timestamp).epsilon(1e-12)); + } + + fs::remove(path); +} + +TEST_CASE("Ros1BagFileReader: livox_ros_driver CustomMsg becomes points with ring = line") +{ + const auto path = fs::temp_directory_path() / "hdmapping_test_ros1_livox.bag"; + const uint64_t timebase = 1'700'000'000'000'000'000ULL; + { + // ROS1 serialization of livox_ros_driver/CustomMsg with three points. + std::vector msg; + const auto put = [&msg](const void* v, size_t n) + { + const auto* b = static_cast(v); + msg.insert(msg.end(), b, b + n); + }; + const uint32_t seq = 0, sec = 1'700'000'000, nsec = 0, frame_len = 0; + put(&seq, 4); + put(&sec, 4); + put(&nsec, 4); + put(&frame_len, 4); + const uint32_t point_num = 3, n_points = 3; + const uint8_t lidar_id = 192, rsvd[3] = {0, 0, 0}; + put(&timebase, 8); + put(&point_num, 4); + put(&lidar_id, 1); + put(rsvd, 3); + put(&n_points, 4); + for (uint32_t i = 0; i < 3; ++i) + { + const uint32_t offset_time = i * 1000; + const float xyz[3] = {1.0f + i, 2.0f, 3.0f}; + const uint8_t reflectivity = static_cast(10 * i), tag = 0x10, line = static_cast(i); + put(&offset_time, 4); + put(xyz, 12); + put(&reflectivity, 1); + put(&tag, 1); + put(&line, 1); + } + rosbags::Ros1BagWriter writer(path); + const auto conn = writer.addConnection("/livox/lidar", "livox_ros_driver/CustomMsg", "e4d6829bdfe657cb6c21a746c86b21a6", ""); + writer.write(conn, timebase, msg); + REQUIRE(writer.close()); + } + + rosbags::Ros1BagFileReader reader(path); + REQUIRE_MESSAGE(reader.isOpen(), reader.error()); + CHECK(reader.topics().lidar == "/livox/lidar"); + CHECK(reader.topics().lidar_schema == rosbags::LidarSchema::LivoxCustomMsg); + std::vector points; + rosbags::Ros1BagFileReader::Callbacks callbacks; + callbacks.onPointCloud = [&](uint64_t, std::vector&& p) + { + points = std::move(p); + }; + REQUIRE(reader.read(callbacks)); + REQUIRE(points.size() == 3); + for (uint32_t i = 0; i < 3; ++i) + { + CHECK(points[i].x == 1.0f + i); + CHECK(points[i].ring == i); + CHECK(points[i].intensity == 10.0f * i); + CHECK(points[i].timestamp == doctest::Approx(1.7e9 + i * 1e-6).epsilon(1e-15)); + } + fs::remove(path); +}