Program Listing for File cloud.hpp
↰ Return to documentation for file (include/draco_point_cloud_transport/cloud.hpp
)
// Copyright (c) 2023, Open Source Robotics Foundation, Inc.
// Copyright (c) 2023, Czech Technical University in Prague
// Copyright (c) 2019, paplhjak
// Copyright (c) 2009, Willow Garage, 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.
#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_