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

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 2 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
56 changes: 44 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 @@ -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)
{
}

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
}
Expand All @@ -197,36 +201,64 @@ 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
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);
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<PointFieldInfo> fields){
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a reason this is void not at least bool?

for(std::vector<PointFieldInfo>::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
Expand Down
14 changes: 14 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 @@ -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<PointFieldInfo> fields);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please mirror the setPointCloud2Fields API here instead of defining a new datatype.

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_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This seems to mirror the local storage of cloud_msg_.point_step ? Can you not query, mutate and reset without adding another data field?

};

namespace impl
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
47 changes: 47 additions & 0 deletions sensor_msgs/include/sensor_msgs/point_field_conversion.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@
#ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_H
#define SENSOR_MSGS_POINT_FIELD_CONVERSION_H

#include<string>

/**
* \brief This file provides a type to enum mapping for the different
* PointField types and methods to read and write in
Expand Down Expand Up @@ -79,6 +81,51 @@ namespace sensor_msgs{
template<> struct typeAsPointFieldType<float> { static const uint8_t value = sensor_msgs::PointField::FLOAT32; };
template<> struct typeAsPointFieldType<double> { static const uint8_t value = sensor_msgs::PointField::FLOAT64; };

/*!
* \Type names of the PointField data type.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please add docblock info for the parameter like the other functions.

*/
int getPointFieldTypeFromString(const std::string field_name){
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since this is not templated it needs to be inline with the implementation in the header. Also please switch to a const reference instead of pass by value of the string.

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;
}

/** 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<sensor_msgs::PointField::INT8>::type );
case sensor_msgs::PointField::UINT8:
return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::UINT8>::type );
case sensor_msgs::PointField::INT16:
return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::INT16>::type );
case sensor_msgs::PointField::UINT16:
return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::UINT16>::type );
case sensor_msgs::PointField::INT32:
return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::INT32>::type );
case sensor_msgs::PointField::UINT32:
return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::UINT32>::type );
case sensor_msgs::PointField::FLOAT32:
return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::FLOAT32>::type );
case sensor_msgs::PointField::FLOAT64:
return sizeof( pointFieldTypeAsType<sensor_msgs::PointField::FLOAT64>::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
Expand Down