dynamic point field creation, docu and cleanup

sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h

@@ -148,6 +148,13 @@ inline void PointCloud2Modifier::clear()
       cloud_msg_.row_step = cloud_msg_.width = cloud_msg_.height = 0;
 }
 
+inline void PointCloud2Modifier::setPointCloud2Fields(const std::vector<PointFieldInfo>& fields)
+{
+  cloud_msg_.fields.clear();
+  current_offset_ = 0;
+  cloud_msg_.fields.reserve(fields.size());
+  addPointCloud2Fields(fields);
+}
 
 /**
  * @brief Function setting some fields in a PointCloud and adjusting the
@@ -232,23 +239,33 @@ inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...)
 }
 
 
-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::addPointCloud2Fields(const std::vector<PointFieldInfo>& fields){
+  bool ret = true;
+  for(std::vector<PointFieldInfo>::const_iterator iter = fields.begin(); iter != fields.end(); ++iter){
+    if(!addPointCloud2Field(*iter)){
+      ret = false;
+    }
   }
+  return ret;
 }
 
-inline bool PointCloud2Modifier::addPointCloud2Field(std::string name, std::string datatype){
-  int type = sensor_msgs::getPointFieldTypeFromString(datatype);
-  if(type == -1){
-    std::cerr << "Could not add the field to the PointCloud2. Unknown datatype \"" << datatype << "\"!" << std::endl;
+inline bool PointCloud2Modifier::addPointCloud2Field(const PointFieldInfo& field){
+  if(field.datatype == -1){
+    std::cerr << "Could not add the field to the PointCloud2. Unknown datatype, name:\""
+              << field.datatype_name << "\", id:\"" << field.datatype << "\"!" << std::endl;
     return false;
   }
-  if(sensor_msgs::hasPointCloud2Field(cloud_msg_, name)){
-    std::cerr << "Could not add the field to the PointCloud2. The field name \"" << name << "\" already exists!" << std::endl;
+  if(sensor_msgs::hasPointCloud2Field(cloud_msg_, field.name)){
+    std::cerr << "Could not add the field to the PointCloud2. The field name \""
+              << field.name << "\" already exists!" << std::endl;
     return false;
   }
-  current_offset_ = addPointField(cloud_msg_, name, 1, type, current_offset_);
+  if(field.name.empty()){
+    // no name given for the field, just add an offset
+    current_offset_ += sizeOfPointField(field.datatype);
+  }else{
+    current_offset_ = addPointField(cloud_msg_, field.name, 1, field.datatype, current_offset_);
+  }
 
   // Resize the point cloud accordingly
   cloud_msg_.point_step = current_offset_;

sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h

@@ -36,6 +36,7 @@
 #define SENSOR_MSGS_POINT_CLOUD2_ITERATOR_H
 
 #include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_field_conversion.h>
 #include <cstdarg>
 #include <string>
 #include <vector>
@@ -131,6 +132,52 @@ class PointCloud2Modifier
    */
   void clear();
 
+  /**
+   * @brief point field info to create new point fields from datatype name or id. This allows to create a dynamic list
+   *        of fields and also for dynamic point cloud creation.
+   */
+  class PointFieldInfo{
+   public:
+    /**
+     * @brief Create a PointFieldInfo using the name of the datafield and the name of the datatype, which can ben one of
+     *        the following names: "int8", "uint8", "int16", "uint16", "int32", "uint32", "float32", "float64"
+     * @param name Name of the datafield, e.g. "x", "y", "z", "r", "rgb", "rgba", "intensity", normal_x", ...
+     * @param datatype_name The name of the datatype names. It must be one of the following name: "int8", "uint8",
+     *                      "int16", "uint16", "int32", "uint32", "float32", "float64"!
+     */
+    PointFieldInfo(const std::string name, const std::string datatype_name)
+        : name(name), datatype_name(datatype_name),
+          datatype(sensor_msgs::getPointFieldTypeFromString(datatype_name)){}
+
+    /**
+     * @brief Create a PointFieldInfo using the datatype enumeration values defined in PointField.msgs
+     *        (sensor_msgs::PointField::INT8, sensor_msgs::PointField::UINT8, ... @sPointField.msg)
+     * @param name Name of the datafield, e.g. "x", "y", "z", "r", "rgb", "rgba", "intensity", normal_x", ...
+     * @param datatype The id of the datatype enumeration value defined in the PointField.msg
+     */
+    PointFieldInfo(const std::string name, const int datatype)
+        : name(name), datatype(datatype), datatype_name(sensor_msgs::getPointFieldNameFromType(datatype)) {}
+
+    //! The point field name, e.g. "x", "y", "z", "rgb", "intensity", "normal_x", etc.
+    std::string name;
+
+    //! The datatype id associated with stored datatype, see PointField.msg
+    int datatype;
+
+    //! The name of the datatype which is associated with the datatype id
+    std::string datatype_name;
+  };
+
+  /**
+   * @brief Sets the point cloud fields, which are given as a vector of PointFieldInfo objects. This allows a dynamic
+   *        creation of variable defined fields at runtime. This is useful to activate or deactivate fields in the
+   *        point cloud dynamically for PointCloud2 messages.
+   *
+   * @param fields A vector of PointFieldInfo objects. A PointFieldInfo object holds all necessary information to
+   *        create a new point field in the PointCloud2 object.
+   */
+  void setPointCloud2Fields(const std::vector<PointFieldInfo>& fields);
+
   /**
    * @brief Function setting some fields in a PointCloud and adjusting the
    *        internals of the PointCloud2
@@ -162,16 +209,29 @@ class PointCloud2Modifier
    */
   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;
-  };
+  /**
+   * @brief Adds the point cloud fields, which are given as a vector of PointFieldInfo objects, to the existing fields
+   *        in the PointCloud2. This allows a dynamic creation of variable defined fields at runtime. This is useful
+   *        to activate or deactivate fields in the point cloud dynamically for PointCloud2 messages.
+   *
+   * @param fields A vector of PointFieldInfo objects. A PointFieldInfo object holds all necessary information to
+   *        create a new point field in the PointCloud2 object.
+   *
+   * @return true, if the fields have been added successfully, false otherwise
+   */
+  bool addPointCloud2Fields(const std::vector<PointFieldInfo>& fields);
 
-  void addPointCloud2Fields(std::vector<PointFieldInfo> fields);
-  bool addPointCloud2Field(std::string name, std::string datatype);
+  /**
+   * @brief Adds the point cloud field, which is given as a PointFieldInfo objects, to the existing fields
+   *        in the PointCloud2. This allows a dynamic creation of variable defined fields at runtime. This is useful
+   *        to activate or deactivate fields in the point cloud dynamically for PointCloud2 messages.
+   *
+   * @param fields A PointFieldInfo object. A PointFieldInfo object holds all necessary information to
+   *        create a new point field in the PointCloud2 object.
+   *
+   * @return true if the field has been added successfully, false otherwise
+   */
+  bool addPointCloud2Field(const PointFieldInfo& field);
 
 protected:
   /** A reference to the original sensor_msgs::PointCloud2 that we read */

sensor_msgs/include/sensor_msgs/point_field_conversion.h

@@ -83,8 +83,9 @@ namespace sensor_msgs{
 
   /*!
    * \Type names of the PointField data type.
+   * @param field_name Type name for a dynamic type initialization in the pointfield
    */
-  int getPointFieldTypeFromString(const std::string field_name){
+  inline 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;
@@ -98,7 +99,36 @@ namespace sensor_msgs{
     return -1;
   }
 
-  /** Return the size of a datatype (which is an enum of sensor_msgs::PointField::) in bytes
+  /*!
+   * Return the string name of a datatype (which is an enum of sensor_msgs::PointField::)
+   * @param datatype one of the enums of sensor_msgs::PointField::
+  */
+  inline const std::string getPointFieldNameFromType(const int datatype)
+  {
+    switch(datatype){
+      case sensor_msgs::PointField::INT8:
+        return "int8";
+      case sensor_msgs::PointField::UINT8:
+        return "uint8";
+      case sensor_msgs::PointField::INT16:
+        return "int16";
+      case sensor_msgs::PointField::UINT16:
+        return "uint16";
+      case sensor_msgs::PointField::INT32:
+        return "int32";
+      case sensor_msgs::PointField::UINT32:
+        return "uint32";
+      case sensor_msgs::PointField::FLOAT32:
+        return "float32";
+      case sensor_msgs::PointField::FLOAT64:
+        return "float64";
+      default:
+        std::cerr << "unknown datatype with the number: " << datatype;
+        return "";
+    }
+  }
+  /*!
+   * 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)