support for dynamic point cloud field creation

sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h

@@ -36,6 +36,9 @@
 #define SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
 
 #include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_field_conversion.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+
 #include <cstdarg>
 #include <string>
 #include <vector>
@@ -98,7 +101,7 @@ inline int addPointField(sensor_msgs::PointCloud2 &cloud_msg, const std::string
 namespace sensor_msgs
 {
 
-inline PointCloud2Modifier::PointCloud2Modifier(PointCloud2& cloud_msg) : cloud_msg_(cloud_msg)
+inline PointCloud2Modifier::PointCloud2Modifier(PointCloud2& cloud_msg) : cloud_msg_(cloud_msg), current_offset_(0)
 {
 }
 
@@ -133,6 +136,7 @@ inline void PointCloud2Modifier::resize(size_t size)
 inline void PointCloud2Modifier::clear()
 {
   cloud_msg_.data.clear();
+  current_offset_ = 0;
 
   // Update height/width
   if (cloud_msg_.height == 1)
@@ -165,21 +169,21 @@ inline void PointCloud2Modifier::clear()
 inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
 {
   cloud_msg_.fields.clear();
+  current_offset_ = 0;
   cloud_msg_.fields.reserve(n_fields);
   va_list vl;
   va_start(vl, n_fields);
-  int offset = 0;
   for (int i = 0; i < n_fields; ++i) {
     // Create the corresponding PointField
     std::string name(va_arg(vl, char*));
     int count(va_arg(vl, int));
     int datatype(va_arg(vl, int));
-    offset = addPointField(cloud_msg_, name, count, datatype, offset);
+    current_offset_ = addPointField(cloud_msg_, name, count, datatype, current_offset_);
   }
   va_end(vl);
 
   // Resize the point cloud accordingly
-  cloud_msg_.point_step = offset;
+  cloud_msg_.point_step = current_offset_;
   cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
   cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
 }
@@ -197,36 +201,64 @@ inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
 inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...)
 {
   cloud_msg_.fields.clear();
+  current_offset_ = 0;
   cloud_msg_.fields.reserve(n_fields);
   va_list vl;
   va_start(vl, n_fields);
-  int offset = 0;
   for (int i = 0; i < n_fields; ++i) {
     // Create the corresponding PointFields
     std::string
     field_name = std::string(va_arg(vl, char*));
     if (field_name == "xyz") {
       sensor_msgs::PointField point_field;
       // Do x, y and z
-      offset = addPointField(cloud_msg_, "x", 1, sensor_msgs::PointField::FLOAT32, offset);
-      offset = addPointField(cloud_msg_, "y", 1, sensor_msgs::PointField::FLOAT32, offset);
-      offset = addPointField(cloud_msg_, "z", 1, sensor_msgs::PointField::FLOAT32, offset);
-      offset += sizeOfPointField(sensor_msgs::PointField::FLOAT32);
+      current_offset_ = addPointField(cloud_msg_, "x", 1, sensor_msgs::PointField::FLOAT32, current_offset_);
+      current_offset_ = addPointField(cloud_msg_, "y", 1, sensor_msgs::PointField::FLOAT32, current_offset_);
+      current_offset_ = addPointField(cloud_msg_, "z", 1, sensor_msgs::PointField::FLOAT32, current_offset_);
+      current_offset_ += sizeOfPointField(sensor_msgs::PointField::FLOAT32);
     } else
       if ((field_name == "rgb") || (field_name == "rgba")) {
-        offset = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, offset);
-        offset += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
+        current_offset_ = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, current_offset_);
+        current_offset_ += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
       } else
         throw std::runtime_error("Field " + field_name + " does not exist");
   }
   va_end(vl);
 
   // Resize the point cloud accordingly
-  cloud_msg_.point_step = offset;
+  cloud_msg_.point_step = current_offset_;
   cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
   cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
 }
 
+
+inline void PointCloud2Modifier::addPointCloud2Fields(std::vector<PointFieldInfo> fields){
+  for(std::vector<PointFieldInfo>::iterator iter = fields.begin(); iter != fields.end(); ++iter){
+    addPointCloud2Field(iter->name, iter->datatype);
+  }
+}
+
+inline bool PointCloud2Modifier::addPointCloud2Field(std::string name, std::string datatype){
+  int type = sensor_msgs::getPointFieldTypeFromString(datatype);
+  if(type == -1){
+    ROS_ERROR("Could not add the field to the PointCloud2. Unknown datatype \"%s\"!", datatype);
+    return false;
+  }
+  if(sensor_msgs::hasPointCloud2Field(cloud_msg_, name)){
+    ROS_ERROR("Could not add the field to the PointCloud2. The field name \"%s\" already exists!", name);
+    return false;
+  }
+  current_offset_ = addPointField(cloud_msg_, name, 1, type, current_offset_);
+
+  // Resize the point cloud accordingly
+  cloud_msg_.point_step = current_offset_;
+  cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
+  cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
+
+  return true;
+}
+
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 namespace impl

sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h

@@ -161,9 +161,23 @@ class PointCloud2Modifier
    * WARNING: THIS FUNCTION DOES ADD ANY NECESSARY PADDING TRANSPARENTLY
    */
   void setPointCloud2FieldsByString(int n_fields, ...);
+
+  class PointFieldInfo{
+    public:
+      PointFieldInfo(std::string name, std::string datatype)
+         : name(name), datatype(datatype){}
+      std::string name;
+      std::string datatype;
+  };
+
+  void addPointCloud2Fields(std::vector<PointFieldInfo> fields);
+  bool addPointCloud2Field(std::string name, std::string datatype);
+
 protected:
   /** A reference to the original sensor_msgs::PointCloud2 that we read */
   PointCloud2& cloud_msg_;
+    /** The current offset for all point fields */
+  int current_offset_;
 };
 
 namespace impl

sensor_msgs/include/sensor_msgs/point_cloud_conversion.h

@@ -41,6 +41,7 @@
 #include <sensor_msgs/PointCloud.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/point_field_conversion.h>
+#include <algorithm>
 
 /** 
   * \brief Convert between the old (sensor_msgs::PointCloud) and the new (sensor_msgs::PointCloud2) format.
@@ -62,6 +63,11 @@ static inline int getPointCloud2FieldIndex (const sensor_msgs::PointCloud2 &clou
   return (-1);
 }
 
+static inline bool hasPointCloud2Field (const sensor_msgs::PointCloud2& cloud, const std::string& field_name)
+{
+  return getPointCloud2FieldIndex(cloud, field_name) != -1;
+}
+
   ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Convert a sensor_msgs::PointCloud message to a sensor_msgs::PointCloud2 message.
     * \param input the message in the sensor_msgs::PointCloud format

sensor_msgs/include/sensor_msgs/point_field_conversion.h

@@ -84,7 +84,7 @@ namespace sensor_msgs{
   /*!
    * \Type names of the PointField data type.
    */
-  int getPointFieldTypeFromString(std::string field_name){
+  int getPointFieldTypeFromString(const std::string field_name){
     if(field_name == "int8")    return sensor_msgs::PointField::INT8;
     if(field_name == "uint8")   return sensor_msgs::PointField::UINT8;
     if(field_name == "int16")   return sensor_msgs::PointField::INT16;
@@ -94,11 +94,38 @@ namespace sensor_msgs{
     if(field_name == "float32") return sensor_msgs::PointField::FLOAT32;
     if(field_name == "float64") return sensor_msgs::PointField::FLOAT64;
 
-    ROS_ERROR_STREAM("Unknown type: \"" << field_name << "\"!");
-    ROS_ERROR_STREAM("Supported types are: int8, uint8, int16, uint16, int32, uint32, float32, float64"); 
+    ROS_ERROR("Unknown type: \"%s\"! Supported types are: int8, uint8, int16, uint16, int32, uint32, float32, float64", field_name); 
     return -1;
   }
 
+  /** Return the size of a datatype (which is an enum of sensor_msgs::PointField::) in bytes
+   * @param datatype one of the enums of sensor_msgs::PointField::
+  */
+  inline int sizeOfPointField(int datatype)
+  {
+      switch(datatype){
+        case sensor_msgs::PointField::INT8:
+          return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::INT8>::type );
+        case sensor_msgs::PointField::UINT8:
+          return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::UINT8>::type );
+        case sensor_msgs::PointField::INT16:
+          return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::INT16>::type );
+        case sensor_msgs::PointField::UINT16:
+          return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::UINT16>::type );
+        case sensor_msgs::PointField::INT32:
+          return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::INT32>::type );
+        case sensor_msgs::PointField::UINT32:
+          return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::UINT32>::type );
+        case sensor_msgs::PointField::FLOAT32:
+          return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::FLOAT32>::type );
+        case sensor_msgs::PointField::FLOAT64:
+          return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::FLOAT64>::type );
+        default:
+          ROS_ERROR("unknown datatype with the number: %s", datatype);
+          return -1;
+      }
+  }
+
   /*!
    * \Converts a value at the given pointer position, interpreted as the datatype
    *  specified by the given template argument point_field_type, to the given