Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Support for dynamic PointCloud2 creation #108

Open
wants to merge 5 commits into
base: jade-devel
Choose a base branch
from
Open

Support for dynamic PointCloud2 creation #108

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8911f10
added string to point-field-type method
spuetz Feb 3, 2017
7928bb5
support for dynamic point cloud field creation
spuetz Aug 14, 2017
89bb853
dynamic point field creation, docu and cleanup
spuetz Oct 10, 2017
695b643
docu cleanup
spuetz Oct 11, 2017
71b7b42
added organized iter access with operator(column, row)
spuetz Oct 11, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
108 changes: 96 additions & 12 deletions sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,9 @@
#define SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_field_conversion.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include <cstdarg>
#include <string>
#include <vector>
Expand Down Expand Up @@ -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)
Expand All @@ -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<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 @@ -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);
}
Expand All @@ -197,36 +207,72 @@ 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
field_name = std::string(va_arg(vl, char*));
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<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(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
Expand All @@ -248,7 +294,7 @@ PointCloud2IteratorBase<T, TT, U, C, V>::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<TT*>(data_char_);
data_end_ = reinterpret_cast<TT*>(&(cloud_msg.data.back()) + 1 + offset);
}
Expand Down Expand Up @@ -318,6 +364,19 @@ V<T> PointCloud2IteratorBase<T, TT, U, C, V>::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<typename T, typename TT, typename U, typename C, template <typename> class V>
V<T>& PointCloud2IteratorBase<T, TT, U, C, V>::operator ()(int column, int row)
{
return reset() += (row * width + column);
}

/** Increase the iterator by a certain amount
* @return a reference to the updated iterator
*/
Expand All @@ -338,6 +397,29 @@ bool PointCloud2IteratorBase<T, TT, U, C, V>::operator !=(const V<T>& iter) cons
return iter.data_ != data_;
}

/** Resets the iterator to the start
* @return The reset iterator
*/
template<typename T, typename TT, typename U, typename C, template <typename> class V>
V<T>& PointCloud2IteratorBase<T, TT, U, C, V>::reset()
{
data_char_ = data_begin_;
data_ = reinterpret_cast<TT*>(data_char_);
return *static_cast<V<T>*>(this);
}

/** Return the start iterator
* @return the start iterator
*/
template<typename T, typename TT, typename U, typename C, template <typename> class V>
V<T> PointCloud2IteratorBase<T, TT, U, C, V>::start() const
{
V<T> res = *static_cast<const V<T>*>(this);
res.data_char_ = data_begin_;
res.data_ = reinterpret_cast<TT*>(data_char_);
return res;
}

/** Return the end iterator
* @return the end iterator (useful when performing normal iterator processing with ++)
*/
Expand All @@ -359,6 +441,8 @@ int PointCloud2IteratorBase<T, TT, U, C, V>::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<sensor_msgs::PointField>::const_iterator field_iter = cloud_msg.fields.begin(), field_end =
cloud_msg.fields.end();
Expand Down
117 changes: 117 additions & 0 deletions sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
View file
Open in desktop
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 @@ -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<PointFieldInfo>& 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_;
Expand Down Expand Up @@ -223,11 +315,30 @@ class PointCloud2IteratorBase
*/
V<T>& 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<T>& 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<T>& iter) const;

/** Reset to start
* @return Resets the iterator to the start
*/
V<T>& reset();

/** Return the start iterator
* @return the start iterator
*/
V<T> start() const;

/** Return the end iterator
* @return the end iterator (useful when performing normal iterator processing with ++)
*/
Expand All @@ -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_;
};
Expand Down
6 changes: 6 additions & 0 deletions sensor_msgs/include/sensor_msgs/point_cloud_conversion.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_field_conversion.h>
#include <algorithm>

/**
* \brief Convert between the old (sensor_msgs::PointCloud) and the new (sensor_msgs::PointCloud2) format.
Expand All @@ -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
Expand Down
Loading