From 5570c30deb061fd5b94e405017692ba5c50af4bc Mon Sep 17 00:00:00 2001 From: duongtd Date: Tue, 21 Jul 2026 14:56:02 +0700 Subject: [PATCH] add pcl --- CMakeLists.txt | 12 +- include/data_convert/data_convert.h | 17 +- include/data_convert/pcl_convert.h | 374 ++++++++++++++++++++++++++++ package.xml | 2 + 4 files changed, 398 insertions(+), 7 deletions(-) create mode 100644 include/data_convert/pcl_convert.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 03b3380..9ff202d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ if (NOT BUILDING_WITH_CATKIN) set(PACKAGES_DIR robot_geometry_msgs + robot_sensor_msgs ) else() @@ -34,14 +35,21 @@ else() # ======================================================== # Catkin specific configuration # ======================================================== + find_package(PCL REQUIRED) find_package(catkin REQUIRED COMPONENTS robot_geometry_msgs + robot_sensor_msgs ) catkin_package( INCLUDE_DIRS include # LIBRARIES không cần vì đây là header-only library - CATKIN_DEPENDS robot_geometry_msgs + CATKIN_DEPENDS + robot_geometry_msgs + robot_sensor_msgs + + DEPENDS + PCL ) include_directories( @@ -64,12 +72,14 @@ if(BUILDING_WITH_CATKIN) INTERFACE $ $ + ${PCL_INCLUDE_DIRS} ) # Link dependencies (header-only, chỉ cần include paths) target_link_libraries(${PROJECT_NAME} INTERFACE ${catkin_LIBRARIES} + ${PCL_LIBRARIES} ) else() diff --git a/include/data_convert/data_convert.h b/include/data_convert/data_convert.h index 6494c1b..d706f4c 100644 --- a/include/data_convert/data_convert.h +++ b/include/data_convert/data_convert.h @@ -1,14 +1,19 @@ -#ifndef COSTMAP_2D_DATA_CONVERT_H -#define COSTMAP_2D_DATA_CONVERT_H +#ifndef ROBOT_DATA_CONVERT_H +#define ROBOT_DATA_CONVERT_H + +#include +#include +#include #include #include #include #include -#include -#include -#include +#include + #include +#include +#include namespace data_convert { @@ -191,4 +196,4 @@ namespace data_convert } } -#endif // DATA_CONVERT_H \ No newline at end of file +#endif // ROBOT_DATA_CONVERT_H \ No newline at end of file diff --git a/include/data_convert/pcl_convert.h b/include/data_convert/pcl_convert.h new file mode 100644 index 0000000..71a2942 --- /dev/null +++ b/include/data_convert/pcl_convert.h @@ -0,0 +1,374 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation, Inc. + * Copyright (c) 2010-2012, 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 Open Source Robotics Foundation, Inc. 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 OWNER 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 ROBOT_PCL_CONVERSIONS_H__ +#define ROBOT_PCL_CONVERSIONS_H__ + +#include + +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include + + +namespace robot_pcl_conversions { + + /** PCLHeader <=> Header **/ + + inline + void fromPCL(const std::uint64_t &pcl_stamp, robot::Time &stamp) + { + stamp.fromNSec(pcl_stamp * 1000ull); // Convert from us to ns + } + + inline + void toPCL(const robot::Time &stamp, std::uint64_t &pcl_stamp) + { + pcl_stamp = stamp.toNSec() / 1000ull; // Convert from ns to us + } + + inline + robot::Time fromPCL(const std::uint64_t &pcl_stamp) + { + robot::Time stamp; + fromPCL(pcl_stamp, stamp); + return stamp; + } + + inline + std::uint64_t toPCL(const robot::Time &stamp) + { + std::uint64_t pcl_stamp; + toPCL(stamp, pcl_stamp); + return pcl_stamp; + } + + /** PCLHeader <=> Header **/ + + inline + void fromPCL(const pcl::PCLHeader &pcl_header, robot_std_msgs::Header &header) + { + fromPCL(pcl_header.stamp, header.stamp); + header.seq = pcl_header.seq; + header.frame_id = pcl_header.frame_id; + } + + inline + void toPCL(const robot_std_msgs::Header &header, pcl::PCLHeader &pcl_header) + { + toPCL(header.stamp, pcl_header.stamp); + pcl_header.seq = header.seq; + pcl_header.frame_id = header.frame_id; + } + + inline + robot_std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) + { + robot_std_msgs::Header header; + fromPCL(pcl_header, header); + return header; + } + + inline + pcl::PCLHeader toPCL(const robot_std_msgs::Header &header) + { + pcl::PCLHeader pcl_header; + toPCL(header, pcl_header); + return pcl_header; + } + + /** PCLImage <=> Image **/ + + inline + void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, robot_sensor_msgs::Image &image) + { + fromPCL(pcl_image.header, image.header); + image.height = pcl_image.height; + image.width = pcl_image.width; + image.encoding = pcl_image.encoding; + image.is_bigendian = pcl_image.is_bigendian; + image.step = pcl_image.step; + } + + inline + void fromPCL(const pcl::PCLImage &pcl_image, robot_sensor_msgs::Image &image) + { + copyPCLImageMetaData(pcl_image, image); + image.data = pcl_image.data; + } + + inline + void moveFromPCL(pcl::PCLImage &pcl_image, robot_sensor_msgs::Image &image) + { + copyPCLImageMetaData(pcl_image, image); + image.data.swap(pcl_image.data); + } + + inline + void copyImageMetaData(const robot_sensor_msgs::Image &image, pcl::PCLImage &pcl_image) + { + toPCL(image.header, pcl_image.header); + pcl_image.height = image.height; + pcl_image.width = image.width; + pcl_image.encoding = image.encoding; + pcl_image.is_bigendian = image.is_bigendian; + pcl_image.step = image.step; + } + + inline + void toPCL(const robot_sensor_msgs::Image &image, pcl::PCLImage &pcl_image) + { + copyImageMetaData(image, pcl_image); + pcl_image.data = image.data; + } + + inline + void moveToPCL(robot_sensor_msgs::Image &image, pcl::PCLImage &pcl_image) + { + copyImageMetaData(image, pcl_image); + pcl_image.data.swap(image.data); + } + + /** PCLPointField <=> PointField **/ + + inline + void fromPCL(const pcl::PCLPointField &pcl_pf, robot_sensor_msgs::PointField &pf) + { + pf.name = pcl_pf.name; + pf.offset = pcl_pf.offset; + pf.datatype = pcl_pf.datatype; + pf.count = pcl_pf.count; + } + + inline + void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) + { + pfs.resize(pcl_pfs.size()); + std::vector::const_iterator it = pcl_pfs.begin(); + int i = 0; + for(; it != pcl_pfs.end(); ++it, ++i) { + fromPCL(*(it), pfs[i]); + } + std::sort(pfs.begin(), pfs.end(), [](const auto& field_a, const auto& field_b) + { + return field_a.offset < field_b.offset; + }); + } + + inline + void toPCL(const robot_sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf) + { + pcl_pf.name = pf.name; + pcl_pf.offset = pf.offset; + pcl_pf.datatype = pf.datatype; + pcl_pf.count = pf.count; + } + + inline + void toPCL(const std::vector &pfs, std::vector &pcl_pfs) + { + pcl_pfs.resize(pfs.size()); + std::vector::const_iterator it = pfs.begin(); + int i = 0; + for(; it != pfs.end(); ++it, ++i) { + toPCL(*(it), pcl_pfs[i]); + } + } + + /** PCLPointCloud2 <=> PointCloud2 **/ + + inline + void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, robot_sensor_msgs::PointCloud2 &pc2) + { + fromPCL(pcl_pc2.header, pc2.header); + pc2.height = pcl_pc2.height; + pc2.width = pcl_pc2.width; + fromPCL(pcl_pc2.fields, pc2.fields); + pc2.is_bigendian = pcl_pc2.is_bigendian; + pc2.point_step = pcl_pc2.point_step; + pc2.row_step = pcl_pc2.row_step; + pc2.is_dense = pcl_pc2.is_dense; + } + + inline + void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, robot_sensor_msgs::PointCloud2 &pc2) + { + copyPCLPointCloud2MetaData(pcl_pc2, pc2); + pc2.data = pcl_pc2.data; + } + + inline + void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, robot_sensor_msgs::PointCloud2 &pc2) + { + copyPCLPointCloud2MetaData(pcl_pc2, pc2); + pc2.data.swap(pcl_pc2.data); + } + + inline + void copyPointCloud2MetaData(const robot_sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { + toPCL(pc2.header, pcl_pc2.header); + pcl_pc2.height = pc2.height; + pcl_pc2.width = pc2.width; + toPCL(pc2.fields, pcl_pc2.fields); + pcl_pc2.is_bigendian = pc2.is_bigendian; + pcl_pc2.point_step = pc2.point_step; + pcl_pc2.row_step = pc2.row_step; + pcl_pc2.is_dense = pc2.is_dense; + } + + inline + void toPCL(const robot_sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { + copyPointCloud2MetaData(pc2, pcl_pc2); + pcl_pc2.data = pc2.data; + } + + inline + void moveToPCL(robot_sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { + copyPointCloud2MetaData(pc2, pcl_pc2); + pcl_pc2.data.swap(pc2.data); + } + +} // namespace robot_pcl_conversions + +namespace pcl { + + /** Provide pcl::toROBOTMsg **/ + + inline + void toROBOTMsg(const robot_sensor_msgs::PointCloud2 &cloud, robot_sensor_msgs::Image &image) + { + pcl::PCLPointCloud2 pcl_cloud; + robot_pcl_conversions::toPCL(cloud, pcl_cloud); + pcl::PCLImage pcl_image; + pcl::toPCLPointCloud2(pcl_cloud, pcl_image); + robot_pcl_conversions::moveFromPCL(pcl_image, image); + } + + inline + void moveToROBOTMsg(robot_sensor_msgs::PointCloud2 &cloud, robot_sensor_msgs::Image &image) + { + pcl::PCLPointCloud2 pcl_cloud; + robot_pcl_conversions::moveToPCL(cloud, pcl_cloud); + pcl::PCLImage pcl_image; + pcl::toPCLPointCloud2(pcl_cloud, pcl_image); + robot_pcl_conversions::moveFromPCL(pcl_image, image); + } + + template void + toROBOTMsg (const pcl::PointCloud &cloud, robot_sensor_msgs::Image& msg) + { + // Ease the user's burden on specifying width/height for unorganized datasets + if (cloud.width == 0 && cloud.height == 0) + { + throw std::runtime_error("Needs to be a dense like cloud!!"); + } + else + { + if (cloud.points.size () != cloud.width * cloud.height) + throw std::runtime_error("The width and height do not match the cloud size!"); + msg.height = cloud.height; + msg.width = cloud.width; + } + + // robot_sensor_msgs::image_encodings::BGR8; + msg.encoding = "bgr8"; + msg.step = msg.width * sizeof (std::uint8_t) * 3; + msg.data.resize (msg.step * msg.height); + for (size_t y = 0; y < cloud.height; y++) + { + for (size_t x = 0; x < cloud.width; x++) + { + std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); + memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(std::uint8_t)); + } + } + } + + /** Provide to/fromROBOTMsg for robot_sensor_msgs::PointCloud2 <=> pcl::PointCloud **/ + + template + void toROBOTMsg(const pcl::PointCloud &pcl_cloud, robot_sensor_msgs::PointCloud2 &cloud) + { + pcl::PCLPointCloud2 pcl_pc2; +#if PCL_VERSION_COMPARE(>=, 1, 14, 1) + // if PCL version is recent enough, request that all padding be removed to make the msg as small as possible + pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2, false); +#else + pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2); +#endif + robot_pcl_conversions::moveFromPCL(pcl_pc2, cloud); + } + + template + void fromROBOTMsg(const robot_sensor_msgs::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + { + pcl::PCLPointCloud2 pcl_pc2; +#if PCL_VERSION_COMPARE(>=, 1, 13, 1) + robot_pcl_conversions::copyPointCloud2MetaData(cloud, pcl_pc2); // Like robot_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, pcl_cloud, field_map, &cloud.data[0]); +#else + robot_pcl_conversions::toPCL(cloud, pcl_pc2); + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); +#endif + } + + template + void moveFromROBOTMsg(robot_sensor_msgs::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + { + pcl::PCLPointCloud2 pcl_pc2; + robot_pcl_conversions::moveToPCL(cloud, pcl_pc2); + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); + } +} // namespace pcl + +#endif /* PCL_CONVERSIONS_H__ */ \ No newline at end of file diff --git a/package.xml b/package.xml index b0d44c0..7509d04 100644 --- a/package.xml +++ b/package.xml @@ -21,5 +21,7 @@ robot_geometry_msgs robot_geometry_msgs + robot_sensor_msgs + robot_sensor_msgs \ No newline at end of file