diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index 024b7948f..311fdb4b6 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -28,28 +28,45 @@ set(dependencies std_msgs ) -include_directories( - include - ${PCL_COMMON_INCLUDE_DIRS} +add_library(${PROJECT_NAME} INTERFACE) + +target_include_directories( + ${PROJECT_NAME} + INTERFACE + "$" + "$" +) + +target_link_libraries( + ${PROJECT_NAME} INTERFACE + rclcpp::rclcpp + message_filters::message_filters + pcl_common + pcl_io + ${std_msgs_TARGETS} + ${pcl_msgs_TARGETS} + ${sensor_msgs_TARGETS} ) -install(DIRECTORY include/ +install( + DIRECTORY include/ DESTINATION include/${PROJECT_NAME} ) +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}_export + INCLUDES DESTINATION include +) + # Add gtest based cpp test target if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp) - ament_target_dependencies(${PROJECT_NAME}-test - ${dependencies} - ) - target_link_libraries(${PROJECT_NAME}-test ${Boost_LIBRARIES} ${PCL_LIBRARIES}) + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() -# Export old-style CMake variables -ament_export_include_directories("include/${PROJECT_NAME}") - +ament_export_targets(${PROJECT_NAME}_export) ament_export_dependencies(${dependencies} PCL) ament_package() diff --git a/pcl_conversions/include/pcl_conversions/pcl_conversions.h b/pcl_conversions/include/pcl_conversions/pcl_conversions.h index b27bc25e5..fe669e637 100644 --- a/pcl_conversions/include/pcl_conversions/pcl_conversions.h +++ b/pcl_conversions/include/pcl_conversions/pcl_conversions.h @@ -41,8 +41,8 @@ #include #include -#include -#include +#include +#include #include @@ -80,20 +80,20 @@ namespace pcl_conversions { /** PCLHeader <=> Header **/ inline - void fromPCL(const std::uint64_t &pcl_stamp, rclcpp::Time &stamp) + void fromPCL(const std::uint64_t & pcl_stamp, rclcpp::Time & stamp) { stamp = rclcpp::Time( - static_cast(pcl_stamp * 1000ull)); // Convert from us to ns + static_cast < rcl_time_point_value_t > (pcl_stamp * 1000ull)); // Convert from us to ns } inline - void toPCL(const rclcpp::Time &stamp, std::uint64_t &pcl_stamp) + void toPCL(const rclcpp::Time & stamp, std::uint64_t & pcl_stamp) { - pcl_stamp = static_cast(stamp.nanoseconds()) / 1000ull; // Convert from ns to us + pcl_stamp = static_cast < std::uint64_t > (stamp.nanoseconds()) / 1000ull; // Convert from ns to us } inline - rclcpp::Time fromPCL(const std::uint64_t &pcl_stamp) + rclcpp::Time fromPCL(const std::uint64_t & pcl_stamp) { rclcpp::Time stamp; fromPCL(pcl_stamp, stamp); @@ -101,7 +101,7 @@ namespace pcl_conversions { } inline - std::uint64_t toPCL(const rclcpp::Time &stamp) + std::uint64_t toPCL(const rclcpp::Time & stamp) { std::uint64_t pcl_stamp; toPCL(stamp, pcl_stamp); @@ -111,14 +111,14 @@ namespace pcl_conversions { /** PCLHeader <=> Header **/ inline - void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::msg::Header &header) + void fromPCL(const pcl::PCLHeader & pcl_header, std_msgs::msg::Header & header) { header.stamp = fromPCL(pcl_header.stamp); header.frame_id = pcl_header.frame_id; } inline - void toPCL(const std_msgs::msg::Header &header, pcl::PCLHeader &pcl_header) + void toPCL(const std_msgs::msg::Header & header, pcl::PCLHeader & pcl_header) { toPCL(header.stamp, pcl_header.stamp); // TODO(clalancette): Seq doesn't exist in the ROS2 header @@ -130,7 +130,7 @@ namespace pcl_conversions { } inline - std_msgs::msg::Header fromPCL(const pcl::PCLHeader &pcl_header) + std_msgs::msg::Header fromPCL(const pcl::PCLHeader & pcl_header) { std_msgs::msg::Header header; fromPCL(pcl_header, header); @@ -138,7 +138,7 @@ namespace pcl_conversions { } inline - pcl::PCLHeader toPCL(const std_msgs::msg::Header &header) + pcl::PCLHeader toPCL(const std_msgs::msg::Header & header) { pcl::PCLHeader pcl_header; toPCL(header, pcl_header); @@ -148,7 +148,7 @@ namespace pcl_conversions { /** PCLImage <=> Image **/ inline - void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image) + void copyPCLImageMetaData(const pcl::PCLImage & pcl_image, sensor_msgs::msg::Image & image) { fromPCL(pcl_image.header, image.header); image.height = pcl_image.height; @@ -159,21 +159,21 @@ namespace pcl_conversions { } inline - void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image) + void fromPCL(const pcl::PCLImage & pcl_image, sensor_msgs::msg::Image & image) { copyPCLImageMetaData(pcl_image, image); image.data = pcl_image.data; } inline - void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image) + void moveFromPCL(pcl::PCLImage & pcl_image, sensor_msgs::msg::Image & image) { copyPCLImageMetaData(pcl_image, image); image.data.swap(pcl_image.data); } inline - void copyImageMetaData(const sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image) + void copyImageMetaData(const sensor_msgs::msg::Image & image, pcl::PCLImage & pcl_image) { toPCL(image.header, pcl_image.header); pcl_image.height = image.height; @@ -184,14 +184,14 @@ namespace pcl_conversions { } inline - void toPCL(const sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image) + void toPCL(const sensor_msgs::msg::Image & image, pcl::PCLImage & pcl_image) { copyImageMetaData(image, pcl_image); pcl_image.data = image.data; } inline - void moveToPCL(sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image) + void moveToPCL(sensor_msgs::msg::Image & image, pcl::PCLImage & pcl_image) { copyImageMetaData(image, pcl_image); pcl_image.data.swap(image.data); @@ -200,7 +200,7 @@ namespace pcl_conversions { /** PCLPointField <=> PointField **/ inline - void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::msg::PointField &pf) + void fromPCL(const pcl::PCLPointField & pcl_pf, sensor_msgs::msg::PointField & pf) { pf.name = pcl_pf.name; pf.offset = pcl_pf.offset; @@ -209,18 +209,24 @@ namespace pcl_conversions { } inline - void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) + void fromPCL( + const std::vector < pcl::PCLPointField > &pcl_pfs, + std::vector < sensor_msgs::msg::PointField > &pfs) { pfs.resize(pcl_pfs.size()); - std::vector::const_iterator it = pcl_pfs.begin(); + std::vector < pcl::PCLPointField > ::const_iterator it = pcl_pfs.begin(); size_t 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 sensor_msgs::msg::PointField &pf, pcl::PCLPointField &pcl_pf) + void toPCL(const sensor_msgs::msg::PointField & pf, pcl::PCLPointField & pcl_pf) { pcl_pf.name = pf.name; pcl_pf.offset = pf.offset; @@ -229,10 +235,12 @@ namespace pcl_conversions { } inline - void toPCL(const std::vector &pfs, std::vector &pcl_pfs) + void toPCL( + const std::vector < sensor_msgs::msg::PointField > &pfs, + std::vector < pcl::PCLPointField > &pcl_pfs) { pcl_pfs.resize(pfs.size()); - std::vector::const_iterator it = pfs.begin(); + std::vector < sensor_msgs::msg::PointField > ::const_iterator it = pfs.begin(); size_t i = 0; for(; it != pfs.end(); ++it, ++i) { toPCL(*(it), pcl_pfs[i]); @@ -242,7 +250,9 @@ namespace pcl_conversions { /** PCLPointCloud2 <=> PointCloud2 **/ inline - void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2) + void copyPCLPointCloud2MetaData( + const pcl::PCLPointCloud2 & pcl_pc2, + sensor_msgs::msg::PointCloud2 & pc2) { fromPCL(pcl_pc2.header, pc2.header); pc2.height = pcl_pc2.height; @@ -255,21 +265,23 @@ namespace pcl_conversions { } inline - void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2) + void fromPCL(const pcl::PCLPointCloud2 & pcl_pc2, sensor_msgs::msg::PointCloud2 & pc2) { copyPCLPointCloud2MetaData(pcl_pc2, pc2); pc2.data = pcl_pc2.data; } inline - void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2) + void moveFromPCL(pcl::PCLPointCloud2 & pcl_pc2, sensor_msgs::msg::PointCloud2 & pc2) { copyPCLPointCloud2MetaData(pcl_pc2, pc2); pc2.data.swap(pcl_pc2.data); } inline - void copyPointCloud2MetaData(const sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + void copyPointCloud2MetaData( + const sensor_msgs::msg::PointCloud2 & pc2, + pcl::PCLPointCloud2 & pcl_pc2) { toPCL(pc2.header, pcl_pc2.header); pcl_pc2.height = pc2.height; @@ -282,14 +294,14 @@ namespace pcl_conversions { } inline - void toPCL(const sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + void toPCL(const sensor_msgs::msg::PointCloud2 & pc2, pcl::PCLPointCloud2 & pcl_pc2) { copyPointCloud2MetaData(pc2, pcl_pc2); pcl_pc2.data = pc2.data; } inline - void moveToPCL(sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + void moveToPCL(sensor_msgs::msg::PointCloud2 & pc2, pcl::PCLPointCloud2 & pcl_pc2) { copyPointCloud2MetaData(pc2, pcl_pc2); pcl_pc2.data.swap(pc2.data); @@ -298,28 +310,28 @@ namespace pcl_conversions { /** pcl::PointIndices <=> pcl_msgs::PointIndices **/ inline - void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::msg::PointIndices &pi) + void fromPCL(const pcl::PointIndices & pcl_pi, pcl_msgs::msg::PointIndices & pi) { fromPCL(pcl_pi.header, pi.header); pi.indices = pcl_pi.indices; } inline - void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::msg::PointIndices &pi) + void moveFromPCL(pcl::PointIndices & pcl_pi, pcl_msgs::msg::PointIndices & pi) { fromPCL(pcl_pi.header, pi.header); pi.indices.swap(pcl_pi.indices); } inline - void toPCL(const pcl_msgs::msg::PointIndices &pi, pcl::PointIndices &pcl_pi) + void toPCL(const pcl_msgs::msg::PointIndices & pi, pcl::PointIndices & pcl_pi) { toPCL(pi.header, pcl_pi.header); pcl_pi.indices = pi.indices; } inline - void moveToPCL(pcl_msgs::msg::PointIndices &pi, pcl::PointIndices &pcl_pi) + void moveToPCL(pcl_msgs::msg::PointIndices & pi, pcl::PointIndices & pcl_pi) { toPCL(pi.header, pcl_pi.header); pcl_pi.indices.swap(pi.indices); @@ -328,28 +340,28 @@ namespace pcl_conversions { /** pcl::ModelCoefficients <=> pcl_msgs::ModelCoefficients **/ inline - void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::msg::ModelCoefficients &mc) + void fromPCL(const pcl::ModelCoefficients & pcl_mc, pcl_msgs::msg::ModelCoefficients & mc) { fromPCL(pcl_mc.header, mc.header); mc.values = pcl_mc.values; } inline - void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::msg::ModelCoefficients &mc) + void moveFromPCL(pcl::ModelCoefficients & pcl_mc, pcl_msgs::msg::ModelCoefficients & mc) { fromPCL(pcl_mc.header, mc.header); mc.values.swap(pcl_mc.values); } inline - void toPCL(const pcl_msgs::msg::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc) + void toPCL(const pcl_msgs::msg::ModelCoefficients & mc, pcl::ModelCoefficients & pcl_mc) { toPCL(mc.header, pcl_mc.header); pcl_mc.values = mc.values; } inline - void moveToPCL(pcl_msgs::msg::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc) + void moveToPCL(pcl_msgs::msg::ModelCoefficients & mc, pcl::ModelCoefficients & pcl_mc) { toPCL(mc.header, pcl_mc.header); pcl_mc.values.swap(mc.values); @@ -359,82 +371,90 @@ namespace pcl_conversions { namespace internal { - template - inline void move(std::vector &a, std::vector &b) + template < class T > + inline void move(std::vector < T > &a, std::vector < T > &b) { b.swap(a); } - template - inline void move(std::vector &a, std::vector &b) + template < class T1, class T2 > + inline void move(std::vector < T1 > &a, std::vector < T2 > &b) { b.assign(a.cbegin(), a.cend()); } } inline - void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert) + void fromPCL(const pcl::Vertices & pcl_vert, pcl_msgs::msg::Vertices & vert) { vert.vertices.assign(pcl_vert.vertices.cbegin(), pcl_vert.vertices.cend()); } inline - void fromPCL(const std::vector &pcl_verts, std::vector &verts) + void fromPCL( + const std::vector < pcl::Vertices > &pcl_verts, + std::vector < pcl_msgs::msg::Vertices > &verts) { verts.resize(pcl_verts.size()); - std::vector::const_iterator it = pcl_verts.begin(); - std::vector::iterator jt = verts.begin(); + std::vector < pcl::Vertices > ::const_iterator it = pcl_verts.begin(); + std::vector < pcl_msgs::msg::Vertices > ::iterator jt = verts.begin(); for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) { fromPCL(*(it), *(jt)); } } inline - void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert) + void moveFromPCL(pcl::Vertices & pcl_vert, pcl_msgs::msg::Vertices & vert) { internal::move(pcl_vert.vertices, vert.vertices); } inline - void fromPCL(std::vector &pcl_verts, std::vector &verts) + void fromPCL( + std::vector < pcl::Vertices > &pcl_verts, + std::vector < pcl_msgs::msg::Vertices > &verts) { verts.resize(pcl_verts.size()); - std::vector::iterator it = pcl_verts.begin(); - std::vector::iterator jt = verts.begin(); + std::vector < pcl::Vertices > ::iterator it = pcl_verts.begin(); + std::vector < pcl_msgs::msg::Vertices > ::iterator jt = verts.begin(); for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) { moveFromPCL(*(it), *(jt)); } } inline - void toPCL(const pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert) + void toPCL(const pcl_msgs::msg::Vertices & vert, pcl::Vertices & pcl_vert) { pcl_vert.vertices.assign(vert.vertices.cbegin(), vert.vertices.cend()); } inline - void toPCL(const std::vector &verts, std::vector &pcl_verts) + void toPCL( + const std::vector < pcl_msgs::msg::Vertices > &verts, + std::vector < pcl::Vertices > &pcl_verts) { pcl_verts.resize(verts.size()); - std::vector::const_iterator it = verts.begin(); - std::vector::iterator jt = pcl_verts.begin(); + std::vector < pcl_msgs::msg::Vertices > ::const_iterator it = verts.begin(); + std::vector < pcl::Vertices > ::iterator jt = pcl_verts.begin(); for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) { toPCL(*(it), *(jt)); } } inline - void moveToPCL(pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert) + void moveToPCL(pcl_msgs::msg::Vertices & vert, pcl::Vertices & pcl_vert) { internal::move(vert.vertices, pcl_vert.vertices); } inline - void moveToPCL(std::vector &verts, std::vector &pcl_verts) + void moveToPCL( + std::vector < pcl_msgs::msg::Vertices > &verts, + std::vector < pcl::Vertices > &pcl_verts) { pcl_verts.resize(verts.size()); - std::vector::iterator it = verts.begin(); - std::vector::iterator jt = pcl_verts.begin(); + std::vector < pcl_msgs::msg::Vertices > ::iterator it = verts.begin(); + std::vector < pcl::Vertices > ::iterator jt = pcl_verts.begin(); for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) { moveToPCL(*(it), *(jt)); } @@ -443,7 +463,7 @@ namespace pcl_conversions { /** pcl::PolygonMesh <=> pcl_msgs::PolygonMesh **/ inline - void fromPCL(const pcl::PolygonMesh &pcl_mesh, pcl_msgs::msg::PolygonMesh &mesh) + void fromPCL(const pcl::PolygonMesh & pcl_mesh, pcl_msgs::msg::PolygonMesh & mesh) { fromPCL(pcl_mesh.header, mesh.header); fromPCL(pcl_mesh.cloud, mesh.cloud); @@ -451,14 +471,14 @@ namespace pcl_conversions { } inline - void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::msg::PolygonMesh &mesh) + void moveFromPCL(pcl::PolygonMesh & pcl_mesh, pcl_msgs::msg::PolygonMesh & mesh) { fromPCL(pcl_mesh.header, mesh.header); moveFromPCL(pcl_mesh.cloud, mesh.cloud); } inline - void toPCL(const pcl_msgs::msg::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh) + void toPCL(const pcl_msgs::msg::PolygonMesh & mesh, pcl::PolygonMesh & pcl_mesh) { toPCL(mesh.header, pcl_mesh.header); toPCL(mesh.cloud, pcl_mesh.cloud); @@ -466,7 +486,7 @@ namespace pcl_conversions { } inline - void moveToPCL(pcl_msgs::msg::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh) + void moveToPCL(pcl_msgs::msg::PolygonMesh & mesh, pcl::PolygonMesh & pcl_mesh) { toPCL(mesh.header, pcl_mesh.header); moveToPCL(mesh.cloud, pcl_mesh.cloud); @@ -479,33 +499,35 @@ namespace pcl { /** Overload pcl::getFieldIndex **/ - inline int getFieldIndex(const sensor_msgs::msg::PointCloud2 &cloud, const std::string &field_name) + inline int getFieldIndex( + const sensor_msgs::msg::PointCloud2 & cloud, + const std::string & field_name) { // Get the index we need for (size_t d = 0; d < cloud.fields.size(); ++d) { if (cloud.fields[d].name == field_name) { - return (static_cast(d)); + return static_cast < int > (d); } } - return (-1); + return -1; } /** Overload pcl::getFieldsList **/ - inline std::string getFieldsList(const sensor_msgs::msg::PointCloud2 &cloud) + inline std::string getFieldsList(const sensor_msgs::msg::PointCloud2 & cloud) { std::string result; for (size_t i = 0; i < cloud.fields.size () - 1; ++i) { result += cloud.fields[i].name + " "; } result += cloud.fields[cloud.fields.size () - 1].name; - return (result); + return result; } /** Provide pcl::toROSMsg **/ inline - void toROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::Image &image) + void toROSMsg(const sensor_msgs::msg::PointCloud2 & cloud, sensor_msgs::msg::Image & image) { pcl::PCLPointCloud2 pcl_cloud; pcl_conversions::toPCL(cloud, pcl_cloud); @@ -515,7 +537,7 @@ namespace pcl { } inline - void moveToROSMsg(sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::Image &image) + void moveToROSMsg(sensor_msgs::msg::PointCloud2 & cloud, sensor_msgs::msg::Image & image) { pcl::PCLPointCloud2 pcl_cloud; pcl_conversions::moveToPCL(cloud, pcl_cloud); @@ -524,30 +546,26 @@ namespace pcl { pcl_conversions::moveFromPCL(pcl_image, image); } - template void - toROSMsg (const pcl::PointCloud &cloud, sensor_msgs::msg::Image& msg) + template < typename T > void + toROSMsg(const pcl::PointCloud < T > &cloud, sensor_msgs::msg::Image & msg) { // Ease the user's burden on specifying width/height for unorganized datasets - if (cloud.width == 0 && cloud.height == 0) - { + 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) + } 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; } // sensor_msgs::image_encodings::BGR8; msg.encoding = "bgr8"; - msg.step = msg.width * sizeof (std::uint8_t) * 3; + 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++) - { + 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)); } @@ -556,22 +574,27 @@ namespace pcl { /** Provide to/fromROSMsg for sensor_msgs::msg::PointCloud2 <=> pcl::PointCloud **/ - template - void toROSMsg(const pcl::PointCloud &pcl_cloud, sensor_msgs::msg::PointCloud2 &cloud) + template < typename T > + void toROSMsg(const pcl::PointCloud < T > &pcl_cloud, sensor_msgs::msg::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 pcl_conversions::moveFromPCL(pcl_pc2, cloud); } - template - void fromROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + template < typename T > + void fromROSMsg(const sensor_msgs::msg::PointCloud2 & cloud, pcl::PointCloud < T > &pcl_cloud) { pcl::PCLPointCloud2 pcl_pc2; #if PCL_VERSION_COMPARE(>=, 1, 13, 1) pcl_conversions::copyPointCloud2MetaData(cloud, 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::createMapping < T > (pcl_pc2.fields, field_map); pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud, field_map, &cloud.data[0]); #else pcl_conversions::toPCL(cloud, pcl_pc2); @@ -579,8 +602,8 @@ namespace pcl { #endif } - template - void moveFromROSMsg(sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + template < typename T > + void moveFromROSMsg(sensor_msgs::msg::PointCloud2 & cloud, pcl::PointCloud < T > &pcl_cloud) { pcl::PCLPointCloud2 pcl_pc2; pcl_conversions::moveToPCL(cloud, pcl_pc2); @@ -589,12 +612,14 @@ namespace pcl { /** Overload pcl::createMapping **/ - template - void createMapping(const std::vector& msg_fields, MsgFieldMap& field_map) + template < typename PointT > + void createMapping( + const std::vector < sensor_msgs::msg::PointField > &msg_fields, + MsgFieldMap & field_map) { - std::vector pcl_msg_fields; + std::vector < pcl::PCLPointField > pcl_msg_fields; pcl_conversions::toPCL(msg_fields, pcl_msg_fields); - return createMapping(pcl_msg_fields, field_map); + return createMapping < PointT > (pcl_msg_fields, field_map); } namespace io { @@ -602,10 +627,11 @@ namespace pcl { /** Overload pcl::io::savePCDFile **/ inline int - savePCDFile(const std::string &file_name, const sensor_msgs::msg::PointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), - const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), - const bool binary_mode = false) + savePCDFile( + const std::string & file_name, const sensor_msgs::msg::PointCloud2 & cloud, + const Eigen::Vector4f & origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf & orientation = Eigen::Quaternionf::Identity (), + const bool binary_mode = false) { pcl::PCLPointCloud2 pcl_cloud; pcl_conversions::toPCL(cloud, pcl_cloud); @@ -613,10 +639,11 @@ namespace pcl { } inline int - destructiveSavePCDFile(const std::string &file_name, sensor_msgs::msg::PointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), - const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), - const bool binary_mode = false) + destructiveSavePCDFile( + const std::string & file_name, sensor_msgs::msg::PointCloud2 & cloud, + const Eigen::Vector4f & origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf & orientation = Eigen::Quaternionf::Identity (), + const bool binary_mode = false) { pcl::PCLPointCloud2 pcl_cloud; pcl_conversions::moveToPCL(cloud, pcl_cloud); @@ -625,7 +652,7 @@ namespace pcl { /** Overload pcl::io::loadPCDFile **/ - inline int loadPCDFile(const std::string &file_name, sensor_msgs::msg::PointCloud2 &cloud) + inline int loadPCDFile(const std::string & file_name, sensor_msgs::msg::PointCloud2 & cloud) { pcl::PCLPointCloud2 pcl_cloud; int ret = pcl::io::loadPCDFile(file_name, pcl_cloud); @@ -638,116 +665,119 @@ namespace pcl { /** Overload asdf **/ inline - bool concatenatePointCloud (const sensor_msgs::msg::PointCloud2 &cloud1, - const sensor_msgs::msg::PointCloud2 &cloud2, - sensor_msgs::msg::PointCloud2 &cloud_out) + bool concatenatePointCloud( + const sensor_msgs::msg::PointCloud2 & cloud1, + const sensor_msgs::msg::PointCloud2 & cloud2, + sensor_msgs::msg::PointCloud2 & cloud_out) { //if one input cloud has no points, but the other input does, just return the cloud with points - if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0) - { + if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0) { cloud_out = cloud2; - return (true); - } - else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0) - { + return true; + } else if (cloud1.width * cloud1.height > 0 && cloud2.width * cloud2.height == 0) { cloud_out = cloud1; - return (true); + return true; } bool strip = false; - for (size_t i = 0; i < cloud1.fields.size (); ++i) - if (cloud1.fields[i].name == "_") + for (size_t i = 0; i < cloud1.fields.size (); ++i) { + if (cloud1.fields[i].name == "_") { strip = true; + } + } - for (size_t i = 0; i < cloud2.fields.size (); ++i) - if (cloud2.fields[i].name == "_") + for (size_t i = 0; i < cloud2.fields.size (); ++i) { + if (cloud2.fields[i].name == "_") { strip = true; + } + } - if (!strip && cloud1.fields.size () != cloud2.fields.size ()) - { - PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ()); - return (false); + if (!strip && cloud1.fields.size () != cloud2.fields.size ()) { + PCL_ERROR ( + "[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", + cloud1.fields.size (), cloud2.fields.size ()); + return false; } // Copy cloud1 into cloud_out cloud_out = cloud1; size_t nrpts = cloud_out.data.size (); // Height = 1 => no more organized - cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height; - cloud_out.height = 1; - if (!cloud1.is_dense || !cloud2.is_dense) + cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height; + cloud_out.height = 1; + cloud_out.row_step = cloud_out.width * cloud_out.point_step; + if (!cloud1.is_dense || !cloud2.is_dense) { cloud_out.is_dense = false; - else + } else { cloud_out.is_dense = true; + } // We need to strip the extra padding fields - if (strip) - { + if (strip) { // Get the field sizes for the second cloud - std::vector fields2; - std::vector fields2_sizes; - for (size_t j = 0; j < cloud2.fields.size (); ++j) - { - if (cloud2.fields[j].name == "_") + std::vector < sensor_msgs::msg::PointField > fields2; + std::vector < size_t > fields2_sizes; + for (size_t j = 0; j < cloud2.fields.size (); ++j) { + if (cloud2.fields[j].name == "_") { continue; + } fields2_sizes.push_back( cloud2.fields[j].count * - static_cast(pcl::getFieldSize(cloud2.fields[j].datatype))); + static_cast < size_t > (pcl::getFieldSize(cloud2.fields[j].datatype))); fields2.push_back(cloud2.fields[j]); } cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step); // Copy the second cloud - for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp) - { + for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp) { size_t i = 0; - for (size_t j = 0; j < fields2.size (); ++j) - { - if (cloud1.fields[i].name == "_") - { + for (size_t j = 0; j < fields2.size (); ++j) { + if (cloud1.fields[i].name == "_") { ++i; continue; } // We're fine with the special RGB vs RGBA use case if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") || - (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") || - (cloud1.fields[i].name == fields2[j].name)) + (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") || + (cloud1.fields[i].name == fields2[j].name)) { - memcpy (reinterpret_cast (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]), - reinterpret_cast (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]), + memcpy (reinterpret_cast < char * > + (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]), + reinterpret_cast < const char * > + (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]), fields2_sizes[j]); ++i; // increment the field size i } } } - } - else - { - for (size_t i = 0; i < cloud1.fields.size (); ++i) - { + } else { + for (size_t i = 0; i < cloud1.fields.size (); ++i) { // We're fine with the special RGB vs RGBA use case if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") || - (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb")) + (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb")) + { continue; + } // Otherwise we need to make sure the names are the same - if (cloud1.fields[i].name != cloud2.fields[i].name) - { - PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ()); - return (false); + if (cloud1.fields[i].name != cloud2.fields[i].name) { + PCL_ERROR ( + "[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", + i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ()); + return false; } } cloud_out.data.resize (nrpts + cloud2.data.size ()); memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ()); } - return (true); + return true; } } // namespace pcl -/* TODO when ROS2 type masquerading is implemented */ +/* TODO when ROS2 type masquerading is implemented */ /** namespace ros { @@ -760,7 +790,7 @@ namespace ros return msg; } }; - + namespace message_traits { template<> diff --git a/pcl_conversions/test/test_pcl_conversions.cpp b/pcl_conversions/test/test_pcl_conversions.cpp index 83a3142ef..85f44feec 100644 --- a/pcl_conversions/test/test_pcl_conversions.cpp +++ b/pcl_conversions/test/test_pcl_conversions.cpp @@ -4,11 +4,13 @@ #include "pcl_conversions/pcl_conversions.h" -namespace { +namespace +{ class PCLConversionTests : public ::testing::Test { protected: - virtual void SetUp() { + virtual void SetUp() + { pcl_image.header.stamp = 3141592653; pcl_image.header.frame_id = "pcl"; pcl_image.height = 1; @@ -29,14 +31,14 @@ class PCLConversionTests : public ::testing::Test { pcl_pc2.is_bigendian = true; pcl_pc2.is_dense = true; pcl_pc2.fields.resize(2); - pcl_pc2.fields[0].name = "XYZ"; - pcl_pc2.fields[0].datatype = pcl::PCLPointField::INT8; - pcl_pc2.fields[0].count = 3; - pcl_pc2.fields[0].offset = 0; - pcl_pc2.fields[1].name = "RGB"; + pcl_pc2.fields[1].name = "XYZ"; pcl_pc2.fields[1].datatype = pcl::PCLPointField::INT8; pcl_pc2.fields[1].count = 3; - pcl_pc2.fields[1].offset = 8 * 3; + pcl_pc2.fields[1].offset = 0; + pcl_pc2.fields[0].name = "RGB"; + pcl_pc2.fields[0].datatype = pcl::PCLPointField::INT8; + pcl_pc2.fields[0].count = 3; + pcl_pc2.fields[0].offset = 8 * 3; pcl_pc2.data.resize(2); pcl_pc2.data[0] = 0x42; pcl_pc2.data[1] = 0x43; @@ -50,7 +52,8 @@ class PCLConversionTests : public ::testing::Test { }; template -void test_image(T &image) { +void test_image(T & image) +{ EXPECT_EQ(std::string("pcl"), image.header.frame_id); EXPECT_EQ(1U, image.height); EXPECT_EQ(2U, image.width); @@ -72,7 +75,8 @@ TEST_F(PCLConversionTests, imageConversion) { } template -void test_pc(T &pc) { +void test_pc(T & pc) +{ EXPECT_EQ(std::string("pcl"), pc.header.frame_id); EXPECT_EQ(1U, pc.height); EXPECT_EQ(2U, pc.width); @@ -110,8 +114,8 @@ struct StampTestData const rclcpp::Time stamp_; rclcpp::Time stamp2_; - explicit StampTestData(const rclcpp::Time &stamp) - : stamp_(stamp) + explicit StampTestData(const rclcpp::Time & stamp) + : stamp_(stamp) { std::uint64_t pcl_stamp; pcl_conversions::toPCL(stamp_, pcl_stamp); @@ -123,35 +127,36 @@ TEST(PCLConversionStamp, Stamps) { { const StampTestData d(rclcpp::Time(1, 1000)); - EXPECT_TRUE(d.stamp_==d.stamp2_); + EXPECT_TRUE(d.stamp_ == d.stamp2_); } { const StampTestData d(rclcpp::Time(1, 999999000)); - EXPECT_TRUE(d.stamp_==d.stamp2_); + EXPECT_TRUE(d.stamp_ == d.stamp2_); } { const StampTestData d(rclcpp::Time(1, 999000000)); - EXPECT_TRUE(d.stamp_==d.stamp2_); + EXPECT_TRUE(d.stamp_ == d.stamp2_); } { const StampTestData d(rclcpp::Time(1423680574, 746000000)); - EXPECT_TRUE(d.stamp_==d.stamp2_); + EXPECT_TRUE(d.stamp_ == d.stamp2_); } { const StampTestData d(rclcpp::Time(1423680629, 901000000)); - EXPECT_TRUE(d.stamp_==d.stamp2_); + EXPECT_TRUE(d.stamp_ == d.stamp2_); } } -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ try { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); - } catch (std::exception &e) { + } catch (std::exception & e) { std::cerr << "Unhandled Exception: " << e.what() << std::endl; } return 1; diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 258f23666..03a4070aa 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -7,10 +7,11 @@ endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -fPIC) endif() +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) ## Find system dependencies find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED QUIET COMPONENTS core common features filters io segmentation surface) +find_package(PCL REQUIRED QUIET COMPONENTS common features filters io segmentation surface) ## Find ROS package dependencies find_package(ament_cmake REQUIRED) @@ -20,20 +21,11 @@ find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) - -set(dependencies - pcl_conversions - rclcpp - sensor_msgs - geometry_msgs - tf2 - tf2_geometry_msgs - tf2_ros - EIGEN3 - PCL -) +find_package(visualization_msgs REQUIRED) +find_package(rosbag2_transport REQUIRED) ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) @@ -41,8 +33,19 @@ target_include_directories(pcl_ros_tf PUBLIC $ $ ) -ament_target_dependencies(pcl_ros_tf - ${dependencies} +target_include_directories(pcl_ros_tf SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) +target_link_libraries(pcl_ros_tf PUBLIC + ${geometry_msgs_TARGETS} + ${PCL_LIBRARIES} + ${sensor_msgs_TARGETS} + Eigen3::Eigen + pcl_conversions::pcl_conversions + rclcpp::rclcpp + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros ) ### Nodelets @@ -79,20 +82,59 @@ ament_target_dependencies(pcl_ros_tf #class_loader_hide_library_symbols(pcl_ros_features) # # + +### Declare library for base filter plugin +add_library(pcl_ros_filter + src/pcl_ros/filters/filter.cpp +) +target_link_libraries(pcl_ros_filter pcl_ros_tf ${PCL_LIBRARIES}) + ### Declare the pcl_ros_filters library -#add_library(pcl_ros_filters -# src/pcl_ros/filters/extract_indices.cpp -# src/pcl_ros/filters/filter.cpp -# src/pcl_ros/filters/passthrough.cpp -# src/pcl_ros/filters/project_inliers.cpp -# src/pcl_ros/filters/radius_outlier_removal.cpp -# src/pcl_ros/filters/statistical_outlier_removal.cpp -# src/pcl_ros/filters/voxel_grid.cpp -# src/pcl_ros/filters/crop_box.cpp -#) -#target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -#add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) -#class_loader_hide_library_symbols(pcl_ros_filters) +add_library(pcl_ros_filters SHARED + src/pcl_ros/filters/extract_indices.cpp + src/pcl_ros/filters/passthrough.cpp + src/pcl_ros/filters/project_inliers.cpp + src/pcl_ros/filters/radius_outlier_removal.cpp + src/pcl_ros/filters/statistical_outlier_removal.cpp + src/pcl_ros/filters/voxel_grid.cpp + src/pcl_ros/filters/crop_box.cpp +) +target_link_libraries(pcl_ros_filters PUBLIC + ${PCL_LIBRARIES} + ${visualization_msgs_TARGETS} + pcl_ros_filter + pcl_ros_tf + rclcpp_components::component +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::ExtractIndices" + EXECUTABLE filter_extract_indices_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::PassThrough" + EXECUTABLE filter_passthrough_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::ProjectInliers" + EXECUTABLE filter_project_inliers_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::RadiusOutlierRemoval" + EXECUTABLE filter_radius_outlier_removal_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::StatisticalOutlierRemoval" + EXECUTABLE filter_statistical_outlier_removal_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::CropBox" + EXECUTABLE filter_crop_box_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::VoxelGrid" + EXECUTABLE filter_voxel_grid_node +) +class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library #add_library (pcl_ros_segmentation @@ -120,18 +162,81 @@ ament_target_dependencies(pcl_ros_tf ### Tools # add_library(pcd_to_pointcloud_lib SHARED tools/pcd_to_pointcloud.cpp) -target_link_libraries(pcd_to_pointcloud_lib - ${PCL_LIBRARIES}) -target_include_directories(pcd_to_pointcloud_lib PUBLIC - ${PCL_INCLUDE_DIRS}) -ament_target_dependencies(pcd_to_pointcloud_lib - rclcpp - rclcpp_components - sensor_msgs - pcl_conversions) +target_include_directories(pcd_to_pointcloud_lib SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) +target_link_libraries(pcd_to_pointcloud_lib PUBLIC + ${PCL_LIBRARIES} + ${sensor_msgs_TARGETS} + pcl_conversions::pcl_conversions + rclcpp::rclcpp + rclcpp_components::component +) rclcpp_components_register_node(pcd_to_pointcloud_lib PLUGIN "pcl_ros::PCDPublisher" EXECUTABLE pcd_to_pointcloud) + +add_library(pointcloud_to_pcd_lib SHARED tools/pointcloud_to_pcd.cpp) +target_include_directories(pointcloud_to_pcd_lib SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) +target_link_libraries(pointcloud_to_pcd_lib PUBLIC + ${PCL_LIBRARIES} + ${sensor_msgs_TARGETS} + pcl_conversions::pcl_conversions + pcl_ros_tf + rclcpp::rclcpp + rclcpp_components::component + tf2_eigen::tf2_eigen + tf2_ros::tf2_ros +) + +rclcpp_components_register_node(pointcloud_to_pcd_lib + PLUGIN "pcl_ros::PointCloudToPCD" + EXECUTABLE pointcloud_to_pcd) + + +# ===================================================== +# CombinedPointCloudToPCD Component +# ===================================================== +add_library(combined_pointcloud_to_pcd_lib SHARED + tools/combined_pointcloud_to_pcd.cpp +) + +# Include directories and link libraries +target_include_directories(combined_pointcloud_to_pcd_lib SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) +target_link_libraries(combined_pointcloud_to_pcd_lib PUBLIC + ${PCL_LIBRARIES} + ${sensor_msgs_TARGETS} + pcl_conversions::pcl_conversions + rclcpp::rclcpp + rclcpp_components::component + tf2_eigen::tf2_eigen + tf2_ros::tf2_ros +) + +add_library(bag_to_pcd_lib SHARED tools/bag_to_pcd.cpp) +target_include_directories(bag_to_pcd_lib PRIVATE + ${PCL_INCLUDE_DIRS} +) +target_link_libraries(bag_to_pcd_lib PRIVATE + ${PCL_LIBRARIES} + ${sensor_msgs_TARGETS} + pcl_conversions::pcl_conversions + rclcpp::rclcpp + rclcpp_components::component + rosbag2_transport::rosbag2_transport +) +rclcpp_components_register_node(bag_to_pcd_lib + PLUGIN "pcl_ros::BagToPCD" + EXECUTABLE bag_to_pcd) + +# Register the component +rclcpp_components_register_node(combined_pointcloud_to_pcd_lib + PLUGIN "pcl_ros::CombinedPointCloudToPCD" + EXECUTABLE combined_pointcloud_to_pcd) # #add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) #target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) @@ -163,6 +268,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests/filters) #add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp) #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) #add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false) @@ -184,11 +290,13 @@ install( pcd_to_pointcloud_lib # pcl_ros_io # pcl_ros_features -# pcl_ros_filters + pcl_ros_filter + pcl_ros_filters # pcl_ros_surface # pcl_ros_segmentation -# pointcloud_to_pcd -# bag_to_pcd + pointcloud_to_pcd_lib + combined_pointcloud_to_pcd_lib + bag_to_pcd_lib # convert_pcd_to_image # convert_pointcloud_to_image EXPORT export_pcl_ros @@ -207,6 +315,19 @@ ament_export_libraries(pcl_ros_tf) # Export modern CMake targets ament_export_targets(export_pcl_ros HAS_LIBRARY_TARGET) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + Eigen3 + geometry_msgs + PCL + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs +) ament_package() diff --git a/pcl_ros/cfg/SACSegmentation_common.py b/pcl_ros/cfg/SACSegmentation_common.py index 9bf8da1c4..0bef192b7 100644 --- a/pcl_ros/cfg/SACSegmentation_common.py +++ b/pcl_ros/cfg/SACSegmentation_common.py @@ -1,8 +1,8 @@ #! /usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import int_t -from dynamic_reconfigure.parameter_generator_catkin import double_t from dynamic_reconfigure.parameter_generator_catkin import bool_t +from dynamic_reconfigure.parameter_generator_catkin import double_t +from dynamic_reconfigure.parameter_generator_catkin import int_t from dynamic_reconfigure.parameter_generator_catkin import str_t # set up parameters that we care about @@ -11,37 +11,37 @@ def add_common_parameters(gen): # add(self, name, paramtype, level, description, default = None, min = None, - # max = None, edit_method = "") - gen.add("max_iterations", int_t, 0, - "The maximum number of iterations the algorithm will run for", + # max = None, edit_method = '') + gen.add('max_iterations', int_t, 0, + 'The maximum number of iterations the algorithm will run for', 50, 0, 100000) - gen.add("probability", double_t, 0, - "The desired probability of choosing at least one sample free from outliers", + gen.add('probability', double_t, 0, + 'The desired probability of choosing at least one sample free from outliers', 0.99, 0.5, 0.99) - gen.add("distance_threshold", double_t, 0, - "The distance to model threshold", + gen.add('distance_threshold', double_t, 0, + 'The distance to model threshold', 0.02, 0, 1.0) - gen.add("optimize_coefficients", bool_t, 0, - "Model coefficient refinement", + gen.add('optimize_coefficients', bool_t, 0, + 'Model coefficient refinement', True) - gen.add("radius_min", double_t, 0, - "The minimum allowed model radius (where applicable)", + gen.add('radius_min', double_t, 0, + 'The minimum allowed model radius (where applicable)', 0.0, 0, 1.0) - gen.add("radius_max", double_t, 0, - "The maximum allowed model radius (where applicable)", + gen.add('radius_max', double_t, 0, + 'The maximum allowed model radius (where applicable)', 0.05, 0, 1.0) - gen.add("eps_angle", double_t, 0, - ("The maximum allowed difference between the model normal " - "and the given axis in radians."), + gen.add('eps_angle', double_t, 0, + ('The maximum allowed difference between the model normal ' + 'and the given axis in radians.'), 0.17, 0.0, 1.5707) - gen.add("min_inliers", int_t, 0, - "The minimum number of inliers a model must have in order to be considered valid.", + gen.add('min_inliers', int_t, 0, + 'The minimum number of inliers a model must have in order to be considered valid.', 0, 0, 100000) - gen.add("input_frame", str_t, 0, - ("The input TF frame the data should be transformed into, " - "if input.header.frame_id is different."), - "") - gen.add("output_frame", str_t, 0, - ("The output TF frame the data should be transformed into, " - "if input.header.frame_id is different."), - "") + gen.add('input_frame', str_t, 0, + ('The input TF frame the data should be transformed into, ' + 'if input.header.frame_id is different.'), + '') + gen.add('output_frame', str_t, 0, + ('The output TF frame the data should be transformed into, ' + 'if input.header.frame_id is different.'), + '') diff --git a/pcl_ros/cfg/common.py b/pcl_ros/cfg/common.py index 43c49554d..27e471077 100644 --- a/pcl_ros/cfg/common.py +++ b/pcl_ros/cfg/common.py @@ -1,8 +1,8 @@ #! /usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import str_t -from dynamic_reconfigure.parameter_generator_catkin import double_t from dynamic_reconfigure.parameter_generator_catkin import bool_t +from dynamic_reconfigure.parameter_generator_catkin import double_t +from dynamic_reconfigure.parameter_generator_catkin import str_t # set up parameters that we care about PACKAGE = 'pcl_ros' @@ -10,27 +10,27 @@ def add_common_parameters(gen): # def add (self, name, paramtype, level, description, default = None, min = None, - # max = None, edit_method = ""): - gen.add("filter_field_name", str_t, 0, "The field name used for filtering", "z") - gen.add("filter_limit_min", double_t, 0, - "The minimum allowed field value a point will be considered from", + # max = None, edit_method = ''): + gen.add('filter_field_name', str_t, 0, 'The field name used for filtering', 'z') + gen.add('filter_limit_min', double_t, 0, + 'The minimum allowed field value a point will be considered from', 0.0, -100000.0, 100000.0) - gen.add("filter_limit_max", double_t, 0, - "The maximum allowed field value a point will be considered from", + gen.add('filter_limit_max', double_t, 0, + 'The maximum allowed field value a point will be considered from', 1.0, -100000.0, 100000.0) - gen.add("filter_limit_negative", bool_t, 0, - ("Set to true if we want to return the data outside " - "[filter_limit_min; filter_limit_max]."), + gen.add('filter_limit_negative', bool_t, 0, + ('Set to true if we want to return the data outside ' + '[filter_limit_min; filter_limit_max].'), False) - gen.add("keep_organized", bool_t, 0, - ("Set whether the filtered points should be kept and set to NaN, " - "or removed from the PointCloud, thus potentially breaking its organized structure."), + gen.add('keep_organized', bool_t, 0, + ('Set whether the filtered points should be kept and set to NaN, ' + 'or removed from the PointCloud, thus potentially breaking its organized structure.'), False) - gen.add("input_frame", str_t, 0, - ("The input TF frame the data should be transformed into before processing, " - "if input.header.frame_id is different."), - "") - gen.add("output_frame", str_t, 0, - ("The output TF frame the data should be transformed into after processing, " - "if input.header.frame_id is different."), - "") + gen.add('input_frame', str_t, 0, + ('The input TF frame the data should be transformed into before processing, ' + 'if input.header.frame_id is different.'), + '') + gen.add('output_frame', str_t, 0, + ('The output TF frame the data should be transformed into after processing, ' + 'if input.header.frame_id is different.'), + '') diff --git a/pcl_ros/include/pcl_ros/features/feature.hpp b/pcl_ros/include/pcl_ros/features/feature.hpp index 8497330ad..cf4272d79 100644 --- a/pcl_ros/include/pcl_ros/features/feature.hpp +++ b/pcl_ros/include/pcl_ros/features/feature.hpp @@ -42,7 +42,6 @@ #include #include -#include // Dynamic reconfigure #include @@ -50,6 +49,7 @@ // PCL conversions #include +#include #include "pcl_ros/pcl_nodelet.hpp" #include "pcl_ros/FeatureConfig.hpp" @@ -117,7 +117,7 @@ class Feature : public PCLNodelet bool use_surface_; /** \brief Parameter for the spatial locator tree. By convention, the values represent: - * 0: ANN (Approximate Nearest Neigbor library) kd-tree + * 0: ANN (Approximate Nearest Neighbor library) kd-tree * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree * 2: Organized spatial dataset index */ diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/pcl_ros/include/pcl_ros/filters/crop_box.hpp index 3665521a6..59fb74f31 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.hpp +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -41,11 +41,10 @@ // PCL includes #include +#include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/CropBoxConfig.hpp" - namespace pcl_ros { /** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box. @@ -57,9 +56,6 @@ namespace pcl_ros class CropBox : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; // TODO(xxx) - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -67,32 +63,29 @@ class CropBox : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); + /** \brief Create publishers for output PointCloud2 data as well as the crop box marker. */ + void createPublishers() override; + /** \brief Update the crop box marker msg. */ + void update_marker_msg(); - /** \brief Dynamic reconfigure service callback. - * \param config the dynamic reconfigure CropBoxConfig object - * \param level the dynamic reconfigure level - */ - void - config_callback(pcl_ros::CropBoxConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + /** \brief The crop box marker msg publisher for visualization and debugging purposes. */ + rclcpp::Publisher::SharedPtr crop_box_marker_publisher_; + /** + * \brief The crop box marker message. + * + * The marker's cube gets updated whenever the min/max points are changed. + * The header is adjusted with every point cloud callback. + */ + visualization_msgs::msg::Marker crop_box_marker_msg_; private: /** \brief The PCL filter implementation used. */ @@ -100,6 +93,8 @@ class CropBox : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit CropBox(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp index 95740f600..8f4bc55aa 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -40,9 +40,8 @@ // PCL includes #include - +#include #include "pcl_ros/filters/filter.hpp" -#include "pcl_ros/ExtractIndicesConfig.hpp" namespace pcl_ros { @@ -53,9 +52,6 @@ namespace pcl_ros class ExtractIndices : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -63,29 +59,16 @@ class ExtractIndices : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - virtual bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure service callback. */ - void - config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -93,6 +76,8 @@ class ExtractIndices : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit ExtractIndices(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index e589c0cb7..08994bb85 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -38,11 +38,10 @@ #ifndef PCL_ROS__FILTERS__FILTER_HPP_ #define PCL_ROS__FILTERS__FILTER_HPP_ -#include -#include +#include #include -#include "pcl_ros/pcl_nodelet.hpp" -#include "pcl_ros/FilterConfig.hpp" +#include +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { @@ -52,19 +51,30 @@ namespace sync_policies = message_filters::sync_policies; * applicable to all filters are defined here as static methods. * \author Radu Bogdan Rusu */ -class Filter : public PCLNodelet +class Filter : public PCLNode { public: - typedef sensor_msgs::PointCloud2 PointCloud2; + typedef sensor_msgs::msg::PointCloud2 PointCloud2; typedef pcl::IndicesPtr IndicesPtr; typedef pcl::IndicesConstPtr IndicesConstPtr; - Filter() {} + /** \brief Filter constructor + * \param node_name node name + * \param options node options + */ + Filter(std::string node_name, const rclcpp::NodeOptions & options); protected: + /** \brief declare and subscribe to param callback for input_frame and output_frame params */ + void + use_frame_params(); + + /** \brief Add common parameters */ + std::vector add_common_params(); + /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + rclcpp::Subscription::SharedPtr sub_input_; message_filters::Subscriber sub_input_filter_; @@ -96,18 +106,7 @@ class Filter : public PCLNodelet std::string tf_output_frame_; /** \brief Internal mutex. */ - boost::mutex mutex_; - - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service - */ - virtual bool - child_init(ros::NodeHandle & nh, bool & has_service) - { - has_service = false; - return true; - } + std::mutex mutex_; /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. @@ -116,7 +115,7 @@ class Filter : public PCLNodelet */ virtual void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, PointCloud2 & output) = 0; /** \brief Lazy transport subscribe routine. */ @@ -127,36 +126,38 @@ class Filter : public PCLNodelet virtual void unsubscribe(); - /** \brief Nodelet initialization routine. */ + /** \brief Create publishers for output PointCloud2 data. */ virtual void - onInit(); + createPublishers(); /** \brief Call the child filter () method, optionally transform the result, and publish it. * \param input the input point cloud dataset. * \param indices a pointer to the vector of point indices to use. */ void - computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices); + computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices); private: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + /** \brief Pointer to parameters callback handle. */ + OnSetParametersCallbackHandle::SharedPtr callback_handle_; /** \brief Synchronized input, and indices.*/ - boost::shared_ptr>> sync_input_indices_e_; - boost::shared_ptr>> sync_input_indices_a_; - /** \brief Dynamic reconfigure service callback. */ - virtual void - config_callback(pcl_ros::FilterConfig & config, uint32_t level); + /** \brief Parameter callback + * \param params parameter values to set + */ + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); /** \brief PointCloud2 + Indices data callback. */ void input_indices_callback( - const PointCloud2::ConstPtr & cloud, - const PointIndicesConstPtr & indices); + const PointCloud2::ConstSharedPtr & cloud, + const PointIndices::ConstSharedPtr & indices); public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index f85d38102..dd18bec84 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.hpp +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -40,6 +40,7 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" namespace pcl_ros @@ -51,9 +52,6 @@ namespace pcl_ros class PassThrough : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -61,32 +59,16 @@ class PassThrough : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure service callback. - * \param config the dynamic reconfigure FilterConfig object - * \param level the dynamic reconfigure level - */ - void - config_callback(pcl_ros::FilterConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -94,6 +76,8 @@ class PassThrough : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit PassThrough(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp index 3c25846e5..ca0a772b0 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -38,9 +38,11 @@ #ifndef PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ #define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ +// PCL includes #include -#include +#include #include "pcl_ros/filters/filter.hpp" +#include namespace pcl_ros @@ -55,8 +57,7 @@ namespace sync_policies = message_filters::sync_policies; class ProjectInliers : public Filter { public: - ProjectInliers() - : model_() {} + explicit ProjectInliers(const rclcpp::NodeOptions & options); protected: /** \brief Call the actual filter. @@ -66,20 +67,8 @@ class ProjectInliers : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); - pcl_conversions::toPCL(*(model_), *(pcl_model)); - impl_.setModelCoefficients(pcl_model); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; private: /** \brief A pointer to the vector of model coefficients. */ @@ -89,25 +78,20 @@ class ProjectInliers : public Filter message_filters::Subscriber sub_model_; /** \brief Synchronized input, indices, and model coefficients.*/ - boost::shared_ptr>> sync_input_indices_model_e_; - boost::shared_ptr>> sync_input_indices_model_a_; /** \brief The PCL filter implementation used. */ pcl::ProjectInliers impl_; - /** \brief Nodelet initialization routine. */ - virtual void - onInit(); - - /** \brief NodeletLazy connection routine. */ - void subscribe(); - void unsubscribe(); + void subscribe() override; + void unsubscribe() override; /** \brief PointCloud2 + Indices + Model data callback. */ void input_indices_model_callback( - const PointCloud2::ConstPtr & cloud, + const PointCloud2::ConstSharedPtr & cloud, const PointIndicesConstPtr & indices, const ModelCoefficientsConstPtr & model); diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp index 420f4df5a..5f7333df4 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/RadiusOutlierRemovalConfig.hpp" - namespace pcl_ros { /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain @@ -55,9 +53,6 @@ namespace pcl_ros class RadiusOutlierRemoval : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -65,29 +60,16 @@ class RadiusOutlierRemoval : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - virtual inline bool child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(pcl_ros::RadiusOutlierRemovalConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -95,6 +77,8 @@ class RadiusOutlierRemoval : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit RadiusOutlierRemoval(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp index 80f4ea64f..f8e14a2ba 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/StatisticalOutlierRemovalConfig.hpp" - namespace pcl_ros { /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more @@ -61,9 +59,6 @@ namespace pcl_ros class StatisticalOutlierRemoval : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -71,30 +66,16 @@ class StatisticalOutlierRemoval : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -102,6 +83,8 @@ class StatisticalOutlierRemoval : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit StatisticalOutlierRemoval(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index 54c7c4596..4b51113c7 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/VoxelGridConfig.hpp" - namespace pcl_ros { /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. @@ -53,38 +51,32 @@ namespace pcl_ros class VoxelGrid : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - - /** \brief The PCL filter implementation used. */ - pcl::VoxelGrid impl_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ - virtual void + inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output); + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void - config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::VoxelGrid impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit VoxelGrid(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index 31c0e1fe8..fefeb4d58 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -37,22 +37,27 @@ #ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_ #define PCL_ROS__IMPL__TRANSFORMS_HPP_ -#include "pcl_ros/transforms.hpp" #include #include -#include -#include -#include -#include -#include -#include #include #include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include #include #include #include -#include -#include + +#include "pcl_ros/transforms.hpp" using pcl_conversions::fromPCL; using pcl_conversions::toPCL; diff --git a/pcl_ros/include/pcl_ros/io/bag_io.hpp b/pcl_ros/include/pcl_ros/io/bag_io.hpp index 6f5add216..bad98c7dd 100644 --- a/pcl_ros/include/pcl_ros/io/bag_io.hpp +++ b/pcl_ros/include/pcl_ros/io/bag_io.hpp @@ -38,11 +38,11 @@ #ifndef PCL_ROS__IO__BAG_IO_HPP_ #define PCL_ROS__IO__BAG_IO_HPP_ -#include #include #include #include #include +#include namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.hpp b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp index 62aae16df..22b3c227e 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_data.hpp +++ b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp @@ -41,13 +41,13 @@ // ROS includes #include #include -#include -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp index 00b53160d..a90ec67e8 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp @@ -40,16 +40,17 @@ // ROS includes #include -#include -#include -#include -#include #include #include #include +#include +#include +#include +#include + namespace pcl_ros { /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of diff --git a/pcl_ros/include/pcl_ros/pcl_node.hpp b/pcl_ros/include/pcl_ros/pcl_node.hpp new file mode 100644 index 000000000..4567bc5c2 --- /dev/null +++ b/pcl_ros/include/pcl_ros/pcl_node.hpp @@ -0,0 +1,295 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, 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 Willow Garage, 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. + * + * $Id: pcl_node.h 33238 2010-03-11 00:46:58Z rusu $ + * + */ + +/** + +\author Radu Bogdan Rusu + +**/ + +#ifndef PCL_ROS__PCL_NODE_HPP_ +#define PCL_ROS__PCL_NODE_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +// #include "pcl_ros/point_cloud.hpp" + +using pcl_conversions::fromPCL; + +namespace pcl_ros +{ +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b PCLNode represents the base PCL Node class. All PCL node should inherit from + * this class. */ +template> +class PCLNode : public rclcpp::Node +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + typedef pcl_msgs::msg::PointIndices PointIndices; + typedef PointIndices::SharedPtr PointIndicesPtr; + typedef PointIndices::ConstSharedPtr PointIndicesConstPtr; + + typedef pcl_msgs::msg::ModelCoefficients ModelCoefficients; + typedef ModelCoefficients::SharedPtr ModelCoefficientsPtr; + typedef ModelCoefficients::ConstSharedPtr ModelCoefficientsConstPtr; + + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; + + /** \brief Empty constructor. */ + PCLNode(std::string node_name, const rclcpp::NodeOptions & options) + : rclcpp::Node(node_name, options), + use_indices_(false), transient_local_indices_(false), + max_queue_size_(3), approximate_sync_(false), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) + { + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "max_queue_size"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + desc.description = "QoS History depth"; + desc.read_only = true; + max_queue_size_ = declare_parameter(desc.name, max_queue_size_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "use_indices"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = "Only process a subset of the point cloud from an indices topic"; + desc.read_only = true; + use_indices_ = declare_parameter(desc.name, use_indices_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "transient_local_indices"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = "Does indices topic use transient local documentation"; + desc.read_only = true; + transient_local_indices_ = declare_parameter(desc.name, transient_local_indices_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "approximate_sync"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = + "Match indices and point cloud messages if time stamps are approximately the same."; + desc.read_only = true; + approximate_sync_ = declare_parameter(desc.name, approximate_sync_, desc); + } + + RCLCPP_DEBUG( + this->get_logger(), "PCL Node successfully created with the following parameters:\n" + " - approximate_sync : %s\n" + " - use_indices : %s\n" + " - transient_local_indices_ : %s\n" + " - max_queue_size : %d", + (approximate_sync_) ? "true" : "false", + (use_indices_) ? "true" : "false", + (transient_local_indices_) ? "true" : "false", + max_queue_size_); + } + +protected: + /** \brief Set to true if point indices are used. + * + * When receiving a point cloud, if use_indices_ is false, the entire + * point cloud is processed for the given operation. If use_indices_ is + * true, then the ~indices topic is read to get the vector of point + * indices specifying the subset of the point cloud that will be used for + * the operation. In the case where use_indices_ is true, the ~input and + * ~indices topics must be synchronised in time, either exact or within a + * specified jitter. See also @ref transient_local_indices_ and approximate_sync. + **/ + bool use_indices_; + /** \brief Set to true if the indices topic has transient_local durability. + * + * If use_indices_ is true, the ~input and ~indices topics generally must + * be synchronised in time. By setting this flag to true, the most recent + * value from ~indices can be used instead of requiring a synchronised + * message. + **/ + bool transient_local_indices_; + + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_input_filter_; + + /** \brief The message filter subscriber for PointIndices. */ + message_filters::Subscriber sub_indices_filter_; + + /** \brief The output PointCloud publisher. Allow each individual class that inherits from this + * to declare the Publisher with their type. + */ + std::shared_ptr pub_output_; + + /** \brief The maximum queue size (default: 3). */ + int max_queue_size_; + + /** \brief True if we use an approximate time synchronizer + * versus an exact one (false by default). + **/ + bool approximate_sync_; + + /** \brief TF listener object. */ + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). + * \param cloud the point cloud to test + * \param topic_name an optional topic name (only used for printing, defaults to "input") + */ + inline bool + isValid(const PointCloud2::ConstSharedPtr & cloud, const std::string & topic_name = "input") + { + if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) { + RCLCPP_WARN( + this->get_logger(), + "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) " + "with stamp %d.%09d, and frame %s on topic %s received!", + cloud->data.size(), cloud->width, cloud->height, cloud->point_step, + cloud->header.stamp.sec, cloud->header.stamp.nanosec, + cloud->header.frame_id.c_str(), topic_name.c_str()); + + return false; + } + return true; + } + + /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). + * \param cloud the point cloud to test + * \param topic_name an optional topic name (only used for printing, defaults to "input") + */ + inline bool + isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input") + { + if (cloud->width * cloud->height != cloud->points.size()) { + RCLCPP_WARN( + this->get_logger(), + "Invalid PointCloud (points = %zu, width = %d, height = %d) " + "with stamp %d.%09d, and frame %s on topic %s received!", + cloud->points.size(), cloud->width, cloud->height, + fromPCL(cloud->header).stamp.sec, fromPCL(cloud->header).stamp.nanosec, + cloud->header.frame_id.c_str(), topic_name.c_str()); + + return false; + } + return true; + } + + /** \brief Test whether a given PointIndices message is "valid" (i.e., has values). + * \param indices the point indices message to test + * \param topic_name an optional topic name (only used for printing, defaults to "indices") + */ + inline bool + isValid( + const PointIndicesConstPtr & /*indices*/, + const std::string & /*topic_name*/ = "indices") + { + /*if (indices->indices.empty ()) + { + RCLCPP_WARN( + this->get_logger(), "Empty indices (values = %zu) with stamp %d.%09d, " + "and frame %s on topic %s received!", + indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec, + indices->header.frame_id.c_str(), topic_name.c_str()); + //return (true); // looks like it should be false + return false; + }*/ + return true; + } + + /** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values). + * \param model the model coefficients to test + * \param topic_name an optional topic name (only used for printing, defaults to "model") + */ + inline bool + isValid( + const ModelCoefficientsConstPtr & /*model*/, + const std::string & /*topic_name*/ = "model") + { + /*if (model->values.empty ()) + { + RCLCPP_WARN( + this->get_logger(), "Empty model (values = %zu) with stamp %d.%09d, " + "and frame %s on topic %s received!", + model->values.size(), model->header.stamp.sec, model->header.stamp.nanosec, + model->header.frame_id.c_str(), topic_name.c_str()); + return (false); + }*/ + return true; + } + + /** \brief Lazy transport subscribe/unsubscribe routine. + * It is optional for backward compatibility. + **/ + virtual void subscribe() {} + virtual void unsubscribe() {} + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace pcl_ros + +#endif // PCL_ROS__PCL_NODE_HPP_ diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp deleted file mode 100644 index 4806338f7..000000000 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp +++ /dev/null @@ -1,260 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010, 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 Willow Garage, 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. - * - * $Id: pcl_nodelet.h 33238 2010-03-11 00:46:58Z rusu $ - * - */ - -/** - -\author Radu Bogdan Rusu - -**/ - -#ifndef PCL_ROS__PCL_NODELET_HPP_ -#define PCL_ROS__PCL_NODELET_HPP_ - -#include -// PCL includes -#include -#include -#include -#include -#include -// ROS Nodelet includes -#include -#include -#include -#include -#include - -// Include TF -#include - -// STL -#include - -#include "pcl_ros/point_cloud.hpp" - -using pcl_conversions::fromPCL; - -namespace pcl_ros -{ -//////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should - * inherit from this class. - **/ -class PCLNodelet : public nodelet_topic_tools::NodeletLazy -{ -public: - typedef sensor_msgs::PointCloud2 PointCloud2; - - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; - - typedef pcl_msgs::PointIndices PointIndices; - typedef PointIndices::Ptr PointIndicesPtr; - typedef PointIndices::ConstPtr PointIndicesConstPtr; - - typedef pcl_msgs::ModelCoefficients ModelCoefficients; - typedef ModelCoefficients::Ptr ModelCoefficientsPtr; - typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; - - typedef pcl::IndicesPtr IndicesPtr; - typedef pcl::IndicesConstPtr IndicesConstPtr; - - /** \brief Empty constructor. */ - PCLNodelet() - : use_indices_(false), latched_indices_(false), - max_queue_size_(3), approximate_sync_(false) {} - -protected: - /** \brief Set to true if point indices are used. - * - * When receiving a point cloud, if use_indices_ is false, the entire - * point cloud is processed for the given operation. If use_indices_ is - * true, then the ~indices topic is read to get the vector of point - * indices specifying the subset of the point cloud that will be used for - * the operation. In the case where use_indices_ is true, the ~input and - * ~indices topics must be synchronised in time, either exact or within a - * specified jitter. See also @ref latched_indices_ and approximate_sync. - **/ - bool use_indices_; - /** \brief Set to true if the indices topic is latched. - * - * If use_indices_ is true, the ~input and ~indices topics generally must - * be synchronised in time. By setting this flag to true, the most recent - * value from ~indices can be used instead of requiring a synchronised - * message. - **/ - bool latched_indices_; - - /** \brief The message filter subscriber for PointCloud2. */ - message_filters::Subscriber sub_input_filter_; - - /** \brief The message filter subscriber for PointIndices. */ - message_filters::Subscriber sub_indices_filter_; - - /** \brief The output PointCloud publisher. */ - ros::Publisher pub_output_; - - /** \brief The maximum queue size (default: 3). */ - int max_queue_size_; - - /** \brief True if we use an approximate time synchronizer - * versus an exact one (false by default). - **/ - bool approximate_sync_; - - /** \brief TF listener object. */ - tf::TransformListener tf_listener_; - - /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). - * \param cloud the point cloud to test - * \param topic_name an optional topic name (only used for printing, defaults to "input") - */ - inline bool - isValid(const PointCloud2::ConstPtr & cloud, const std::string & topic_name = "input") - { - if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) { - NODELET_WARN( - "[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) " - "with stamp %f, and frame %s on topic %s received!", - getName().c_str(), - cloud->data.size(), cloud->width, cloud->height, cloud->point_step, - cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - topic_name).c_str()); - - return false; - } - return true; - } - - /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). - * \param cloud the point cloud to test - * \param topic_name an optional topic name (only used for printing, defaults to "input") - */ - inline bool - isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input") - { - if (cloud->width * cloud->height != cloud->points.size()) { - NODELET_WARN( - "[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) " - "with stamp %f, and frame %s on topic %s received!", - getName().c_str(), cloud->points.size(), cloud->width, cloud->height, - fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), - pnh_->resolveName(topic_name).c_str()); - - return false; - } - return true; - } - - /** \brief Test whether a given PointIndices message is "valid" (i.e., has values). - * \param indices the point indices message to test - * \param topic_name an optional topic name (only used for printing, defaults to "indices") - */ - inline bool - isValid(const PointIndicesConstPtr & indices, const std::string & topic_name = "indices") - { - /*if (indices->indices.empty ()) - { - NODELET_WARN ("[%s] Empty indices (values = %zu) " - "with stamp %f, and frame %s on topic %s received!", - getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), - indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); - return (true); - }*/ - return true; - } - - /** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values). - * \param model the model coefficients to test - * \param topic_name an optional topic name (only used for printing, defaults to "model") - */ - inline bool - isValid(const ModelCoefficientsConstPtr & model, const std::string & topic_name = "model") - { - /*if (model->values.empty ()) - { - NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, " - "and frame %s on topic %s received!", - getName ().c_str (), model->values.size (), model->header.stamp.toSec (), - model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); - return (false); - }*/ - return true; - } - - /** \brief Lazy transport subscribe/unsubscribe routine. - * It is optional for backward compatibility. - **/ - virtual void subscribe() {} - virtual void unsubscribe() {} - - /** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */ - virtual void - onInit() - { - nodelet_topic_tools::NodeletLazy::onInit(); - - // Parameters that we care about only at startup - pnh_->getParam("max_queue_size", max_queue_size_); - - // ---[ Optional parameters - pnh_->getParam("use_indices", use_indices_); - pnh_->getParam("latched_indices", latched_indices_); - pnh_->getParam("approximate_sync", approximate_sync_); - - NODELET_DEBUG( - "[%s::onInit] PCL Nodelet successfully created with the following parameters:\n" - " - approximate_sync : %s\n" - " - use_indices : %s\n" - " - latched_indices : %s\n" - " - max_queue_size : %d", - getName().c_str(), - (approximate_sync_) ? "true" : "false", - (use_indices_) ? "true" : "false", - (latched_indices_) ? "true" : "false", - max_queue_size_); - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; -} // namespace pcl_ros - -#endif // PCL_ROS__PCL_NODELET_HPP_ diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index 435cc7170..16e45c2d3 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -43,12 +43,12 @@ #endif #include -#include // for PCL_VERSION_COMPARE +#include // for PCL_VERSION_COMPARE #if PCL_VERSION_COMPARE(>=, 1, 11, 0) #include #else #include -#endif // PCL_VERSION_COMPARE(>=, 1, 11, 0) +#endif // PCL_VERSION_COMPARE(>=, 1, 11, 0) #include #include #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED @@ -59,9 +59,6 @@ #endif #endif #include -#include -#include -#include #include #include #include @@ -69,6 +66,10 @@ #include #include #endif +#include +#include // for BOOST_FOREACH +#include +#include namespace pcl { @@ -126,7 +127,7 @@ namespace ros // In ROS 1.3.1+, we can specialize the functor used to create PointCloud objects // on the subscriber side. This allows us to generate the mapping between message // data and object fields only once and reuse it. -#if 0 //ROS_VERSION_MINIMUM(1, 3, 1) +#if 0 // ROS_VERSION_MINIMUM(1, 3, 1) template struct DefaultMessageCreator> { diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp index b60f7e40f..3abb0b4aa 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp @@ -38,11 +38,11 @@ #ifndef PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ #define PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ -#include -#include -#include #include #include +#include +#include +#include #include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp" #include "pcl_ros/pcl_nodelet.hpp" diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp index 4699eee69..ca4ca34fe 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -38,10 +38,10 @@ #ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ #define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ -#include #include #include #include +#include #include "pcl_ros/pcl_nodelet.hpp" #include "pcl_ros/SACSegmentationConfig.hpp" #include "pcl_ros/SACSegmentationFromNormalsConfig.hpp" diff --git a/pcl_ros/include/pcl_ros/transforms.hpp b/pcl_ros/include/pcl_ros/transforms.hpp index 1b5312f32..b96d49009 100644 --- a/pcl_ros/include/pcl_ros/transforms.hpp +++ b/pcl_ros/include/pcl_ros/transforms.hpp @@ -37,15 +37,17 @@ #ifndef PCL_ROS__TRANSFORMS_HPP_ #define PCL_ROS__TRANSFORMS_HPP_ -#include -#include +#include #include #include + +#include +#include + +#include #include #include #include -#include -#include namespace pcl_ros { diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 1f9cbb7c7..df8c80cdd 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -39,7 +39,10 @@ geometry_msgs tf2 tf2_geometry_msgs + tf2_eigen tf2_ros + visualization_msgs + rosbag2_transport libpcl-common libpcl-features @@ -51,6 +54,12 @@ ament_lint_auto ament_lint_common ament_cmake_gtest + ament_cmake_pytest + launch + launch_ros + launch_testing + launch_testing_ros + sensor_msgs