Program Listing for File point_cloud2_iterator.hpp

↰ Return to documentation for file (/tmp/ws/src/common_interfaces/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.hpp)

// Copyright (c) 2013, Open Source Robotics Foundation, Inc.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the copyright holder nor the names of its
//      contributors may be used to endorse or promote products derived from
//      this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

// This file is originally from:
// https://github.com/ros/common_msgs/blob/50ee957/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h

#ifndef SENSOR_MSGS__IMPL__POINT_CLOUD2_ITERATOR_HPP_
#define SENSOR_MSGS__IMPL__POINT_CLOUD2_ITERATOR_HPP_

#include <sensor_msgs/msg/point_cloud2.hpp>
#include <cstdarg>
#include <sstream>
#include <string>
#include <vector>

namespace
{
inline int sizeOfPointField(int datatype)
{
  if ((datatype == sensor_msgs::msg::PointField::INT8) ||
    (datatype == sensor_msgs::msg::PointField::UINT8))
  {
    return 1;
  } else if ((datatype == sensor_msgs::msg::PointField::INT16) ||  // NOLINT
    (datatype == sensor_msgs::msg::PointField::UINT16))
  {
    return 2;
  } else if ((datatype == sensor_msgs::msg::PointField::INT32) ||  // NOLINT
    (datatype == sensor_msgs::msg::PointField::UINT32) ||
    (datatype == sensor_msgs::msg::PointField::FLOAT32))
  {
    return 4;
  } else if (datatype == sensor_msgs::msg::PointField::FLOAT64) {
    return 8;
  } else {
    std::stringstream err;
    err << "PointField of type " << datatype << " does not exist";
    throw std::runtime_error(err.str());
  }
  return -1;
}

inline int addPointField(
  sensor_msgs::msg::PointCloud2 & cloud_msg,
  const std::string & name, int count, int datatype,
  int offset)
{
  sensor_msgs::msg::PointField point_field;
  point_field.name = name;
  point_field.count = count;
  point_field.datatype = datatype;
  point_field.offset = offset;
  cloud_msg.fields.push_back(point_field);

  // Update the offset
  return offset + point_field.count * sizeOfPointField(datatype);
}
}  // namespace


