diff --git a/apps/console_tools/CMakeLists.txt b/apps/console_tools/CMakeLists.txt index d4d8cb52..9fdab971 100644 --- a/apps/console_tools/CMakeLists.txt +++ b/apps/console_tools/CMakeLists.txt @@ -82,6 +82,39 @@ target_link_libraries( ${PLATFORM_MISCELLANEOUS_LIBS} ${CORE_LIBRARIES}) +# mcap_to_laz: the inverse of laz_to_mcap -- MCAP/ROS2 bag -> mandeye session +# directory. Needs neither lidar_odometry_utils.cpp nor core: reading is +# rosbags/McapReader and writing is rosbags/MandeyeSessionWriter, so laszip + +# mcap + spdlog is the whole dependency set. +# +# McapWriter.cpp is compiled in even though nothing here writes bags: it holds +# the single MCAP_IMPLEMENTATION definition, which McapReader.cpp deliberately +# does not repeat (that would be a duplicate-symbol link error in targets, like +# the unit tests, that use both). +add_executable( + mcap_to_laz mcap_to_laz.cpp + ${REPOSITORY_DIRECTORY}/rosbags/McapReader.h ${REPOSITORY_DIRECTORY}/rosbags/McapReader.cpp + ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.h ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.cpp + ${REPOSITORY_DIRECTORY}/rosbags/MandeyeSessionWriter.h ${REPOSITORY_DIRECTORY}/rosbags/MandeyeSessionWriter.cpp +) + +target_include_directories( + mcap_to_laz + PRIVATE ${REPOSITORY_DIRECTORY}/rosbags + ${LASZIP_INCLUDE_DIR}/LASzip/include) + +target_link_libraries( + mcap_to_laz + PRIVATE + mcap + spdlog::spdlog + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS}) + +if (MSVC) + target_compile_options(mcap_to_laz PRIVATE /bigobj) +endif() + if (MSVC) target_compile_options(laz_to_mcap PRIVATE /bigobj) endif() \ No newline at end of file diff --git a/apps/console_tools/mcap_to_laz.cpp b/apps/console_tools/mcap_to_laz.cpp new file mode 100644 index 00000000..ad7a06c4 --- /dev/null +++ b/apps/console_tools/mcap_to_laz.cpp @@ -0,0 +1,410 @@ +// MCAP/ROS2-bag -> mandeye session directory importer: the inverse of +// laz_to_mcap. +// +// Deliberately does not link lidar_odometry_utils.cpp. Nothing here needs +// load_point_cloud()/load_imu(); reading is handled by rosbags/McapReader and +// writing by rosbags/MandeyeSessionWriter, which keeps this tool free of +// core/TBB/glm/toml++/vqf. +#include "MandeyeSessionWriter.h" +#include "McapReader.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace fs = std::filesystem; + +namespace +{ + + // 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 + // two streams having to agree on a time origin -- which they cannot, since which + // stream a bag's first message belongs to depends on its read order. + int64_t timeBin(double timestamp_s, double chunk_seconds) + { + if (chunk_seconds <= 0.0) + return 0; // splitting disabled: everything lands in one chunk + return static_cast(std::floor(timestamp_s / chunk_seconds)); + } + + 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(" 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:"); + spdlog::error(" --chunk-seconds chunk length in seconds (default: 20, mandeye's own"); + spdlog::error(" cadence; 0 = one single chunk)"); + spdlog::error(" --lidar-topic PointCloud2 topic (default: the file's only one)"); + spdlog::error(" --imu-topic Imu topic (default: the file's only one)"); + spdlog::error(" --sn-topic serial-number String topic (default: the file's only one)"); + spdlog::error(" --lidar-type PointCloud2 record layout: auto|hesai|ouster (default: auto)"); + spdlog::error(" auto reads each message's own field offsets and datatypes and is"); + 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(" --list print the file's channels and exit"); + spdlog::error(""); + 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) + { + const auto& channels = reader.channels(); + if (channels.empty()) + { + spdlog::error("No channels found: {}", reader.error()); + return EXIT_FAILURE; + } + + spdlog::info("{} channel(s):", channels.size()); + for (const auto& c : channels) + spdlog::info(" {:<28} {:<32} encoding={:<10} messages={}", c.topic, c.schema_name, c.message_encoding, c.message_count); + + if (reader.isOpen()) + { + const auto& topics = reader.topics(); + spdlog::info("Resolved: lidar='{}' imu='{}' sn='{}'", topics.lidar, topics.imu, topics.sn); + } + else + { + spdlog::warn("Topics could not be resolved: {}", reader.error()); + } + return EXIT_SUCCESS; + } + + struct ImportStats + { + size_t lidar_messages = 0; + bool out_of_order = false; + }; + + // Reads every IMU sample (and the serial number, if present) into memory. The + // 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) + { + rosbags::McapFileReader::Callbacks callbacks; + callbacks.onImu = [&](const rosbags::McapImuSample& sample) + { + out.push_back(sample); + }; + callbacks.onSn = [&](uint64_t, const std::string& sn) + { + if (serial_number.empty()) + serial_number = sn; + }; + + if (!reader.read(callbacks)) + return false; + + std::sort( + out.begin(), + out.end(), + [](const auto& a, const auto& b) + { + return a.timestamp < b.timestamp; + }); + return true; + } + + // 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. + bool write_lidar_chunks( + rosbags::McapFileReader& reader, + rosbags::MandeyeSessionWriter& writer, + double chunk_seconds, + std::vector& chunk_bins, + ImportStats& stats) + { + bool ok = true; + int64_t current_bin = 0; + double last_stamp = -std::numeric_limits::infinity(); + + rosbags::McapFileReader::Callbacks callbacks; + callbacks.onPointCloud = [&](uint64_t /*log_time_ns*/, std::vector&& points) + { + if (!ok || points.empty()) + return; + + // Bin on the cloud's first point timestamp, not on the MCAP log time: + // point timestamps are what lands in the laz files and what IMU samples + // are binned by, so this keeps both streams on one clock. The cloud is + // the atomic unit -- splitting one across two laz files would be + // pointless churn given load_data() concatenates the chunks anyway. + const double stamp_s = points.front().timestamp; + if (stamp_s < last_stamp) + stats.out_of_order = true; + last_stamp = stamp_s; + + const int64_t bin = timeBin(stamp_s, chunk_seconds); + if (chunk_bins.empty() || bin != current_bin) + { + if (!writer.beginChunk(static_cast(chunk_bins.size()))) + { + ok = false; + return; + } + chunk_bins.push_back(bin); + current_bin = bin; + } + + if (!writer.addPoints(points)) + { + ok = false; + return; + } + ++stats.lidar_messages; + }; + + if (!reader.read(callbacks)) + return false; + if (!writer.endChunk()) + return false; + return ok; + } + + // Routes every buffered IMU sample into the chunk that was open at its + // timestamp: the last chunk whose bin is <= the sample's bin, clamped to the + // first chunk for samples that predate all lidar data. No sample is dropped -- + // load_data() concatenates every imuNNNN.csv into one stream, so which chunk a + // sample lands in only affects file layout, not the resulting IMU trajectory. + bool write_imu_chunks( + rosbags::MandeyeSessionWriter& writer, + const std::vector& samples, + const std::vector& chunk_bins, + double chunk_seconds) + { + std::vector> per_chunk(chunk_bins.size()); + + // chunk_bins is ascending for any bag whose clouds arrive in time order, + // but a file-order read of an out-of-order bag can break that -- and + // upper_bound on an unsorted range would route samples arbitrarily. Sort a + // (bin, chunk index) view instead of assuming. + std::vector> bin_to_chunk; + bin_to_chunk.reserve(chunk_bins.size()); + for (size_t i = 0; i < chunk_bins.size(); ++i) + bin_to_chunk.emplace_back(chunk_bins[i], i); + std::sort(bin_to_chunk.begin(), bin_to_chunk.end()); + + for (const auto& sample : samples) + { + const int64_t bin = timeBin(sample.timestamp, chunk_seconds); + const auto it = std::upper_bound( + bin_to_chunk.begin(), + bin_to_chunk.end(), + bin, + [](int64_t value, const std::pair& entry) + { + return value < entry.first; + }); + const size_t index = (it == bin_to_chunk.begin()) ? bin_to_chunk.front().second : std::prev(it)->second; + per_chunk[index].push_back(sample); + } + + for (size_t i = 0; i < per_chunk.size(); ++i) + { + if (!writer.writeImuChunk(static_cast(i), per_chunk[i])) + return false; + } + return true; + } + +} // namespace + +int main(const int argc, const char** argv) +{ + if (argc < 2) + { + print_usage(argv[0]); + return EXIT_FAILURE; + } + + const std::string mcap_path = argv[1]; + std::string session_dir; + rosbags::McapReaderOptions options; + double chunk_seconds = 20.0; + bool list_only = false; + + for (int i = 2; i < argc; ++i) + { + const std::string arg = argv[i]; + const bool hasValue = i + 1 < argc; + + if (arg == "--list") + list_only = true; + else if (arg == "--lidar-topic" && hasValue) + options.lidar_topic = argv[++i]; + else if (arg == "--imu-topic" && hasValue) + options.imu_topic = argv[++i]; + else if (arg == "--sn-topic" && hasValue) + options.sn_topic = argv[++i]; + else if (arg == "--lidar-type" && hasValue) + { + const std::string type = argv[++i]; + if (type == "auto") + options.lidar_preset = rosbags::Pc2Preset::Auto; + else if (type == "hesai") + options.lidar_preset = rosbags::Pc2Preset::Hesai; + else if (type == "ouster") + options.lidar_preset = rosbags::Pc2Preset::Ouster; + else + { + spdlog::error("Unknown --lidar-type '{}' (expected auto|hesai|ouster)", type); + return EXIT_FAILURE; + } + } + else if (arg == "--chunk-seconds" && hasValue) + { + const std::string value = argv[++i]; + try + { + chunk_seconds = std::stod(value); + } catch (const std::exception&) + { + spdlog::error("Invalid --chunk-seconds '{}' (expected a number)", value); + return EXIT_FAILURE; + } + if (!std::isfinite(chunk_seconds) || chunk_seconds < 0.0) + { + spdlog::error("Invalid --chunk-seconds '{}' (expected >= 0; 0 = one single chunk)", value); + return EXIT_FAILURE; + } + } + else if (!arg.starts_with("--") && session_dir.empty()) + session_dir = arg; + else + { + spdlog::error("Unrecognized argument '{}'", arg); + print_usage(argv[0]); + return EXIT_FAILURE; + } + } + + if (fs::path(mcap_path).extension() != ".mcap") + { + spdlog::error("Invalid extension for input file {} - expected .mcap", mcap_path); + return EXIT_FAILURE; + } + if (!fs::exists(mcap_path)) + { + spdlog::error("Input file {} does not exist", mcap_path); + return EXIT_FAILURE; + } + if (!list_only && session_dir.empty()) + { + spdlog::error("Missing "); + print_usage(argv[0]); + return EXIT_FAILURE; + } + + rosbags::McapFileReader reader(mcap_path, 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; + } + + const auto& topics = reader.topics(); + if (topics.lidar.empty()) + { + spdlog::error("No sensor_msgs/msg/PointCloud2 topic found in {}", mcap_path); + return EXIT_FAILURE; + } + 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); + + std::vector imu; + std::string serial_number; + if (!topics.imu.empty() && !collect_imu(reader, imu, serial_number)) + { + spdlog::error("Failed reading IMU stream: {}", reader.error()); + return EXIT_FAILURE; + } + if (!serial_number.empty()) + spdlog::info("Bag serial number: {}", serial_number); + spdlog::info("Read {} IMU sample(s)", imu.size()); + + rosbags::MandeyeSessionWriter writer(session_dir); + if (!writer.isOpen()) + { + spdlog::error("Cannot write session directory: {}", writer.error()); + return EXIT_FAILURE; + } + + std::vector chunk_bins; + ImportStats stats; + if (!write_lidar_chunks(reader, writer, chunk_seconds, chunk_bins, stats)) + { + spdlog::error("Failed writing lidar chunks: {}", writer.error().empty() ? reader.error() : writer.error()); + 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()); + + if (chunk_bins.empty()) + { + spdlog::error("No point cloud messages decoded from '{}' - nothing to write", topics.lidar); + if (options.lidar_preset != rosbags::Pc2Preset::Auto) + spdlog::error("Try --lidar-type auto, which takes the record layout from the messages themselves."); + return EXIT_FAILURE; + } + if (!write_imu_chunks(writer, imu, chunk_bins, chunk_seconds)) + { + spdlog::error("Failed writing IMU chunks: {}", writer.error()); + return EXIT_FAILURE; + } + + spdlog::info( + "Wrote {} chunk(s) to {}: {} point(s) from {} message(s), {} IMU sample(s)", + chunk_bins.size(), + session_dir, + writer.pointsWritten(), + stats.lidar_messages, + writer.imuSamplesWritten()); + + if (stats.out_of_order) + spdlog::warn("Point cloud messages were not in ascending time order - chunk boundaries may not be monotonic"); + + // 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 + // chunks contributes little or nothing there. + if (chunk_bins.size() < 3) + spdlog::warn( + "Only {} chunk(s): lidar_odometry_step_1 requires more than two files and discards the first chunk's " + "points - lower --chunk-seconds to split this bag further", + chunk_bins.size()); + + if (!imu.empty()) + spdlog::info( + "IMU timestampUnix is written as 0: the bag format carries only one IMU timestamp. It feeds the step-1 " + "trajectory csv only."); + + return EXIT_SUCCESS; +} diff --git a/rosbags/MandeyeSessionWriter.cpp b/rosbags/MandeyeSessionWriter.cpp new file mode 100644 index 00000000..8aff3f13 --- /dev/null +++ b/rosbags/MandeyeSessionWriter.cpp @@ -0,0 +1,225 @@ +#include "MandeyeSessionWriter.h" + +#include + +#include +#include +#include +#include +#include +#include + +namespace rosbags +{ + +namespace +{ + +// Same scale mandeye/HDMapping LAZ files use (see LazWriter::open in +// core/include/Core/export_laz.h): 0.1 mm, i.e. an int32 X/Y/Z spans +-214 km, +// far beyond any sensor-local coordinate. +constexpr double kCoordScale = 0.0001; + +std::string chunkFileName(const char* prefix, int index, const char* ext) +{ + std::array buffer{}; + std::snprintf(buffer.data(), buffer.size(), "%s%04d%s", prefix, index, ext); + return std::string(buffer.data()); +} + +} // namespace + +struct MandeyeSessionWriter::Impl +{ + std::filesystem::path dir; + std::string error; + bool open = false; + + laszip_POINTER writer = nullptr; + laszip_point* point = nullptr; + int currentChunk = -1; + + uint64_t points = 0; + uint64_t imuSamples = 0; + + bool fail(std::string message) + { + if(error.empty()) + error = std::move(message); + return false; + } +}; + +MandeyeSessionWriter::MandeyeSessionWriter(const std::filesystem::path& dir) + : impl_(std::make_unique()) +{ + impl_->dir = dir; + + std::error_code ec; + std::filesystem::create_directories(dir, ec); + if(ec) + { + impl_->fail("failed to create output directory " + dir.string() + ": " + ec.message()); + return; + } + if(!std::filesystem::is_directory(dir)) + { + impl_->fail(dir.string() + " exists but is not a directory"); + return; + } + + impl_->open = true; +} + +MandeyeSessionWriter::~MandeyeSessionWriter() +{ + if(impl_ && impl_->writer) + endChunk(); +} + +bool MandeyeSessionWriter::isOpen() const +{ + return impl_ && impl_->open; +} + +const std::string& MandeyeSessionWriter::error() const +{ + return impl_->error; +} + +bool MandeyeSessionWriter::beginChunk(int index) +{ + if(!isOpen()) + return false; + if(impl_->writer && !endChunk()) + return false; + + const auto path = impl_->dir / chunkFileName("lidar", index, ".laz"); + + if(laszip_create(&impl_->writer)) + { + impl_->writer = nullptr; + return impl_->fail("failed to create laszip writer"); + } + + laszip_header* header = nullptr; + if(laszip_get_header_pointer(impl_->writer, &header)) + return impl_->fail("failed to get laszip header pointer"); + + header->file_source_ID = 4711; + header->global_encoding = (1 << 0); + header->version_major = 1; + header->version_minor = 2; + header->point_data_format = 1; // XYZ + intensity + user_data + gps_time + header->point_data_record_length = 28; + header->number_of_point_records = 0; + header->number_of_points_by_return[0] = 0; + header->number_of_points_by_return[1] = 0; + header->x_scale_factor = kCoordScale; + header->y_scale_factor = kCoordScale; + header->z_scale_factor = kCoordScale; + // Bag points are sensor-local, so no georeferencing offset is applied. + header->x_offset = 0.0; + header->y_offset = 0.0; + header->z_offset = 0.0; + + if(laszip_open_writer(impl_->writer, path.string().c_str(), /*compress=*/1)) + return impl_->fail("failed to open laszip writer for " + path.string()); + + if(laszip_get_point_pointer(impl_->writer, &impl_->point)) + return impl_->fail("failed to get laszip point pointer"); + + impl_->currentChunk = index; + return true; +} + +bool MandeyeSessionWriter::addPoints(const std::vector& points) +{ + if(!isOpen()) + return false; + if(!impl_->writer) + return impl_->fail("addPoints() called with no chunk open"); + + for(const auto& p : points) + { + impl_->point->intensity = static_cast(std::clamp(p.intensity, 0.0f, 65535.0f)); + impl_->point->return_number = 1; + impl_->point->number_of_returns = 1; + // Unscaled: load_point_cloud() assigns p.timestamp = point->gps_time directly. + impl_->point->gps_time = p.timestamp; + // Single-lidar sessions only, so the lidar id load_point_cloud() reads out of + // 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; + + laszip_F64 coordinates[3] = {p.x, p.y, p.z}; + if(laszip_set_coordinates(impl_->writer, coordinates)) + return impl_->fail("failed to set laszip coordinates"); + if(laszip_write_point(impl_->writer)) + return impl_->fail("failed to write laszip point"); + if(laszip_update_inventory(impl_->writer)) + return impl_->fail("failed to update laszip inventory"); + + ++impl_->points; + } + return true; +} + +bool MandeyeSessionWriter::endChunk() +{ + if(!impl_->writer) + return true; + + bool ok = true; + if(laszip_close_writer(impl_->writer)) + ok = impl_->fail("failed to close laszip writer for chunk " + std::to_string(impl_->currentChunk)); + if(laszip_destroy(impl_->writer)) + ok = impl_->fail("failed to destroy laszip writer for chunk " + std::to_string(impl_->currentChunk)); + + impl_->writer = nullptr; + impl_->point = nullptr; + impl_->currentChunk = -1; + return ok; +} + +bool MandeyeSessionWriter::writeImuChunk(int index, const std::vector& samples) +{ + if(!isOpen()) + return false; + + const auto path = impl_->dir / chunkFileName("imu", index, ".csv"); + std::ofstream out(path); + if(!out.is_open()) + return impl_->fail("failed to open " + path.string()); + + // Column set and order per concatenate_multi_livox.cpp; load_imu() looks the + // columns up by name, so the order is only a convention. + out << "timestamp timestampUnix accX accY accZ gyroX gyroY gyroZ\n"; + out << std::setprecision(std::numeric_limits::max_digits10); + + for(const auto& s : samples) + { + // timestampUnix is 0: McapImuSample has no slot for load_imu()'s second + // (wall-clock) timestamp, so the exporter never carried it into the bag. + out << static_cast(s.timestamp * 1e9) << " " << 0 << " " << s.acc_x << " " << s.acc_y << " " << s.acc_z << " " << s.gyro_x + << " " << s.gyro_y << " " << s.gyro_z << "\n"; + ++impl_->imuSamples; + } + + out.flush(); + if(!out) + return impl_->fail("failed to write " + path.string()); + return true; +} + +uint64_t MandeyeSessionWriter::pointsWritten() const +{ + return impl_->points; +} + +uint64_t MandeyeSessionWriter::imuSamplesWritten() const +{ + return impl_->imuSamples; +} + +} // namespace rosbags diff --git a/rosbags/MandeyeSessionWriter.h b/rosbags/MandeyeSessionWriter.h new file mode 100644 index 00000000..84d488f2 --- /dev/null +++ b/rosbags/MandeyeSessionWriter.h @@ -0,0 +1,61 @@ +#pragma once +#include "McapWriter.h" // McapPoint, McapImuSample +#include +#include +#include +#include +#include + +namespace rosbags +{ + +// 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. +// +// The LAZ side deliberately does NOT go through export_laz.h's LazWriter, which +// stores gps_time as `timestamp * 1e9` -- whereas load_point_cloud() +// (lidar_odometry_utils.cpp) reads timestamps straight out of gps_time. Reusing +// it would produce a session whose timestamps are off by a factor of 1e9. +// +// Point record layout written here, matching what load_point_cloud() reads back: +// gps_time = McapPoint::timestamp (absolute seconds, unscaled) +// user_data = 0 (lidar id; single sensor per session) +// intensity = McapPoint::intensity (clamped to uint16) +// +// 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 +// integer nanoseconds (load_imu divides by 1e9). +class MandeyeSessionWriter +{ +public: + // Creates `dir` if it does not exist. + explicit MandeyeSessionWriter(const std::filesystem::path& dir); + ~MandeyeSessionWriter(); + + // Non-copyable + MandeyeSessionWriter(const MandeyeSessionWriter&) = delete; + MandeyeSessionWriter& operator=(const MandeyeSessionWriter&) = delete; + + bool isOpen() const; + const std::string& error() const; + + // Opens lidar.laz. Any chunk still open is closed first. + bool beginChunk(int index); + bool addPoints(const std::vector& points); + bool endChunk(); + + // Writes imu.csv in one shot. Samples are written in the order + // given; an empty vector still produces a header-only file, because + // load_data() reports a missing csv for every laz it cannot pair. + bool writeImuChunk(int index, const std::vector& samples); + + uint64_t pointsWritten() const; + uint64_t imuSamplesWritten() const; + +private: + struct Impl; + std::unique_ptr impl_; +}; + +} // namespace rosbags diff --git a/rosbags/McapReader.cpp b/rosbags/McapReader.cpp new file mode 100644 index 00000000..5db5c984 --- /dev/null +++ b/rosbags/McapReader.cpp @@ -0,0 +1,237 @@ +// No MCAP_IMPLEMENTATION define here: McapWriter.cpp already provides the mcap +// library's single implementation, and every target that compiles McapReader.cpp +// also compiles McapWriter.cpp (see apps/console_tools/CMakeLists.txt and +// rosbags/tests/CMakeLists.txt). Defining it in both would be a duplicate-symbol +// link error. +#include "McapReader.h" +#include "cdr_serializer.hpp" + +#include + +#include +#include +#include + +namespace rosbags +{ + +namespace +{ + +// ros2 schema names are "sensor_msgs/msg/PointCloud2"; ros1-style bags spell the +// same type "sensor_msgs/PointCloud2". Comparing only the trailing type name +// accepts both. +std::string_view schemaTypeName(std::string_view schema_name) +{ + const auto slash = schema_name.rfind('/'); + return slash == std::string_view::npos ? schema_name : schema_name.substr(slash + 1); +} + +} // namespace + +struct McapFileReader::Impl +{ + mcap::McapReader reader; + McapReaderOptions options; + ResolvedTopics resolved; + std::vector channels; + std::string error; + std::string lidarLayoutWarning; + bool open = false; + bool timeOrdered = false; + + // Resolves one stream to a topic name. `requested` non-empty means "use this + // topic, and fail if it isn't in the file"; empty means auto-detect the + // single channel carrying `type_name`. `required_unique` streams error out on + // ambiguity, optional ones give up quietly. + bool resolveTopic(const std::string& requested, std::string_view type_name, bool required_unique, std::string& out) + { + if(!requested.empty()) + { + const auto it = std::find_if(channels.begin(), channels.end(), [&](const ChannelInfo& c) { return c.topic == requested; }); + if(it == channels.end()) + { + error = "topic '" + requested + "' is not present in the file"; + return false; + } + if(it->message_encoding != "cdr") + { + error = "topic '" + requested + "' uses message encoding '" + it->message_encoding + "', expected 'cdr'"; + return false; + } + out = requested; + return true; + } + + std::vector candidates; + for(const auto& c : channels) + { + if(schemaTypeName(c.schema_name) == type_name && c.message_encoding == "cdr") + candidates.push_back(&c); + } + + if(candidates.empty()) + return true; // absent stream, `out` stays empty + if(candidates.size() > 1) + { + if(!required_unique) + return true; + error = "found " + std::to_string(candidates.size()) + " " + std::string(type_name) + " 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; + } +}; + +McapFileReader::McapFileReader(const std::filesystem::path& path, const McapReaderOptions& options) + : impl_(std::make_unique()) +{ + impl_->options = options; + + auto status = impl_->reader.open(path.string()); + if(!status.ok()) + { + impl_->error = "failed to open " + path.string() + ": " + status.message; + return; + } + + // AllowFallbackScan so files written without a summary section (or truncated + // mid-recording) still enumerate their channels. + status = impl_->reader.readSummary(mcap::ReadSummaryMethod::AllowFallbackScan); + if(!status.ok()) + { + impl_->error = "failed to read summary of " + path.string() + ": " + status.message; + return; + } + + const auto& schemas = impl_->reader.schemas(); + const auto& statistics = impl_->reader.statistics(); + for(const auto& [id, channel] : impl_->reader.channels()) + { + ChannelInfo info; + info.topic = channel->topic; + info.message_encoding = channel->messageEncoding; + if(const auto it = schemas.find(channel->schemaId); it != schemas.end()) + info.schema_name = it->second->name; + if(statistics) + { + if(const auto it = statistics->channelMessageCounts.find(id); it != statistics->channelMessageCounts.end()) + info.message_count = it->second; + } + impl_->channels.push_back(std::move(info)); + } + std::sort(impl_->channels.begin(), impl_->channels.end(), [](const ChannelInfo& a, const ChannelInfo& b) { return a.topic < b.topic; }); + + if(!impl_->resolveTopic(options.lidar_topic, "PointCloud2", /*required_unique=*/true, impl_->resolved.lidar) || + !impl_->resolveTopic(options.imu_topic, "Imu", /*required_unique=*/true, impl_->resolved.imu) || + !impl_->resolveTopic(options.sn_topic, "String", /*required_unique=*/false, impl_->resolved.sn)) + return; + + // Ascending-log-time iteration needs per-chunk message indexes; this mirrors + // the precondition mcap's own IndexedMessageReader checks, so testing it here + // lets us fall back to file order instead of failing mid-iteration. + const auto& chunkIndexes = impl_->reader.chunkIndexes(); + impl_->timeOrdered = !chunkIndexes.empty() && chunkIndexes.front().messageIndexLength != 0; + + impl_->open = true; +} + +McapFileReader::~McapFileReader() +{ + if(impl_) + impl_->reader.close(); +} + +bool McapFileReader::isOpen() const +{ + return impl_ && impl_->open; +} + +const std::string& McapFileReader::error() const +{ + return impl_->error; +} + +const ResolvedTopics& McapFileReader::topics() const +{ + return impl_->resolved; +} + +const std::vector& McapFileReader::channels() const +{ + return impl_->channels; +} + +bool McapFileReader::timeOrdered() const +{ + return impl_ && impl_->timeOrdered; +} + +const std::string& McapFileReader::lidarLayoutWarning() const +{ + return impl_->lidarLayoutWarning; +} + +bool McapFileReader::read(const Callbacks& callbacks) +{ + if(!isOpen()) + return false; + + const std::string& lidarTopic = impl_->resolved.lidar; + const std::string& imuTopic = impl_->resolved.imu; + const std::string& snTopic = impl_->resolved.sn; + + const bool wantLidar = callbacks.onPointCloud && !lidarTopic.empty(); + const bool wantImu = callbacks.onImu && !imuTopic.empty(); + const bool wantSn = callbacks.onSn && !snTopic.empty(); + if(!wantLidar && !wantImu && !wantSn) + return true; + + mcap::ReadMessageOptions read_options; + read_options.readOrder = impl_->timeOrdered ? mcap::ReadMessageOptions::ReadOrder::LogTimeOrder + : mcap::ReadMessageOptions::ReadOrder::FileOrder; + read_options.topicFilter = [&](std::string_view topic) { + return (wantLidar && topic == lidarTopic) || (wantImu && topic == imuTopic) || (wantSn && topic == snTopic); + }; + + bool failed = false; + const auto onProblem = [&](const mcap::Status& status) { + if(impl_->error.empty()) + impl_->error = status.message; + failed = true; + }; + + auto messages = impl_->reader.readMessages(onProblem, read_options); + for(const auto& view : messages) + { + const auto* data = reinterpret_cast(view.message.data); + const size_t size = view.message.dataSize; + const std::string& topic = view.channel->topic; + + if(wantLidar && topic == lidarTopic) + { + // Only the first complaint is kept: a layout problem is a property of + // the file, so it would otherwise repeat once per message. + auto points = decodePc2(data, size, impl_->options.lidar_preset, &impl_->lidarLayoutWarning); + if(!points.empty()) + callbacks.onPointCloud(view.message.logTime, std::move(points)); + } + else if(wantImu && topic == imuTopic) + { + if(const auto sample = decodeImu(data, size)) + callbacks.onImu(*sample); + } + else if(wantSn && topic == snTopic) + { + callbacks.onSn(view.message.logTime, decodeSn(data, size)); + } + } + + return !failed; +} + +} // namespace rosbags diff --git a/rosbags/McapReader.h b/rosbags/McapReader.h new file mode 100644 index 00000000..08b51b6b --- /dev/null +++ b/rosbags/McapReader.h @@ -0,0 +1,112 @@ +#pragma once +#include "McapWriter.h" // McapPoint, McapImuSample -- the same structs the writer consumes +#include "cdr_serializer.hpp" // Pc2Preset +#include +#include +#include +#include +#include +#include + +namespace rosbags +{ + +// Topic selection. An empty name means "auto-detect": the reader looks for +// exactly one channel whose schema is the matching ros2 message type. More than +// one candidate is an error for the lidar/IMU streams (picking one silently +// would quietly export the wrong sensor) but not for the serial-number stream, +// which is cosmetic and simply stays unresolved. +struct McapReaderOptions +{ + std::string lidar_topic; // sensor_msgs/msg/PointCloud2 + std::string imu_topic; // sensor_msgs/msg/Imu + std::string sn_topic; // std_msgs/msg/String + + // How PointCloud2 point records are unpacked. Auto reads the layout from each + // message's own `fields` array and is right for any bag that fills it in + // honestly; a preset forces a known driver layout for bags that do not. + Pc2Preset lidar_preset = Pc2Preset::Auto; +}; + +// The topics actually in use after auto-detection; empty means "not present". +struct ResolvedTopics +{ + std::string lidar; + std::string imu; + std::string sn; +}; + +// One channel as found in the file, for --list output and diagnostics. +struct ChannelInfo +{ + std::string topic; + std::string schema_name; + std::string message_encoding; + uint64_t message_count = 0; // 0 if the file carries no statistics record +}; + +// Reads ros2msg/CDR-encoded MCAP files back into the McapPoint/McapImuSample +// structs McapFileWriter writes, i.e. the inverse of McapFileWriter. +// +// Decoding is delegated to decodePc2()/decodeImu()/decodeSn() in +// cdr_serializer.hpp, which parse the PointCloud2 `fields` array at runtime and +// therefore handle every PointCloudLayout the writer can emit. +// +// Messages are delivered through callbacks rather than returned in bulk: a real +// recording is far too large to hold in memory, so callers see one message at a +// time. Delivery is in ascending log time when the file has message indexes +// (see timeOrdered()), otherwise in file order. +class McapFileReader +{ +public: + explicit McapFileReader(const std::filesystem::path& path, const McapReaderOptions& options = {}); + ~McapFileReader(); + + // Non-copyable + McapFileReader(const McapFileReader&) = delete; + McapFileReader& operator=(const McapFileReader&) = delete; + + // False if the file could not be opened or a requested/auto-detected topic + // could not be resolved; error() then says why. + bool isOpen() const; + const std::string& error() const; + + const ResolvedTopics& topics() const; + const std::vector& channels() const; + + // True when read() delivers messages in ascending log time. False means the + // file has no message indexes and messages arrive in file order instead. + bool timeOrdered() const; + + // The first point-cloud decoding complaint seen during read(), or empty. + // Most importantly, this is where a forced lidar_preset that disagrees with + // the file's own field descriptions gets reported: decoding still proceeds + // with the preset, so the caller must surface this rather than ignore it. + const std::string& lidarLayoutWarning() const; + + // Any callback may be left empty; its topic is then skipped without being + // decoded. + // + // `log_time_ns` is the message's MCAP log time -- the recorder's clock, not the + // sensor clock the payload timestamps are on. Prefer + // McapPoint/McapImuSample::timestamp for anything that has to agree with the + // data. + // + // onPointCloud is only called with a non-empty vector. + struct Callbacks + { + std::function&&)> onPointCloud; + std::function onImu; + std::function onSn; + }; + + // Streams the whole file through `callbacks`. Returns false on a read error + // (see error()); a file with no matching messages is not an error. + bool read(const Callbacks& callbacks); + +private: + struct Impl; + std::unique_ptr impl_; +}; + +} // namespace rosbags diff --git a/rosbags/McapWriter.h b/rosbags/McapWriter.h index 85fd4873..1291feee 100644 --- a/rosbags/McapWriter.h +++ b/rosbags/McapWriter.h @@ -2,8 +2,8 @@ // cmake/cpu_optimizations.cmake defines _HAS_STD_BYTE=0 project-wide for // MSVC (worked around a std::byte/pybind11 conflict on Windows -- see commit -// 2af71be); mcap's headers (pulled in transitively by McapWriter.cpp and -// rosbags/tests/test_mcap_writer.cpp) genuinely need real std::byte, so +// 2af71be); mcap's headers (pulled in transitively by McapWriter.cpp, +// McapReader.cpp and rosbags/tests/*) genuinely need real std::byte, so // re-enable it here before any standard header can lock the disabled value // in for these translation units. #if defined(_MSC_VER) diff --git a/rosbags/cdr_serializer.hpp b/rosbags/cdr_serializer.hpp index bdfdf622..b0e28210 100644 --- a/rosbags/cdr_serializer.hpp +++ b/rosbags/cdr_serializer.hpp @@ -222,16 +222,271 @@ class CdrReader namespace rosbags { +// sensor_msgs/msg/PointField datatype codes. None is our own "field absent" +// marker, not part of the ROS message. +enum class Pc2Type : uint8_t +{ + None = 0, + Int8 = 1, + UInt8 = 2, + Int16 = 3, + UInt16 = 4, + Int32 = 5, + UInt32 = 6, + Float32 = 7, + Float64 = 8, +}; + +inline uint32_t pc2TypeSize(Pc2Type type) +{ + switch(type) + { + case Pc2Type::Int8: + case Pc2Type::UInt8: return 1; + case Pc2Type::Int16: + case Pc2Type::UInt16: return 2; + case Pc2Type::Int32: + case Pc2Type::UInt32: + case Pc2Type::Float32: return 4; + case Pc2Type::Float64: return 8; + case Pc2Type::None: + default: return 0; + } +} + +struct Pc2Field +{ + uint32_t offset = 0; + Pc2Type type = Pc2Type::None; + + bool present() const + { + return type != Pc2Type::None; + } +}; + +// Reads one field out of a point record and widens it to double, so callers do +// not care which of the eight PointField datatypes a driver happened to use. +inline double readPc2Field(const uint8_t* point, const Pc2Field& field) +{ + const uint8_t* p = point + field.offset; + switch(field.type) + { + case Pc2Type::Int8: + { + int8_t v = 0; + std::memcpy(&v, p, 1); + return static_cast(v); + } + case Pc2Type::UInt8: return static_cast(*p); + case Pc2Type::Int16: + { + int16_t v = 0; + std::memcpy(&v, p, 2); + return static_cast(v); + } + case Pc2Type::UInt16: + { + uint16_t v = 0; + std::memcpy(&v, p, 2); + return static_cast(v); + } + case Pc2Type::Int32: + { + int32_t v = 0; + std::memcpy(&v, p, 4); + return static_cast(v); + } + case Pc2Type::UInt32: + { + uint32_t v = 0; + std::memcpy(&v, p, 4); + return static_cast(v); + } + case Pc2Type::Float32: + { + float v = 0; + std::memcpy(&v, p, 4); + return static_cast(v); + } + case Pc2Type::Float64: + { + double v = 0; + std::memcpy(&v, p, 8); + return v; + } + case Pc2Type::None: + default: return 0.0; + } +} + +// The PointCloud2 fields McapPoint can represent. Three mutually exclusive +// per-point time fields are recognized, distinguished by name because the +// message carries no unit metadata: +// timestamp - absolute seconds (Hesai convention) +// time - seconds relative to the message stamp +// t - relative; nanoseconds if an integer type, seconds if floating +// Everything else in a record (reflectivity, ambient, range, ...) is ignored. +struct Pc2Layout +{ + uint32_t point_step = 0; + Pc2Field x, y, z, intensity, ring, laser_id, time, t, timestamp; +}; + +// Selects how a PointCloud2's point records are interpreted. +// Auto - trust the message's own `fields` array (correct for every bag +// laz_to_mcap writes, and for any driver that fills it in honestly) +// Hesai - force the HesaiLidar_ROS_2.0 Pandar record layout +// Ouster - force the ouster_ros Point record layout +// The presets exist for bags whose `fields` array is missing, truncated, or +// mislabelled; Auto is otherwise strictly better, since it adapts per message. +enum class Pc2Preset +{ + Auto, + Hesai, + Ouster, +}; + +// Verified against a real HesaiLidar_ROS_2.0 recording (/hesai/pandar): the +// PCL-style 4-byte pad after z puts intensity at 16, not 12, and point_step is +// 48 -- notably NOT the same packing PointCloudLayout::Hesai writes. +inline Pc2Layout hesaiPandarLayout() +{ + Pc2Layout l; + l.point_step = 48; + l.x = {0, Pc2Type::Float32}; + l.y = {4, Pc2Type::Float32}; + l.z = {8, Pc2Type::Float32}; + l.intensity = {16, Pc2Type::Float32}; + l.timestamp = {24, Pc2Type::Float64}; + l.ring = {32, Pc2Type::UInt16}; + return l; +} + +// ouster_ros' Point struct: PCL_ADD_POINT4D (16 bytes) + intensity, t, +// reflectivity, ring, ambient, range, padded to 48. Transcribed from the +// driver's published layout rather than measured, and ring's width has moved +// between driver releases -- decodePc2 cross-checks any preset against the +// message's own fields array and reports a mismatch, so a wrong guess here +// surfaces as a warning instead of as silently shifted data. +inline Pc2Layout ousterRosLayout() +{ + Pc2Layout l; + l.point_step = 48; + l.x = {0, Pc2Type::Float32}; + l.y = {4, Pc2Type::Float32}; + l.z = {8, Pc2Type::Float32}; + l.intensity = {16, Pc2Type::Float32}; + l.t = {20, Pc2Type::UInt32}; + l.ring = {26, Pc2Type::UInt8}; + return l; +} + +// False for Auto, which has no fixed layout. +inline bool pc2PresetLayout(Pc2Preset preset, Pc2Layout& out) +{ + switch(preset) + { + case Pc2Preset::Hesai: out = hesaiPandarLayout(); return true; + case Pc2Preset::Ouster: out = ousterRosLayout(); return true; + case Pc2Preset::Auto: + default: return false; + } +} + +inline const char* pc2PresetName(Pc2Preset preset) +{ + switch(preset) + { + case Pc2Preset::Hesai: return "hesai"; + case Pc2Preset::Ouster: return "ouster"; + case Pc2Preset::Auto: + default: return "auto"; + } +} + +// Every present field must lie wholly inside one point record, or decoding +// would read into the following point (or off the end of the buffer for the +// last one). Only reachable with a forced preset whose point_step disagrees +// with the file. +inline bool pc2FieldFits(const Pc2Field& field, uint32_t point_step) +{ + if(!field.present()) + return true; + return static_cast(field.offset) + pc2TypeSize(field.type) <= point_step; +} + +inline bool pc2LayoutFits(const Pc2Layout& l) +{ + return pc2FieldFits(l.x, l.point_step) && pc2FieldFits(l.y, l.point_step) && pc2FieldFits(l.z, l.point_step) && + pc2FieldFits(l.intensity, l.point_step) && pc2FieldFits(l.ring, l.point_step) && pc2FieldFits(l.laser_id, l.point_step) && + pc2FieldFits(l.time, l.point_step) && pc2FieldFits(l.t, l.point_step) && pc2FieldFits(l.timestamp, l.point_step); +} + +namespace detail +{ + +// Appends a human-readable " vs " note for one +// field whose preset and message descriptions disagree. +inline void describePc2Mismatch(const char* name, const Pc2Field& preset, const Pc2Field& message, std::string& out) +{ + if(preset.offset == message.offset && preset.type == message.type) + return; + if(!preset.present() && !message.present()) + return; + if(!out.empty()) + out += ", "; + out += name; + out += " preset@" + std::to_string(preset.offset) + "/type" + std::to_string(static_cast(preset.type)); + out += " vs file@" + std::to_string(message.offset) + "/type" + std::to_string(static_cast(message.type)); +} + +inline std::string comparePc2Layouts(const Pc2Layout& preset, const Pc2Layout& message) +{ + std::string diff; + describePc2Mismatch("x", preset.x, message.x, diff); + describePc2Mismatch("y", preset.y, message.y, diff); + describePc2Mismatch("z", preset.z, message.z, diff); + describePc2Mismatch("intensity", preset.intensity, message.intensity, diff); + describePc2Mismatch("ring", preset.ring, message.ring, diff); + describePc2Mismatch("laser_id", preset.laser_id, message.laser_id, diff); + describePc2Mismatch("time", preset.time, message.time, diff); + describePc2Mismatch("t", preset.t, message.t, diff); + describePc2Mismatch("timestamp", preset.timestamp, message.timestamp, diff); + if(preset.point_step != message.point_step) + { + if(!diff.empty()) + diff += ", "; + diff += "point_step preset=" + std::to_string(preset.point_step) + " vs file=" + std::to_string(message.point_step); + } + return diff; +} + +} // namespace detail + // sensor_msgs/msg/PointCloud2 → std::vector -// Decodes by field name/offset (as parsed from the message's own `fields` -// array) rather than a fixed per-layout struct, since several -// PointCloudLayout variants share the same point_step (26 bytes) but place -// different fields at different offsets. Recognizes the field names -// McapWriter emits for any PointCloudLayout: x,y,z,intensity (always), -// ring, laser_id, time (relative f64), t (relative u32 ns, Ouster), -// timestamp (absolute f64, Hesai). Unrecognized fields are ignored. -inline std::vector decodePc2(const uint8_t* data, size_t size) +// +// By default (Pc2Preset::Auto) the record layout is taken from the message's own +// `fields` array -- offsets *and* datatypes -- so one decoder handles every +// PointCloudLayout McapWriter emits as well as foreign driver bags, which pack +// the same field names at different offsets and widths. Recognized names are +// x, y, z (required), intensity, ring, laser_id, and the three time fields +// described on Pc2Layout; anything else is ignored. +// +// Passing a preset instead forces that driver's layout and ignores the file's +// fields array, for bags whose array is missing or wrong. When the file does +// describe its fields, the preset is still compared against it and any +// disagreement is reported through `warning` -- forcing a layout that does not +// match the data is the one way this decoder can silently produce plausible +// nonsense, so it is never done quietly. +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); + }; + std::vector points; CdrReader r(data, size); @@ -242,89 +497,126 @@ inline std::vector decodePc2(const uint8_t* data, size_t size) const double stamp_s = static_cast(stamp_sec) + static_cast(stamp_nsec) * 1e-9; r.read_u32(); // height - const uint32_t width = r.read_u32(); - - struct FieldInfo - { - uint32_t offset; - bool present = false; - }; - FieldInfo xF, yF, zF, intensityF, ringF, laserIdF, timeF, tF, timestampF; + 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(); - r.read_u8(); // datatype (implied by field name for our own writer's output) - r.read_u32(); // count + const auto type = static_cast(r.read_u8()); + const uint32_t count = r.read_u32(); - FieldInfo info{offset, true}; + // 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") - xF = info; + msg.x = field; else if(name == "y") - yF = info; + msg.y = field; else if(name == "z") - zF = info; + msg.z = field; else if(name == "intensity") - intensityF = info; + msg.intensity = field; else if(name == "ring") - ringF = info; + msg.ring = field; else if(name == "laser_id") - laserIdF = info; + msg.laser_id = field; else if(name == "time") - timeF = info; + msg.time = field; else if(name == "t") - tF = info; + msg.t = field; else if(name == "timestamp") - timestampF = info; + msg.timestamp = field; } - r.read_bool(); - const uint32_t point_step = r.read_u32(); + 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 || point_step == 0 || !xF.present || !yF.present || !zF.present || !intensityF.present) + 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; + } - const uint32_t nPts = dataLen / point_step; - (void)width; + Pc2Layout layout; + if(pc2PresetLayout(preset, layout)) + { + if(nFields > 0) + { + const std::string diff = detail::comparePc2Layouts(layout, msg); + if(!diff.empty()) + warn(std::string("forced '") + pc2PresetName(preset) + "' layout disagrees with the file's own fields (" + diff + ")"); + } + } + else + { + layout = msg; + } + + if(layout.point_step == 0) + { + warn("PointCloud2 has a zero point_step"); + return points; + } + if(!layout.x.present() || !layout.y.present() || !layout.z.present()) + { + warn("PointCloud2 has no x/y/z fields"); + return points; + } + if(!pc2LayoutFits(layout)) + { + warn(std::string("'") + pc2PresetName(preset) + "' layout has fields reaching past point_step " + + std::to_string(layout.point_step)); + return points; + } + + const uint32_t nPts = dataLen / layout.point_step; points.reserve(nPts); for(uint32_t i = 0; i < nPts; ++i) { - const uint8_t* p = rawData + i * point_step; + const uint8_t* p = rawData + static_cast(i) * layout.point_step; McapPoint pt{}; - std::memcpy(&pt.x, p + xF.offset, 4); - std::memcpy(&pt.y, p + yF.offset, 4); - std::memcpy(&pt.z, p + zF.offset, 4); - std::memcpy(&pt.intensity, p + intensityF.offset, 4); - - if(ringF.present) - std::memcpy(&pt.ring, p + ringF.offset, 2); - if(laserIdF.present) - std::memcpy(&pt.laser_id, p + laserIdF.offset, 1); - - if(timestampF.present) + pt.x = static_cast(readPc2Field(p, layout.x)); + pt.y = static_cast(readPc2Field(p, layout.y)); + pt.z = static_cast(readPc2Field(p, layout.z)); + if(layout.intensity.present()) + pt.intensity = static_cast(readPc2Field(p, layout.intensity)); + if(layout.ring.present()) + pt.ring = static_cast(readPc2Field(p, layout.ring)); + if(layout.laser_id.present()) + pt.laser_id = static_cast(readPc2Field(p, layout.laser_id)); + + if(layout.timestamp.present()) { - // Absolute timestamp (Hesai) -- no offset from the message stamp. - std::memcpy(&pt.timestamp, p + timestampF.offset, 8); + pt.timestamp = readPc2Field(p, layout.timestamp); } - else if(timeF.present) + else if(layout.time.present()) { - double rel_time = 0.0; - std::memcpy(&rel_time, p + timeF.offset, 8); - pt.timestamp = stamp_s + rel_time; + pt.timestamp = stamp_s + readPc2Field(p, layout.time); } - else if(tF.present) + else if(layout.t.present()) { - uint32_t rel_ns = 0; - std::memcpy(&rel_ns, p + tF.offset, 4); - pt.timestamp = stamp_s + static_cast(rel_ns) * 1e-9; + // Integer t is nanoseconds (ouster_ros); floating t is seconds. + const double raw = readPc2Field(p, layout.t); + const bool integral = layout.t.type != Pc2Type::Float32 && layout.t.type != Pc2Type::Float64; + pt.timestamp = stamp_s + (integral ? raw * 1e-9 : raw); } else { diff --git a/rosbags/tests/CMakeLists.txt b/rosbags/tests/CMakeLists.txt index ebfee54e..d0ac8dff 100644 --- a/rosbags/tests/CMakeLists.txt +++ b/rosbags/tests/CMakeLists.txt @@ -2,20 +2,28 @@ cmake_minimum_required(VERSION 4.0.0) project(hdmapping_rosbags_tests) -# Unit tests for rosbags/ (LAZ/IMU -> MCAP/ROS2-bag exporter support code). -# Uses doctest, same as shared/tests -- see that CMakeLists.txt for why. +# Unit tests for rosbags/ (LAZ/IMU <-> MCAP/ROS2-bag support code, both +# directions). 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 ${CMAKE_CURRENT_SOURCE_DIR}/../McapWriter.h ${CMAKE_CURRENT_SOURCE_DIR}/../McapWriter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../McapReader.h + ${CMAKE_CURRENT_SOURCE_DIR}/../McapReader.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../MandeyeSessionWriter.h + ${CMAKE_CURRENT_SOURCE_DIR}/../MandeyeSessionWriter.cpp ) target_include_directories(hdmapping_rosbags_tests PRIVATE ${THIRDPARTY_DIRECTORY}/doctest ${CMAKE_CURRENT_SOURCE_DIR}/.. + ${LASZIP_INCLUDE_DIR}/LASzip/include ) -target_link_libraries(hdmapping_rosbags_tests PRIVATE mcap) +target_link_libraries(hdmapping_rosbags_tests PRIVATE mcap ${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 new file mode 100644 index 00000000..8ce96fb5 --- /dev/null +++ b/rosbags/tests/test_mcap_reader.cpp @@ -0,0 +1,787 @@ +// See rosbags/McapWriter.h for why this needs to come before any standard +// header (including , which pulls in plenty of its own) gets a +// chance to lock in the project-wide _HAS_STD_BYTE=0 MSVC workaround. +#if defined(_MSC_VER) +# undef _HAS_STD_BYTE +# define _HAS_STD_BYTE 1 +#endif + +// No DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN here: test_mcap_writer.cpp (linked into +// the same binary) provides doctest's main once. +#include + +#include "MandeyeSessionWriter.h" +#include "McapReader.h" +#include "McapWriter.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fs = std::filesystem; + +namespace +{ + +fs::path tempPath(const char* name) +{ + return fs::temp_directory_path() / name; +} + +std::vector makeReaderPoints(size_t n, double t0) +{ + std::mt19937 rng(7); + std::uniform_real_distribution coord(-50.0f, 50.0f); + + std::vector points; + points.reserve(n); + for (size_t i = 0; i < n; ++i) + { + rosbags::McapPoint p{}; + p.x = coord(rng); + p.y = coord(rng); + p.z = coord(rng); + p.intensity = static_cast(i % 256); + p.ring = static_cast(i % 16); + p.laser_id = static_cast(1 + i % 3); // non-zero, so the round trip can be checked + p.timestamp = t0 + static_cast(i) * 1e-6; + points.push_back(p); + } + return points; +} + +std::vector makeImuSamples(size_t n, double t0) +{ + std::vector samples; + samples.reserve(n); + for (size_t i = 0; i < n; ++i) + { + rosbags::McapImuSample s{}; + s.timestamp = t0 + static_cast(i) * 0.005; + s.gyro_x = 0.01f * static_cast(i); + s.gyro_y = -0.02f * static_cast(i); + s.gyro_z = 0.03f * static_cast(i); + s.acc_x = 0.1f; + s.acc_y = 0.2f; + s.acc_z = 9.81f; + samples.push_back(s); + } + return samples; +} + +// One point as stored in a LAZ file, decoded exactly the way +// load_point_cloud() (apps/lidar_odometry_step_1/lidar_odometry_utils.cpp) does: +// coordinates from the header scale/offset, timestamp straight out of gps_time, +// lidar id out of user_data. +struct LazPoint +{ + double x, y, z; + double gps_time; + uint16_t intensity; + uint8_t user_data; +}; + +std::vector readLaz(const fs::path& path) +{ + std::vector out; + + laszip_POINTER reader = nullptr; + REQUIRE(laszip_create(&reader) == 0); + + laszip_BOOL is_compressed = 0; + REQUIRE(laszip_open_reader(reader, path.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); + + out.reserve(header->number_of_point_records); + for (laszip_U32 i = 0; i < header->number_of_point_records; ++i) + { + REQUIRE(laszip_read_point(reader) == 0); + LazPoint p{}; + p.x = header->x_offset + header->x_scale_factor * static_cast(point->X); + p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y); + p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); + p.gps_time = point->gps_time; + p.intensity = point->intensity; + p.user_data = point->user_data; + out.push_back(p); + } + + laszip_close_reader(reader); + laszip_destroy(reader); + return out; +} + +std::vector readLines(const fs::path& path) +{ + std::vector lines; + std::ifstream in(path); + std::string line; + while (std::getline(in, line)) + { + if (!line.empty()) + lines.push_back(line); + } + return lines; +} + +} // namespace + +TEST_CASE("McapFileReader: PointCloud2 round-trip through every layout") +{ + struct Case + { + const char* name; + rosbags::PointCloudLayout layout; + bool carries_laser_id; + }; + const Case cases[] = { + {"hdmapping_reader_generic.mcap", rosbags::PointCloudLayout::Generic, true}, + {"hdmapping_reader_velodyne.mcap", rosbags::PointCloudLayout::Velodyne, false}, + {"hdmapping_reader_ouster.mcap", rosbags::PointCloudLayout::Ouster, false}, + {"hdmapping_reader_hesai.mcap", rosbags::PointCloudLayout::Hesai, false}, + }; + + for (const auto& c : cases) + { + CAPTURE(c.name); + const auto path = tempPath(c.name); + const auto points = makeReaderPoints(300, 1000.0); + + { + rosbags::McapWriterOptions options; + options.lidar_layout = c.layout; + rosbags::McapFileWriter writer(path, options); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(points.front().timestamp * 1e9), points); + } + + std::vector decoded; + size_t messages = 0; + { + // Scoped so the reader closes the file before fs::remove: Windows + // refuses to delete a file that still has an open handle. + rosbags::McapFileReader reader(path); + REQUIRE_MESSAGE(reader.isOpen(), reader.error()); + + rosbags::McapFileReader::Callbacks callbacks; + callbacks.onPointCloud = [&](uint64_t, std::vector&& pts) + { + ++messages; + decoded.insert(decoded.end(), pts.begin(), pts.end()); + }; + REQUIRE(reader.read(callbacks)); + } + + CHECK(messages == 1); + REQUIRE(decoded.size() == points.size()); + for (size_t i = 0; i < points.size(); ++i) + { + CHECK(decoded[i].x == doctest::Approx(points[i].x)); + CHECK(decoded[i].y == doctest::Approx(points[i].y)); + CHECK(decoded[i].z == doctest::Approx(points[i].z)); + CHECK(decoded[i].intensity == doctest::Approx(points[i].intensity)); + CHECK(decoded[i].ring == points[i].ring); + CHECK(decoded[i].laser_id == (c.carries_laser_id ? points[i].laser_id : 0)); + CHECK(decoded[i].timestamp == doctest::Approx(points[i].timestamp).epsilon(1e-8)); + } + + fs::remove(path); + } +} + +TEST_CASE("McapFileReader: IMU round-trip and serial number") +{ + const auto path = tempPath("hdmapping_reader_imu.mcap"); + const auto samples = makeImuSamples(20, 5000.0); + + { + rosbags::McapFileWriter writer(path); + REQUIRE(writer.isOpen()); + writer.writeSn(static_cast(5000.0 * 1e9), "SN-ABC-123"); + writer.writeImu(samples); + } + + std::vector decoded; + std::string serial; + { + // Scoped so the reader closes the file before fs::remove (Windows). + rosbags::McapFileReader reader(path); + REQUIRE_MESSAGE(reader.isOpen(), reader.error()); + + rosbags::McapFileReader::Callbacks callbacks; + callbacks.onImu = [&](const rosbags::McapImuSample& s) { decoded.push_back(s); }; + callbacks.onSn = [&](uint64_t, const std::string& sn) { serial = sn; }; + REQUIRE(reader.read(callbacks)); + } + + CHECK(serial == "SN-ABC-123"); + REQUIRE(decoded.size() == samples.size()); + for (size_t i = 0; i < samples.size(); ++i) + { + CHECK(decoded[i].gyro_x == doctest::Approx(samples[i].gyro_x)); + CHECK(decoded[i].gyro_z == doctest::Approx(samples[i].gyro_z)); + CHECK(decoded[i].acc_z == doctest::Approx(samples[i].acc_z)); + CHECK(decoded[i].timestamp == doctest::Approx(samples[i].timestamp).epsilon(1e-8)); + } + + fs::remove(path); +} + +TEST_CASE("McapFileReader: topic resolution") +{ + const auto path = tempPath("hdmapping_reader_topics.mcap"); + const auto points = makeReaderPoints(10, 42.0); + + rosbags::McapWriterOptions write_options; + write_options.lidar_topic = "/custom/points"; + write_options.imu_topic = "/custom/imu"; + write_options.sn_topic = "/custom/sn"; + { + rosbags::McapFileWriter writer(path, write_options); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(points.front().timestamp * 1e9), points); + } + + SUBCASE("auto-detection finds the single channel of each type") + { + rosbags::McapFileReader reader(path); + REQUIRE_MESSAGE(reader.isOpen(), reader.error()); + CHECK(reader.topics().lidar == "/custom/points"); + CHECK(reader.topics().imu == "/custom/imu"); + CHECK(reader.topics().sn == "/custom/sn"); + CHECK(reader.channels().size() == 3); + } + + SUBCASE("an explicit topic is honored") + { + rosbags::McapReaderOptions options; + options.lidar_topic = "/custom/points"; + rosbags::McapFileReader reader(path, options); + REQUIRE_MESSAGE(reader.isOpen(), reader.error()); + CHECK(reader.topics().lidar == "/custom/points"); + } + + SUBCASE("a topic that is not in the file is an error, and channels stay listable") + { + rosbags::McapReaderOptions options; + options.lidar_topic = "/does/not/exist"; + rosbags::McapFileReader reader(path, options); + CHECK_FALSE(reader.isOpen()); + CHECK(reader.error().find("/does/not/exist") != std::string::npos); + CHECK(reader.channels().size() == 3); // --list still works on an unresolvable file + } + + fs::remove(path); +} + +TEST_CASE("McapFileReader: the same reader can be read twice") +{ + // mcap_to_laz relies on this: one pass buffers the IMU stream, a second + // streams the point clouds. + const auto path = tempPath("hdmapping_reader_two_pass.mcap"); + const auto points = makeReaderPoints(50, 100.0); + const auto samples = makeImuSamples(10, 100.0); + + { + rosbags::McapFileWriter writer(path); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(points.front().timestamp * 1e9), points); + writer.writeImu(samples); + } + + size_t imu_count = 0; + size_t point_count = 0; + { + // Scoped so the reader closes the file before fs::remove (Windows). + rosbags::McapFileReader reader(path); + REQUIRE_MESSAGE(reader.isOpen(), reader.error()); + + rosbags::McapFileReader::Callbacks imu_pass; + imu_pass.onImu = [&](const rosbags::McapImuSample&) { ++imu_count; }; + REQUIRE(reader.read(imu_pass)); + + rosbags::McapFileReader::Callbacks lidar_pass; + lidar_pass.onPointCloud = [&](uint64_t, std::vector&& pts) { point_count += pts.size(); }; + REQUIRE(reader.read(lidar_pass)); + } + + CHECK(imu_count == samples.size()); + CHECK(point_count == points.size()); + + fs::remove(path); +} + +TEST_CASE("MandeyeSessionWriter: LAZ stores unscaled gps_time, zero lidar id and intensity") +{ + const auto dir = tempPath("hdmapping_session_laz"); + fs::remove_all(dir); + + const auto points = makeReaderPoints(200, 1234.5); + { + rosbags::MandeyeSessionWriter writer(dir); + REQUIRE_MESSAGE(writer.isOpen(), writer.error()); + REQUIRE(writer.beginChunk(0)); + REQUIRE(writer.addPoints(points)); + REQUIRE(writer.endChunk()); + CHECK(writer.pointsWritten() == points.size()); + } + + const auto laz = dir / "lidar0000.laz"; + REQUIRE(fs::exists(laz)); + + const auto decoded = readLaz(laz); + REQUIRE(decoded.size() == points.size()); + for (size_t i = 0; i < points.size(); ++i) + { + // 0.1 mm coordinate scale, so agreement is absolute to within one step, + // not relative -- points near the origin have no significant digits to + // measure a relative tolerance against. + CHECK(std::abs(decoded[i].x - points[i].x) <= 1e-4); + CHECK(std::abs(decoded[i].y - points[i].y) <= 1e-4); + CHECK(std::abs(decoded[i].z - points[i].z) <= 1e-4); + + // The unit contract: load_point_cloud() reads p.timestamp = gps_time + // directly, so a 1e9 factor here would silently break every consumer. + // LAS point format 1 stores gps_time as a full float64, so this is exact. + CHECK(decoded[i].gps_time == points[i].timestamp); + CHECK(decoded[i].gps_time < 1e6); + + // Single-lidar sessions: the lidar id load_point_cloud() reads out of + // user_data is always 0, whatever laser_id the bag carried. A non-zero id + // with no calibration entry would make it drop the points. + CHECK(decoded[i].user_data == 0); + CHECK(decoded[i].intensity == static_cast(points[i].intensity)); + } + + fs::remove_all(dir); +} + +TEST_CASE("MandeyeSessionWriter: IMU csv carries the columns load_imu() requires") +{ + const auto dir = tempPath("hdmapping_session_csv"); + fs::remove_all(dir); + + const auto samples = makeImuSamples(5, 10.0); + { + rosbags::MandeyeSessionWriter writer(dir); + REQUIRE_MESSAGE(writer.isOpen(), writer.error()); + REQUIRE(writer.writeImuChunk(3, samples)); + CHECK(writer.imuSamplesWritten() == samples.size()); + } + + const auto csv = dir / "imu0003.csv"; + REQUIRE(fs::exists(csv)); + + const auto lines = readLines(csv); + REQUIRE(lines.size() == samples.size() + 1); + CHECK(lines.front() == "timestamp timestampUnix accX accY accZ gyroX gyroY gyroZ"); + + for (size_t i = 0; i < samples.size(); ++i) + { + std::istringstream iss(lines[i + 1]); + uint64_t ts = 0, ts_unix = 1; + float acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0; + REQUIRE(bool(iss >> ts >> ts_unix >> acc_x >> acc_y >> acc_z >> gyro_x >> gyro_y >> gyro_z)); + + // load_imu() divides the timestamp columns by 1e9. + CHECK(static_cast(ts) / 1e9 == doctest::Approx(samples[i].timestamp).epsilon(1e-9)); + CHECK(ts_unix == 0); // not carried by the bag format + CHECK(acc_z == doctest::Approx(samples[i].acc_z)); + CHECK(gyro_x == doctest::Approx(samples[i].gyro_x)); + CHECK(gyro_z == doctest::Approx(samples[i].gyro_z)); + } + + fs::remove_all(dir); +} + +TEST_CASE("MandeyeSessionWriter: bag -> session round trip preserves points per chunk") +{ + const auto path = tempPath("hdmapping_session_roundtrip.mcap"); + const auto dir = tempPath("hdmapping_session_roundtrip"); + fs::remove_all(dir); + + // Two clouds 30 s apart, so a 20 s chunk grid puts them in separate chunks. + const auto cloud_a = makeReaderPoints(120, 2000.0); + const auto cloud_b = makeReaderPoints(80, 2030.0); + { + rosbags::McapFileWriter writer(path); + REQUIRE(writer.isOpen()); + writer.writePointCloud(static_cast(cloud_a.front().timestamp * 1e9), cloud_a); + writer.writePointCloud(static_cast(cloud_b.front().timestamp * 1e9), cloud_b); + } + + int chunk = 0; + { + // Scoped so the reader/writer close their files before fs::remove and + // fs::remove_all below: Windows refuses to delete an open file. + rosbags::McapFileReader reader(path); + REQUIRE_MESSAGE(reader.isOpen(), reader.error()); + + rosbags::MandeyeSessionWriter writer(dir); + REQUIRE_MESSAGE(writer.isOpen(), writer.error()); + + rosbags::McapFileReader::Callbacks callbacks; + callbacks.onPointCloud = [&](uint64_t, std::vector&& pts) + { + REQUIRE(writer.beginChunk(chunk++)); + REQUIRE(writer.addPoints(pts)); + }; + REQUIRE(reader.read(callbacks)); + REQUIRE(writer.endChunk()); + } + + REQUIRE(chunk == 2); + const auto chunk_a = readLaz(dir / "lidar0000.laz"); + const auto chunk_b = readLaz(dir / "lidar0001.laz"); + CHECK(chunk_a.size() == cloud_a.size()); + CHECK(chunk_b.size() == cloud_b.size()); + CHECK(chunk_a.front().gps_time == cloud_a.front().timestamp); + CHECK(chunk_b.front().gps_time == cloud_b.front().timestamp); + + fs::remove(path); + fs::remove_all(dir); +} + +// --------------------------------------------------------------------------- +// decodePc2: datatype awareness and forced layout presets +// --------------------------------------------------------------------------- + +namespace +{ + +struct Pc2TestField +{ + std::string name; + uint32_t offset; + uint8_t datatype; +}; + +// Builds a sensor_msgs/msg/PointCloud2 CDR payload with an arbitrary field +// description and raw record blob, so a test can present layouts McapWriter +// itself never emits (foreign datatypes, padding gaps, malformed offsets). +std::vector buildPc2Message( + double stamp_s, const std::vector& fields, uint32_t point_step, const std::vector& raw, + bool is_bigendian = false) +{ + CdrWriter w; + const auto ns = static_cast(stamp_s * 1e9); + w.write_i32(static_cast(ns / 1000000000ULL)); + w.write_u32(static_cast(ns % 1000000000ULL)); + w.write_string("test_frame"); + + w.write_u32(1); // height + w.write_u32(point_step ? static_cast(raw.size() / point_step) : 0); // width + + w.write_u32(static_cast(fields.size())); + for (const auto& f : fields) + { + w.write_string(f.name); + w.write_u32(f.offset); + w.write_u8(f.datatype); + w.write_u32(1); // count + } + + w.write_bool(is_bigendian); + w.write_u32(point_step); + w.write_u32(static_cast(raw.size())); // row_step + w.write_u32(static_cast(raw.size())); // data length + w.write_raw(raw.data(), raw.size()); + w.write_bool(true); // is_dense + return w.data(); +} + +template +void poke(std::vector& buffer, size_t offset, T value) +{ + REQUIRE(offset + sizeof(T) <= buffer.size()); + std::memcpy(buffer.data() + offset, &value, sizeof(T)); +} + +constexpr uint8_t PF_UINT8 = 2; +constexpr uint8_t PF_UINT16 = 4; +constexpr uint8_t PF_UINT32 = 6; +constexpr uint8_t PF_FLOAT32 = 7; +constexpr uint8_t PF_FLOAT64 = 8; + +// The record layout of a real HesaiLidar_ROS_2.0 /hesai/pandar message, as +// measured from a recording: a 4-byte pad after z, so intensity sits at 16 and +// point_step is 48 -- not the packing PointCloudLayout::Hesai writes. +std::vector buildRealHesaiMessage(double stamp_s, size_t n) +{ + constexpr uint32_t kStep = 48; + std::vector raw(kStep * n, 0); + for (size_t i = 0; i < n; ++i) + { + const size_t base = i * kStep; + poke(raw, base + 0, 1.0f + static_cast(i)); + poke(raw, base + 4, 2.0f + static_cast(i)); + poke(raw, base + 8, 3.0f + static_cast(i)); + poke(raw, base + 16, static_cast(100 + i)); + poke(raw, base + 24, stamp_s + static_cast(i) * 1e-6); + poke(raw, base + 32, static_cast(i % 32)); + } + + const std::vector fields = { + {"x", 0, PF_FLOAT32}, + {"y", 4, PF_FLOAT32}, + {"z", 8, PF_FLOAT32}, + {"intensity", 16, PF_FLOAT32}, + {"timestamp", 24, PF_FLOAT64}, + {"ring", 32, PF_UINT16}, + }; + return buildPc2Message(stamp_s, fields, kStep, raw); +} + +} // namespace + +TEST_CASE("decodePc2: real Hesai Pandar layout, auto and forced preset agree") +{ + const double t0 = 1720513004.923256397; + const auto msg = buildRealHesaiMessage(t0, 5); + + std::string auto_warning; + const auto by_auto = rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Auto, &auto_warning); + + std::string preset_warning; + const auto by_preset = rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Hesai, &preset_warning); + + CHECK(auto_warning.empty()); + CHECK_MESSAGE(preset_warning.empty(), preset_warning); // preset matches the file exactly + REQUIRE(by_auto.size() == 5); + REQUIRE(by_preset.size() == 5); + + for (size_t i = 0; i < by_auto.size(); ++i) + { + CHECK(by_auto[i].x == doctest::Approx(1.0 + static_cast(i))); + CHECK(by_auto[i].y == doctest::Approx(2.0 + static_cast(i))); + CHECK(by_auto[i].z == doctest::Approx(3.0 + static_cast(i))); + CHECK(by_auto[i].intensity == doctest::Approx(100.0 + static_cast(i))); + CHECK(by_auto[i].ring == static_cast(i % 32)); + // Absolute f64 timestamp, so exact -- and nowhere near the message stamp + // offset an intensity@12 misread would produce. + CHECK(by_auto[i].timestamp == t0 + static_cast(i) * 1e-6); + + CHECK(by_preset[i].x == by_auto[i].x); + CHECK(by_preset[i].intensity == by_auto[i].intensity); + CHECK(by_preset[i].ring == by_auto[i].ring); + CHECK(by_preset[i].timestamp == by_auto[i].timestamp); + } +} + +TEST_CASE("decodePc2: a preset that disagrees with the file is reported, not silent") +{ + const auto msg = buildRealHesaiMessage(1720513004.5, 4); + + std::string warning; + const auto points = rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Ouster, &warning); + + // Decoding still proceeds under the forced layout -- the point is that the + // caller is told the layout does not match. + CHECK(points.size() == 4); + REQUIRE_FALSE(warning.empty()); + CHECK(warning.find("ouster") != std::string::npos); + CHECK(warning.find("ring") != std::string::npos); +} + +TEST_CASE("decodePc2: honors each field's datatype rather than assuming float32") +{ + // float64 coordinates and uint8 intensity/ring: every field is a different + // width from what McapWriter emits, so a fixed-width decoder reads garbage. + constexpr uint32_t kStep = 27; + std::vector raw(kStep * 3, 0); + for (size_t i = 0; i < 3; ++i) + { + const size_t base = i * kStep; + poke(raw, base + 0, -10.5 - static_cast(i)); + poke(raw, base + 8, 20.25 + static_cast(i)); + poke(raw, base + 16, 0.125 * static_cast(i)); + poke(raw, base + 24, static_cast(200 + i)); + poke(raw, base + 25, static_cast(500 + i)); + } + + const std::vector fields = { + {"x", 0, PF_FLOAT64}, + {"y", 8, PF_FLOAT64}, + {"z", 16, PF_FLOAT64}, + {"intensity", 24, PF_UINT8}, + {"ring", 25, PF_UINT16}, + }; + const auto msg = buildPc2Message(1000.0, fields, kStep, raw); + + std::string warning; + const auto points = rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Auto, &warning); + + CHECK(warning.empty()); + REQUIRE(points.size() == 3); + for (size_t i = 0; i < points.size(); ++i) + { + CHECK(points[i].x == doctest::Approx(-10.5 - static_cast(i))); + CHECK(points[i].y == doctest::Approx(20.25 + static_cast(i))); + CHECK(points[i].z == doctest::Approx(0.125 * static_cast(i))); + CHECK(points[i].intensity == doctest::Approx(200.0 + static_cast(i))); + CHECK(points[i].ring == static_cast(500 + i)); + CHECK(points[i].timestamp == doctest::Approx(1000.0)); // no time field -> message stamp + } +} + +TEST_CASE("decodePc2: integer 't' is nanoseconds, float 't' is seconds, both relative") +{ + constexpr uint32_t kStep = 16; + + SUBCASE("uint32 t (ouster convention)") + { + std::vector raw(kStep, 0); + poke(raw, 0, 1.0f); + poke(raw, 4, 2.0f); + poke(raw, 8, 3.0f); + poke(raw, 12, 250000000u); // 0.25 s in ns + const auto msg = buildPc2Message( + 500.0, {{"x", 0, PF_FLOAT32}, {"y", 4, PF_FLOAT32}, {"z", 8, PF_FLOAT32}, {"t", 12, PF_UINT32}}, kStep, raw); + + const auto points = rosbags::decodePc2(msg.data(), msg.size()); + REQUIRE(points.size() == 1); + CHECK(points[0].timestamp == doctest::Approx(500.25)); + } + + SUBCASE("float32 t is already seconds") + { + std::vector raw(kStep, 0); + poke(raw, 0, 1.0f); + poke(raw, 4, 2.0f); + poke(raw, 8, 3.0f); + poke(raw, 12, 0.25f); + const auto msg = buildPc2Message( + 500.0, {{"x", 0, PF_FLOAT32}, {"y", 4, PF_FLOAT32}, {"z", 8, PF_FLOAT32}, {"t", 12, PF_FLOAT32}}, kStep, raw); + + const auto points = rosbags::decodePc2(msg.data(), msg.size()); + REQUIRE(points.size() == 1); + CHECK(points[0].timestamp == doctest::Approx(500.25)); + } +} + +TEST_CASE("decodePc2: intensity is optional") +{ + // Many public datasets publish geometry only. These used to decode to nothing. + constexpr uint32_t kStep = 12; + std::vector raw(kStep * 2, 0); + poke(raw, 0, 7.0f); + poke(raw, 4, 8.0f); + poke(raw, 8, 9.0f); + + const auto msg = buildPc2Message(42.0, {{"x", 0, PF_FLOAT32}, {"y", 4, PF_FLOAT32}, {"z", 8, PF_FLOAT32}}, kStep, raw); + + std::string warning; + const auto points = rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Auto, &warning); + + CHECK(warning.empty()); + REQUIRE(points.size() == 2); + CHECK(points[0].x == doctest::Approx(7.0)); + CHECK(points[0].intensity == 0.0f); + CHECK(points[0].timestamp == doctest::Approx(42.0)); +} + +TEST_CASE("decodePc2: malformed clouds are refused with a reason") +{ + const std::vector xyz = {{"x", 0, PF_FLOAT32}, {"y", 4, PF_FLOAT32}, {"z", 8, PF_FLOAT32}}; + + SUBCASE("big-endian is refused rather than misread") + { + const auto msg = buildPc2Message(1.0, xyz, 12, std::vector(12, 0), /*is_bigendian=*/true); + std::string warning; + CHECK(rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Auto, &warning).empty()); + CHECK(warning.find("big-endian") != std::string::npos); + } + + SUBCASE("zero point_step") + { + const auto msg = buildPc2Message(1.0, xyz, 0, std::vector(12, 0)); + std::string warning; + CHECK(rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Auto, &warning).empty()); + CHECK(warning.find("point_step") != std::string::npos); + } + + SUBCASE("no x/y/z") + { + const auto msg = buildPc2Message(1.0, {{"intensity", 0, PF_FLOAT32}}, 4, std::vector(4, 0)); + std::string warning; + CHECK(rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Auto, &warning).empty()); + CHECK(warning.find("x/y/z") != std::string::npos); + } + + SUBCASE("a field reaching past point_step would read into the next record") + { + // z occupies bytes 8..12 but point_step is only 8. + const auto msg = buildPc2Message(1.0, xyz, 8, std::vector(16, 0)); + std::string warning; + CHECK(rosbags::decodePc2(msg.data(), msg.size(), rosbags::Pc2Preset::Auto, &warning).empty()); + CHECK(warning.find("past point_step") != std::string::npos); + } +} + +TEST_CASE("decodePc2: point timestamps come from the payload header stamp") +{ + // The contract mcap_to_laz's chunk binning depends on: a relative per-point + // time is measured from the PointCloud2 header stamp inside the payload. + // McapFileWriter derives that stamp from its own timestamp_ns argument, so the + // message is hand-built here to set the two independently. + constexpr double kSensorTime = 563.706196350; + constexpr uint32_t kStep = 20; + + std::vector raw(kStep * 3, 0); + for (size_t i = 0; i < 3; ++i) + { + const size_t base = i * kStep; + poke(raw, base + 0, 1.0f + static_cast(i)); + poke(raw, base + 4, 2.0f); + poke(raw, base + 8, 3.0f); + poke(raw, base + 12, static_cast(i) * 0.01); // "time", relative seconds + } + const auto msg = buildPc2Message( + kSensorTime, + {{"x", 0, PF_FLOAT32}, {"y", 4, PF_FLOAT32}, {"z", 8, PF_FLOAT32}, {"time", 12, PF_FLOAT64}}, + kStep, + raw); + + const auto points = rosbags::decodePc2(msg.data(), msg.size()); + REQUIRE(points.size() == 3); + for (size_t i = 0; i < points.size(); ++i) + CHECK(points[i].timestamp == doctest::Approx(kSensorTime + static_cast(i) * 0.01).epsilon(1e-12)); +} + +TEST_CASE("decodePc2: an absolute per-point timestamp ignores the header stamp entirely") +{ + // A Hesai `timestamp` field is already absolute, so it must be used as-is and + // never added to the header stamp. The two are set far apart here so a stray + // offset could not hide in the noise. + constexpr double kHeaderStamp = 1720513004.0; + constexpr double kPointTime = 563.706196350; + constexpr uint32_t kStep = 20; + + std::vector raw(kStep, 0); + poke(raw, 0, 1.0f); + poke(raw, 4, 2.0f); + poke(raw, 8, 3.0f); + poke(raw, 12, kPointTime); + + const auto msg = buildPc2Message( + kHeaderStamp, + {{"x", 0, PF_FLOAT32}, {"y", 4, PF_FLOAT32}, {"z", 8, PF_FLOAT32}, {"timestamp", 12, PF_FLOAT64}}, + kStep, + raw); + + const auto points = rosbags::decodePc2(msg.data(), msg.size()); + REQUIRE(points.size() == 1); + CHECK(points[0].timestamp == kPointTime); +}