FIX: issue with XYZIRT to ROS conversion

MRPT · Jan 9, 2024 · 0c35071 · 0c35071
1 parent 4167fd5
commit 0c35071
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 3 deletions.
diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md
@@ -6,6 +6,7 @@
     - mrpt::obs::CObservation::load() is now protected with a std::mutex for safe multi-threading usage.
 - BUG FIXES:
   - Fix wrong filenames in `rawlog-edit --externalize` when sensor labels contain the `/` character (e.g. mimicking ROS topic names).
+  - Fix crash in mrpt::ros2bridge::toROS() for XYZIRT point clouds.
 
 # Version 2.11.5: Released Dec 21st, 2023
 - Changes in libraries:

diff --git a/libs/ros2bridge/src/point_cloud2.cpp b/libs/ros2bridge/src/point_cloud2.cpp
@@ -358,18 +358,21 @@ bool mrpt::ros2bridge::toROS(
 
 	if (obj.hasIntensityField())
 	{
+		ASSERT_EQUAL_(obj.getPointsBufferRef_intensity()->size(), obj.size());
 		names.push_back("intensity");
 		offsets.push_back(msg.point_step);
 		msg.point_step += sizeof(float);
 	}
 	if (obj.hasTimeField())
 	{
+		ASSERT_EQUAL_(obj.getPointsBufferRef_timestamp()->size(), obj.size());
 		names.push_back("time");
 		offsets.push_back(msg.point_step);
 		msg.point_step += sizeof(float);
 	}
 	if (obj.hasRingField())
 	{
+		ASSERT_EQUAL_(obj.getPointsBufferRef_ring()->size(), obj.size());
 		names.push_back("ring");
 		offsets.push_back(msg.point_step);
 		msg.point_step += sizeof(uint16_t);
@@ -402,9 +405,9 @@ bool mrpt::ros2bridge::toROS(
 	const auto& xs = obj.getPointsBufferRef_x();
 	const auto& ys = obj.getPointsBufferRef_y();
 	const auto& zs = obj.getPointsBufferRef_z();
-	const auto& Is = obj.getPointsBufferRef_intensity();
-	const auto& Rs = obj.getPointsBufferRef_ring();
-	const auto& Ts = obj.getPointsBufferRef_timestamp();
+	const auto& Is = *obj.getPointsBufferRef_intensity();
+	const auto& Rs = *obj.getPointsBufferRef_ring();
+	const auto& Ts = *obj.getPointsBufferRef_timestamp();
 
 	uint8_t* pointDest = msg.data.data();
 	for (size_t i = 0; i < xs.size(); i++)