namespace sensor_msgs
{

inline PointCloud2Modifier::PointCloud2Modifier(
  sensor_msgs::msg::PointCloud2 & cloud_msg)
: cloud_msg_(cloud_msg)
{
}

inline size_t PointCloud2Modifier::size() const
{
  return cloud_msg_.data.size() / cloud_msg_.point_step;
}

inline void PointCloud2Modifier::reserve(size_t size)
{
  cloud_msg_.data.reserve(size * cloud_msg_.point_step);
}

inline void PointCloud2Modifier::resize(size_t size)
{
  cloud_msg_.data.resize(size * cloud_msg_.point_step);

  // Update height/width
  if (cloud_msg_.height == 1) {
    cloud_msg_.width = static_cast<uint32_t>(size);
    cloud_msg_.row_step = static_cast<uint32_t>(size * cloud_msg_.point_step);
  } else {
    if (cloud_msg_.width == 1) {
      cloud_msg_.height = static_cast<uint32_t>(size);
    } else {
      cloud_msg_.height = 1;
      cloud_msg_.width = static_cast<uint32_t>(size);
      cloud_msg_.row_step = static_cast<uint32_t>(size * cloud_msg_.point_step);
    }
  }
}

inline void PointCloud2Modifier::clear()
{
  cloud_msg_.data.clear();

  // Update height/width
  if (cloud_msg_.height == 1) {
    cloud_msg_.row_step = cloud_msg_.width = 0;
  } else {
    if (cloud_msg_.width == 1) {
      cloud_msg_.height = 0;
    } else {
      cloud_msg_.row_step = cloud_msg_.width = cloud_msg_.height = 0;
    }
  }
}


inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
{
  cloud_msg_.fields.clear();
  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);
  }
  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 void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...)
{
  cloud_msg_.fields.clear();
  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::msg::PointField point_field;
      // Do x, y and z
      offset = addPointField(
        cloud_msg_, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
      offset = addPointField(
        cloud_msg_, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
      offset = addPointField(
        cloud_msg_, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
      offset += sizeOfPointField(sensor_msgs::msg::PointField::FLOAT32);
    } else {
      if ((field_name == "rgb") || (field_name == "rgba")) {
        offset = addPointField(
          cloud_msg_, field_name, 1, sensor_msgs::msg::PointField::FLOAT32,
          offset);
        offset += 3 * sizeOfPointField(sensor_msgs::msg::PointField::FLOAT32);
      } else {
        va_end(vl);
        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);
}


namespace impl
{

template<typename T, typename TT, typename U, typename C, template<typename> class V>
PointCloud2IteratorBase<T, TT, U, C, V>::PointCloud2IteratorBase()
: data_char_(0), data_(0), data_end_(0)
{
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
PointCloud2IteratorBase<T, TT, U, C, V>::PointCloud2IteratorBase(
  C & cloud_msg, const std::string & field_name)
{
  int offset = set_field(cloud_msg, field_name);

  data_char_ = &(cloud_msg.data.front()) + offset;
  data_ = reinterpret_cast<TT *>(data_char_);
  data_end_ = reinterpret_cast<TT *>(&(cloud_msg.data.back()) + 1 + offset);
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
V<T> & PointCloud2IteratorBase<T, TT, U, C, V>::operator=(const V<T> & iter)
{
  if (this != &iter) {
    point_step_ = iter.point_step_;
    data_char_ = iter.data_char_;
    data_ = iter.data_;
    data_end_ = iter.data_end_;
    is_bigendian_ = iter.is_bigendian_;
  }

  return *this;
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
TT & PointCloud2IteratorBase<T, TT, U, C, V>::operator[](size_t i) const
{
  return *(data_ + i);
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
TT & PointCloud2IteratorBase<T, TT, U, C, V>::operator*() const
{
  return *data_;
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
V<T> & PointCloud2IteratorBase<T, TT, U, C, V>::operator++()
{
  data_char_ += point_step_;
  data_ = reinterpret_cast<TT *>(data_char_);
  return *static_cast<V<T> *>(this);
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
V<T> PointCloud2IteratorBase<T, TT, U, C, V>::operator+(int i)
{
  V<T> res = *static_cast<V<T> *>(this);

  res.data_char_ += i * point_step_;
  res.data_ = reinterpret_cast<TT *>(res.data_char_);

  return res;
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
V<T> & PointCloud2IteratorBase<T, TT, U, C, V>::operator+=(int i)
{
  data_char_ += i * point_step_;
  data_ = reinterpret_cast<TT *>(data_char_);
  return *static_cast<V<T> *>(this);
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
bool PointCloud2IteratorBase<T, TT, U, C, V>::operator!=(const V<T> & iter) const
{
  return iter.data_ != data_;
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
V<T> PointCloud2IteratorBase<T, TT, U, C, V>::end() const
{
  V<T> res = *static_cast<const V<T> *>(this);
  res.data_ = data_end_;
  return res;
}

template<typename T, typename TT, typename U, typename C, template<typename> class V>
int PointCloud2IteratorBase<T, TT, U, C, V>::set_field(
  const sensor_msgs::msg::PointCloud2 & cloud_msg, const std::string & field_name)
{
  is_bigendian_ = cloud_msg.is_bigendian;
  point_step_ = cloud_msg.point_step;
  // make sure the channel is valid
  std::vector<sensor_msgs::msg::PointField>::const_iterator field_iter = cloud_msg.fields.begin(),
    field_end =
    cloud_msg.fields.end();
  while ((field_iter != field_end) && (field_iter->name != field_name)) {
    ++field_iter;
  }

  if (field_iter == field_end) {
    // Handle the special case of r,g,b,a (we assume they are understood as the
    // channels of an rgb or rgba field)
    if ((field_name == "r") || (field_name == "g") || (field_name == "b") || (field_name == "a")) {
      // Check that rgb or rgba is present
      field_iter = cloud_msg.fields.begin();
      while ((field_iter != field_end) && (field_iter->name != "rgb") &&
        (field_iter->name != "rgba"))
      {
        ++field_iter;
      }
      if (field_iter == field_end) {
        throw std::runtime_error("Field " + field_name + " does not exist");
      }
      if (field_name == "r") {
        if (is_bigendian_) {
          return field_iter->offset + 1;
        } else {
          return field_iter->offset + 2;
        }
      }
      if (field_name == "g") {
        if (is_bigendian_) {
          return field_iter->offset + 2;
        } else {
          return field_iter->offset + 1;
        }
      }
      if (field_name == "b") {
        if (is_bigendian_) {
          return field_iter->offset + 3;
        } else {
          return field_iter->offset + 0;
        }
      }
      if (field_name == "a") {
        if (is_bigendian_) {
          return field_iter->offset + 0;
        } else {
          return field_iter->offset + 3;
        }
      }
    } else {
      throw std::runtime_error("Field " + field_name + " does not exist");
    }
  }

  return field_iter->offset;
}

}  // namespace impl
}  // namespace sensor_msgs

#endif  // SENSOR_MSGS__IMPL__POINT_CLOUD2_ITERATOR_HPP_