Program Listing for File cloud.hpp

↰ Return to documentation for file (/tmp/ws/src/point_cloud_transport_plugins/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud.hpp)

/*
 * Copyright (c) 2023, Czech Technical University in Prague
 * Copyright (c) 2019, paplhjak
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * 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.
 */

#ifndef DRACO_POINT_CLOUD_TRANSPORT__CLOUD_HPP_
#define DRACO_POINT_CLOUD_TRANSPORT__CLOUD_HPP_

#include <limits>
#include <string>

#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/point_field.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>

#include "cloud/impl/cloud.hpp"

namespace cras
{

typedef ::sensor_msgs::msg::PointCloud2 Cloud;

typedef ::sensor_msgs::PointCloud2Iterator<float> CloudIter;

typedef ::sensor_msgs::PointCloud2Iterator<int> CloudIndexIter;

typedef ::sensor_msgs::PointCloud2ConstIterator<float> CloudConstIter;

typedef ::sensor_msgs::PointCloud2ConstIterator<int> CloudIndexConstIter;

typedef ::sensor_msgs::PointCloud2Modifier CloudModifier;

typedef ::cras::impl::GenericCloudIterator<> GenericCloudIter;

typedef ::cras::impl::GenericCloudConstIterator<> GenericCloudConstIter;

inline size_t numPoints(const ::cras::Cloud & cloud)
{
  return static_cast<size_t>(cloud.height) * static_cast<size_t>(cloud.width);
}

bool hasField(const ::cras::Cloud & cloud, const ::std::string & fieldName);

::sensor_msgs::msg::PointField & getField(::cras::Cloud & cloud, const ::std::string & fieldName);

const ::sensor_msgs::msg::PointField & getField(
  const ::cras::Cloud & cloud,
  const ::std::string & fieldName);

size_t sizeOfPointField(int datatype);

size_t sizeOfPointField(const ::sensor_msgs::msg::PointField & field);

void copyChannelData(
  const ::cras::Cloud & in, ::cras::Cloud & out,
  const ::std::string & fieldName);

#define CREATE_FILTERED_CLOUD(IN, OUT, KEEP_ORGANIZED, FILTER) { \
    const auto inputIsOrganized = (IN).height > 1; \
    const auto outIsOrganized = (KEEP_ORGANIZED) && inputIsOrganized; \
 \
    (OUT).header = (IN).header; \
    (OUT).fields = (IN).fields; \
    (OUT).point_step = (IN).point_step; \
    (OUT).height = outIsOrganized ? (IN).height : 1; \
    (OUT).width = outIsOrganized ? (IN).width : 0; \
 \
    (OUT).data.resize(0); \
    (OUT).data.reserve((IN).data.size()); \
 \
    ::cras::CloudConstIter x_it((IN), "x"); \
    ::cras::CloudConstIter y_it((IN), "y"); \
    ::cras::CloudConstIter z_it((IN), "z"); \
 \
    const auto numPoints = ::cras::numPoints(IN); \
 \
    if (!outIsOrganized) { \
      for (size_t i = 0; i < numPoints; ++i, ++x_it, ++y_it, ++z_it) { \
        if (FILTER) { \
          size_t from = (i / (IN).width) * (IN).row_step + (i % (IN).width) * (IN).point_step; \
          size_t to = from + (IN).point_step; \
          (OUT).data.insert( \
            (OUT).data.end(), (IN).data.begin() + from, \
            (IN).data.begin() + to); \
          (OUT).width++; \
        } \
      } \
      (OUT).is_dense = true; \
    } else { \
      (OUT).data.insert((OUT).data.end(), (IN).data.begin(), (IN).data.end()); \
 \
      ::cras::CloudIter x_out_it((OUT), "x"); \
      ::cras::CloudIter y_out_it((OUT), "y"); \
      ::cras::CloudIter z_out_it((OUT), "z"); \
      const auto invalidValue = std::numeric_limits<float>::quiet_NaN(); \
 \
      for (size_t i = 0; i < numPoints; \
        ++i, ++x_it, ++y_it, ++z_it, ++x_out_it, ++y_out_it, ++z_out_it) { \
        if (!(FILTER)) { \
          *x_out_it = *y_out_it = *z_out_it = invalidValue; \
          (OUT).is_dense = false; \
        } \
      } \
    } \
 \
    (OUT).row_step = (OUT).width * (OUT).point_step; \
}
}  // namespace cras

#endif  // DRACO_POINT_CLOUD_TRANSPORT__CLOUD_HPP_