MCAP Livox MID360 ROS2

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<McapPoint> 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)
 		{

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.

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<LivoxPoint> 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<McapPoint> 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<McapPoint> 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<double>(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<float>(pt.reflectivity);
+        mcap_pt.ring = static_cast<uint16_t>(pt.line);
+        mcap_pt.laser_id = pt.tag;
+
+        // compute timestamp of point: timebase + offset_time
+        const double offset_sec = static_cast<double>(pt.offset_time) * 1e-9;        
+        mcap_pt.timestamp = timebase_sec + offset_sec;
+
+        points.push_back(mcap_pt);
+    }
+
+    return points;
+}
+
 inline std::vector<McapPoint> decodePc2(
 	const uint8_t* data, size_t size, Pc2Preset preset = Pc2Preset::Auto, std::string* warning = nullptr)
 {