ros2bridge: handle PointCloud2 with uint32 timestamps

MRPT · Sep 24, 2024 · c271521 · c271521
1 parent 485a2ea
commit c271521
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 8 deletions.
diff --git 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
@@ -93,7 +93,7 @@ std::set<std::string> 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
@@ -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<const uint32_t*>(&data[field->offset]));
+  }
+  else
+    output = 0;
+}
+}  // namespace
 
 std::set<std::string> mrpt::ros2bridge::extractFields(const sensor_msgs::msg::PointCloud2& msg)
 {
@@ -97,7 +112,7 @@ std::set<std::string> 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: