BUGFIX: ros convert to XYZI clouds

MRPT · Aug 5, 2024 · fca04ca · fca04ca
1 parent 5b6f10b
commit fca04ca
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 5 deletions.
diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md
@@ -10,6 +10,8 @@
   - This main MRPT repository is no longer directly built as a ROS package. Please, use the wrappers for better modularity:
     - https://github.com/MRPT/mrpt_ros
     - https://github.com/MRPT/python_mrpt_ros
+- BUG FIXES:
+  - ros1bridge and ros2bridge: fix conversion to PointCloud XYZI.
 
 # Version 2.13.4: Released July 24th, 2024
 - Fix docs typos.

diff --git a/libs/maps/src/maps/CPointsMapXYZI.cpp b/libs/maps/src/maps/CPointsMapXYZI.cpp
@@ -166,7 +166,7 @@ void CPointsMapXYZI::setPointRGB(size_t index, float x, float y, float z, float
 
 void CPointsMapXYZI::setPointIntensity(size_t index, float I)
 {
-  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
+  if (index >= m_intensity.size()) THROW_EXCEPTION("Index out of bounds");
   this->m_intensity[index] = I;
   // mark_as_modified();  // No need to rebuild KD-trees, etc...
 }

diff --git a/libs/ros1bridge/src/point_cloud2.cpp b/libs/ros1bridge/src/point_cloud2.cpp
@@ -167,8 +167,7 @@ bool mrpt::ros1bridge::fromROS(const sensor_msgs::PointCloud2& msg, CPointsMapXY
       get_float_from_field(z_field, msg_data, z);
       get_float_from_field(i_field, msg_data, i);
       obj.insertPoint(x, y, z);
-
-      obj.setPointIntensity(obj.size() - 1, i);
+      obj.insertPointField_Intensity(i);
     }
   }
   return true;

diff --git a/libs/ros2bridge/src/point_cloud2.cpp b/libs/ros2bridge/src/point_cloud2.cpp
@@ -171,8 +171,7 @@ bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CPoints
       get_float_from_field(z_field, msg_data, z);
       get_float_from_field(i_field, msg_data, i);
       obj.insertPoint(x, y, z);
-
-      obj.setPointIntensity(obj.size() - 1, i);
+      obj.insertPointField_Intensity(i);
     }
   }
   return true;