diff --git a/.vscode/settings.json b/.vscode/settings.json index 97b51ae..6969664 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,6 +1,91 @@ { "files.associations": { "algorithm": "cpp", - "memory": "cpp" + "memory": "cpp", + "compare": "cpp", + "array": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "string_view": "cpp", + "regex": "cpp", + "*.ipp": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "*.tcc": "cpp", + "slist": "cpp", + "valarray": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "bitset": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "coroutine": "cpp", + "cstdint": "cpp", + "map": "cpp", + "set": "cpp", + "exception": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "source_location": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "ranges": "cpp", + "scoped_allocator": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp", + "expected": "cpp" } } \ No newline at end of file diff --git a/organized_point_cloud_transport/CMakeLists.txt b/organized_point_cloud_transport/CMakeLists.txt new file mode 100644 index 0000000..dbc2984 --- /dev/null +++ b/organized_point_cloud_transport/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.10.2) + +set(CMAKE_CXX_STANDARD 17) + +project(organized_point_cloud_transport) + +find_package(ament_cmake REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(point_cloud_interfaces REQUIRED) +find_package(point_cloud_transport REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +set(dependencies + pcl_conversions + pcl_ros + pluginlib + point_cloud_interfaces + point_cloud_transport + rclcpp + sensor_msgs + tf2_geometry_msgs + tf2_ros +) + + +include_directories( + include +) + +add_library(${PROJECT_NAME} + SHARED + src/manifest.cpp + src/organize_cloud.cpp + src/organized_publisher.cpp + src/organized_subscriber.cpp +) + +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +pluginlib_export_plugin_description_file(point_cloud_transport organized_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}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/organized_point_cloud_transport/include/organized_point_cloud_transport/organized_publisher.hpp b/organized_point_cloud_transport/include/organized_point_cloud_transport/organized_publisher.hpp new file mode 100644 index 0000000..71587f4 --- /dev/null +++ b/organized_point_cloud_transport/include/organized_point_cloud_transport/organized_publisher.hpp @@ -0,0 +1,89 @@ +/* + * Copyright (c) 2023, John D'Angelo + * 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 PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_PUBLISHER_HPP_ +#define PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_PUBLISHER_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace organized_point_cloud_transport +{ + +class OrganizedPublisher + : public point_cloud_transport::SimplePublisherPlugin< + point_cloud_interfaces::msg::CompressedPointCloud2> +{ +public: + std::string getTransportName() const override; + + void declareParameters(const std::string & base_topic) override; + + TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override; + + std::string getDataType() const override + { + return "point_cloud_interfaces/msg/CompressedPointCloud2"; + } + +private: + + void encodeOrganizedPointCloud2(const sensor_msgs::msg::PointCloud2& cloud, std::vector& compressed_data) const; + + void organizePointCloud2(const sensor_msgs::msg::PointCloud2& cloud, sensor_msgs::msg::PointCloud2& organized) const; + + // tf2 machinery + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // params for planar projection + sensor_msgs::msg::CameraInfo projector_info_; + + // params for compression + int png_level_; + +}; +} // namespace organized_point_cloud_transport + +#endif // PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_PUBLISHER_HPP_ diff --git a/organized_point_cloud_transport/include/organized_point_cloud_transport/organized_subscriber.hpp b/organized_point_cloud_transport/include/organized_point_cloud_transport/organized_subscriber.hpp new file mode 100644 index 0000000..c2e0064 --- /dev/null +++ b/organized_point_cloud_transport/include/organized_point_cloud_transport/organized_subscriber.hpp @@ -0,0 +1,67 @@ +/* + * Copyright (c) 2023, John D'Angelo + * 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 PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_SUBSCRIBER_HPP_ +#define PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_SUBSCRIBER_HPP_ + +#include +#include + +#include + +#include +#include +#include + +namespace organized_point_cloud_transport +{ + +class OrganizedSubscriber + : public point_cloud_transport::SimpleSubscriberPlugin< + point_cloud_interfaces::msg::CompressedPointCloud2> +{ +public: + std::string getTransportName() const override; + + void declareParameters() override; + + DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed) + const override; + + std::string getDataType() const override + { + return "point_cloud_interfaces/msg/CompressedPointCloud2"; + } + +}; +} // namespace organized_point_cloud_transport + +#endif // PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_SUBSCRIBER_HPP_ diff --git a/organized_point_cloud_transport/organized_plugins.xml b/organized_point_cloud_transport/organized_plugins.xml new file mode 100644 index 0000000..2b40ffd --- /dev/null +++ b/organized_point_cloud_transport/organized_plugins.xml @@ -0,0 +1,23 @@ + + + + This plugin publishes a CompressedPointCloud2 message as a png-compressed, organized + pointcloud. If the + pointcloud is not already organized, it is organized via pin-hole projection. The + projection is configurable. + + + + + + This plugin decompresses a png-compressed, organized CompressedPointCloud2 message into + a PointCloud2 message. + + + \ No newline at end of file diff --git a/organized_point_cloud_transport/package.xml b/organized_point_cloud_transport/package.xml new file mode 100644 index 0000000..bd4387e --- /dev/null +++ b/organized_point_cloud_transport/package.xml @@ -0,0 +1,38 @@ + + organized_point_cloud_transport + 1.0.5 + + organized_point_cloud_transport provides a plugin to point_cloud_transport for sending point clouds + compressed by projecting the cloud onto an image and compressing the image like its a png image. + Adapted from: https://arxiv.org/pdf/2202.00719.pdf + + John D'Angelo + John D'Angelo + BSD + + https://github.com/ctu-vras/draco_point_cloud_transport + https://wiki.ros.org/draco_point_cloud_transport + https://github.com/ctu-vras/draco_point_cloud_transport/issues + + ament_cmake + + pluginlib + + pluginlib + + libpcl-dev + pcl_conversions + point_cloud_interfaces + point_cloud_transport + rclcpp + sensor_msgs + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/organized_point_cloud_transport/src/manifest.cpp b/organized_point_cloud_transport/src/manifest.cpp new file mode 100644 index 0000000..d38d419 --- /dev/null +++ b/organized_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( + organized_point_cloud_transport::OrganizedPublisher, + point_cloud_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS( + organized_point_cloud_transport::OrganizedSubscriber, + point_cloud_transport::SubscriberPlugin) diff --git a/organized_point_cloud_transport/src/organize_cloud.cpp b/organized_point_cloud_transport/src/organize_cloud.cpp new file mode 100644 index 0000000..21fa09d --- /dev/null +++ b/organized_point_cloud_transport/src/organize_cloud.cpp @@ -0,0 +1,164 @@ +/* + * Copyright (c) 2023, John D'Angelo + * 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 + +namespace organized_point_cloud_transport +{ + void OrganizedPublisher::organizePointCloud2(const sensor_msgs::msg::PointCloud2 &unorganized_cloud, sensor_msgs::msg::PointCloud2 &organized_cloud) const + { + // get the transform between the frame_id of the pointcloud and the frame_id of the view point + geometry_msgs::msg::TransformStamped transform_stamped; + if (unorganized_cloud.header.frame_id == projector_info_.header.frame_id) + { + transform_stamped.transform.rotation.w = 1; + } + else + { + try + { + // impatiently lookup the transform + transform_stamped = tf_buffer_->lookupTransform( + projector_info_.header.frame_id, unorganized_cloud.header.frame_id, unorganized_cloud.header.stamp, rclcpp::Duration::from_seconds(0.1)); + } + catch (tf2::TransformException &ex) + { + RCLCPP_ERROR_STREAM(getLogger(), "Failed to lookup transform from " << projector_info_.header.frame_id << " to " << unorganized_cloud.header.frame_id << "!"); + RCLCPP_ERROR_STREAM(getLogger(), "Using identity transform instead!"); + } + } + + // Check if the cloud has rgb data + const auto predicate = [](const auto &field) + { return field.name == "rgb"; }; + const auto result = std::find_if(unorganized_cloud.fields.cbegin(), unorganized_cloud.fields.cend(), predicate); + const bool has_rgb = result != unorganized_cloud.fields.cend(); + + // initialize the pointcloud accordingly + organized_cloud.is_dense = true; + organized_cloud.header.frame_id = projector_info_.header.frame_id; + organized_cloud.header.stamp = unorganized_cloud.header.stamp; + organized_cloud.height = projector_info_.height; + organized_cloud.width = projector_info_.width; + organized_cloud.is_bigendian = unorganized_cloud.is_bigendian; + + sensor_msgs::PointCloud2Modifier modifier(organized_cloud); + if (has_rgb) + { + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); // Define fields as XYZ and RGB + } + else + { + modifier.setPointCloud2FieldsByString(1, "xyz"); // Define fields as XYZ + } + modifier.resize(organized_cloud.width * organized_cloud.height); + + // create iterators for accessing the unorganized pointcloud data + sensor_msgs::PointCloud2ConstIterator u_iter_x(unorganized_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator u_iter_y(unorganized_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator u_iter_z(unorganized_cloud, "z"); + sensor_msgs::PointCloud2ConstIterator u_iter_r(unorganized_cloud, "r"); + sensor_msgs::PointCloud2ConstIterator u_iter_g(unorganized_cloud, "g"); + sensor_msgs::PointCloud2ConstIterator u_iter_b(unorganized_cloud, "b"); + + // initialize the point objects for the transform + geometry_msgs::msg::PointStamped old_point, new_point; + + for (; u_iter_x != u_iter_x.end();) + { + old_point.point.x = *u_iter_x; + old_point.point.y = *u_iter_y; + old_point.point.z = *u_iter_z; + + // skip points that are not finite + if (!std::isfinite(old_point.point.x) || !std::isfinite(old_point.point.y) || !std::isfinite(old_point.point.z)) + { + continue; + } + + // transform the point into the view point frame + tf2::doTransform(old_point, new_point, transform_stamped); + + if (new_point.point.z <= 0) + { + // the imaginary camera doesnt see this point + continue; + } + + const int col = new_point.point.x / new_point.point.z * projector_info_.k[0] + projector_info_.k[2]; + const int row = new_point.point.y / new_point.point.z * projector_info_.k[4] + projector_info_.k[5]; + + if (col < 0 || row < 0 || col >= projector_info_.width || row >= projector_info_.height) + { + // the imaginary camera doesnt see this point + continue; + } + + // get index into organized pointcloud + const int index = row * organized_cloud.row_step + col * organized_cloud.point_step; + + // get the current z coordinate of this cell in the organized pointcloud + // reinterprety the uint8_t memory as float + float *z_ptr = reinterpret_cast(&organized_cloud.data[index + 2 * 4]); + + if (*z_ptr < new_point.point.z) + { + // this point is further than the one already in the image at this pixel + continue; + } + + if (has_rgb) + { + // copy the xyz and rgb values + *reinterpret_cast(&organized_cloud.data[index + 0 * 4]) = new_point.point.x; + *reinterpret_cast(&organized_cloud.data[index + 1 * 4]) = new_point.point.y; + *z_ptr = new_point.point.z; + // TODO (john-maidbot): This might be wrong + organized_cloud.data[index + 3 * 4 + 1] = *u_iter_r; + organized_cloud.data[index + 3 * 4 + 2] = *u_iter_g; + organized_cloud.data[index + 3 * 4 + 3] = *u_iter_b; + ++u_iter_r, ++u_iter_g, ++u_iter_b; + } + else + { + *reinterpret_cast(&organized_cloud.data[index + 0 * 4]) = new_point.point.x; + *reinterpret_cast(&organized_cloud.data[index + 1 * 4]) = new_point.point.y; + *z_ptr = new_point.point.z; + } + ++u_iter_x, ++u_iter_y, ++u_iter_z; + } + } +} // namespace organized_point_cloud_transport diff --git a/organized_point_cloud_transport/src/organized_publisher.cpp b/organized_point_cloud_transport/src/organized_publisher.cpp new file mode 100644 index 0000000..76efcc8 --- /dev/null +++ b/organized_point_cloud_transport/src/organized_publisher.cpp @@ -0,0 +1,151 @@ +/* + * Copyright (c) 2023, John D'Angelo + * 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 organized_point_cloud_transport +{ + + void OrganizedPublisher::declareParameters(const std::string &base_topic) + { + // params for planar projection + projector_info_.height = 720; + projector_info_.width = 1080; + projector_info_.k[2] = static_cast(projector_info_.width) / 2.0; + projector_info_.k[5] = static_cast(projector_info_.height) / 2.0; + const float fov = M_PI_2; + projector_info_.k[0] = projector_info_.width / (2 * std::tan(fov / 2.0)); + projector_info_.k[4] = projector_info_.height / (2 * std::tan(fov / 2.0)); + // compression params + png_level_ = 3; + + // if this is the first time receieving a message, setup the tf2 machinery + if (!tf_buffer_) + { + auto node_ptr = getNode(); + if (node_ptr == nullptr) + { + RCLCPP_ERROR_STREAM(getLogger(), "Node pointer is null!"); + } + tf_buffer_ = std::make_shared(node_ptr->get_clock()); + // disable intra process communication in case someone is using this in a composition + rclcpp::SubscriptionOptions tf2_subscription_options; + tf2_subscription_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + tf_listener_ = std::make_shared( + *tf_buffer_, node_ptr, true, tf2_ros::DynamicListenerQoS(), tf2_ros::StaticListenerQoS(), + tf2_subscription_options, tf2_subscription_options); + } + } + + std::string OrganizedPublisher::getTransportName() const + { + return "organized"; + } + + OrganizedPublisher::TypedEncodeResult OrganizedPublisher::encodeTyped( + const sensor_msgs::msg::PointCloud2 &raw) const + { + point_cloud_interfaces::msg::CompressedPointCloud2 compressed; + + if (raw.is_dense) + { + encodeOrganizedPointCloud2(raw, compressed.compressed_data); + compressed.header = raw.header; + compressed.height = raw.height; + compressed.width = raw.width; + compressed.fields = raw.fields; + compressed.is_bigendian = raw.is_bigendian; + } + else + { + sensor_msgs::msg::PointCloud2 organized; + organizePointCloud2(raw, organized); + encodeOrganizedPointCloud2(organized, compressed.compressed_data); + // Populate the msg metadata + compressed.header = organized.header; + compressed.height = organized.height; + compressed.width = organized.width; + compressed.fields = organized.fields; + compressed.is_bigendian = organized.is_bigendian; + } + compressed.format = getTransportName(); + + return compressed; + } + + void OrganizedPublisher::encodeOrganizedPointCloud2(const sensor_msgs::msg::PointCloud2 &cloud, std::vector &compressed_data) const + { + const auto predicate = [](const auto &field) + { return field.name == "rgb"; }; + const auto result = std::find_if(cloud.fields.cbegin(), cloud.fields.cend(), predicate); + const bool has_rgb = result != cloud.fields.cend(); + + // PCL has a nice solution for organized pointclouds, so let's not reinvent the wheel + pcl::PCLPointCloud2 pcl_pc2; + pcl_conversions::toPCL(cloud, pcl_pc2); + std::ostringstream compressed_data_stream; + if (has_rgb) + { + auto temp_cloud = pcl::make_shared>(); + pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); + pcl::io::OrganizedPointCloudCompression encoder; + encoder.encodePointCloud(temp_cloud, + compressed_data_stream, + true, + false, + false, + png_level_); + } + else + { + auto temp_cloud = pcl::make_shared>(); + pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); + pcl::io::OrganizedPointCloudCompression encoder; + encoder.encodePointCloud(temp_cloud, + compressed_data_stream, + false, + false, + false, + png_level_); + } + compressed_data = std::vector(compressed_data_stream.str().begin(), compressed_data_stream.str().end()); + } + +} // namespace organized_point_cloud_transport diff --git a/organized_point_cloud_transport/src/organized_subscriber.cpp b/organized_point_cloud_transport/src/organized_subscriber.cpp new file mode 100644 index 0000000..58bed9e --- /dev/null +++ b/organized_point_cloud_transport/src/organized_subscriber.cpp @@ -0,0 +1,85 @@ +/* + * Copyright (c) 2023, John D'Angelo + * 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 + +namespace organized_point_cloud_transport +{ +void OrganizedSubscriber::declareParameters() +{ +} + +std::string OrganizedSubscriber::getTransportName() const +{ + return "organized"; +} + +OrganizedSubscriber::DecodeResult OrganizedSubscriber::decodeTyped( + const point_cloud_interfaces::msg::CompressedPointCloud2 & msg) const +{ + auto result = std::make_shared(); + + // determine if this is an xyz only or an xyzrgb cloud + const auto predicate = [](const auto& field) { return field.name == "rgb"; }; + const auto field_result = std::find_if(msg.fields.cbegin (), msg.fields.cend (), predicate); + const bool has_rgb = (field_result != msg.fields.cend()); + + // decompress the cloud + std::istringstream stream(reinterpret_cast(msg.compressed_data.data())); + if(has_rgb){ + auto temp_cloud = pcl::make_shared>(); + pcl::io::OrganizedPointCloudCompression decoder; + decoder.decodePointCloud (stream, temp_cloud, false); + pcl::PCLPointCloud2::Ptr temp_cloud_pcl2 = pcl::make_shared(); + pcl::toPCLPointCloud2(*temp_cloud, *temp_cloud_pcl2); + pcl_conversions::moveFromPCL(*temp_cloud_pcl2, *result); + }else{ + auto temp_cloud = pcl::make_shared>(); + pcl::io::OrganizedPointCloudCompression decoder; + decoder.decodePointCloud (stream, temp_cloud, false); + pcl::PCLPointCloud2::Ptr temp_cloud_pcl2 = pcl::make_shared(); + pcl::toPCLPointCloud2(*temp_cloud, *temp_cloud_pcl2); + pcl_conversions::moveFromPCL(*temp_cloud_pcl2, *result); + } + result->header = msg.header; + + return result; +} + +} // namespace organized_point_cloud_transport diff --git a/point_cloud_interfaces/CMakeLists.txt b/point_cloud_interfaces/CMakeLists.txt index 5eb2508..10a5c0a 100644 --- a/point_cloud_interfaces/CMakeLists.txt +++ b/point_cloud_interfaces/CMakeLists.txt @@ -3,6 +3,7 @@ project(point_cloud_interfaces) find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -13,7 +14,7 @@ set(msg_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} - DEPENDENCIES sensor_msgs std_msgs builtin_interfaces + DEPENDENCIES geometry_msgs sensor_msgs std_msgs builtin_interfaces ) if(BUILD_TESTING) diff --git a/point_cloud_interfaces/package.xml b/point_cloud_interfaces/package.xml index d9e7385..43ffdde 100644 --- a/point_cloud_interfaces/package.xml +++ b/point_cloud_interfaces/package.xml @@ -16,6 +16,7 @@ builtin_interfaces rosidl_default_generators + geometry_msgs sensor_msgs std_msgs diff --git a/zlib_point_cloud_transport/src/zlib_publisher.cpp b/zlib_point_cloud_transport/src/zlib_publisher.cpp index de702e5..e3935fd 100644 --- a/zlib_point_cloud_transport/src/zlib_publisher.cpp +++ b/zlib_point_cloud_transport/src/zlib_publisher.cpp @@ -115,6 +115,8 @@ ZlibPublisher::TypedEncodeResult ZlibPublisher::encodeTyped( compressed.fields = raw.fields; compressed.format = getTransportName(); + compressed.format = getTransportName(); + return compressed; }