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..9092eea0 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 @@ -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,10 +201,10 @@ 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 @@ -208,25 +212,53 @@ 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); + 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 fields){ + for(std::vector::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){ + std::cerr << "Could not add the field to the PointCloud2. Unknown datatype \"" << 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; + 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 diff --git a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h b/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h index 5b9c7a1b..ca142049 100644 --- a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h +++ b/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 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 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 24cd5f21..d3fe2596 100644 --- a/sensor_msgs/include/sensor_msgs/point_field_conversion.h +++ b/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"); + std::cerr << "Unknown type: \"" << field_name << "\"! Supported types are: int8, uint8, int16, uint16, int32, uint32, float32, float64" << std::endl; 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::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; + } + } + /*! * \Converts a value at the given pointer position, interpreted as the datatype * specified by the given template argument point_field_type, to the given