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 c8d7de8a..4aa76088 100644 --- a/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h +++ b/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h @@ -36,6 +36,9 @@ #define SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H #include +#include +#include + #include #include #include @@ -133,6 +136,7 @@ inline void PointCloud2Modifier::resize(size_t size) inline void PointCloud2Modifier::clear() { cloud_msg_.data.clear(); + cloud_msg_.point_step = 0; // Update height/width if (cloud_msg_.height == 1) @@ -144,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& 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 @@ -165,21 +176,20 @@ inline void PointCloud2Modifier::clear() inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...) { cloud_msg_.fields.clear(); + cloud_msg_.point_step = 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); + 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 = offset; cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step; cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step); } @@ -197,10 +207,10 @@ inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...) inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...) { cloud_msg_.fields.clear(); + cloud_msg_.point_step = 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 @@ -208,25 +218,61 @@ inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...) 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); + 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")) { - offset = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, offset); - 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 = offset; cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step; cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step); } + +inline bool PointCloud2Modifier::addPointCloud2Fields(const std::vector& fields){ + bool ret = true; + for(std::vector::const_iterator iter = fields.begin(); iter != fields.end(); ++iter){ + if(!addPointCloud2Field(*iter)){ + ret = false; + } + } + return ret; +} + +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_, field.name)){ + std::cerr << "Could not add the field to the PointCloud2. The field name \"" + << field.name << "\" already exists!" << std::endl; + return false; + } + 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_.row_step = cloud_msg_.width * cloud_msg_.point_step; + cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step); + + return true; +} + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// namespace impl @@ -248,7 +294,7 @@ PointCloud2IteratorBase::PointCloud2IteratorBase(C &cloud_msg, c { int offset = set_field(cloud_msg, field_name); - data_char_ = &(cloud_msg.data.front()) + offset; + data_begin_ = data_char_ = &(cloud_msg.data.front()) + offset; data_ = reinterpret_cast(data_char_); data_end_ = reinterpret_cast(&(cloud_msg.data.back()) + 1 + offset); } @@ -318,6 +364,19 @@ V PointCloud2IteratorBase::operator +(int i) return res; } +/** + * Organized point cloud access, access the iterator at the associated position (column, row), where 0 <= column < width + * and 0 <= row <= height. + * @param column The column in which the iterator should be accessed, iterator at (column, row) + * @param row The row in whcih the iterator should be accessed, iterator at (column, row) + * @return an iterator at (column, row) + */ +template class V> +V& PointCloud2IteratorBase::operator ()(int column, int row) +{ + return reset() += (row * width + column); +} + /** Increase the iterator by a certain amount * @return a reference to the updated iterator */ @@ -338,6 +397,29 @@ bool PointCloud2IteratorBase::operator !=(const V& iter) cons return iter.data_ != data_; } +/** Resets the iterator to the start + * @return The reset iterator + */ +template class V> +V& PointCloud2IteratorBase::reset() +{ + data_char_ = data_begin_; + data_ = reinterpret_cast(data_char_); + return *static_cast*>(this); +} + +/** Return the start iterator + * @return the start iterator + */ +template class V> +V PointCloud2IteratorBase::start() const +{ + V res = *static_cast*>(this); + res.data_char_ = data_begin_; + res.data_ = reinterpret_cast(data_char_); + return res; +} + /** Return the end iterator * @return the end iterator (useful when performing normal iterator processing with ++) */ @@ -359,6 +441,8 @@ int PointCloud2IteratorBase::set_field(const sensor_msgs::PointC { is_bigendian_ = cloud_msg.is_bigendian; point_step_ = cloud_msg.point_step; + width = cloud_msg.width; + height = cloud_msg.height; // make sure the channel is valid std::vector::const_iterator field_iter = cloud_msg.fields.begin(), field_end = cloud_msg.fields.end(); diff --git a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h b/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h index 5b9c7a1b..0b71be31 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 +#include #include #include #include @@ -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& fields); + /** * @brief Function setting some fields in a PointCloud and adjusting the * internals of the PointCloud2 @@ -161,6 +228,31 @@ class PointCloud2Modifier * WARNING: THIS FUNCTION DOES ADD ANY NECESSARY PADDING TRANSPARENTLY */ void setPointCloud2FieldsByString(int n_fields, ...); + + /** + * @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& fields); + + /** + * @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_; @@ -223,11 +315,30 @@ class PointCloud2IteratorBase */ V& operator +=(int i); + /** + * Organized point cloud access, access the iterator at the associated position (column, row), where 0 <= column < width + * and 0 <= row <= height. + * @param column The column in which the iterator should be accessed, iterator at (column, row) + * @param row The row in whcih the iterator should be accessed, iterator at (column, row) + * @return an iterator at (column, row) + */ + V& operator ()(int column, int row); + /** Compare to another iterator * @return whether the current iterator points to a different address than the other one */ bool operator !=(const V& iter) const; + /** Reset to start + * @return Resets the iterator to the start + */ + V& reset(); + + /** Return the start iterator + * @return the start iterator + */ + V start() const; + /** Return the end iterator * @return the end iterator (useful when performing normal iterator processing with ++) */ @@ -243,12 +354,18 @@ class PointCloud2IteratorBase /** The "point_step" of the original cloud */ int point_step_; + /** The "width" of the original cloud */ + int width; + /** The "height" of the original cloud */ + int height; /** The raw data in uchar* where the iterator is */ U* data_char_; /** The cast data where the iterator is */ TT* data_; /** The end() pointer of the iterator */ TT* data_end_; + /** pointer to first value in field */ + U* data_begin_; /** Whether the fields are stored as bigendian */ bool is_bigendian_; }; diff --git a/sensor_msgs/include/sensor_msgs/point_cloud_conversion.h b/sensor_msgs/include/sensor_msgs/point_cloud_conversion.h index 439145da..a5725a48 100644 --- a/sensor_msgs/include/sensor_msgs/point_cloud_conversion.h +++ b/sensor_msgs/include/sensor_msgs/point_cloud_conversion.h @@ -41,6 +41,7 @@ #include #include #include +#include /** * \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 diff --git a/sensor_msgs/include/sensor_msgs/point_field_conversion.h b/sensor_msgs/include/sensor_msgs/point_field_conversion.h index 473b8530..815ef0b6 100644 --- a/sensor_msgs/include/sensor_msgs/point_field_conversion.h +++ b/sensor_msgs/include/sensor_msgs/point_field_conversion.h @@ -46,6 +46,8 @@ #ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_H #define SENSOR_MSGS_POINT_FIELD_CONVERSION_H +#include + /** * \brief This file provides a type to enum mapping for the different * PointField types and methods to read and write in @@ -54,7 +56,7 @@ */ namespace sensor_msgs{ /*! - * \Enum to type mapping. + * \brief Enum to type mapping. */ template struct pointFieldTypeAsType {}; template<> struct pointFieldTypeAsType { typedef int8_t type; }; @@ -67,7 +69,7 @@ namespace sensor_msgs{ template<> struct pointFieldTypeAsType { typedef double type; }; /*! - * \Type to enum mapping. + * \brief Type to enum mapping. */ template struct typeAsPointFieldType {}; template<> struct typeAsPointFieldType { static const uint8_t value = sensor_msgs::PointField::INT8; }; @@ -80,7 +82,82 @@ namespace sensor_msgs{ template<> struct typeAsPointFieldType { static const uint8_t value = sensor_msgs::PointField::FLOAT64; }; /*! - * \Converts a value at the given pointer position, interpreted as the datatype + * \brief Type names of the PointField data type. + * \param field_name Type name for a dynamic type initialization in the pointfield + */ + 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; + if(field_name == "uint16") return sensor_msgs::PointField::UINT16; + if(field_name == "int32") return sensor_msgs::PointField::INT32; + if(field_name == "uint32") return sensor_msgs::PointField::UINT32; + if(field_name == "float32") return sensor_msgs::PointField::FLOAT32; + if(field_name == "float64") return sensor_msgs::PointField::FLOAT64; + + std::cerr << "Unknown type: \"" << field_name << "\"! Supported types are: int8, uint8, int16, uint16, int32, uint32, float32, float64" << std::endl; + return -1; + } + + /*! + * \brief Returns 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 ""; + } + } + /*! + * \brief Returns 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::type ); + case sensor_msgs::PointField::UINT8: + return sizeof( pointFieldTypeAsType::type ); + case sensor_msgs::PointField::INT16: + return sizeof( pointFieldTypeAsType::type ); + case sensor_msgs::PointField::UINT16: + return sizeof( pointFieldTypeAsType::type ); + case sensor_msgs::PointField::INT32: + return sizeof( pointFieldTypeAsType::type ); + case sensor_msgs::PointField::UINT32: + return sizeof( pointFieldTypeAsType::type ); + case sensor_msgs::PointField::FLOAT32: + return sizeof( pointFieldTypeAsType::type ); + case sensor_msgs::PointField::FLOAT64: + return sizeof( pointFieldTypeAsType::type ); + default: + std::cerr << "unknown datatype with the number: " << datatype; + return -1; + } + } + + /*! + * \brief Converts a value at the given pointer position, interpreted as the datatype * specified by the given template argument point_field_type, to the given * template type T and returns it. * \param data_ptr pointer into the point cloud 2 buffer @@ -94,7 +171,7 @@ namespace sensor_msgs{ } /*! - * \Converts a value at the given pointer position interpreted as the datatype + * \brief Converts a value at the given pointer position interpreted as the datatype * specified by the given datatype parameter to the given template type and returns it. * \param data_ptr pointer into the point cloud 2 buffer * \param datatype sensor_msgs::PointField datatype value @@ -123,7 +200,7 @@ namespace sensor_msgs{ } /*! - * \Inserts a given value at the given point position interpreted as the datatype + * \brief 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 @@ -137,7 +214,7 @@ namespace sensor_msgs{ } /*! - * \Inserts a given value at the given point position interpreted as the datatype + * \brief 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