From e5b6dbba7d8956171edf413bb0ca7b7ee79b1a8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 26 Jun 2023 13:36:04 +0200 Subject: [PATCH 1/6] Added common tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- CONTRIBUTING.md | 3 + draco_point_cloud_transport/CMakeLists.txt | 7 +- .../draco_point_cloud_transport/cloud.hpp | 149 +++++--- .../cloud/impl/cloud.hpp | 99 +++-- .../conversion_utilities.h | 65 +++- .../draco_publisher.hpp | 60 ++- .../draco_subscriber.hpp | 52 ++- draco_point_cloud_transport/package.xml | 4 +- draco_point_cloud_transport/src/cloud.cpp | 121 +++++-- .../src/draco_publisher.cpp | 341 +++++++++--------- .../src/draco_subscriber.cpp | 100 +++-- draco_point_cloud_transport/src/manifest.cpp | 41 ++- point_cloud_interfaces/CMakeLists.txt | 5 + point_cloud_interfaces/package.xml | 3 + 14 files changed, 682 insertions(+), 368 deletions(-) create mode 100644 CONTRIBUTING.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..309be1e --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the 3-Clause BSD License, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/draco_point_cloud_transport/CMakeLists.txt b/draco_point_cloud_transport/CMakeLists.txt index 95318b9..f8dffce 100644 --- a/draco_point_cloud_transport/CMakeLists.txt +++ b/draco_point_cloud_transport/CMakeLists.txt @@ -51,6 +51,11 @@ install( pluginlib_export_plugin_description_file(point_cloud_transport draco_plugins.xml) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + # TODO: Fix tests # if (CATKIN_ENABLE_TESTING) # find_package(roslint REQUIRED) @@ -88,4 +93,4 @@ pluginlib_export_plugin_description_file(point_cloud_transport draco_plugins.xml ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies(${dependencies}) -ament_package() \ No newline at end of file +ament_package() diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud.hpp b/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud.hpp index f9470a3..5fe900b 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud.hpp +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud.hpp @@ -1,13 +1,46 @@ -#pragma once +/* + * 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_ /** * \file * \brief Utilities for comfortable working with PointCloud2 messages. * \author Martin Pecka - * SPDX-License-Identifier: BSD-3-Clause - * SPDX-FileCopyrightText: Czech Technical University in Prague */ +#include #include #include @@ -65,7 +98,7 @@ typedef ::cras::impl::GenericCloudConstIterator<> GenericCloudConstIter; * \param[in] cloud The cloud to examine. * \return The number of points. */ -inline size_t numPoints(const ::cras::Cloud& cloud) +inline size_t numPoints(const ::cras::Cloud & cloud) { return static_cast(cloud.height) * static_cast(cloud.width); } @@ -76,7 +109,7 @@ inline size_t numPoints(const ::cras::Cloud& cloud) * \param[in] fieldName Name of the field. * \return Whether the field is there or not. */ -bool hasField(const ::cras::Cloud& cloud, const ::std::string& fieldName); +bool hasField(const ::cras::Cloud & cloud, const ::std::string & fieldName); /** * \brief Return the sensor_msgs::msg::PointField with the given name. @@ -85,7 +118,7 @@ bool hasField(const ::cras::Cloud& cloud, const ::std::string& fieldName); * \return Reference to the field. * \throws std::runtime_error if the field doesn't exist. */ -::sensor_msgs::msg::PointField& getField(::cras::Cloud& cloud, const ::std::string& fieldName); +::sensor_msgs::msg::PointField & getField(::cras::Cloud & cloud, const ::std::string & fieldName); /** * \brief Return the sensor_msgs::msg::PointField with the given name. @@ -94,7 +127,9 @@ ::sensor_msgs::msg::PointField& getField(::cras::Cloud& cloud, const ::std::stri * \return Reference to the field. * \throws std::runtime_error if the field doesn't exist. */ -const ::sensor_msgs::msg::PointField& getField(const ::cras::Cloud& cloud, const ::std::string& fieldName); +const ::sensor_msgs::msg::PointField & getField( + const ::cras::Cloud & cloud, + const ::std::string & fieldName); /** * \brief Return the size (in bytes) of a sensor_msgs::msg::PointField datatype. @@ -110,7 +145,7 @@ size_t sizeOfPointField(int datatype); * \return Size of the data. * \throws std::runtime_error if wrong datatype is passed. */ -size_t sizeOfPointField(const ::sensor_msgs::msg::PointField& field); +size_t sizeOfPointField(const ::sensor_msgs::msg::PointField & field); /** * \brief Copy data belonging to the given field from `in` cloud to `out` cloud. @@ -121,7 +156,9 @@ size_t sizeOfPointField(const ::sensor_msgs::msg::PointField& field); * \throws std::runtime_error If the output cloud is smaller (in nr. of points) than the input cloud. * \throws std::runtime_error If the given field doesn't exist in either of the clouds. */ -void copyChannelData(const ::cras::Cloud& in, ::cras::Cloud& out, const ::std::string& fieldName); +void copyChannelData( + const ::cras::Cloud & in, ::cras::Cloud & out, + const ::std::string & fieldName); /** * \brief Create a pointcloud that contains a subset of points of `IN` defined by @@ -129,52 +166,56 @@ void copyChannelData(const ::cras::Cloud& in, ::cras::Cloud& out, const ::std::s * which can use the following: `i`: index of the point, `x_it, y_it, z_it` iterators to XYZ coordinates. * Points for which FILTER is true are part of the final pointcloud. */ -#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++; \ +#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::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).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::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;\ -} + \ + (OUT).row_step = (OUT).width * (OUT).point_step; \ } +} // namespace cras + +#endif // DRACO_POINT_CLOUD_TRANSPORT__CLOUD_HPP_ diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud/impl/cloud.hpp b/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud/impl/cloud.hpp index 0ff2029..4cffcf8 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud/impl/cloud.hpp +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/cloud/impl/cloud.hpp @@ -1,13 +1,44 @@ -#pragma once +/* + * Copyright (c) 2023, Czech Technical University in Prague + * 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. + */ + /** * \file * \brief Utilities for comfortable working with PointCloud2 messages. * \author Martin Pecka - * SPDX-License-Identifier: BSD-3-Clause - * SPDX-FileCopyrightText: Czech Technical University in Prague */ +#ifndef DRACO_POINT_CLOUD_TRANSPORT__CLOUD__IMPL__CLOUD_HPP_ +#define DRACO_POINT_CLOUD_TRANSPORT__CLOUD__IMPL__CLOUD_HPP_ + #include #include @@ -32,24 +63,23 @@ template cla class GenericCloudIteratorBase : public ::sensor_msgs::impl::PointCloud2IteratorBase { public: - /** * \param[in] cloud_msg The PointCloud2 to iterate upon. * \param[in] field_name The field to iterate upon. */ - GenericCloudIteratorBase(C& cloudMsg, const ::std::string& fieldName); + GenericCloudIteratorBase(C & cloudMsg, const ::std::string & fieldName); /** * \brief Get the byte size of the field which this iterator iterates. * \return Size in bytes. */ - inline size_t getFieldSize() const { return this->fieldSize; } + inline size_t getFieldSize() const {return this->fieldSize;} /** * \brief Return a pointer to the raw data of this field. Only `getFieldSize()` bytes after this pointer are valid. * \return The pointer. */ - U* rawData() const; + U * rawData() const; protected: //! \brief The byte size of the field which this iterator iterates. @@ -60,8 +90,8 @@ class GenericCloudIteratorBase : public ::sensor_msgs::impl::PointCloud2Iterator * \brief Generic const cloud iterator which can return the data in the raw type. */ template -class GenericCloudConstIterator : - public GenericCloudIteratorBase { public: @@ -69,8 +99,10 @@ class GenericCloudConstIterator : * \param[in] cloud_msg The PointCloud2 to iterate upon. * \param[in] field_name The field to iterate upon. */ - GenericCloudConstIterator(const ::sensor_msgs::msg::PointCloud2 &cloud_msg, const ::std::string &field_name) : - GenericCloudIteratorBase::GenericCloudIteratorBase(cloud_msg, field_name) { } @@ -82,12 +114,14 @@ class GenericCloudConstIterator : * \throws std::runtime_error If sizeof(D) is not the same as getFieldSize(). */ template - const D* dataAs() const + const D * dataAs() const { - if (sizeof(D) != this->getFieldSize()) - throw ::std::runtime_error("Cannot convert field of size " + ::std::to_string(this->getFieldSize()) + - " to a type of size " + ::std::to_string(sizeof(D))); - return reinterpret_cast(this->rawData()); + if (sizeof(D) != this->getFieldSize()) { + throw ::std::runtime_error( + "Cannot convert field of size " + ::std::to_string(this->getFieldSize()) + + " to a type of size " + ::std::to_string(sizeof(D))); + } + return reinterpret_cast(this->rawData()); } }; @@ -95,16 +129,19 @@ class GenericCloudConstIterator : * \brief Generic non-const cloud iterator which can return the data in the raw type. */ template -class GenericCloudIterator : - public GenericCloudIteratorBase +class GenericCloudIterator + : public GenericCloudIteratorBase { public: /** * \param[in] cloud_msg The PointCloud2 to iterate upon. * \param[in] field_name The field to iterate upon. */ - GenericCloudIterator(::sensor_msgs::msg::PointCloud2 &cloud_msg, const ::std::string &field_name) : - GenericCloudIteratorBase::GenericCloudIteratorBase(cloud_msg, field_name) { } @@ -116,26 +153,30 @@ class GenericCloudIterator : * \throws std::runtime_error If sizeof(D) is not the same as getFieldSize(). */ template - D* dataAs() const + D * dataAs() const { - if (sizeof(D) != this->getFieldSize()) - throw ::std::runtime_error("Cannot convert field of size " + ::std::to_string(this->getFieldSize()) + - " to a type of size " + ::std::to_string(sizeof(D))); - return reinterpret_cast(this->rawData()); + if (sizeof(D) != this->getFieldSize()) { + throw ::std::runtime_error( + "Cannot convert field of size " + ::std::to_string(this->getFieldSize()) + + " to a type of size " + ::std::to_string(sizeof(D))); + } + return reinterpret_cast(this->rawData()); } /** * \brief Copy all values of this field from another iterator. * \param otherIter The other iterator. */ - void copyData(const ::cras::impl::GenericCloudConstIterator& otherIter) const; + void copyData(const ::cras::impl::GenericCloudConstIterator & otherIter) const; /** * \brief Copy all values of this field from another iterator. * \param otherIter The other iterator. */ - void copyData(const ::cras::impl::GenericCloudIterator& otherIter) const; + void copyData(const ::cras::impl::GenericCloudIterator & otherIter) const; }; -} -} +} // namespace impl +} // namespace cras + +#endif // DRACO_POINT_CLOUD_TRANSPORT__CLOUD__IMPL__CLOUD_HPP_ diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/conversion_utilities.h b/draco_point_cloud_transport/include/draco_point_cloud_transport/conversion_utilities.h index 628589e..f840f26 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/conversion_utilities.h +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/conversion_utilities.h @@ -1,23 +1,54 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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. + */ -#pragma once +#ifndef DRACO_POINT_CLOUD_TRANSPORT__CONVERSION_UTILITIES_H_ +#define DRACO_POINT_CLOUD_TRANSPORT__CONVERSION_UTILITIES_H_ namespace draco_point_cloud_transport { - //! copies header, width, ... between clouds -template -void copyCloudMetadata(PC1& target, const PC2& source) -{ - target.header = source.header; - target.height = source.height; - target.width = source.width; - target.fields = source.fields; - target.is_bigendian = source.is_bigendian; - target.point_step = source.point_step; - target.row_step = source.row_step; - target.is_dense = source.is_dense; -} + template < typename PC1, typename PC2 > + void copyCloudMetadata(PC1 & target, const PC2 & source) + { + target.header = source.header; + target.height = source.height; + target.width = source.width; + target.fields = source.fields; + target.is_bigendian = source.is_bigendian; + target.point_step = source.point_step; + target.row_step = source.row_step; + target.is_dense = source.is_dense; + } + +} // namespace draco_point_cloud_transport -} +#endif // DRACO_POINT_CLOUD_TRANSPORT__CONVERSION_UTILITIES_H_ diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp index 7b81fa7..9e50077 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp @@ -1,13 +1,43 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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. + */ -#pragma once +#ifndef DRACO_POINT_CLOUD_TRANSPORT__DRACO_PUBLISHER_HPP_ +#define DRACO_POINT_CLOUD_TRANSPORT__DRACO_PUBLISHER_HPP_ + +#include #include #include -#include - #include #include @@ -18,22 +48,25 @@ namespace draco_point_cloud_transport { class DracoPublisher - : public point_cloud_transport::SimplePublisherPlugin + : public point_cloud_transport::SimplePublisherPlugin< + point_cloud_interfaces::msg::CompressedPointCloud2> { public: std::string getTransportName() const override; - TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2& raw) const override; + TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override; - static void registerPositionField(const std::string& field); - static void registerColorField(const std::string& field); - static void registerNormalField(const std::string& field); + static void registerPositionField(const std::string & field); + static void registerColorField(const std::string & field); + static void registerNormalField(const std::string & field); private: cras::expected, std::string> convertPC2toDraco( - const sensor_msgs::msg::PointCloud2& PC2, const std::string& topic, bool deduplicate, bool expert_encoding) const; + const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate, + bool expert_encoding) const; - struct DracoPublisherConfig{ + struct DracoPublisherConfig + { int encode_speed = 7; int decode_speed = 7; int method_enum = 0; @@ -51,5 +84,6 @@ class DracoPublisher DracoPublisherConfig config_; }; +} // namespace draco_point_cloud_transport -} +#endif // DRACO_POINT_CLOUD_TRANSPORT__DRACO_PUBLISHER_HPP_ diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp index a76dabc..d71d415 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp @@ -1,14 +1,43 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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__DRACO_SUBSCRIBER_HPP_ +#define DRACO_POINT_CLOUD_TRANSPORT__DRACO_SUBSCRIBER_HPP_ -#pragma once +#include #include #include -#include - #include #include @@ -16,14 +45,17 @@ namespace draco_point_cloud_transport { class DracoSubscriber - : public point_cloud_transport::SimpleSubscriberPlugin + : public point_cloud_transport::SimpleSubscriberPlugin< + point_cloud_interfaces::msg::CompressedPointCloud2> { public: std::string getTransportName() const override; - DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2& compressed) const override; + DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed) + const override; - struct DracoSubscriberConfig{ + struct DracoSubscriberConfig + { bool SkipDequantizationPOSITION = false; bool SkipDequantizationNORMAL = false; bool SkipDequantizationCOLOR = false; @@ -32,7 +64,7 @@ class DracoSubscriber }; DracoSubscriberConfig config_; - }; +} // namespace draco_point_cloud_transport -} +#endif // DRACO_POINT_CLOUD_TRANSPORT__DRACO_SUBSCRIBER_HPP_ diff --git a/draco_point_cloud_transport/package.xml b/draco_point_cloud_transport/package.xml index 7b3a47d..ec51583 100644 --- a/draco_point_cloud_transport/package.xml +++ b/draco_point_cloud_transport/package.xml @@ -27,8 +27,10 @@ sensor_msgs std_msgs + ament_lint_auto + ament_lint_common + ament_cmake - diff --git a/draco_point_cloud_transport/src/cloud.cpp b/draco_point_cloud_transport/src/cloud.cpp index dd45836..9b4a258 100644 --- a/draco_point_cloud_transport/src/cloud.cpp +++ b/draco_point_cloud_transport/src/cloud.cpp @@ -1,117 +1,160 @@ -/** - * \file - * \brief Utilities for comfortable working with PointCloud2 messages. - * \author Martin Pecka - * SPDX-License-Identifier: BSD-3-Clause - * SPDX-FileCopyrightText: Czech Technical University in Prague +/* + * Copyright (c) 2023, Czech Technical University in Prague + * 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. */ // HACK: we need to access PointCloud2IteratorBase::data_char_ which is private +#include +#include #include +#include + #define private protected #include #undef private -#include -#include -#include #include namespace cras { -bool hasField(const ::cras::Cloud &cloud, const std::string &fieldName) +bool hasField(const ::cras::Cloud & cloud, const std::string & fieldName) { - return std::any_of(cloud.fields.begin(), cloud.fields.end(), - [&fieldName](const sensor_msgs::msg::PointField& f) {return f.name == fieldName;}); + return std::any_of( + cloud.fields.begin(), cloud.fields.end(), + [&fieldName](const sensor_msgs::msg::PointField & f) {return f.name == fieldName;}); } -sensor_msgs::msg::PointField& getField(::cras::Cloud& cloud, const std::string& fieldName) +sensor_msgs::msg::PointField & getField(::cras::Cloud & cloud, const std::string & fieldName) { - for (auto &field : cloud.fields) { - if (field.name == fieldName) + for (auto & field : cloud.fields) { + if (field.name == fieldName) { return field; + } } throw std::runtime_error(std::string("Field ") + fieldName + " does not exist."); } -const sensor_msgs::msg::PointField& getField(const ::cras::Cloud& cloud, const std::string& fieldName) +const sensor_msgs::msg::PointField & getField( + const ::cras::Cloud & cloud, + const std::string & fieldName) { - for (const auto &field : cloud.fields) { - if (field.name == fieldName) + for (const auto & field : cloud.fields) { + if (field.name == fieldName) { return field; + } } throw std::runtime_error(std::string("Field ") + fieldName + " does not exist."); } -size_t sizeOfPointField(const ::sensor_msgs::msg::PointField& field) +size_t sizeOfPointField(const ::sensor_msgs::msg::PointField & field) { return sizeOfPointField(field.datatype); } size_t sizeOfPointField(int datatype) { - if ((datatype == sensor_msgs::msg::PointField::INT8) || (datatype == sensor_msgs::msg::PointField::UINT8)) + if ((datatype == sensor_msgs::msg::PointField::INT8) || + (datatype == sensor_msgs::msg::PointField::UINT8)) + { return 1u; - else if ((datatype == sensor_msgs::msg::PointField::INT16) || (datatype == sensor_msgs::msg::PointField::UINT16)) + } else if ((datatype == sensor_msgs::msg::PointField::INT16) || // NOLINT + (datatype == sensor_msgs::msg::PointField::UINT16)) + { return 2u; - else if ((datatype == sensor_msgs::msg::PointField::INT32) || (datatype == sensor_msgs::msg::PointField::UINT32) || - (datatype == sensor_msgs::msg::PointField::FLOAT32)) + } else if ((datatype == sensor_msgs::msg::PointField::INT32) || // NOLINT + (datatype == sensor_msgs::msg::PointField::UINT32) || + (datatype == sensor_msgs::msg::PointField::FLOAT32)) + { return 4u; - else if (datatype == sensor_msgs::msg::PointField::FLOAT64) + } else if (datatype == sensor_msgs::msg::PointField::FLOAT64) { return 8u; - else - throw std::runtime_error(std::string("PointField of type ") + std::to_string(datatype) + " does not exist"); + } else { + throw std::runtime_error( + std::string("PointField of type ") + std::to_string( + datatype) + " does not exist"); + } } -void copyChannelData(const ::cras::Cloud& in, ::cras::Cloud& out, const std::string& fieldName) +void copyChannelData(const ::cras::Cloud & in, ::cras::Cloud & out, const std::string & fieldName) { - if (numPoints(out) < numPoints(in)) - throw std::runtime_error("Output cloud needs to be resized to fit the number of points of the input cloud."); + if (numPoints(out) < numPoints(in)) { + throw std::runtime_error( + "Output cloud needs to be resized to fit the number of points of the input cloud."); + } GenericCloudConstIter dataIn(in, fieldName); GenericCloudIter dataOut(out, fieldName); - for (; dataIn != dataIn.end(); ++dataIn, ++dataOut) + for (; dataIn != dataIn.end(); ++dataIn, ++dataOut) { dataOut.copyData(dataIn); + } } namespace impl { template class V> -GenericCloudIteratorBase::GenericCloudIteratorBase(C& cloudMsg, const std::string& fieldName) - : sensor_msgs::impl::PointCloud2IteratorBase(cloudMsg, fieldName) +GenericCloudIteratorBase::GenericCloudIteratorBase( + C & cloudMsg, + const std::string & fieldName) +: sensor_msgs::impl::PointCloud2IteratorBase(cloudMsg, fieldName) { this->fieldSize = sizeOfPointField(getField(cloudMsg, fieldName)); } template class V> -U* GenericCloudIteratorBase::rawData() const +U * GenericCloudIteratorBase::rawData() const { return this->data_char_; } template -void GenericCloudIterator::copyData(const GenericCloudConstIterator& otherIter) const +void GenericCloudIterator::copyData(const GenericCloudConstIterator & otherIter) const { memcpy(this->rawData(), otherIter.rawData(), this->fieldSize); } template -void GenericCloudIterator::copyData(const GenericCloudIterator& otherIter) const +void GenericCloudIterator::copyData(const GenericCloudIterator & otherIter) const { memcpy(this->rawData(), otherIter.rawData(), this->fieldSize); } // explicitly instantiate template class GenericCloudIteratorBase; + sensor_msgs::msg::PointCloud2, GenericCloudIterator>; template class GenericCloudIteratorBase; + const sensor_msgs::msg::PointCloud2, GenericCloudConstIterator>; template class GenericCloudIterator<>; template class GenericCloudConstIterator<>; -} +} // namespace impl -} +} // namespace cras diff --git a/draco_point_cloud_transport/src/draco_publisher.cpp b/draco_point_cloud_transport/src/draco_publisher.cpp index 702a90a..6cb9a19 100644 --- a/draco_point_cloud_transport/src/draco_publisher.cpp +++ b/draco_point_cloud_transport/src/draco_publisher.cpp @@ -1,5 +1,39 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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. + */ + +#include +#include +#include +#include #include #include @@ -10,42 +44,38 @@ #include #include -#include -#include -#include - #include -#include #include namespace draco_point_cloud_transport { static std::unordered_map attributeTypes = { - {"x", draco::GeometryAttribute::Type::POSITION}, - {"y", draco::GeometryAttribute::Type::POSITION}, - {"z", draco::GeometryAttribute::Type::POSITION}, - {"pos", draco::GeometryAttribute::Type::POSITION}, - {"position", draco::GeometryAttribute::Type::POSITION}, - {"vp_x", draco::GeometryAttribute::Type::POSITION}, - {"vp_y", draco::GeometryAttribute::Type::POSITION}, - {"vp_z", draco::GeometryAttribute::Type::POSITION}, - {"rgb", draco::GeometryAttribute::Type::COLOR}, - {"rgba", draco::GeometryAttribute::Type::COLOR}, - {"r", draco::GeometryAttribute::Type::COLOR}, - {"g", draco::GeometryAttribute::Type::COLOR}, - {"b", draco::GeometryAttribute::Type::COLOR}, - {"a", draco::GeometryAttribute::Type::COLOR}, - {"nx", draco::GeometryAttribute::Type::NORMAL}, - {"ny", draco::GeometryAttribute::Type::NORMAL}, - {"nz", draco::GeometryAttribute::Type::NORMAL}, - {"normal_x", draco::GeometryAttribute::Type::NORMAL}, - {"normal_y", draco::GeometryAttribute::Type::NORMAL}, - {"normal_z", draco::GeometryAttribute::Type::NORMAL}, + {"x", draco::GeometryAttribute::Type::POSITION}, + {"y", draco::GeometryAttribute::Type::POSITION}, + {"z", draco::GeometryAttribute::Type::POSITION}, + {"pos", draco::GeometryAttribute::Type::POSITION}, + {"position", draco::GeometryAttribute::Type::POSITION}, + {"vp_x", draco::GeometryAttribute::Type::POSITION}, + {"vp_y", draco::GeometryAttribute::Type::POSITION}, + {"vp_z", draco::GeometryAttribute::Type::POSITION}, + {"rgb", draco::GeometryAttribute::Type::COLOR}, + {"rgba", draco::GeometryAttribute::Type::COLOR}, + {"r", draco::GeometryAttribute::Type::COLOR}, + {"g", draco::GeometryAttribute::Type::COLOR}, + {"b", draco::GeometryAttribute::Type::COLOR}, + {"a", draco::GeometryAttribute::Type::COLOR}, + {"nx", draco::GeometryAttribute::Type::NORMAL}, + {"ny", draco::GeometryAttribute::Type::NORMAL}, + {"nz", draco::GeometryAttribute::Type::NORMAL}, + {"normal_x", draco::GeometryAttribute::Type::NORMAL}, + {"normal_y", draco::GeometryAttribute::Type::NORMAL}, + {"normal_z", draco::GeometryAttribute::Type::NORMAL}, }; cras::expected, std::string> DracoPublisher::convertPC2toDraco( - const sensor_msgs::msg::PointCloud2& PC2, const std::string& topic, bool deduplicate, bool expert_encoding) const + const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate, + bool expert_encoding) const { // object for conversion into Draco Point Cloud format draco::PointCloudBuilder builder; @@ -70,138 +100,119 @@ cras::expected, std::string> DracoPublisher:: std::string expert_attribute_data_type; // fill in att_ids with attributes from PointField[] fields - for (const auto& field : PC2.fields) - { - if (expert_encoding) // find attribute type in user specified parameters - { + for (const auto & field : PC2.fields) { + if (expert_encoding) { // find attribute type in user specified parameters rgba_tweak = false; - if (getParam(topic + "/draco/attribute_mapping/attribute_type/" + field.name, - expert_attribute_data_type)) + if (getParam( + topic + "/draco/attribute_mapping/attribute_type/" + field.name, + expert_attribute_data_type)) { - if (expert_attribute_data_type == "POSITION") // if data type is POSITION - { + if (expert_attribute_data_type == "POSITION") { // if data type is POSITION attribute_type = draco::GeometryAttribute::POSITION; - } - else if (expert_attribute_data_type == "NORMAL") // if data type is NORMAL - { + } else if (expert_attribute_data_type == "NORMAL") { // if data type is NORMAL attribute_type = draco::GeometryAttribute::NORMAL; - } - else if (expert_attribute_data_type == "COLOR") // if data type is COLOR - { + } else if (expert_attribute_data_type == "COLOR") { // if data type is COLOR attribute_type = draco::GeometryAttribute::COLOR; getParam(topic + "/draco/attribute_mapping/rgba_tweak/" + field.name, rgba_tweak); - } - else if (expert_attribute_data_type == "TEX_COORD") // if data type is TEX_COORD - { + } else if (expert_attribute_data_type == "TEX_COORD") { // if data type is TEX_COORD attribute_type = draco::GeometryAttribute::TEX_COORD; - } - else if (expert_attribute_data_type == "GENERIC") // if data type is GENERIC - { + } else if (expert_attribute_data_type == "GENERIC") { // if data type is GENERIC attribute_type = draco::GeometryAttribute::GENERIC; - } - else - { - RCLCPP_ERROR_STREAM(getLogger(), "Attribute data type not recognized for " + field.name + " field entry. " - "Using regular type recognition instead."); + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "Attribute data type not recognized for " + field.name + " field entry. " + "Using regular type recognition instead."); expert_settings_ok = false; } - } - else - { - RCLCPP_ERROR_STREAM(getLogger(), "Attribute data type not specified for " + field.name + " field entry." - "Using regular type recognition instead."); - RCLCPP_INFO_STREAM(getLogger(), "To set attribute type for " + field.name + " field entry, set " + topic + - "/draco/attribute_mapping/attribute_type/" + field.name); + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "Attribute data type not specified for " + field.name + " field entry." + "Using regular type recognition instead."); + RCLCPP_INFO_STREAM( + getLogger(), "To set attribute type for " + field.name + " field entry, set " + topic + + "/draco/attribute_mapping/attribute_type/" + field.name); expert_settings_ok = false; } } // find attribute type in recognized names - if ((!expert_encoding) || (!expert_settings_ok)) - { + if ((!expert_encoding) || (!expert_settings_ok)) { rgba_tweak = field.name == "rgb" || field.name == "rgba"; attribute_type = draco::GeometryAttribute::GENERIC; - const auto& it = attributeTypes.find(field.name); - if (it != attributeTypes.end()) - { + const auto & it = attributeTypes.find(field.name); + if (it != attributeTypes.end()) { attribute_type = it->second; } } // attribute data type switch - switch (field.datatype) - { - case sensor_msgs::msg::PointField::INT8:attribute_data_type = draco::DT_INT8; + switch (field.datatype) { + case sensor_msgs::msg::PointField::INT8: attribute_data_type = draco::DT_INT8; break; - case sensor_msgs::msg::PointField::UINT8:attribute_data_type = draco::DT_UINT8; + case sensor_msgs::msg::PointField::UINT8: attribute_data_type = draco::DT_UINT8; break; - case sensor_msgs::msg::PointField::INT16:attribute_data_type = draco::DT_INT16; + case sensor_msgs::msg::PointField::INT16: attribute_data_type = draco::DT_INT16; break; - case sensor_msgs::msg::PointField::UINT16:attribute_data_type = draco::DT_UINT16; + case sensor_msgs::msg::PointField::UINT16: attribute_data_type = draco::DT_UINT16; break; - case sensor_msgs::msg::PointField::INT32:attribute_data_type = draco::DT_INT32; + case sensor_msgs::msg::PointField::INT32: attribute_data_type = draco::DT_INT32; rgba_tweak_64bit = false; break; - case sensor_msgs::msg::PointField::UINT32:attribute_data_type = draco::DT_UINT32; + case sensor_msgs::msg::PointField::UINT32: attribute_data_type = draco::DT_UINT32; rgba_tweak_64bit = false; break; - case sensor_msgs::msg::PointField::FLOAT32:attribute_data_type = draco::DT_FLOAT32; + case sensor_msgs::msg::PointField::FLOAT32: attribute_data_type = draco::DT_FLOAT32; rgba_tweak_64bit = false; break; - case sensor_msgs::msg::PointField::FLOAT64:attribute_data_type = draco::DT_FLOAT64; + case sensor_msgs::msg::PointField::FLOAT64: attribute_data_type = draco::DT_FLOAT64; rgba_tweak_64bit = true; break; - default:return cras::make_unexpected("Invalid data type in PointCloud2 to Draco conversion"); - } // attribute data type switch end + default: return cras::make_unexpected("Invalid data type in PointCloud2 to Draco conversion"); + } + // attribute data type switch end // add attribute to point cloud builder - if (rgba_tweak) // attribute is rgb/rgba color - { - if (rgba_tweak_64bit) // attribute data type is 64bits long, each color is encoded in 16bits - { + if (rgba_tweak) { + // attribute is rgb/rgba color + if (rgba_tweak_64bit) { + // attribute data type is 64bits long, each color is encoded in 16bits att_ids.push_back(builder.AddAttribute(attribute_type, 4 * field.count, draco::DT_UINT16)); - } - else // attribute data type is 32bits long, each color is encoded in 8bits - { + } else { + // attribute data type is 32bits long, each color is encoded in 8bits att_ids.push_back(builder.AddAttribute(attribute_type, 4 * field.count, draco::DT_UINT8)); } - } - else // attribute is not rgb/rgba color, this is the default behavior - { + } else { + // attribute is not rgb/rgba color, this is the default behavior att_ids.push_back(builder.AddAttribute(attribute_type, field.count, attribute_data_type)); } // Set attribute values for the last added attribute - if ((!att_ids.empty()) && (attribute_data_type != draco::DT_INVALID)) - { + if ((!att_ids.empty()) && (attribute_data_type != draco::DT_INVALID)) { builder.SetAttributeValuesForAllPoints( - static_cast(att_ids.back()), &PC2.data[0] + field.offset, PC2.point_step); + static_cast(att_ids.back()), &PC2.data[0] + field.offset, PC2.point_step); } } // finalize point cloud *** builder.Finalize(bool deduplicate) *** std::unique_ptr pc = builder.Finalize(deduplicate); - if (pc == nullptr) - { - return cras::make_unexpected("Conversion from sensor_msgs::msg::PointCloud2 to Draco::PointCloud failed"); + if (pc == nullptr) { + return cras::make_unexpected( + "Conversion from sensor_msgs::msg::PointCloud2 to Draco::PointCloud failed"); } // add metadata to point cloud std::unique_ptr metadata = std::make_unique(); - if (deduplicate) - { + if (deduplicate) { metadata->AddEntryInt("deduplicate", 1); // deduplication=true flag - } - else - { + } else { metadata->AddEntryInt("deduplicate", 0); // deduplication=false flag } pc->AddMetadata(std::move(metadata)); - if ((pc->num_points() != number_of_points) && !deduplicate) - { - return cras::make_unexpected("Number of points in Draco::PointCloud differs from sensor_msgs::msg::PointCloud2!"); + if ((pc->num_points() != number_of_points) && !deduplicate) { + return cras::make_unexpected( + "Number of points in Draco::PointCloud differs from sensor_msgs::msg::PointCloud2!"); } return std::move(pc); // std::move() has to be here for GCC 7 @@ -213,36 +224,38 @@ std::string DracoPublisher::getTransportName() const } DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( - const sensor_msgs::msg::PointCloud2& raw) const + const sensor_msgs::msg::PointCloud2 & raw) const { // Remove invalid points if the cloud contains them - draco cannot cope with them sensor_msgs::msg::PointCloud2::SharedPtr rawCleaned; if (!raw.is_dense) { rawCleaned = std::make_shared(); - CREATE_FILTERED_CLOUD(raw, *rawCleaned, false, std::isfinite(*x_it) && std::isfinite(*y_it) && std::isfinite(*z_it)) + CREATE_FILTERED_CLOUD( + raw, *rawCleaned, false, std::isfinite(*x_it) && std::isfinite( + *y_it) && std::isfinite(*z_it)) } - const sensor_msgs::msg::PointCloud2& rawDense = raw.is_dense ? raw : *rawCleaned; + const sensor_msgs::msg::PointCloud2 & rawDense = raw.is_dense ? raw : *rawCleaned; // Compressed message point_cloud_interfaces::msg::CompressedPointCloud2 compressed; copyCloudMetadata(compressed, rawDense); - auto res = convertPC2toDraco(rawDense, base_topic_, config_.deduplicate, config_.expert_attribute_types); - if (!res) - { + auto res = convertPC2toDraco( + rawDense, base_topic_, config_.deduplicate, + config_.expert_attribute_types); + if (!res) { return cras::make_unexpected(res.error()); } - const auto& pc = res.value(); + const auto & pc = res.value(); - if (config_.deduplicate) - { - compressed.height = 1; - compressed.width = pc->num_points(); - compressed.row_step = compressed.width * compressed.point_step; - compressed.is_dense = true; + if (config_.deduplicate) { + compressed.height = 1; + compressed.width = pc->num_points(); + compressed.row_step = compressed.width * compressed.point_step; + compressed.is_dense = true; } draco::EncoderBuffer encode_buffer; @@ -251,48 +264,41 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( bool expert_settings_ok = true; // expert encoder - if (config_.expert_quantization) - { + if (config_.expert_quantization) { draco::ExpertEncoder expert_encoder(*pc); expert_encoder.SetSpeedOptions(config_.encode_speed, config_.decode_speed); // default - if ((config_.encode_method == 0) && (!config_.force_quantization)) - { + if ((config_.encode_method == 0) && (!config_.force_quantization)) { // let draco handle method selection - } + } else if ((config_.encode_method == 1) || (config_.force_quantization)) { // force kd tree - else if ((config_.encode_method == 1) || (config_.force_quantization)) - { - if (config_.force_quantization) - { + if (config_.force_quantization) { // keep track of which attribute is being processed int att_id = 0; int attribute_quantization_bits; - for (const auto& field : rawDense.fields) - { - if (getParam(base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name, - attribute_quantization_bits)) + for (const auto & field : rawDense.fields) { + if (getParam( + base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name, + attribute_quantization_bits)) { expert_encoder.SetAttributeQuantization(att_id, attribute_quantization_bits); - } - else - { - RCLCPP_ERROR_STREAM(getLogger(), "Attribute quantization not specified for " + field.name + " field entry. " - "Using regular encoder instead."); - RCLCPP_INFO_STREAM(getLogger(), "To set quantization for " + field.name + " field entry, set " + base_topic_ + - "/draco/attribute_mapping/quantization_bits/" + field.name); + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "Attribute quantization not specified for " + field.name + + " field entry. Using regular encoder instead."); + RCLCPP_INFO_STREAM( + getLogger(), "To set quantization for " + field.name + " field entry, set " + + base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name); expert_settings_ok = false; } att_id++; } } expert_encoder.SetEncodingMethod(draco::POINT_CLOUD_KD_TREE_ENCODING); - } + } else { // force sequential encoding - else - { expert_encoder.SetEncodingMethod(draco::POINT_CLOUD_SEQUENTIAL_ENCODING); } @@ -300,77 +306,80 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( // draco::Status status = encoder.EncodePointCloudToBuffer(*pc, &encode_buffer); draco::Status status = expert_encoder.EncodeToBuffer(&encode_buffer); - if (status.code() != 0) - { - // TODO: Fix with proper format + if (status.code() != 0) { + // TODO(anyone): Fix with proper format return cras::make_unexpected( - "Draco encoder returned code "+std::to_string(status.code())+": "+status.error_msg()+"."); + "Draco encoder returned code " + std::to_string( + status.code()) + ": " + status.error_msg() + "."); } } // expert encoder end // regular encoder - if ((!config_.expert_quantization) || (!expert_settings_ok)) - { + if ((!config_.expert_quantization) || (!expert_settings_ok)) { draco::Encoder encoder; encoder.SetSpeedOptions(config_.encode_speed, config_.decode_speed); // default - if ((config_.encode_method == 0) && (!config_.force_quantization)) - { + if ((config_.encode_method == 0) && (!config_.force_quantization)) { // let draco handle method selection - } + } else if ((config_.encode_method == 1) || (config_.force_quantization)) { // force kd tree - else if ((config_.encode_method == 1) || (config_.force_quantization)) - { - if (config_.force_quantization) - { - encoder.SetAttributeQuantization(draco::GeometryAttribute::POSITION, config_.quantization_POSITION); - encoder.SetAttributeQuantization(draco::GeometryAttribute::NORMAL, config_.quantization_NORMAL); - encoder.SetAttributeQuantization(draco::GeometryAttribute::COLOR, config_.quantization_COLOR); - encoder.SetAttributeQuantization(draco::GeometryAttribute::TEX_COORD, config_.quantization_TEX_COORD); - encoder.SetAttributeQuantization(draco::GeometryAttribute::GENERIC, config_.quantization_GENERIC); + if (config_.force_quantization) { + encoder.SetAttributeQuantization( + draco::GeometryAttribute::POSITION, + config_.quantization_POSITION); + encoder.SetAttributeQuantization( + draco::GeometryAttribute::NORMAL, + config_.quantization_NORMAL); + encoder.SetAttributeQuantization( + draco::GeometryAttribute::COLOR, + config_.quantization_COLOR); + encoder.SetAttributeQuantization( + draco::GeometryAttribute::TEX_COORD, + config_.quantization_TEX_COORD); + encoder.SetAttributeQuantization( + draco::GeometryAttribute::GENERIC, + config_.quantization_GENERIC); } encoder.SetEncodingMethod(draco::POINT_CLOUD_KD_TREE_ENCODING); - } + } else { // force sequential encoding - else - { encoder.SetEncodingMethod(draco::POINT_CLOUD_SEQUENTIAL_ENCODING); } // encodes point cloud and raises error if encoding fails draco::Status status = encoder.EncodePointCloudToBuffer(*pc, &encode_buffer); - if (!status.ok()) - { + if (!status.ok()) { return cras::make_unexpected( - "Draco encoder returned code "+std::to_string(status.code())+": "+status.error_msg()+"."); + "Draco encoder returned code " + std::to_string( + status.code()) + ": " + status.error_msg() + "."); } } // regular encoder end uint32_t compressed_data_size = encode_buffer.size(); - auto cast_buffer = reinterpret_cast(encode_buffer.data()); + auto cast_buffer = reinterpret_cast(encode_buffer.data()); std::vector vec_data(cast_buffer, cast_buffer + compressed_data_size); compressed.compressed_data = vec_data; return compressed; } -void DracoPublisher::registerPositionField(const std::string& field) +void DracoPublisher::registerPositionField(const std::string & field) { attributeTypes[field] = draco::GeometryAttribute::Type::POSITION; } -void DracoPublisher::registerColorField(const std::string& field) +void DracoPublisher::registerColorField(const std::string & field) { attributeTypes[field] = draco::GeometryAttribute::Type::COLOR; } -void DracoPublisher::registerNormalField(const std::string& field) +void DracoPublisher::registerNormalField(const std::string & field) { attributeTypes[field] = draco::GeometryAttribute::Type::NORMAL; } -} +} // namespace draco_point_cloud_transport diff --git a/draco_point_cloud_transport/src/draco_subscriber.cpp b/draco_point_cloud_transport/src/draco_subscriber.cpp index e3d0b50..d688404 100644 --- a/draco_point_cloud_transport/src/draco_subscriber.cpp +++ b/draco_point_cloud_transport/src/draco_subscriber.cpp @@ -1,5 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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. + */ + +#include +#include +#include #include #include @@ -10,10 +43,6 @@ #include #include -#include -#include - -#include #include namespace draco_point_cloud_transport @@ -21,9 +50,9 @@ namespace draco_point_cloud_transport //! Method for converting into sensor_msgs::msg::PointCloud2 cras::expected convertDracoToPC2( - const draco::PointCloud& pc, - const point_cloud_interfaces::msg::CompressedPointCloud2& compressed_PC2, - sensor_msgs::msg::PointCloud2& PC2) + const draco::PointCloud & pc, + const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed_PC2, + sensor_msgs::msg::PointCloud2 & PC2) { // number of all attributes of point cloud int32_t number_of_attributes = pc.num_attributes(); @@ -34,23 +63,27 @@ cras::expected convertDracoToPC2( PC2.data.resize(number_of_points * compressed_PC2.point_step); // for each attribute - for (int32_t att_id = 0; att_id < number_of_attributes; att_id++) - { + for (int32_t att_id = 0; att_id < number_of_attributes; att_id++) { // get attribute - const draco::PointAttribute* attribute = pc.attribute(att_id); + const draco::PointAttribute * attribute = pc.attribute(att_id); // check if attribute is valid - if (!attribute->IsValid()) - return cras::make_unexpected("In point_cloud_transport::DracoToPC2, attribute of Draco pointcloud is not valid!"); + if (!attribute->IsValid()) { + return cras::make_unexpected( + std::string( + "In point_cloud_transport::DracoToPC2, attribute of Draco pointcloud is not valid!")); + } // get offset of attribute in data structure uint32_t attribute_offset = compressed_PC2.fields[att_id].offset; // for each point in point cloud - for (draco::PointIndex::ValueType point_index = 0; point_index < number_of_points; point_index++) + for (draco::PointIndex::ValueType point_index = 0; point_index < number_of_points; + point_index++) { // get pointer to corresponding memory in PointCloud2 data - uint8_t* out_data = &PC2.data[static_cast(compressed_PC2.point_step * point_index + attribute_offset)]; + uint8_t * out_data = + &PC2.data[static_cast(compressed_PC2.point_step * point_index + attribute_offset)]; // read value from Draco pointcloud to out_data attribute->GetValue(draco::AttributeValueIndex(point_index), out_data); @@ -69,14 +102,15 @@ std::string DracoSubscriber::getTransportName() const } DracoSubscriber::DecodeResult DracoSubscriber::decodeTyped( - const point_cloud_interfaces::msg::CompressedPointCloud2& compressed) const + const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed) const { // get size of buffer with compressed data in Bytes uint32_t compressed_data_size = compressed.compressed_data.size(); // empty buffer - if (compressed_data_size == 0) + if (compressed_data_size == 0) { return cras::make_unexpected("Received compressed Draco message with zero length."); + } draco::DecoderBuffer decode_buffer; std::vector vec_data = compressed.compressed_data; @@ -84,46 +118,44 @@ DracoSubscriber::DecodeResult DracoSubscriber::decodeTyped( // Sets the buffer's internal data. Note that no copy of the input data is // made so the data owner needs to keep the data valid and unchanged for // runtime of the decoder. - decode_buffer.Init(reinterpret_cast(&vec_data[0]), compressed_data_size); + decode_buffer.Init(reinterpret_cast(&vec_data[0]), compressed_data_size); // create decoder object draco::Decoder decoder; // set decoder from dynamic reconfiguration - if (config_.SkipDequantizationPOSITION) - { + if (config_.SkipDequantizationPOSITION) { decoder.SetSkipAttributeTransform(draco::GeometryAttribute::POSITION); } - if (config_.SkipDequantizationNORMAL) - { + if (config_.SkipDequantizationNORMAL) { decoder.SetSkipAttributeTransform(draco::GeometryAttribute::NORMAL); } - if (config_.SkipDequantizationCOLOR) - { + if (config_.SkipDequantizationCOLOR) { decoder.SetSkipAttributeTransform(draco::GeometryAttribute::COLOR); } - if (config_.SkipDequantizationTEX_COORD) - { + if (config_.SkipDequantizationTEX_COORD) { decoder.SetSkipAttributeTransform(draco::GeometryAttribute::TEX_COORD); } - if (config_.SkipDequantizationGENERIC) - { + if (config_.SkipDequantizationGENERIC) { decoder.SetSkipAttributeTransform(draco::GeometryAttribute::GENERIC); } // decode buffer into draco point cloud const auto res = decoder.DecodePointCloudFromBuffer(&decode_buffer); - if (!res.ok()) + if (!res.ok()) { return cras::make_unexpected( - "Draco decoder returned code "+std::to_string(res.status().code())+": "+res.status().error_msg()+"."); + "Draco decoder returned code " + std::to_string( + res.status().code()) + ": " + res.status().error_msg() + "."); + } - const std::unique_ptr& decoded_pc = res.value(); + const std::unique_ptr & decoded_pc = res.value(); auto message = std::make_shared(); const auto convertRes = convertDracoToPC2(*decoded_pc, compressed, *message); - if (!convertRes) + if (!convertRes) { return cras::make_unexpected(convertRes.error()); + } return message; } -} +} // namespace draco_point_cloud_transport diff --git a/draco_point_cloud_transport/src/manifest.cpp b/draco_point_cloud_transport/src/manifest.cpp index f0ad133..4054ac1 100644 --- a/draco_point_cloud_transport/src/manifest.cpp +++ b/draco_point_cloud_transport/src/manifest.cpp @@ -1,5 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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. + */ #include @@ -9,5 +38,9 @@ #include #include -PLUGINLIB_EXPORT_CLASS(draco_point_cloud_transport::DracoPublisher, point_cloud_transport::PublisherPlugin) -PLUGINLIB_EXPORT_CLASS(draco_point_cloud_transport::DracoSubscriber, point_cloud_transport::SubscriberPlugin) +PLUGINLIB_EXPORT_CLASS( + draco_point_cloud_transport::DracoPublisher, + point_cloud_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS( + draco_point_cloud_transport::DracoSubscriber, + point_cloud_transport::SubscriberPlugin) diff --git a/point_cloud_interfaces/CMakeLists.txt b/point_cloud_interfaces/CMakeLists.txt index 300df6f..5eb2508 100644 --- a/point_cloud_interfaces/CMakeLists.txt +++ b/point_cloud_interfaces/CMakeLists.txt @@ -16,6 +16,11 @@ rosidl_generate_interfaces(${PROJECT_NAME} DEPENDENCIES sensor_msgs std_msgs builtin_interfaces ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/point_cloud_interfaces/package.xml b/point_cloud_interfaces/package.xml index 22ff763..fbe57cf 100644 --- a/point_cloud_interfaces/package.xml +++ b/point_cloud_interfaces/package.xml @@ -24,6 +24,9 @@ rosidl_interface_packages + ament_lint_auto + ament_lint_common + ament_cmake From 44f2adc23ba9a2e32c2187860615aca108108b55 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 4 Jul 2023 00:02:09 +0200 Subject: [PATCH 2/6] Added dynamic parameters MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- draco_point_cloud_transport/README.md | 12 +- .../draco_publisher.hpp | 2 + .../draco_subscriber.hpp | 2 + .../src/draco_publisher.cpp | 146 +++++++++++++++++- .../src/draco_subscriber.cpp | 35 +++++ 5 files changed, 184 insertions(+), 13 deletions(-) diff --git a/draco_point_cloud_transport/README.md b/draco_point_cloud_transport/README.md index 67c99fb..33a9ef2 100644 --- a/draco_point_cloud_transport/README.md +++ b/draco_point_cloud_transport/README.md @@ -42,9 +42,9 @@ By adjusting the **encode_speed** and **dedode_speed** parameters, one can adjus - "ny" - NORMAL - "nz" - NORMAL - all others are encoded as GENERIC - + To specify custom quantization, one can either edit the list of recognized names or use **expert_quantization** and **expert_attribute_type** options. - + ### Expert Quantization **Expert_quantization** option tell the encoder to use custom quantization values for point cloud attributes. Multiple POSITION attribute can therefore be encoded with varying quantization levels. @@ -55,7 +55,7 @@ To set a quantization for a PointField entry "x" of point cloud which will be ad Example: ```bash -$ rosparam set /base_topic/draco/attribute_mapping/quantization_bits/x 16 +ros2 param set //draco/attribute_mapping/quantization_bits/x 16 ``` When using **expert_quantization**, user must specify the quantization bits for all PointField entries of point cloud. @@ -70,11 +70,11 @@ To set a type for a PointField entry "x" of point cloud which will be advertised Example: ```bash -$ rosparam set /base_topic/draco/attribute_mapping/attribute_type/x "'POSITION'" +ros2 param set //draco/attribute_mapping/attribute_type/x "'POSITION'" ``` When using **expert_attribute_types**, user must specify the type for all PointField entries of point cloud. Accepted types are: - - POSITION + - POSITION - NORMAL - COLOR - TEX_COORD @@ -83,7 +83,7 @@ When using **expert_attribute_types**, user must specify the type for all PointF When encoding rgb/rgba COLOR, user can specify to use the common rgba tweak of ROS (encoding rgba as 4 instances of 1 Byte instead of 1 instance of float32). To inform the encoder, that PointField entry "rgb" should be handled with the tweak, set parameter: ```bash -$ rosparam set /base_topic/draco/attribute_mapping/rgba_tweak/rgb true +ros2 param set //draco/attribute_mapping/rgba_tweak/rgb true ``` ## Subscriber diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp index 9e50077..4ac8f32 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp @@ -54,6 +54,8 @@ class DracoPublisher public: std::string getTransportName() const override; + void declareParameters(const std::string & base_topic) override; + TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override; static void registerPositionField(const std::string & field); diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp index d71d415..a9d2105 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp @@ -51,6 +51,8 @@ class DracoSubscriber public: std::string getTransportName() const override; + void declareParameters() override; + DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed) const override; diff --git a/draco_point_cloud_transport/src/draco_publisher.cpp b/draco_point_cloud_transport/src/draco_publisher.cpp index 6cb9a19..00d64c7 100644 --- a/draco_point_cloud_transport/src/draco_publisher.cpp +++ b/draco_point_cloud_transport/src/draco_publisher.cpp @@ -73,6 +73,140 @@ static std::unordered_map attribute {"normal_z", draco::GeometryAttribute::Type::NORMAL}, }; +void DracoPublisher::declareParameters(const std::string & base_topic) +{ + declareParam(std::string("encode_speed"), 7); + declareParam(std::string("decode_speed"), 7); + declareParam(std::string("encode_method"), 0); + declareParam(std::string("deduplicate"), true); + declareParam(std::string("force_quantization"), true); + declareParam(std::string("quantization_POSITION"), 14); + declareParam(std::string("quantization_NORMAL"), 14); + declareParam(std::string("quantization_COLOR"), 14); + declareParam(std::string("quantization_TEX_COORD"), 14); + declareParam(std::string("quantization_GENERIC"), 14); + declareParam(std::string("expert_quantization"), true); + declareParam(std::string("expert_attribute_types"), true); + + declareParam(base_topic + "/attribute_mapping/attribute_type/x", "POSITION"); + declareParam(base_topic + "/attribute_mapping/attribute_type/y", "POSITION"); + declareParam(base_topic + "/attribute_mapping/attribute_type/z", "POSITION"); + declareParam(base_topic + "/attribute_mapping/quantization_bits/x", 16); + declareParam(base_topic + "/attribute_mapping/quantization_bits/y", 16); + declareParam(base_topic + "/attribute_mapping/quantization_bits/z", 16); + declareParam(base_topic + "/attribute_mapping/quantization_bits/rgb", 16); + declareParam(base_topic + "/attribute_mapping/rgba_tweak/rgb", true); + declareParam(base_topic + "/attribute_mapping/rgba_tweak/rgba", false); + + auto param_change_callback = + [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult + { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + for (auto parameter : parameters) { + if (parameter.get_name() == "expert_quantization") { + config_.expert_quantization = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "force_quantization") { + config_.force_quantization = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "encode_speed") { + int value = parameter.as_int(); + if (value >= 0 && value <= 10) + { + config_.encode_speed = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "encode_speed value range should be between [0, 10] "); + } + return result; + } else if (parameter.get_name() == "decode_speed") { + int value = parameter.as_int(); + if (value >= 0 && value <= 10) + { + config_.decode_speed = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "decode_speed value range should be between [0, 10] "); + } + return result; + } else if (parameter.get_name() == "method_enum") { + int value = parameter.as_int(); + if (value >= 0 && value <= 2) + { + config_.method_enum = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "method_enum value range should be between [0, 2], " + "0 = auto, 1 = KD-tree, 2 = sequential "); + } + return result; + } else if (parameter.get_name() == "encode_method") { + config_.encode_method = parameter.as_int(); + return result; + } else if (parameter.get_name() == "deduplicate") { + config_.deduplicate = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "quantization_POSITION") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) + { + config_.quantization_POSITION = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_POSITION value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "quantization_NORMAL") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) + { + config_.quantization_NORMAL = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_NORMAL value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "quantization_COLOR") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) + { + config_.quantization_COLOR = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_COLOR value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "quantization_TEX_COORD") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) + { + config_.quantization_TEX_COORD = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_TEX_COORD value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "quantization_GENERIC") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) + { + config_.quantization_GENERIC = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_GENERIC value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "expert_attribute_types") { + config_.expert_attribute_types = parameter.as_bool(); + return result; + } + } + return result; + }; + setParamCallback(param_change_callback); +} + cras::expected, std::string> DracoPublisher::convertPC2toDraco( const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate, bool expert_encoding) const @@ -103,9 +237,8 @@ cras::expected, std::string> DracoPublisher:: for (const auto & field : PC2.fields) { if (expert_encoding) { // find attribute type in user specified parameters rgba_tweak = false; - if (getParam( - topic + "/draco/attribute_mapping/attribute_type/" + field.name, + topic + "/attribute_mapping/attribute_type/" + field.name, expert_attribute_data_type)) { if (expert_attribute_data_type == "POSITION") { // if data type is POSITION @@ -114,7 +247,7 @@ cras::expected, std::string> DracoPublisher:: attribute_type = draco::GeometryAttribute::NORMAL; } else if (expert_attribute_data_type == "COLOR") { // if data type is COLOR attribute_type = draco::GeometryAttribute::COLOR; - getParam(topic + "/draco/attribute_mapping/rgba_tweak/" + field.name, rgba_tweak); + getParam(topic + "/attribute_mapping/rgba_tweak/" + field.name, rgba_tweak); } else if (expert_attribute_data_type == "TEX_COORD") { // if data type is TEX_COORD attribute_type = draco::GeometryAttribute::TEX_COORD; } else if (expert_attribute_data_type == "GENERIC") { // if data type is GENERIC @@ -131,7 +264,7 @@ cras::expected, std::string> DracoPublisher:: "Using regular type recognition instead."); RCLCPP_INFO_STREAM( getLogger(), "To set attribute type for " + field.name + " field entry, set " + topic + - "/draco/attribute_mapping/attribute_type/" + field.name); + "/attribute_mapping/attribute_type/" + field.name); expert_settings_ok = false; } } @@ -280,7 +413,7 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( for (const auto & field : rawDense.fields) { if (getParam( - base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name, + base_topic_ + "/attribute_mapping/quantization_bits/" + field.name, attribute_quantization_bits)) { expert_encoder.SetAttributeQuantization(att_id, attribute_quantization_bits); @@ -290,7 +423,7 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( " field entry. Using regular encoder instead."); RCLCPP_INFO_STREAM( getLogger(), "To set quantization for " + field.name + " field entry, set " + - base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name); + base_topic_ + "/attribute_mapping/quantization_bits/" + field.name); expert_settings_ok = false; } att_id++; @@ -314,7 +447,6 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( } } // expert encoder end - // regular encoder if ((!config_.expert_quantization) || (!expert_settings_ok)) { draco::Encoder encoder; diff --git a/draco_point_cloud_transport/src/draco_subscriber.cpp b/draco_point_cloud_transport/src/draco_subscriber.cpp index d688404..8c3b974 100644 --- a/draco_point_cloud_transport/src/draco_subscriber.cpp +++ b/draco_point_cloud_transport/src/draco_subscriber.cpp @@ -47,6 +47,41 @@ namespace draco_point_cloud_transport { +void DracoSubscriber::declareParameters() +{ + declareParam(std::string("SkipDequantizationPOSITION"), false); + declareParam(std::string("SkipDequantizationNORMAL"), false); + declareParam(std::string("SkipDequantizationCOLOR"), false); + declareParam(std::string("SkipDequantizationTEX_COORD"), false); + declareParam(std::string("SkipDequantizationGENERIC"), false); + + auto param_change_callback = + [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult + { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + for (auto parameter : parameters) { + if (parameter.get_name() == "SkipDequantizationPOSITION") { + config_.SkipDequantizationPOSITION = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "SkipDequantizationNORMAL") { + config_.SkipDequantizationNORMAL = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "SkipDequantizationCOLOR") { + config_.SkipDequantizationCOLOR = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "SkipDequantizationTEX_COORD") { + config_.SkipDequantizationTEX_COORD = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "SkipDequantizationGENERIC") { + config_.SkipDequantizationGENERIC = parameter.as_bool(); + return result; + } + } + return result; + }; + setParamCallback(param_change_callback); +} //! Method for converting into sensor_msgs::msg::PointCloud2 cras::expected convertDracoToPC2( From 7889237ea5f738c28fb678a29a448cd41bd62290 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 4 Jul 2023 00:14:12 +0200 Subject: [PATCH 3/6] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../src/draco_publisher.cpp | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/draco_point_cloud_transport/src/draco_publisher.cpp b/draco_point_cloud_transport/src/draco_publisher.cpp index 00d64c7..619fbb7 100644 --- a/draco_point_cloud_transport/src/draco_publisher.cpp +++ b/draco_point_cloud_transport/src/draco_publisher.cpp @@ -112,8 +112,7 @@ void DracoPublisher::declareParameters(const std::string & base_topic) return result; } else if (parameter.get_name() == "encode_speed") { int value = parameter.as_int(); - if (value >= 0 && value <= 10) - { + if (value >= 0 && value <= 10) { config_.encode_speed = value; } else { RCLCPP_ERROR_STREAM( @@ -122,8 +121,7 @@ void DracoPublisher::declareParameters(const std::string & base_topic) return result; } else if (parameter.get_name() == "decode_speed") { int value = parameter.as_int(); - if (value >= 0 && value <= 10) - { + if (value >= 0 && value <= 10) { config_.decode_speed = value; } else { RCLCPP_ERROR_STREAM( @@ -132,8 +130,7 @@ void DracoPublisher::declareParameters(const std::string & base_topic) return result; } else if (parameter.get_name() == "method_enum") { int value = parameter.as_int(); - if (value >= 0 && value <= 2) - { + if (value >= 0 && value <= 2) { config_.method_enum = value; } else { RCLCPP_ERROR_STREAM( @@ -149,8 +146,7 @@ void DracoPublisher::declareParameters(const std::string & base_topic) return result; } else if (parameter.get_name() == "quantization_POSITION") { int value = parameter.as_int(); - if (value >= 1 && value <= 31) - { + if (value >= 1 && value <= 31) { config_.quantization_POSITION = value; } else { RCLCPP_ERROR_STREAM( @@ -159,8 +155,7 @@ void DracoPublisher::declareParameters(const std::string & base_topic) return result; } else if (parameter.get_name() == "quantization_NORMAL") { int value = parameter.as_int(); - if (value >= 1 && value <= 31) - { + if (value >= 1 && value <= 31) { config_.quantization_NORMAL = value; } else { RCLCPP_ERROR_STREAM( @@ -169,8 +164,7 @@ void DracoPublisher::declareParameters(const std::string & base_topic) return result; } else if (parameter.get_name() == "quantization_COLOR") { int value = parameter.as_int(); - if (value >= 1 && value <= 31) - { + if (value >= 1 && value <= 31) { config_.quantization_COLOR = value; } else { RCLCPP_ERROR_STREAM( @@ -179,8 +173,7 @@ void DracoPublisher::declareParameters(const std::string & base_topic) return result; } else if (parameter.get_name() == "quantization_TEX_COORD") { int value = parameter.as_int(); - if (value >= 1 && value <= 31) - { + if (value >= 1 && value <= 31) { config_.quantization_TEX_COORD = value; } else { RCLCPP_ERROR_STREAM( @@ -189,8 +182,7 @@ void DracoPublisher::declareParameters(const std::string & base_topic) return result; } else if (parameter.get_name() == "quantization_GENERIC") { int value = parameter.as_int(); - if (value >= 1 && value <= 31) - { + if (value >= 1 && value <= 31) { config_.quantization_GENERIC = value; } else { RCLCPP_ERROR_STREAM( From 9d1602d07aae2827e66f8e7a2c5018b7bc1bde14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 4 Jul 2023 09:54:05 +0200 Subject: [PATCH 4/6] cleanup MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- draco_point_cloud_transport/CMakeLists.txt | 34 --------------- .../cfg/DracoPublisher.cfg | 42 ------------------- .../cfg/DracoSubscriber.cfg | 15 ------- 3 files changed, 91 deletions(-) delete mode 100755 draco_point_cloud_transport/cfg/DracoPublisher.cfg delete mode 100755 draco_point_cloud_transport/cfg/DracoSubscriber.cfg diff --git a/draco_point_cloud_transport/CMakeLists.txt b/draco_point_cloud_transport/CMakeLists.txt index f8dffce..48dda28 100644 --- a/draco_point_cloud_transport/CMakeLists.txt +++ b/draco_point_cloud_transport/CMakeLists.txt @@ -56,40 +56,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -# TODO: Fix tests -# if (CATKIN_ENABLE_TESTING) -# find_package(roslint REQUIRED) - -# # catkin_lint - checks validity of package.xml and CMakeLists.txt -# # ROS buildfarm calls this without any environment and with empty rosdep cache, -# # so we have problems reading the list of packages from env -# # see https://github.com/ros-infrastructure/ros_buildfarm/issues/923 -# if(DEFINED ENV{ROS_HOME}) -# #catkin_lint: ignore_once env_var -# set(ROS_HOME "$ENV{ROS_HOME}") -# else() -# #catkin_lint: ignore_once env_var -# set(ROS_HOME "$ENV{HOME}/.ros") -# endif() -# #catkin_lint: ignore_once env_var -# if(DEFINED ENV{ROS_ROOT} AND EXISTS "${ROS_HOME}/rosdep/sources.cache") -# roslint_custom(catkin_lint "-W2" .) -# endif() - -# # Roslint C++ - checks formatting and some other rules for C++ files - -# file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp) -# file(GLOB_RECURSE ROSLINT_SRC src/*.cpp src/*.hpp src/*.h) -# #file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) - -# set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ -# -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\ -# -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces") -# roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC}) - -# roslint_add_test() -# endif() - ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies(${dependencies}) diff --git a/draco_point_cloud_transport/cfg/DracoPublisher.cfg b/draco_point_cloud_transport/cfg/DracoPublisher.cfg deleted file mode 100755 index eb05913..0000000 --- a/draco_point_cloud_transport/cfg/DracoPublisher.cfg +++ /dev/null @@ -1,42 +0,0 @@ -#! /usr/bin/env python - -PACKAGE='draco_point_cloud_transport' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - - - -gen.add("encode_speed", int_t, 0, "0 = slowest speed, but the best compression 10 = fastest, but the worst compression.", 7, 0, 10) -gen.add("decode_speed", int_t, 0, "0 = slowest speed, but the best compression 10 = fastest, but the worst compression.", 7, 0, 10) - -method_enum = gen.enum([ gen.const("Auto", int_t, 0, "Draco chooses appropriate compression"), - gen.const("KD_tree", int_t, 1, "Force KD-tree"), - gen.const("Sequential", int_t, 2, "Force Sequential")], - "An enum to set method of encoding") - -gen.add("encode_method", int_t, 0, "Encoding process method, 0 = auto, 1 = KD-tree, 2 = sequential", 0, 0, 2, edit_method=method_enum) - -#deduplicate_enum = gen.enum([ gen.const("Deduplication_OFF", bool_t, False, "Do NOT remove duplicate point entries."), -# gen.const("DEDUPLICATION_ON", bool_t, True, "Remove duplicate point entries.")], -# "An enum to enable/disable deduplication of point entries") - -gen.add("deduplicate", bool_t, 0, "Remove duplicate point entries.", True)#, edit_method=deduplicate_enum) - -#force_quantization_enum = gen.enum([ gen.const("Quantization_OFF", bool_t, False, "Do NOT quantize attribute values."), -# gen.const("Quantization_ON", bool_t, True, "Quantize attribute values.")], -# "An enum to enable/disable quantization of attribute values") - -gen.add("force_quantization", bool_t, 0, "Force attribute quantization. Attributes of type float32 must be quantized for kd-tree encoding.", True)#, edit_method=force_quantization_enum) - -gen.add("quantization_POSITION", int_t, 0, "Number of bits for quantization of POSITION type attributes.", 14, 1, 31) -gen.add("quantization_NORMAL", int_t, 0, "Number of bits for quantization of NORMAL type attributes.", 14, 1, 31) -gen.add("quantization_COLOR", int_t, 0, "Number of bits for quantization of COLOR type attributes.", 14, 1, 31) -gen.add("quantization_TEX_COORD", int_t, 0, "Number of bits for quantization of TEX_COORD type attributes.", 14, 1, 31) -gen.add("quantization_GENERIC", int_t, 0, "Number of bits for quantization of GENERIC type attributes.", 14, 1, 31) - -gen.add("expert_quantization", bool_t, 0, "WARNING: Apply user specified quantization for PointField entries. User must specify all entries at parameter server.", False) -gen.add("expert_attribute_types", bool_t, 0, "WARNING: Apply user specified attribute types for PointField entries. User must specify all entries at parameter server.", False) - -exit(gen.generate(PACKAGE, "DracoPublisher", "DracoPublisher")) diff --git a/draco_point_cloud_transport/cfg/DracoSubscriber.cfg b/draco_point_cloud_transport/cfg/DracoSubscriber.cfg deleted file mode 100755 index cc9fbdf..0000000 --- a/draco_point_cloud_transport/cfg/DracoSubscriber.cfg +++ /dev/null @@ -1,15 +0,0 @@ -#! /usr/bin/env python - -PACKAGE='draco_point_cloud_transport' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("SkipDequantizationPOSITION", bool_t, 0, "Tells decoder to skip dequantization of POSITION attributes", False) -gen.add("SkipDequantizationNORMAL", bool_t, 0, "Tells decoder to skip dequantization of NORMAL attributes", False) -gen.add("SkipDequantizationCOLOR", bool_t, 0, "Tells decoder to skip dequantization of COLOR attributes", False) -gen.add("SkipDequantizationTEX_COORD", bool_t, 0, "Tells decoder to skip dequantization of TEX_COORD attributes", False) -gen.add("SkipDequantizationGENERIC", bool_t, 0, "Tells decoder to skip dequantization of GENERIC attributes", False) - -exit(gen.generate(PACKAGE, "DracoSubscriber", "DracoSubscriber")) From 571e756e196afc3e5db46696e53e7c8432172499 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 7 Jul 2023 17:44:53 +0200 Subject: [PATCH 5/6] Type adaptation PCL MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- pcl_point_cloud_transport/CMakeLists.txt | 84 +++++++ .../examples/subscriber.cpp | 94 ++++++++ .../pcl_publisher.hpp | 68 ++++++ ...l_sensor_msgs_pointcloud2_type_adapter.hpp | 225 ++++++++++++++++++ .../pcl_subscriber.hpp | 63 +++++ pcl_point_cloud_transport/package.xml | 32 +++ pcl_point_cloud_transport/pcl_plugins.xml | 19 ++ pcl_point_cloud_transport/src/manifest.cpp | 46 ++++ .../src/pcl_publisher.cpp | 77 ++++++ ...l_sensor_msgs_pointcloud2_type_adapter.cpp | 202 ++++++++++++++++ .../src/pcl_subscriber.cpp | 76 ++++++ 11 files changed, 986 insertions(+) create mode 100644 pcl_point_cloud_transport/CMakeLists.txt create mode 100644 pcl_point_cloud_transport/examples/subscriber.cpp create mode 100644 pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp create mode 100644 pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp create mode 100644 pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_subscriber.hpp create mode 100644 pcl_point_cloud_transport/package.xml create mode 100644 pcl_point_cloud_transport/pcl_plugins.xml create mode 100644 pcl_point_cloud_transport/src/manifest.cpp create mode 100644 pcl_point_cloud_transport/src/pcl_publisher.cpp create mode 100644 pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp create mode 100644 pcl_point_cloud_transport/src/pcl_subscriber.cpp diff --git a/pcl_point_cloud_transport/CMakeLists.txt b/pcl_point_cloud_transport/CMakeLists.txt new file mode 100644 index 0000000..c421c2e --- /dev/null +++ b/pcl_point_cloud_transport/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.5) + +project(pcl_point_cloud_transport) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(PCL REQUIRED QUIET COMPONENTS common io) +find_package(pcl_conversions REQUIRED) +find_package(pluginlib REQUIRED) +find_package(point_cloud_transport REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +set(dependencies + pcl_conversions + pluginlib + point_cloud_transport + rclcpp + sensor_msgs + std_msgs +) + +include_directories(include ${PCL_COMMON_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} + SHARED + src/manifest.cpp + src/pcl_publisher.cpp + src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp + src/pcl_subscriber.cpp +) + +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) + +add_executable(pclcontainer_subscriber + examples/subscriber.cpp + src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp +) +ament_target_dependencies(pclcontainer_subscriber + pcl_conversions + point_cloud_transport + rclcpp + sensor_msgs +) +target_link_libraries(pclcontainer_subscriber ${PCL_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +# Install executables +install( + TARGETS pclcontainer_subscriber + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +pluginlib_export_plugin_description_file(point_cloud_transport pcl_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME} PCL) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/pcl_point_cloud_transport/examples/subscriber.cpp b/pcl_point_cloud_transport/examples/subscriber.cpp new file mode 100644 index 0000000..b07e40c --- /dev/null +++ b/pcl_point_cloud_transport/examples/subscriber.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl_point_cloud_transport::PCLContainer, + sensor_msgs::msg::PointCloud2); + +class ShowPointCloud : public rclcpp::Node +{ +public: + explicit ShowPointCloud() + : Node("showpointcloud") + { + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + initialize(); + } + +private: + void initialize() + { + // Set quality of service profile based on command line options. + auto qos = rclcpp::QoS( + rclcpp::QoSInitialization( + // The history policy determines how messages are saved until taken by + // the reader. + // KEEP_ALL saves all messages until they are taken. + // KEEP_LAST enforces a limit on the number of messages that are saved, + // specified by the "depth" parameter. + history_policy_, + // Depth represents how many messages to store in history when the + // history policy is KEEP_LAST. + depth_ + )); + // The reliability policy can be reliable, meaning that the underlying transport layer will try + // ensure that every message gets received in order, or best effort, meaning that the transport + // makes no guarantees about the order or reliability of delivery. + qos.reliability(reliability_policy_); + auto callback = + [this](const pcl_point_cloud_transport::PCLContainer & container) { + process_image(container, show_image_, this->get_logger()); + }; + + RCLCPP_INFO(this->get_logger(), "Subscribing to topic '/pct/point_cloud/pcl'"); + sub_ = create_subscription("/pct/point_cloud/pcl", rclcpp::SensorDataQoS(), callback); + + if (window_name_ == "") { + // If no custom window name is given, use the topic name + window_name_ = sub_->get_topic_name(); + } + } + + /// Convert the ROS Image message to an OpenCV matrix and display it to the user. + // \param[in] container The image message to show. + void process_image( + const pcl_point_cloud_transport::PCLContainer & container, bool show_image, rclcpp::Logger logger) + { + RCLCPP_INFO(logger, "Received image #%s", container.header().frame_id.c_str()); + } + + rclcpp::Subscription::SharedPtr sub_; + size_t depth_ = rmw_qos_profile_default.depth; + rmw_qos_reliability_policy_t reliability_policy_ = rmw_qos_profile_default.reliability; + rmw_qos_history_policy_t history_policy_ = rmw_qos_profile_default.history; + bool show_image_ = true; + std::string window_name_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp new file mode 100644 index 0000000..3014919 --- /dev/null +++ b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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 PCL_POINT_CLOUD_TRANSPORT__PCL_PUBLISHER_HPP_ +#define PCL_POINT_CLOUD_TRANSPORT__PCL_PUBLISHER_HPP_ + +#include +#include + +#include + +#include +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +#include + + +namespace pcl_point_cloud_transport +{ + +class PCLPublisher + : public point_cloud_transport::SimplePublisherPlugin< + pcl_point_cloud_transport::PCLContainer> +{ +public: + std::string getTransportName() const override; + + void declareParameters(const std::string & base_topic) override; + + TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override; + +// private: +// cras::expected, std::string> convertPC2toPCL( +// const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate, +// bool expert_encoding) const; +}; +} // namespace pcl_point_cloud_transport + +#endif // pcl_point_cloud_transport__DRACO_PUBLISHER_HPP_ diff --git a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp new file mode 100644 index 0000000..7c0ac6b --- /dev/null +++ b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp @@ -0,0 +1,225 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef IMAGE_TOOLS__CV_MAT_sensor_msgs_point_cloud2_TYPE_ADAPTER_HPP_ +#define IMAGE_TOOLS__CV_MAT_sensor_msgs_point_cloud2_TYPE_ADAPTER_HPP_ + +#include +#include +#include // NOLINT[build/include_order] + +#include +#include + +#include + +#include +#include + +namespace pcl_point_cloud_transport +{ +namespace detail +{ +// TODO(audrow): Replace with std::endian when C++ 20 is available +// https://en.cppreference.com/w/cpp/types/endian +enum class endian +{ +#ifdef _WIN32 + little = 0, + big = 1, + native = little +#else + little = __ORDER_LITTLE_ENDIAN__, + big = __ORDER_BIG_ENDIAN__, + native = __BYTE_ORDER__ +#endif +}; + +} // namespace detail + +class PCLContainer +{ + static constexpr bool is_bigendian_system = detail::endian::native == detail::endian::big; + +public: + using PointCloud2MsgsImageStorageType = std::variant< + std::nullptr_t, + std::unique_ptr, + std::shared_ptr + >; + + PCLContainer() = default; + + explicit PCLContainer(const PCLContainer & other) + : header_(other.header_), frame_(other.frame_), is_bigendian_(other.is_bigendian_) + { + if (std::holds_alternative>(other.storage_)) { + storage_ = std::get>(other.storage_); + } else if (std::holds_alternative>(other.storage_)) { + storage_ = std::make_unique( + *std::get>(other.storage_)); + } + } + + PCLContainer & operator=(const PCLContainer & other) + { + if (this != &other) { + header_ = other.header_; + pcl::copyPointCloud(other.frame_, frame_); + is_bigendian_ = other.is_bigendian_; + if (std::holds_alternative>(other.storage_)) { + storage_ = std::get>(other.storage_); + } else if (std::holds_alternative>(other.storage_)) { + storage_ = std::make_unique( + *std::get>(other.storage_)); + } else if (std::holds_alternative(other.storage_)) { + storage_ = nullptr; + } + } + return *this; + } + + /// Store an owning pointer to a sensor_msg::msg::PointCloud2, and create a pcl::PCLPointCloud2 that references it. + explicit PCLContainer(std::unique_ptr unique_sensor_msgs_point_cloud2); + + /// Store an owning pointer to a sensor_msg::msg::PointCloud2, and create a pcl::PCLPointCloud2 that references it. + explicit PCLContainer(std::shared_ptr shared_sensor_msgs_point_cloud2); + + /// Shallow copy the given pcl::PCLPointCloud2 into this class, but do not own the data directly. + PCLContainer( + const pcl::PCLPointCloud2 & pcl_frame, + const std_msgs::msg::Header & header, + bool is_bigendian = is_bigendian_system); + + /// Move the given pcl::PCLPointCloud2 into this class. + PCLContainer( + pcl::PCLPointCloud2 && pcl_frame, + const std_msgs::msg::Header & header, + bool is_bigendian = is_bigendian_system); + + /// Copy the sensor_msgs::msg::PointCloud2 into this contain and create a pcl::PCLPointCloud2 that references it. + explicit PCLContainer(const sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2); + + /// Return true if this class owns the data the cv_mat references. + /** + * Note that this does not check if the pcl::PCLPointCloud2 owns its own data, only if + * this class owns a sensor_msgs::msg::PointCloud2 that the pcl::PCLPointCloud2 references. + */ + bool + is_owning() const; + + /// Const access the pcl::PCLPointCloud2 in this class. + const pcl::PCLPointCloud2 & + pcl() const; + + /// Get a shallow copy of the pcl::PCLPointCloud2 that is in this class. + /** + * Note that if you want to let this container go out of scope you should + * make a deep copy with pcl::PCLPointCloud2::clone() beforehand. + */ + pcl::PCLPointCloud2 + pcl(); + + /// Const access the ROS Header. + const std_msgs::msg::Header & + header() const; + + /// Access the ROS Header. + std_msgs::msg::Header & + header(); + + /// Get shared const pointer to the sensor_msgs::msg::PointCloud2 if available, otherwise nullptr. + std::shared_ptr + get_sensor_msgs_msg_point_cloud2_pointer() const; + + /// Get copy as a unique pointer to the sensor_msgs::msg::PointCloud2. + std::unique_ptr + get_sensor_msgs_msg_point_cloud2_pointer_copy() const; + + /// Get a copy of the image as a sensor_msgs::msg::PointCloud2. + sensor_msgs::msg::PointCloud2 + get_sensor_msgs_msg_point_cloud2_copy() const; + + /// Get a copy of the image as a sensor_msgs::msg::PointCloud2. + void + get_sensor_msgs_msg_point_cloud2_copy(sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) const; + + /// Return true if the data is stored in big endian, otherwise return false. + bool + is_bigendian() const; + +private: + std_msgs::msg::Header header_; + pcl::PCLPointCloud2 frame_; + PointCloud2MsgsImageStorageType storage_; + bool is_bigendian_; +}; + +} // namespace image_tools + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = pcl_point_cloud_transport::PCLContainer; + using ros_message_type = sensor_msgs::msg::PointCloud2; + + static + void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + sensor_msgs::msg::PointCloud2 msg; + pcl::PCLPointCloud2 cloud = source.pcl(); + // pcl::toROSMsg(cloud, msg); + pcl_conversions::moveFromPCL(cloud, destination); + + + // destination.height = source.cv_mat().rows; + // destination.width = source.cv_mat().cols; + // switch (source.cv_mat().type()) { + // case CV_8UC1: + // destination.encoding = "mono8"; + // break; + // case CV_8UC3: + // destination.encoding = "bgr8"; + // break; + // case CV_16SC1: + // destination.encoding = "mono16"; + // break; + // case CV_8UC4: + // destination.encoding = "rgba8"; + // break; + // default: + // throw std::runtime_error("unsupported encoding type"); + // } + // destination.step = static_cast(source.cv_mat().step); + // size_t size = source.cv_mat().step * source.cv_mat().rows; + // destination.data.resize(size); + // memcpy(&destination.data[0], source.cv_mat().data, size); + // destination.header = source.header(); + } + + static + void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = pcl_point_cloud_transport::PCLContainer(source); + } +}; + +#endif // IMAGE_TOOLS__CV_MAT_sensor_msgs_point_cloud2_TYPE_ADAPTER_HPP_ diff --git a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_subscriber.hpp b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_subscriber.hpp new file mode 100644 index 0000000..4a1421f --- /dev/null +++ b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_subscriber.hpp @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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 PCL_POINT_CLOUD_TRANSPORT__PCL_SUBSCRIBER_HPP_ +#define PCL_POINT_CLOUD_TRANSPORT__PCL_SUBSCRIBER_HPP_ + +#include + +#include +#include + +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl_point_cloud_transport::PCLContainer, + sensor_msgs::msg::PointCloud2); + +namespace pcl_point_cloud_transport +{ + +class PCLSubscriber + : public point_cloud_transport::SimpleSubscriberPlugin< + pcl_point_cloud_transport::PCLContainer> +{ +public: + std::string getTransportName() const override; + + void declareParameters() override; + + DecodeResult decodeTyped(const pcl_point_cloud_transport::PCLContainer & compressed) + const override; +}; +} // namespace pcl_point_cloud_transport + +#endif // DRACO_POINT_CLOUD_TRANSPORT__DRACO_SUBSCRIBER_HPP_ diff --git a/pcl_point_cloud_transport/package.xml b/pcl_point_cloud_transport/package.xml new file mode 100644 index 0000000..2947338 --- /dev/null +++ b/pcl_point_cloud_transport/package.xml @@ -0,0 +1,32 @@ + + pcl_point_cloud_transport + 1.0.5 + + Add descrioptuio + + Jakub Paplham + Alejandro Hernandez Cordero + BSD + + + + + + ament_cmake + + libpcl-common + libpcl-io + pcl_conversions + pluginlib + point_cloud_transport + rclcpp + sensor_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/pcl_point_cloud_transport/pcl_plugins.xml b/pcl_point_cloud_transport/pcl_plugins.xml new file mode 100644 index 0000000..24bb5ae --- /dev/null +++ b/pcl_point_cloud_transport/pcl_plugins.xml @@ -0,0 +1,19 @@ + + + + PCL publisher + + + + + + PCL + + + diff --git a/pcl_point_cloud_transport/src/manifest.cpp b/pcl_point_cloud_transport/src/manifest.cpp new file mode 100644 index 0000000..d2cbf37 --- /dev/null +++ b/pcl_point_cloud_transport/src/manifest.cpp @@ -0,0 +1,46 @@ +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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. + */ + +#include + +#include +#include + +#include +#include + +PLUGINLIB_EXPORT_CLASS( + pcl_point_cloud_transport::PCLPublisher, + point_cloud_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS( + pcl_point_cloud_transport::PCLSubscriber, + point_cloud_transport::SubscriberPlugin) diff --git a/pcl_point_cloud_transport/src/pcl_publisher.cpp b/pcl_point_cloud_transport/src/pcl_publisher.cpp new file mode 100644 index 0000000..afc4f9e --- /dev/null +++ b/pcl_point_cloud_transport/src/pcl_publisher.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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. + */ + +#include +#include +#include +#include +#include + +#include +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl_point_cloud_transport::PCLContainer, + sensor_msgs::msg::PointCloud2); + +namespace pcl_point_cloud_transport +{ + +void PCLPublisher::declareParameters(const std::string & base_topic) +{ +} + +// cras::expected, std::string> PCLPublisher::convertPC2toPCL( +// const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate, +// bool expert_encoding) const +// { +// // std_msgs::msg::Header header; +// // header.frame_id = frame_id_; +// // header.stamp = this->now(); +// // pcl_point_cloud_transport::PCLContainer container(PC2); +// // return container.get_sensor_msgs_msg_point_cloud2_pointer_copy(); +// } + +std::string PCLPublisher::getTransportName() const +{ + return "pcl"; +} + +PCLPublisher::TypedEncodeResult PCLPublisher::encodeTyped( + const sensor_msgs::msg::PointCloud2 & raw) const +{ + pcl_point_cloud_transport::PCLContainer container(raw); + return std::optional(container); + // cras::make_unexpected("lol"); +} + +} // namespace draco_point_cloud_transport diff --git a/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp b/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp new file mode 100644 index 0000000..a25ac51 --- /dev/null +++ b/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp @@ -0,0 +1,202 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include // NOLINT[build/include_order] + +#include +#include + +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +namespace pcl_point_cloud_transport +{ + +namespace +{ +// int +// encoding2mat_type(const std::string & encoding) +// { +// } + +template +struct NotNull +{ + NotNull(const T * pointer_in, const char * msg) + : pointer(pointer_in) + { + if (pointer == nullptr) { + throw std::invalid_argument(msg); + } + } + + const T * pointer; +}; + +} // namespace + +PCLContainer::PCLContainer( + std::unique_ptr unique_sensor_msgs_point_cloud2) +: header_(NotNull( + unique_sensor_msgs_point_cloud2.get(), + "unique_sensor_msgs_point_cloud2 cannot be nullptr" +).pointer->header) + + // storage_(std::move(unique_sensor_msgs_point_cloud2)) +{ + // pcl::PCLPointCloud2 pcl_pc2; + // pcl_conversions::copyPointCloud2MetaData(*unique_sensor_msgs_point_cloud2.get(), pcl_pc2); // Like pcl_conversions::toPCL, but does not copy the binary data + // pcl::MsgFieldMap field_map; + // pcl::createMapping (pcl_pc2.fields, field_map); + // pcl::fromPCLPointCloud2(pcl_pc2, frame_, field_map, &(*unique_sensor_msgs_point_cloud2.get(),).data[0]); + + pcl_conversions::toPCL(*unique_sensor_msgs_point_cloud2.get(), frame_); + + storage_ = std::move(unique_sensor_msgs_point_cloud2); + + // + // pcl::copyPointCloud(other.frame_, frame_); + +} + +PCLContainer::PCLContainer( + std::shared_ptr shared_sensor_msgs_point_cloud2) +: header_(shared_sensor_msgs_point_cloud2->header) +// frame_( +// shared_sensor_msgs_point_cloud2->height, +// shared_sensor_msgs_point_cloud2->width, +// encoding2mat_type(shared_sensor_msgs_point_cloud2->encoding), +// shared_sensor_msgs_point_cloud2->data.data(), +// shared_sensor_msgs_point_cloud2->step), +// storage_(shared_sensor_msgs_point_cloud2) +{ + + pcl_conversions::toPCL(*shared_sensor_msgs_point_cloud2.get(), frame_); + + storage_ = shared_sensor_msgs_point_cloud2; + + +} + +PCLContainer::PCLContainer( + const pcl::PCLPointCloud2 & pcl_frame, + const std_msgs::msg::Header & header, + bool is_bigendian) +: header_(header), + frame_(pcl_frame), + storage_(nullptr), + is_bigendian_(is_bigendian) +{} + +PCLContainer::PCLContainer( + pcl::PCLPointCloud2 && pcl_frame, + const std_msgs::msg::Header & header, + bool is_bigendian) +: header_(header), + frame_(std::forward(pcl_frame)), + storage_(nullptr), + is_bigendian_(is_bigendian) +{} + +PCLContainer::PCLContainer( + const sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) +: PCLContainer(std::make_unique(sensor_msgs_point_cloud2)) +{} + +bool +PCLContainer::is_owning() const +{ + return std::holds_alternative(storage_); +} + +const pcl::PCLPointCloud2 & +PCLContainer::pcl() const +{ + return frame_; +} + +pcl::PCLPointCloud2 +PCLContainer::pcl() +{ + return frame_; +} + +const std_msgs::msg::Header & +PCLContainer::header() const +{ + return header_; +} + +std_msgs::msg::Header & +PCLContainer::header() +{ + return header_; +} + +std::shared_ptr +PCLContainer::get_sensor_msgs_msg_point_cloud2_pointer() const +{ + if (!std::holds_alternative>(storage_)) { + return nullptr; + } + return std::get>(storage_); +} + +std::unique_ptr +PCLContainer::get_sensor_msgs_msg_point_cloud2_pointer_copy() const +{ + auto unique_image = std::make_unique(); + this->get_sensor_msgs_msg_point_cloud2_copy(*unique_image); + return unique_image; +} + +void +PCLContainer::get_sensor_msgs_msg_point_cloud2_copy( + sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) const +{ + // sensor_msgs_point_cloud2.height = frame_.rows; + // sensor_msgs_point_cloud2.width = frame_.cols; + // switch (frame_.type()) { + // case CV_8UC1: + // sensor_msgs_point_cloud2.encoding = "mono8"; + // break; + // case CV_8UC3: + // sensor_msgs_point_cloud2.encoding = "bgr8"; + // break; + // case CV_16SC1: + // sensor_msgs_point_cloud2.encoding = "mono16"; + // break; + // case CV_8UC4: + // sensor_msgs_point_cloud2.encoding = "rgba8"; + // break; + // default: + // throw std::runtime_error("unsupported encoding type"); + // } + // sensor_msgs_point_cloud2.step = static_cast(frame_.step); + // size_t size = frame_.step * frame_.rows; + // sensor_msgs_point_cloud2.data.resize(size); + // memcpy(&sensor_msgs_point_cloud2.data[0], frame_.data, size); + // sensor_msgs_point_cloud2.header = header_; +} + +bool +PCLContainer::is_bigendian() const +{ + return is_bigendian_; +} + +} // namespace image_tools diff --git a/pcl_point_cloud_transport/src/pcl_subscriber.cpp b/pcl_point_cloud_transport/src/pcl_subscriber.cpp new file mode 100644 index 0000000..84d62cf --- /dev/null +++ b/pcl_point_cloud_transport/src/pcl_subscriber.cpp @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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. + */ + +#include +#include +#include + +#include + +#include +#include +#include + +namespace pcl_point_cloud_transport +{ +void PCLSubscriber::declareParameters() +{ +} + +std::string PCLSubscriber::getTransportName() const +{ + return "pcl"; +} + +PCLSubscriber::DecodeResult PCLSubscriber::decodeTyped( + const pcl_point_cloud_transport::PCLContainer & container) const +{ + auto message = std::make_shared(); + + pcl::PCLPointCloud2 pcl_pc2 = container.pcl(); + + std::stringstream ss; + ss << "output.pcd"; + std::cerr << "Data saved to " << ss.str() << std::endl; + + pcl::io::savePCDFile( + ss.str(), pcl_pc2, Eigen::Vector4f::Zero(), + Eigen::Quaternionf::Identity(), true); + + pcl_conversions::moveFromPCL(pcl_pc2, *message.get()); + + return message; + // pcl_point_cloud_transport::PCLContainer::ConstPtr l; + // return message; +} + +} // namespace pcl_point_cloud_transport From 10eff5603ab6934dc10a9055f13c7337c490fdc0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 13 Jul 2023 18:23:54 +0200 Subject: [PATCH 6/6] cleanup code MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- pcl_point_cloud_transport/CMakeLists.txt | 1 - .../examples/subscriber.cpp | 21 +++--- .../pcl_publisher.hpp | 51 ++++---------- ...l_sensor_msgs_pointcloud2_type_adapter.hpp | 68 +++++++------------ .../pcl_subscriber.hpp | 47 ++++--------- pcl_point_cloud_transport/package.xml | 11 +-- pcl_point_cloud_transport/src/manifest.cpp | 44 ++++-------- .../src/pcl_publisher.cpp | 60 ++++------------ ...l_sensor_msgs_pointcloud2_type_adapter.cpp | 57 +--------------- .../src/pcl_subscriber.cpp | 53 +++++---------- 10 files changed, 117 insertions(+), 296 deletions(-) diff --git a/pcl_point_cloud_transport/CMakeLists.txt b/pcl_point_cloud_transport/CMakeLists.txt index c421c2e..8cedad7 100644 --- a/pcl_point_cloud_transport/CMakeLists.txt +++ b/pcl_point_cloud_transport/CMakeLists.txt @@ -77,7 +77,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() - ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME} PCL) ament_export_dependencies(${dependencies}) diff --git a/pcl_point_cloud_transport/examples/subscriber.cpp b/pcl_point_cloud_transport/examples/subscriber.cpp index b07e40c..d91e037 100644 --- a/pcl_point_cloud_transport/examples/subscriber.cpp +++ b/pcl_point_cloud_transport/examples/subscriber.cpp @@ -28,7 +28,7 @@ RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( class ShowPointCloud : public rclcpp::Node { public: - explicit ShowPointCloud() + ShowPointCloud() : Node("showpointcloud") { setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -57,32 +57,27 @@ class ShowPointCloud : public rclcpp::Node qos.reliability(reliability_policy_); auto callback = [this](const pcl_point_cloud_transport::PCLContainer & container) { - process_image(container, show_image_, this->get_logger()); + process_pointcloud(container, this->get_logger()); }; RCLCPP_INFO(this->get_logger(), "Subscribing to topic '/pct/point_cloud/pcl'"); - sub_ = create_subscription("/pct/point_cloud/pcl", rclcpp::SensorDataQoS(), callback); - - if (window_name_ == "") { - // If no custom window name is given, use the topic name - window_name_ = sub_->get_topic_name(); - } + sub_ = create_subscription( + "/pct/point_cloud/pcl", + rclcpp::SensorDataQoS(), callback); } /// Convert the ROS Image message to an OpenCV matrix and display it to the user. // \param[in] container The image message to show. - void process_image( - const pcl_point_cloud_transport::PCLContainer & container, bool show_image, rclcpp::Logger logger) + void process_pointcloud( + const pcl_point_cloud_transport::PCLContainer & container, rclcpp::Logger logger) { - RCLCPP_INFO(logger, "Received image #%s", container.header().frame_id.c_str()); + RCLCPP_INFO(logger, "Received point_cloud #%s", container.header().frame_id.c_str()); } rclcpp::Subscription::SharedPtr sub_; size_t depth_ = rmw_qos_profile_default.depth; rmw_qos_reliability_policy_t reliability_policy_ = rmw_qos_profile_default.reliability; rmw_qos_history_policy_t history_policy_ = rmw_qos_profile_default.history; - bool show_image_ = true; - std::string window_name_; }; int main(int argc, char * argv[]) diff --git a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp index 3014919..e4789c5 100644 --- a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp +++ b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp @@ -1,34 +1,16 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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. - */ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef PCL_POINT_CLOUD_TRANSPORT__PCL_PUBLISHER_HPP_ #define PCL_POINT_CLOUD_TRANSPORT__PCL_PUBLISHER_HPP_ @@ -57,12 +39,7 @@ class PCLPublisher void declareParameters(const std::string & base_topic) override; TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override; - -// private: -// cras::expected, std::string> convertPC2toPCL( -// const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate, -// bool expert_encoding) const; }; } // namespace pcl_point_cloud_transport -#endif // pcl_point_cloud_transport__DRACO_PUBLISHER_HPP_ +#endif // PCL_POINT_CLOUD_TRANSPORT__PCL_PUBLISHER_HPP_ diff --git a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp index 7c0ac6b..eda60c7 100644 --- a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp +++ b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_TOOLS__CV_MAT_sensor_msgs_point_cloud2_TYPE_ADAPTER_HPP_ -#define IMAGE_TOOLS__CV_MAT_sensor_msgs_point_cloud2_TYPE_ADAPTER_HPP_ - -#include -#include -#include // NOLINT[build/include_order] +#ifndef PCL_POINT_CLOUD_TRANSPORT__PCL_SENSOR_MSGS_POINTCLOUD2_TYPE_ADAPTER_HPP_ +#define PCL_POINT_CLOUD_TRANSPORT__PCL_SENSOR_MSGS_POINTCLOUD2_TYPE_ADAPTER_HPP_ #include #include #include +#include +#include +#include // NOLINT[build/include_order] + #include #include @@ -66,7 +66,9 @@ class PCLContainer { if (std::holds_alternative>(other.storage_)) { storage_ = std::get>(other.storage_); - } else if (std::holds_alternative>(other.storage_)) { + } else if ( // NOLINT + std::holds_alternative>(other.storage_)) + { storage_ = std::make_unique( *std::get>(other.storage_)); } @@ -80,7 +82,9 @@ class PCLContainer is_bigendian_ = other.is_bigendian_; if (std::holds_alternative>(other.storage_)) { storage_ = std::get>(other.storage_); - } else if (std::holds_alternative>(other.storage_)) { + } else if ( // NOLINT + std::holds_alternative>(other.storage_)) + { storage_ = std::make_unique( *std::get>(other.storage_)); } else if (std::holds_alternative(other.storage_)) { @@ -90,11 +94,15 @@ class PCLContainer return *this; } - /// Store an owning pointer to a sensor_msg::msg::PointCloud2, and create a pcl::PCLPointCloud2 that references it. - explicit PCLContainer(std::unique_ptr unique_sensor_msgs_point_cloud2); + /// Store an owning pointer to a sensor_msg::msg::PointCloud2, and create a pcl::PCLPointCloud2 + /// that references it. + explicit PCLContainer( + std::unique_ptr unique_sensor_msgs_point_cloud2); - /// Store an owning pointer to a sensor_msg::msg::PointCloud2, and create a pcl::PCLPointCloud2 that references it. - explicit PCLContainer(std::shared_ptr shared_sensor_msgs_point_cloud2); + /// Store an owning pointer to a sensor_msg::msg::PointCloud2, and create a pcl::PCLPointCloud2 + /// that references it. + explicit PCLContainer( + std::shared_ptr shared_sensor_msgs_point_cloud2); /// Shallow copy the given pcl::PCLPointCloud2 into this class, but do not own the data directly. PCLContainer( @@ -108,7 +116,8 @@ class PCLContainer const std_msgs::msg::Header & header, bool is_bigendian = is_bigendian_system); - /// Copy the sensor_msgs::msg::PointCloud2 into this contain and create a pcl::PCLPointCloud2 that references it. + /// Copy the sensor_msgs::msg::PointCloud2 into this contain and create a pcl::PCLPointCloud2 + /// that references it. explicit PCLContainer(const sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2); /// Return true if this class owns the data the cv_mat references. @@ -153,7 +162,8 @@ class PCLContainer /// Get a copy of the image as a sensor_msgs::msg::PointCloud2. void - get_sensor_msgs_msg_point_cloud2_copy(sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) const; + get_sensor_msgs_msg_point_cloud2_copy(sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) + const; /// Return true if the data is stored in big endian, otherwise return false. bool @@ -166,7 +176,7 @@ class PCLContainer bool is_bigendian_; }; -} // namespace image_tools +} // namespace pcl_point_cloud_transport template<> struct rclcpp::TypeAdapter @@ -185,31 +195,6 @@ struct rclcpp::TypeAdapter(cloud, msg); pcl_conversions::moveFromPCL(cloud, destination); - - - // destination.height = source.cv_mat().rows; - // destination.width = source.cv_mat().cols; - // switch (source.cv_mat().type()) { - // case CV_8UC1: - // destination.encoding = "mono8"; - // break; - // case CV_8UC3: - // destination.encoding = "bgr8"; - // break; - // case CV_16SC1: - // destination.encoding = "mono16"; - // break; - // case CV_8UC4: - // destination.encoding = "rgba8"; - // break; - // default: - // throw std::runtime_error("unsupported encoding type"); - // } - // destination.step = static_cast(source.cv_mat().step); - // size_t size = source.cv_mat().step * source.cv_mat().rows; - // destination.data.resize(size); - // memcpy(&destination.data[0], source.cv_mat().data, size); - // destination.header = source.header(); } static @@ -221,5 +206,4 @@ struct rclcpp::TypeAdapter pcl_point_cloud_transport - 1.0.5 + 0.0.0 - Add descrioptuio + pcl_point_cloud_transport provides a plugin to point_cloud_transport for sending point clouds + encoded with PCL Jakub Paplham Alejandro Hernandez Cordero BSD - - - + https://github.com/john-maidbot/point_cloud_transport_plugins + https://github.com/john-maidbot/point_cloud_transport_plugins + https://github.com/john-maidbot/point_cloud_transport_plugins/issues ament_cmake diff --git a/pcl_point_cloud_transport/src/manifest.cpp b/pcl_point_cloud_transport/src/manifest.cpp index d2cbf37..c2337bc 100644 --- a/pcl_point_cloud_transport/src/manifest.cpp +++ b/pcl_point_cloud_transport/src/manifest.cpp @@ -1,34 +1,16 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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. - */ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include diff --git a/pcl_point_cloud_transport/src/pcl_publisher.cpp b/pcl_point_cloud_transport/src/pcl_publisher.cpp index afc4f9e..d114686 100644 --- a/pcl_point_cloud_transport/src/pcl_publisher.cpp +++ b/pcl_point_cloud_transport/src/pcl_publisher.cpp @@ -1,34 +1,16 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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. - */ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include @@ -46,21 +28,10 @@ RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( namespace pcl_point_cloud_transport { -void PCLPublisher::declareParameters(const std::string & base_topic) +void PCLPublisher::declareParameters(const std::string & /*base_topic*/) { } -// cras::expected, std::string> PCLPublisher::convertPC2toPCL( -// const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate, -// bool expert_encoding) const -// { -// // std_msgs::msg::Header header; -// // header.frame_id = frame_id_; -// // header.stamp = this->now(); -// // pcl_point_cloud_transport::PCLContainer container(PC2); -// // return container.get_sensor_msgs_msg_point_cloud2_pointer_copy(); -// } - std::string PCLPublisher::getTransportName() const { return "pcl"; @@ -71,7 +42,6 @@ PCLPublisher::TypedEncodeResult PCLPublisher::encodeTyped( { pcl_point_cloud_transport::PCLContainer container(raw); return std::optional(container); - // cras::make_unexpected("lol"); } -} // namespace draco_point_cloud_transport +} // namespace pcl_point_cloud_transport diff --git a/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp b/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp index a25ac51..aadebce 100644 --- a/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp +++ b/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. +// Copyright 2023 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -28,11 +28,6 @@ namespace pcl_point_cloud_transport namespace { -// int -// encoding2mat_type(const std::string & encoding) -// { -// } - template struct NotNull { @@ -55,41 +50,17 @@ PCLContainer::PCLContainer( unique_sensor_msgs_point_cloud2.get(), "unique_sensor_msgs_point_cloud2 cannot be nullptr" ).pointer->header) - - // storage_(std::move(unique_sensor_msgs_point_cloud2)) { - // pcl::PCLPointCloud2 pcl_pc2; - // pcl_conversions::copyPointCloud2MetaData(*unique_sensor_msgs_point_cloud2.get(), pcl_pc2); // Like pcl_conversions::toPCL, but does not copy the binary data - // pcl::MsgFieldMap field_map; - // pcl::createMapping (pcl_pc2.fields, field_map); - // pcl::fromPCLPointCloud2(pcl_pc2, frame_, field_map, &(*unique_sensor_msgs_point_cloud2.get(),).data[0]); - pcl_conversions::toPCL(*unique_sensor_msgs_point_cloud2.get(), frame_); - storage_ = std::move(unique_sensor_msgs_point_cloud2); - - // - // pcl::copyPointCloud(other.frame_, frame_); - } PCLContainer::PCLContainer( std::shared_ptr shared_sensor_msgs_point_cloud2) : header_(shared_sensor_msgs_point_cloud2->header) -// frame_( -// shared_sensor_msgs_point_cloud2->height, -// shared_sensor_msgs_point_cloud2->width, -// encoding2mat_type(shared_sensor_msgs_point_cloud2->encoding), -// shared_sensor_msgs_point_cloud2->data.data(), -// shared_sensor_msgs_point_cloud2->step), -// storage_(shared_sensor_msgs_point_cloud2) { - pcl_conversions::toPCL(*shared_sensor_msgs_point_cloud2.get(), frame_); - storage_ = shared_sensor_msgs_point_cloud2; - - } PCLContainer::PCLContainer( @@ -168,29 +139,7 @@ void PCLContainer::get_sensor_msgs_msg_point_cloud2_copy( sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) const { - // sensor_msgs_point_cloud2.height = frame_.rows; - // sensor_msgs_point_cloud2.width = frame_.cols; - // switch (frame_.type()) { - // case CV_8UC1: - // sensor_msgs_point_cloud2.encoding = "mono8"; - // break; - // case CV_8UC3: - // sensor_msgs_point_cloud2.encoding = "bgr8"; - // break; - // case CV_16SC1: - // sensor_msgs_point_cloud2.encoding = "mono16"; - // break; - // case CV_8UC4: - // sensor_msgs_point_cloud2.encoding = "rgba8"; - // break; - // default: - // throw std::runtime_error("unsupported encoding type"); - // } - // sensor_msgs_point_cloud2.step = static_cast(frame_.step); - // size_t size = frame_.step * frame_.rows; - // sensor_msgs_point_cloud2.data.resize(size); - // memcpy(&sensor_msgs_point_cloud2.data[0], frame_.data, size); - // sensor_msgs_point_cloud2.header = header_; + // TODO(ahcorde): Implement this emthod } bool @@ -199,4 +148,4 @@ PCLContainer::is_bigendian() const return is_bigendian_; } -} // namespace image_tools +} // namespace pcl_point_cloud_transport diff --git a/pcl_point_cloud_transport/src/pcl_subscriber.cpp b/pcl_point_cloud_transport/src/pcl_subscriber.cpp index 84d62cf..d3ac839 100644 --- a/pcl_point_cloud_transport/src/pcl_subscriber.cpp +++ b/pcl_point_cloud_transport/src/pcl_subscriber.cpp @@ -1,34 +1,19 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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. - */ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include #include #include @@ -37,8 +22,6 @@ #include #include -#include -#include namespace pcl_point_cloud_transport { @@ -65,12 +48,10 @@ PCLSubscriber::DecodeResult PCLSubscriber::decodeTyped( pcl::io::savePCDFile( ss.str(), pcl_pc2, Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), true); - + pcl_conversions::moveFromPCL(pcl_pc2, *message.get()); return message; - // pcl_point_cloud_transport::PCLContainer::ConstPtr l; - // return message; } } // namespace pcl_point_cloud_transport