Skip to content

Commit

Permalink
dynamic point field creation, docu and cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@spuetz
spuetz committed Oct 11, 2017
1 parent 7928bb5 commit 89bb853
Show file tree
Hide file tree
Showing 3 changed files with 159 additions and 37 deletions.
62 changes: 38 additions & 24 deletions sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
}

Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand All @@ -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);
Expand All @@ -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);
}
Expand All @@ -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);
Expand All @@ -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);

Expand Down
100 changes: 89 additions & 11 deletions sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
34 changes: 32 additions & 2 deletions sensor_msgs/include/sensor_msgs/point_field_conversion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)
Expand Down

0 comments on commit 89bb853

Please sign in to comment.