From c2715211199bae4a21405fd90553f28667a87a47 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 24 Sep 2024 22:54:47 +0200 Subject: [PATCH] ros2bridge: handle PointCloud2 with uint32 timestamps --- doc/source/doxygen-docs/changelog.md | 2 ++ libs/ros1bridge/src/point_cloud2.cpp | 4 +-- libs/ros2bridge/src/point_cloud2.cpp | 41 ++++++++++++++++++++++++---- 3 files changed, 39 insertions(+), 8 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 0d8e9b977d..78161c4d01 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -7,6 +7,8 @@ - \ref mrpt_opengl_grp: - New method mrpt::opengl::CAssimpModel::split_triangles_rendering_bbox() to enable a new feature in Assimp 3D models: splitting into smaller triangle sets for correct z-ordering of semitransparent objects; e.g. required for trees with masked leaves. - SkyBox shader changed its internal ID so it gets rendered before potentially transparent elements, fixing render artifacts. + - \ref mrpt_ros2bridge_grp: + - Handle PointCloud2 with uint32 timestamps, interpreted as nanoseconds. # Version 2.14.0: Released Sep 15th, 2024 - Changes in libraries: diff --git a/libs/ros1bridge/src/point_cloud2.cpp b/libs/ros1bridge/src/point_cloud2.cpp index 0ca9a7e763..cc3de959dc 100644 --- a/libs/ros1bridge/src/point_cloud2.cpp +++ b/libs/ros1bridge/src/point_cloud2.cpp @@ -93,7 +93,7 @@ std::set mrpt::ros1bridge::extractFields(const sensor_msgs::PointCl /** Convert sensor_msgs/PointCloud2 -> mrpt::slam::CSimplePointsMap * - * \return true on sucessful conversion, false on any error. + * \return true on successful conversion, false on any error. */ bool mrpt::ros1bridge::fromROS(const sensor_msgs::PointCloud2& msg, CSimplePointsMap& obj) { @@ -243,7 +243,7 @@ bool mrpt::ros1bridge::fromROS(const sensor_msgs::PointCloud2& msg, CPointsMapXY // Convention: // I only found one case (NTU Viral dataset) using uint32_t for time, // and times ranged from 0 to ~99822766 = 100,000,000 = 1e8 - // so they seems to be nanoseconds: + // so they seem to be nanoseconds: obj.setPointTime(idx, t * 1e-9); } diff --git a/libs/ros2bridge/src/point_cloud2.cpp b/libs/ros2bridge/src/point_cloud2.cpp index e83f9234e9..4f2e906d9c 100644 --- a/libs/ros2bridge/src/point_cloud2.cpp +++ b/libs/ros2bridge/src/point_cloud2.cpp @@ -22,7 +22,9 @@ using namespace mrpt::maps; -static bool check_field( +namespace +{ +bool check_field( const sensor_msgs::msg::PointField& input_field, std::string check_name, const sensor_msgs::msg::PointField** output) @@ -33,6 +35,7 @@ static bool check_field( if (input_field.datatype != sensor_msgs::msg::PointField::FLOAT32 && input_field.datatype != sensor_msgs::msg::PointField::FLOAT64 && input_field.datatype != sensor_msgs::msg::PointField::UINT16 && + input_field.datatype != sensor_msgs::msg::PointField::UINT32 && input_field.datatype != sensor_msgs::msg::PointField::UINT8) { *output = nullptr; @@ -46,7 +49,7 @@ static bool check_field( return coherence_error; } -static void get_float_from_field( +void get_float_from_field( const sensor_msgs::msg::PointField* field, const unsigned char* data, float& output) { if (field != nullptr) @@ -60,7 +63,7 @@ static void get_float_from_field( output = 0.0; } -static void get_double_from_field( +void get_double_from_field( const sensor_msgs::msg::PointField* field, const unsigned char* data, double& output) { if (field != nullptr) @@ -74,7 +77,7 @@ static void get_double_from_field( output = 0.0; } -static void get_uint16_from_field( +void get_uint16_from_field( const sensor_msgs::msg::PointField* field, const unsigned char* data, uint16_t& output) { if (field != nullptr) @@ -87,6 +90,18 @@ static void get_uint16_from_field( else output = 0; } +void get_uint32_from_field( + const sensor_msgs::msg::PointField* field, const unsigned char* data, uint32_t& output) +{ + if (field != nullptr) + { + if (field->datatype == sensor_msgs::msg::PointField::UINT32) + output = *(reinterpret_cast(&data[field->offset])); + } + else + output = 0; +} +} // namespace std::set mrpt::ros2bridge::extractFields(const sensor_msgs::msg::PointCloud2& msg) { @@ -97,7 +112,7 @@ std::set mrpt::ros2bridge::extractFields(const sensor_msgs::msg::Po /** Convert sensor_msgs/PointCloud2 -> mrpt::slam::CSimplePointsMap * - * \return true on sucessful conversion, false on any error. + * \return true on successful conversion, false on any error. */ bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CSimplePointsMap& obj) { @@ -233,7 +248,21 @@ bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CPoints if (t_field) { double t = 0; - get_double_from_field(t_field, msg_data, t); + + if (t_field->datatype == sensor_msgs::msg::PointField::FLOAT32 || + t_field->datatype == sensor_msgs::msg::PointField::FLOAT64) + { + get_double_from_field(t_field, msg_data, t); + } + else + { + uint32_t tim = 0; + + get_uint32_from_field(t_field, msg_data, tim); + + // Convention: they seem to be nanoseconds: + t = tim * 1e-9; + } // If the sensor uses absolute timestamp, convert them to relative // since otherwise precision is lost in the double->float conversion: