diff --git a/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h b/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h
index 9092eea0..a9697089 100644
--- a/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h
+++ b/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h
@@ -101,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), current_offset_(0)
+inline PointCloud2Modifier::PointCloud2Modifier(PointCloud2& cloud_msg) : cloud_msg_(cloud_msg)
 {
 }
 
@@ -136,7 +136,7 @@ inline void PointCloud2Modifier::resize(size_t size)
 inline void PointCloud2Modifier::clear()
 {
   cloud_msg_.data.clear();
-  current_offset_ = 0;
+  cloud_msg_.point_step = 0;
 
   // Update height/width
   if (cloud_msg_.height == 1)
@@ -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();
+  cloud_msg_.point_step = 0;
+  cloud_msg_.fields.reserve(fields.size());
+  addPointCloud2Fields(fields);
+}
 
 /**
  * @brief Function setting some fields in a PointCloud and adjusting the
@@ -169,7 +176,7 @@ inline void PointCloud2Modifier::clear()
 inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
 {
   cloud_msg_.fields.clear();
-  current_offset_ = 0;
+  cloud_msg_.point_step = 0;
   cloud_msg_.fields.reserve(n_fields);
   va_list vl;
   va_start(vl, n_fields);
@@ -178,12 +185,11 @@ inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
     std::string name(va_arg(vl, char*));
     int count(va_arg(vl, int));
     int datatype(va_arg(vl, int));
-    current_offset_ = addPointField(cloud_msg_, name, count, datatype, current_offset_);
+    cloud_msg_.point_step = addPointField(cloud_msg_, name, count, datatype, cloud_msg_.point_step);
   }
   va_end(vl);
 
   // 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);
 }
@@ -201,7 +207,7 @@ inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
 inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...)
 {
   cloud_msg_.fields.clear();
-  current_offset_ = 0;
+  cloud_msg_.point_step = 0;
   cloud_msg_.fields.reserve(n_fields);
   va_list vl;
   va_start(vl, n_fields);
@@ -212,46 +218,54 @@ inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...)
     if (field_name == "xyz") {
       sensor_msgs::PointField point_field;
       // Do x, y and z
-      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);
+      cloud_msg_.point_step = addPointField(cloud_msg_, "x", 1, sensor_msgs::PointField::FLOAT32, cloud_msg_.point_step);
+      cloud_msg_.point_step = addPointField(cloud_msg_, "y", 1, sensor_msgs::PointField::FLOAT32, cloud_msg_.point_step);
+      cloud_msg_.point_step = addPointField(cloud_msg_, "z", 1, sensor_msgs::PointField::FLOAT32, cloud_msg_.point_step);
+      cloud_msg_.point_step += sizeOfPointField(sensor_msgs::PointField::FLOAT32);
     } else
       if ((field_name == "rgb") || (field_name == "rgba")) {
-        current_offset_ = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, current_offset_);
-        current_offset_ += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
+        cloud_msg_.point_step = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, cloud_msg_.point_step);
+        cloud_msg_.point_step += 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 = 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::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
+    cloud_msg_.point_step += sizeOfPointField(field.datatype);
+  }else{
+    cloud_msg_.point_step = addPointField(cloud_msg_, field.name, 1, field.datatype, cloud_msg_.point_step);
+  }
 
   // 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);
 
diff --git a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h b/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
index ca142049..3dc18408 100644
--- a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
+++ b/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,72 @@ 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 be 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 a name of the datatype and the datatype enumeration values defined in
+     *        PointField.msgs (sensor_msgs::PointField::INT8, sensor_msgs::PointField::UINT8, ... see PointField.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)) {}
+
+    /**
+     * @brief Create a PointFieldInfo using the name of the datatype, which can be one of
+     *        the following names: "int8", "uint8", "int16", "uint16", "int32", "uint32", "float32", "float64"
+     *        Adding this nameless field will cause a padding in the cloud buffer with the size for the datatype.
+     * @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 datatype_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, ... see PointField.msg)
+     *        Adding this nameless field will cause a padding in the cloud buffer with the size for the datatype.
+     * @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 int datatype)
+        : 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,22 +229,33 @@ 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 */
   PointCloud2& cloud_msg_;
-    /** The current offset for all point fields */
-  int current_offset_;
 };
 
 namespace impl
diff --git a/sensor_msgs/include/sensor_msgs/point_field_conversion.h b/sensor_msgs/include/sensor_msgs/point_field_conversion.h
index d3fe2596..3dd423d0 100644
--- a/sensor_msgs/include/sensor_msgs/point_field_conversion.h
+++ b/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)