From a1a03e0e15041abd4654d896d05daeee3fc2adf7 Mon Sep 17 00:00:00 2001 From: Aleksandr Ivanishchev Date: Mon, 7 Sep 2026 21:09:20 +0500 Subject: [PATCH] only reading Livox MID360 ROS2 mcap --- rosbags/McapReader.cpp | 41 ++++++++++++-- rosbags/McapReader.h | 11 ++++ rosbags/cdr_serializer.hpp | 113 +++++++++++++++++++++++++++++++++++++ 3 files changed, 160 insertions(+), 5 deletions(-) diff --git a/rosbags/McapReader.cpp b/rosbags/McapReader.cpp index 5db5c984..80d5a835 100644 --- a/rosbags/McapReader.cpp +++ b/rosbags/McapReader.cpp @@ -127,10 +127,36 @@ McapFileReader::McapFileReader(const std::filesystem::path& path, const McapRead 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.lidar_topic, "CustomMsg", /*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; + // resolveTopic() only records *which topic* the lidar stream is on; it + // doesn't tell read() *which schema* that topic carries, and the two + // calls above happily aim at the same output field. Look the resolved + // topic back up in `channels` and record its schema, so read() can pick + // the matching decoder instead of guessing. + if (!impl_->resolved.lidar.empty()) + { + const auto it = std::find_if( + impl_->channels.begin(), + impl_->channels.end(), + [&](const ChannelInfo& c) + { + return c.topic == impl_->resolved.lidar; + }); + if (it != impl_->channels.end()) + { + const auto type = schemaTypeName(it->schema_name); + if (type == "PointCloud2") + impl_->resolved.lidar_schema = LidarSchema::PointCloud2; + else if (type == "CustomMsg") + impl_->resolved.lidar_schema = LidarSchema::LivoxCustomMsg; + } + + } + // 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. @@ -214,11 +240,16 @@ bool McapFileReader::read(const Callbacks& callbacks) 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)); + // Only the first complaint is kept: a layout problem is a property of + // the file, so it would otherwise repeat once per message. + std::vector points; + if (impl_->resolved.lidar_schema == LidarSchema::LivoxCustomMsg) + points = decodeLivoxCustomMsg(data, size, 0.0, &impl_->lidarLayoutWarning); + else + 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) { diff --git a/rosbags/McapReader.h b/rosbags/McapReader.h index 08b51b6b..49ecb2bc 100644 --- a/rosbags/McapReader.h +++ b/rosbags/McapReader.h @@ -28,12 +28,23 @@ struct McapReaderOptions Pc2Preset lidar_preset = Pc2Preset::Auto; }; +// Which ros2 message type the resolved lidar topic actually carries. Needed +// because read() must call a different CDR decoder for each: decodePc2() and +// decodeLivoxCustomMsg() are not interchangeable. +enum class LidarSchema +{ + Unknown, + PointCloud2, + LivoxCustomMsg, +}; + // The topics actually in use after auto-detection; empty means "not present". struct ResolvedTopics { std::string lidar; std::string imu; std::string sn; + LidarSchema lidar_schema = LidarSchema::Unknown; }; // One channel as found in the file, for --list output and diagnostics. diff --git a/rosbags/cdr_serializer.hpp b/rosbags/cdr_serializer.hpp index b0e28210..8a17e4d0 100644 --- a/rosbags/cdr_serializer.hpp +++ b/rosbags/cdr_serializer.hpp @@ -149,6 +149,13 @@ class CdrReader safe_copy(&v, 4); return v; } + uint64_t read_u64() + { + align(8); + uint64_t v = 0; + safe_copy(&v, 8); + return v; + } float read_f32() { align(4); @@ -265,6 +272,27 @@ struct Pc2Field } }; +// LivoxPoint +struct LivoxPoint +{ + uint32_t offset_time; + float x; + float y; + float z; + uint8_t reflectivity; + uint8_t tag; + uint8_t line; +}; + +// Livox CustomMsg +struct LivoxCustomMsg +{ + uint64_t timebase; + uint32_t point_num; + uint8_t lidar_id; + std::vector points; +}; + // 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) @@ -479,6 +507,91 @@ inline std::string comparePc2Layouts(const Pc2Layout& preset, const Pc2Layout& m // 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. + +// decode livox_ros_driver2/msg/CustomMsg +// +inline std::vector decodeLivoxCustomMsg( + const uint8_t* data, + size_t size, + double fallback_stamp = 0.0, + 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); + + if (!r.ok()) + { + warn("Livox CustomMsg data is truncated"); + return points; + } + + // reading header (std ROS2 header) + const int32_t stamp_sec = r.read_i32(); + const uint32_t stamp_nsec = r.read_u32(); + r.read_string(); // frame_id + + (void)stamp_sec; + (void)stamp_nsec; + (void)fallback_stamp; + + // reading Livox CustomMsg + const uint64_t timebase = r.read_u64(); + const uint32_t point_num = r.read_u32(); + r.read_u8(); // lidar_id + r.skip(3); // uint8[3] rsvd + + const uint32_t seq_len = r.read_u32(); + if (seq_len != point_num) + warn("CustomMsg point_num (" + std::to_string(point_num) + + ") != points sequence length (" + std::to_string(seq_len) + ")"); + const uint32_t n = std::min(point_num, seq_len); + + // reading points + const double timebase_sec = static_cast(timebase) * 1e-9; + + points.reserve(n); + for (uint32_t i = 0; i < n; ++i) + { + if (!r.ok()) + { + warn("Truncated point data at index " + std::to_string(i)); + break; + } + + LivoxPoint pt; + pt.offset_time = r.read_u32(); + pt.x = r.read_f32(); + pt.y = r.read_f32(); + pt.z = r.read_f32(); + pt.reflectivity = r.read_u8(); + pt.tag = r.read_u8(); + pt.line = r.read_u8(); + + // to McapPoint + McapPoint mcap_pt; + mcap_pt.x = pt.x; + mcap_pt.y = pt.y; + mcap_pt.z = pt.z; + mcap_pt.intensity = static_cast(pt.reflectivity); + mcap_pt.ring = static_cast(pt.line); + mcap_pt.laser_id = pt.tag; + + // compute timestamp of point: timebase + offset_time + const double offset_sec = static_cast(pt.offset_time) * 1e-9; + mcap_pt.timestamp = timebase_sec + offset_sec; + + points.push_back(mcap_pt); + } + + return points; +} + inline std::vector decodePc2( const uint8_t* data, size_t size, Pc2Preset preset = Pc2Preset::Auto, std::string* warning = nullptr) {