From fca04ca12dbb6db6a4261f6535d7f96761927c30 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 5 Aug 2024 09:16:04 +0200 Subject: [PATCH] BUGFIX: ros convert to XYZI clouds --- doc/source/doxygen-docs/changelog.md | 2 ++ libs/maps/src/maps/CPointsMapXYZI.cpp | 2 +- libs/ros1bridge/src/point_cloud2.cpp | 3 +-- libs/ros2bridge/src/point_cloud2.cpp | 3 +-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index f9e4caf509..b2bd28eabd 100644 --- 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 index c08de2f45e..f23a67e6ed 100644 --- 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 index 8224e2ce87..160f5c4062 100644 --- 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 index b5806c8538..34685ae174 100644 --- 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;