point_iterator_safe.h

Go to the documentation of this file.

#include <boost/foreach.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointField.h>

namespace point_cloud_color {

template <bool SAFE> class PointCloud2Iterator {
public:
  PointCloud2Iterator(sensor_msgs::PointCloud2::Ptr pcl, std::vector<std::string> fieldNames);
  inline bool isFieldAvailable(uint32_t fieldIndex) {
      return fieldsAvailable[fieldIndex];
  }
  template <class T> inline bool isFieldCompatible<T>(uint32_t fieldIndex) {
      return sizeof(T) == fieldDataSizes[fieldIndex];
  }
  template <> inline bool isFieldCompatible<float>(uint32_t fieldIndex) {
      return fieldDataTypes[fieldIndex] == sensor_msgs::PointField::FLOAT32;
  }
  template <bool SAFE, class T> T getField<T>(uint32_t pointIndex, uint32_t fieldIndex);
  template <class T> void setField<T>(uint32_t pointIndex, uint32_t fieldIndex, T value);
private:
  inline void checkFieldAvailable(uint32_t fieldIndex) {
      if (SAFE && !isFieldAvailable(fieldIndex)) {
          throw std::runtime_error("Field not available.");
      }
  }
  template <class T> inline void checkFieldCompatible<T>(uint32_t fieldIndex) {
      if (SAFE && isFieldCompatible<T>(fieldIndex)) {
          throw std::runtime_error("Field not compatible.");
      }
  }
  sensor_msgs::PointCloud2::Ptr pcl;
  std::vector<std::string> fieldNames;
  std::vector<bool> fieldsAvailable;
  std::vector<uint32_t> fieldOffsets;
  std::vector<uint8_t> fieldDataTypes;
  std::vector<size_t> fieldDataSizes;
};

} // namespace point_cloud_color