Address reviewer feedback, expand test.

Signed-off-by: Michael Carroll <[email protected]>

Author: mjcarroll

sensor_msgs/include/sensor_msgs/point_cloud_conversion.hpp

@@ -133,8 +133,6 @@ static inline bool convertPointCloud2ToPointCloud(
   int y_idx = getPointCloud2FieldIndex(input, "y");
   int z_idx = getPointCloud2FieldIndex(input, "z");
   if (x_idx == -1 || y_idx == -1 || z_idx == -1) {
-    std::cerr << "x/y/z coordinates not found! Cannot convert to sensor_msgs::msg::PointCloud!" <<
-      std::endl;
     return false;
   }
   int x_offset = input.fields[x_idx].offset;

sensor_msgs/include/sensor_msgs/point_field_conversion.hpp

@@ -147,61 +147,5 @@ inline T readPointCloud2BufferValue(const unsigned char * data_ptr, const unsign
   // This should never be reached, but return statement added to avoid compiler warning. (#84)
   return T();
 }
-
-/*!
- * \Inserts a given value at the given point position interpreted as the datatype
- *  specified by the template argument point_field_type.
- * \param data_ptr            pointer into the point cloud 2 buffer
- * \param value               the value to insert
- * \tparam point_field_type   sensor_msgs::PointField datatype value
- * \tparam T                  type of the value to insert
- */
-template<int point_field_type, typename T>
-inline void writePointCloud2BufferValue(unsigned char * data_ptr, T value)
-{
-  typedef typename pointFieldTypeAsType<point_field_type>::type type;
-  *(reinterpret_cast<type *>(data_ptr)) = static_cast<type>(value);
-}
-
-/*!
- * \Inserts a given value at the given point position interpreted as the datatype
- *  specified by the given datatype parameter.
- * \param data_ptr    pointer into the point cloud 2 buffer
- * \param datatype    sensor_msgs::PointField datatype value
- * \param value       the value to insert
- * \tparam T          type of the value to insert
- */
-template<typename T>
-inline void writePointCloud2BufferValue(
-  unsigned char * data_ptr, const unsigned char datatype,
-  T value)
-{
-  switch (datatype) {
-    case sensor_msgs::msg::PointField::INT8:
-      writePointCloud2BufferValue<sensor_msgs::msg::PointField::INT8, T>(data_ptr, value);
-      break;
-    case sensor_msgs::msg::PointField::UINT8:
-      writePointCloud2BufferValue<sensor_msgs::msg::PointField::UINT8, T>(data_ptr, value);
-      break;
-    case sensor_msgs::msg::PointField::INT16:
-      writePointCloud2BufferValue<sensor_msgs::msg::PointField::INT16, T>(data_ptr, value);
-      break;
-    case sensor_msgs::msg::PointField::UINT16:
-      writePointCloud2BufferValue<sensor_msgs::msg::PointField::UINT16, T>(data_ptr, value);
-      break;
-    case sensor_msgs::msg::PointField::INT32:
-      writePointCloud2BufferValue<sensor_msgs::msg::PointField::INT32, T>(data_ptr, value);
-      break;
-    case sensor_msgs::msg::PointField::UINT32:
-      writePointCloud2BufferValue<sensor_msgs::msg::PointField::UINT32, T>(data_ptr, value);
-      break;
-    case sensor_msgs::msg::PointField::FLOAT32:
-      writePointCloud2BufferValue<sensor_msgs::msg::PointField::FLOAT32, T>(data_ptr, value);
-      break;
-    case sensor_msgs::msg::PointField::FLOAT64:
-      writePointCloud2BufferValue<sensor_msgs::msg::PointField::FLOAT64, T>(data_ptr, value);
-      break;
-  }
-}
 }  // namespace sensor_msgs
 #endif  // SENSOR_MSGS__POINT_FIELD_CONVERSION_HPP_

sensor_msgs/test/test_pointcloud.cpp

@@ -23,26 +23,63 @@
 
 TEST(PointCloudConversions, CloudToCloud2)
 {
+  // Build a PointCloud
   sensor_msgs::msg::PointCloud cloud;
 
+  cloud.header.stamp.sec = 100;
+  cloud.header.stamp.nanosec = 500;
+  cloud.header.frame_id = "cloud_frame";
+
+  sensor_msgs::msg::ChannelFloat32 intensity, range;
+  intensity.name = "intensity";
+  range.name = "range";
+
   for (size_t ii = 0; ii < 100; ++ii) {
     auto pt = geometry_msgs::msg::Point32();
     pt.x = ii;
-    pt.y = ii;
-    pt.z = ii;
+    pt.y = 2 * ii;
+    pt.z = 3 * ii;
     cloud.points.push_back(pt);
+
+    intensity.values.push_back(4 * ii);
+    range.values.push_back(5* ii);
   }
+  cloud.channels.push_back(intensity);
+  cloud.channels.push_back(range);
 
+  // Convert to PointCloud2
   sensor_msgs::msg::PointCloud2 cloud2;
-
-  auto ret = sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2);
-  EXPECT_TRUE(ret);
+  auto ret_cc2 = sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2);
+  ASSERT_TRUE(ret_cc2);
 
   EXPECT_EQ(1u, cloud2.height);
   EXPECT_EQ(100u, cloud2.width);
 
   EXPECT_EQ(cloud2.fields[0].name, "x");
   EXPECT_EQ(cloud2.fields[1].name, "y");
   EXPECT_EQ(cloud2.fields[2].name, "z");
-}
+  EXPECT_EQ(cloud2.fields[3].name, "intensity");
+  EXPECT_EQ(cloud2.fields[4].name, "range");
 
+  EXPECT_EQ(cloud2.fields.size(), 5u);
+  EXPECT_EQ(cloud2.header.frame_id, "cloud_frame");
+  EXPECT_EQ(cloud2.header.stamp.sec, 100);
+  EXPECT_EQ(cloud2.header.stamp.nanosec, 500u);
+
+  // Convert back to PointCloud
+  sensor_msgs::msg::PointCloud cloud3;
+  auto ret_c2c = sensor_msgs::convertPointCloud2ToPointCloud(cloud2, cloud3);
+  ASSERT_TRUE(ret_c2c);
+  EXPECT_EQ(cloud3.points.size(), 100u);
+  EXPECT_EQ(cloud3.channels.size(), 2u);
+
+  EXPECT_EQ(cloud3.channels[0].name, "intensity");
+  EXPECT_EQ(cloud3.channels[0].values.size(), 100u);
+  EXPECT_FLOAT_EQ(cloud3.channels[0].values[0], 0.0);
+  EXPECT_FLOAT_EQ(cloud3.channels[0].values[10], 40.0);
+
+  EXPECT_EQ(cloud3.channels[1].name, "range");
+  EXPECT_EQ(cloud3.channels[1].values.size(), 100u);
+  EXPECT_FLOAT_EQ(cloud3.channels[1].values[0], 0.0);
+  EXPECT_FLOAT_EQ(cloud3.channels[1].values[10], 50.0);
+}