From 649d1ca6ce62922043262b09a34e6024ee04cd25 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Jun 2013 15:41:31 -0700 Subject: [PATCH 001/405] Initial code commit --- CMakeLists.txt | 27 ++++ include/pcl_conversions/pcl_conversions.h | 169 ++++++++++++++++++++++ package.xml | 25 ++++ test/test_pcl_conversions.cpp | 5 + 4 files changed, 226 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 include/pcl_conversions/pcl_conversions.h create mode 100644 package.xml create mode 100644 test/test_pcl_conversions.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..d47230db --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pcl_conversions) + +find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs) + +find_package(PCL REQUIRED COMPONENTS COMMON) + +include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS sensor_msgs std_msgs + DEPENDS pcl +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Add gtest based cpp test target +find_package(GTEST QUIET) +if(GTEST_FOUND) + catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp) +endif() diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h new file mode 100644 index 00000000..a082a243 --- /dev/null +++ b/include/pcl_conversions/pcl_conversions.h @@ -0,0 +1,169 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation, Inc. nor + * the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +namespace pcl_conversions { + +/** PCLHeader <=> Header **/ + +void fromPCLHeaderToHeader(const pcl_std_msgs::PCLHeader &pcl_header, std_msgs::Header &header) +{ + header.stamp.fromNSec(pcl_header.stamp); + header.seq = pcl_header.seq; + header.frame_id = pcl_header.frame_id; +} + +void fromHeaderToPCLHeader(const std_msgs::Header &header, pcl_std_msgs::PCLHeader &pcl_header) +{ + pcl_header.stamp = header.stamp.toNSec(); + pcl_header.seq = header.seq; + pcl_header.frame_id = header.frame_id; +} + +/** PCLImage <=> Image **/ + +void fromPCLImageToImage(const pcl_sensor_msgs::PCLImage &pcl_image, sensor_msgs::Image &image) +{ + fromPCLHeaderToHeader(pcl_image.header, image.header); + image.height = pcl_image.height; + image.width = pcl_image.width; + image.encoding = pcl_image.encoding; + image.is_bigendian = pcl_image.is_bigendian; + image.step = pcl_image.step; + image.data = pcl_image.data; +} + +class ConvertedImage +{ +public: + ConvertedImage(boost::shared_ptr pcl_image_ptr) + : image(), _pcl_image_ptr(pcl_image_ptr) + { + fromPCLHeaderToHeader(pcl_image_ptr->header, image.header); + image.height = pcl_image_ptr->height; + image.width = pcl_image_ptr->width; + image.encoding = pcl_image_ptr->encoding; + image.is_bigendian = pcl_image_ptr->is_bigendian; + image.step = pcl_image_ptr->step; + } + + sensor_msgs::Image image; +private: + boost::shared_ptr _pcl_image_ptr; +}; + +void fromImageToPCLImage(const sensor_msgs::Image &image, pcl_sensor_msgs::PCLImage &pcl_image) +{ + fromHeaderToPCLHeader(image.header, pcl_image.header); + pcl_image.height = image.height; + pcl_image.width = image.width; + pcl_image.encoding = image.encoding; + pcl_image.is_bigendian = image.is_bigendian; + pcl_image.step = image.step; + pcl_image.data = image.data; +} + +/** PCLPointCloud2 <=> PointCloud2 **/ + +void fromPCLPointFieldToPointField(const pcl_sensor_msgs::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) +{ + pf.name = pcl_pf.name; + pf.offset = pcl_pf.offset; + pf.datatype = pcl_pf.datatype; + pf.count = pcl_pf.count; +} + +void fromPointFieldToPCLPointField(const sensor_msgs::PointField &pf, pcl_sensor_msgs::PCLPointField &pcl_pf) +{ + pcl_pf.name = pf.name; + pcl_pf.offset = pf.offset; + pcl_pf.datatype = pf.datatype; + pcl_pf.count = pf.count; +} + +/** PCLPointCloud2 <=> PointCloud2 **/ + +void fromPCLPointCloud2ToPointCloud2(const pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) +{ + fromPCLHeaderToHeader(pcl_pc2.header, pc2.header); + pc2.height = pcl_pc2.height; + pc2.width = pcl_pc2.width; + pc2.fields.resize(pcl_pc2.fields.size()); + std::vector::const_iterator it = pcl_pc2.fields.begin(); + int i = 0; + for(; it != pcl_pc2.fields.end(); ++it, ++i) { + fromPCLPointFieldToPointField(*(it), pc2.fields[i]); + } + pc2.is_bigendian = pcl_pc2.is_bigendian; + pc2.point_step = pcl_pc2.point_step; + pc2.row_step = pcl_pc2.row_step; + pc2.data = pcl_pc2.data; + pc2.is_dense = pcl_pc2.is_dense; +} + +void fromPointCloud2ToPCLPointCloud2(const sensor_msgs::PointCloud2 &pc2, pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2) +{ + fromHeaderToPCLHeader(pc2.header, pcl_pc2.header); + pcl_pc2.height = pc2.height; + pcl_pc2.width = pc2.width; + pcl_pc2.fields.resize(pc2.fields.size()); + std::vector::const_iterator it = pc2.fields.begin(); + int i = 0; + for(; it != pc2.fields.end(); ++it, ++i) { + fromPointFieldToPCLPointField(*(it), pcl_pc2.fields[i]); + } + pcl_pc2.is_bigendian = pc2.is_bigendian; + pcl_pc2.point_step = pc2.point_step; + pcl_pc2.row_step = pc2.row_step; + pcl_pc2.data = pc2.data; + pcl_pc2.is_dense = pc2.is_dense; +} + +} // namespace pcl_conversions diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..d68fe7b0 --- /dev/null +++ b/package.xml @@ -0,0 +1,25 @@ + + + pcl_conversions + 0.1.0 + Provides conversions from PCL data types and ROS message types + William Woodall + + BSD + + https://github.com/ros-perception/pcl_conversions + https://github.com/ros-perception/pcl_conversions/issues + + William Woodall + + catkin + + sensor_msgs + std_msgs + pcl + + sensor_msgs + std_msgs + pcl + + \ No newline at end of file diff --git a/test/test_pcl_conversions.cpp b/test/test_pcl_conversions.cpp new file mode 100644 index 00000000..180623f5 --- /dev/null +++ b/test/test_pcl_conversions.cpp @@ -0,0 +1,5 @@ +#include "gtest/gtest.h" + +#include "pcl_conversions/pcl_conversions.h" + + From c09b1a25de161c4fb0fa9ccec0082063ba699465 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Jun 2013 15:45:02 -0700 Subject: [PATCH 002/405] PCL uses microseconds rather than nanoseconds Also removed some vestigial code --- include/pcl_conversions/pcl_conversions.h | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index a082a243..e4955001 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -55,14 +55,14 @@ namespace pcl_conversions { void fromPCLHeaderToHeader(const pcl_std_msgs::PCLHeader &pcl_header, std_msgs::Header &header) { - header.stamp.fromNSec(pcl_header.stamp); + header.stamp.fromNSec(pcl_header.stamp * 1e3); // Convert from us to ns header.seq = pcl_header.seq; header.frame_id = pcl_header.frame_id; } void fromHeaderToPCLHeader(const std_msgs::Header &header, pcl_std_msgs::PCLHeader &pcl_header) { - pcl_header.stamp = header.stamp.toNSec(); + pcl_header.stamp = header.stamp.toNSec() / 1e3; // Convert from ns to us pcl_header.seq = header.seq; pcl_header.frame_id = header.frame_id; } @@ -80,25 +80,6 @@ void fromPCLImageToImage(const pcl_sensor_msgs::PCLImage &pcl_image, sensor_msgs image.data = pcl_image.data; } -class ConvertedImage -{ -public: - ConvertedImage(boost::shared_ptr pcl_image_ptr) - : image(), _pcl_image_ptr(pcl_image_ptr) - { - fromPCLHeaderToHeader(pcl_image_ptr->header, image.header); - image.height = pcl_image_ptr->height; - image.width = pcl_image_ptr->width; - image.encoding = pcl_image_ptr->encoding; - image.is_bigendian = pcl_image_ptr->is_bigendian; - image.step = pcl_image_ptr->step; - } - - sensor_msgs::Image image; -private: - boost::shared_ptr _pcl_image_ptr; -}; - void fromImageToPCLImage(const sensor_msgs::Image &image, pcl_sensor_msgs::PCLImage &pcl_image) { fromHeaderToPCLHeader(image.header, pcl_image.header); From 06bdd261738b8a0718cc808b07b93e5153facded Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Jun 2013 15:51:38 -0700 Subject: [PATCH 003/405] Boost is no longer used. --- include/pcl_conversions/pcl_conversions.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index e4955001..0ded89d2 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -35,8 +35,6 @@ #include -#include - #include #include From f265bb7aa8f8c0bca1b15dc51a66e9c7af615dd3 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Jun 2013 17:08:00 -0700 Subject: [PATCH 004/405] Added some simple tests. --- CMakeLists.txt | 6 +- test/test_pcl_conversions.cpp | 106 ++++++++++++++++++++++++++++++++++ 2 files changed, 108 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d47230db..d5083c25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,5 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) # Add gtest based cpp test target -find_package(GTEST QUIET) -if(GTEST_FOUND) - catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp) -endif() +catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp) +target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES}) diff --git a/test/test_pcl_conversions.cpp b/test/test_pcl_conversions.cpp index 180623f5..df647af2 100644 --- a/test/test_pcl_conversions.cpp +++ b/test/test_pcl_conversions.cpp @@ -1,5 +1,111 @@ +#include + #include "gtest/gtest.h" #include "pcl_conversions/pcl_conversions.h" +namespace { +class PCLConversionTests : public ::testing::Test { +protected: + virtual void SetUp() { + pcl_image.header.frame_id = "pcl"; + pcl_image.height = 1; + pcl_image.width = 2; + pcl_image.step = 1; + pcl_image.is_bigendian = true; + pcl_image.encoding = "bgr8"; + pcl_image.data.resize(2); + pcl_image.data[0] = 0x42; + pcl_image.data[1] = 0x43; + + pcl_pc2.header.frame_id = "pcl"; + pcl_pc2.height = 1; + pcl_pc2.width = 2; + pcl_pc2.point_step = 1; + pcl_pc2.row_step = 1; + 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_sensor_msgs::PCLPointField::INT8; + pcl_pc2.fields[0].count = 3; + pcl_pc2.fields[0].offset = 0; + pcl_pc2.fields[1].name = "RGB"; + pcl_pc2.fields[1].datatype = pcl_sensor_msgs::PCLPointField::INT8; + pcl_pc2.fields[1].count = 3; + pcl_pc2.fields[1].offset = 8 * 3; + pcl_pc2.data.resize(2); + pcl_pc2.data[0] = 0x42; + pcl_pc2.data[1] = 0x43; + } + + pcl_sensor_msgs::PCLImage pcl_image; + sensor_msgs::Image image; + + pcl_sensor_msgs::PCLPointCloud2 pcl_pc2; + sensor_msgs::PointCloud2 pc2; +}; + +template +void test_image(T &image) { + EXPECT_EQ(std::string("pcl"), image.header.frame_id); + EXPECT_EQ(1, image.height); + EXPECT_EQ(2, image.width); + EXPECT_EQ(1, image.step); + EXPECT_TRUE(image.is_bigendian); + EXPECT_EQ(std::string("bgr8"), image.encoding); + EXPECT_EQ(2, image.data.size()); + EXPECT_EQ(0x42, image.data[0]); + EXPECT_EQ(0x43, image.data[1]); +} + +TEST_F(PCLConversionTests, imageConversion) { + pcl_conversions::fromPCLImageToImage(pcl_image, image); + test_image(image); + pcl_sensor_msgs::PCLImage pcl_image2; + pcl_conversions::fromImageToPCLImage(image, pcl_image2); + test_image(pcl_image2); +} + +template +void test_pc(T &pc) { + EXPECT_EQ(std::string("pcl"), pc.header.frame_id); + EXPECT_EQ(1, pc.height); + EXPECT_EQ(2, pc.width); + EXPECT_EQ(1, pc.point_step); + EXPECT_EQ(1, pc.row_step); + EXPECT_TRUE(pc.is_bigendian); + EXPECT_TRUE(pc.is_dense); + EXPECT_EQ("XYZ", pc.fields[0].name); + EXPECT_EQ(pcl_sensor_msgs::PCLPointField::INT8, pc.fields[0].datatype); + EXPECT_EQ(3, pc.fields[0].count); + EXPECT_EQ(0, pc.fields[0].offset); + EXPECT_EQ("RGB", pc.fields[1].name); + EXPECT_EQ(pcl_sensor_msgs::PCLPointField::INT8, pc.fields[1].datatype); + EXPECT_EQ(3, pc.fields[1].count); + EXPECT_EQ(8 * 3, pc.fields[1].offset); + EXPECT_EQ(2, pc.data.size()); + EXPECT_EQ(0x42, pc.data[0]); + EXPECT_EQ(0x43, pc.data[1]); +} + +TEST_F(PCLConversionTests, pointcloud2Conversion) { + pcl_conversions::fromPCLPointCloud2ToPointCloud2(pcl_pc2, pc2); + test_pc(pc2); + pcl_sensor_msgs::PCLPointCloud2 pcl_pc2_2; + pcl_conversions::fromPointCloud2ToPCLPointCloud2(pc2, pcl_pc2_2); + test_pc(pcl_pc2_2); +} + +} // namespace + +int main(int argc, char **argv) { + try { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); + } catch (std::exception &e) { + std::cerr << "Unhandled Exception: " << e.what() << std::endl; + } + return 1; +} From 21ed95ce4e6512bce0039529146c1c9dc3b36f2d Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Jun 2013 17:11:54 -0700 Subject: [PATCH 005/405] Adding a README --- README.rst | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 README.rst diff --git a/README.rst b/README.rst new file mode 100644 index 00000000..b6dff4af --- /dev/null +++ b/README.rst @@ -0,0 +1,22 @@ +pcl_conversions +=============== + +This package provides conversions from PCL data types and ROS message types. + + +Code & tickets +-------------- + +Build status: |Build Status| + +.. |Build Status| image:: https://secure.travis-ci.org/ros-perception/pcl_conversions.png + :target: http://travis-ci.org/ros-perception/pcl_conversions + ++-----------------+------------------------------------------------------------+ +| pcl_conversions | http://ros.org/wiki/pcl_conversions | ++-----------------+------------------------------------------------------------+ +| Issues | http://github.com/ros-perception/pcl_conversions/issues | ++-----------------+------------------------------------------------------------+ +..| Documentation | http://ros-perception.github.com/pcl_conversions/doc | +..+-----------------+------------------------------------------------------------+ + From d6c254cb4e9a131593289e2481e355e5f4937649 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Jun 2013 17:12:46 -0700 Subject: [PATCH 006/405] Update README.rst --- README.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.rst b/README.rst index b6dff4af..9858405e 100644 --- a/README.rst +++ b/README.rst @@ -17,6 +17,6 @@ Build status: |Build Status| +-----------------+------------------------------------------------------------+ | Issues | http://github.com/ros-perception/pcl_conversions/issues | +-----------------+------------------------------------------------------------+ -..| Documentation | http://ros-perception.github.com/pcl_conversions/doc | -..+-----------------+------------------------------------------------------------+ +.. | Documentation | http://ros-perception.github.com/pcl_conversions/doc | +.. +-----------------+------------------------------------------------------------+ From 034680bd5bd3c8a6e99ee7bb1965276e12b52b60 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Jun 2013 17:15:16 -0700 Subject: [PATCH 007/405] Update README.rst --- README.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.rst b/README.rst index 9858405e..4ec7b6a5 100644 --- a/README.rst +++ b/README.rst @@ -7,9 +7,9 @@ This package provides conversions from PCL data types and ROS message types. Code & tickets -------------- -Build status: |Build Status| +.. Build status: |Build Status| -.. |Build Status| image:: https://secure.travis-ci.org/ros-perception/pcl_conversions.png +.. .. |Build Status| image:: https://secure.travis-ci.org/ros-perception/pcl_conversions.png :target: http://travis-ci.org/ros-perception/pcl_conversions +-----------------+------------------------------------------------------------+ From 17662604b8def70831716ea2a7db38881a859395 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 28 Jun 2013 12:07:30 -0700 Subject: [PATCH 008/405] Shorten the names of functions by popular demand. --- include/pcl_conversions/pcl_conversions.h | 28 +++++++++++------------ test/test_pcl_conversions.cpp | 8 +++---- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 0ded89d2..f142372e 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -51,14 +51,14 @@ namespace pcl_conversions { /** PCLHeader <=> Header **/ -void fromPCLHeaderToHeader(const pcl_std_msgs::PCLHeader &pcl_header, std_msgs::Header &header) +void fromPCL(const pcl_std_msgs::PCLHeader &pcl_header, std_msgs::Header &header) { header.stamp.fromNSec(pcl_header.stamp * 1e3); // Convert from us to ns header.seq = pcl_header.seq; header.frame_id = pcl_header.frame_id; } -void fromHeaderToPCLHeader(const std_msgs::Header &header, pcl_std_msgs::PCLHeader &pcl_header) +void toPCL(const std_msgs::Header &header, pcl_std_msgs::PCLHeader &pcl_header) { pcl_header.stamp = header.stamp.toNSec() / 1e3; // Convert from ns to us pcl_header.seq = header.seq; @@ -67,9 +67,9 @@ void fromHeaderToPCLHeader(const std_msgs::Header &header, pcl_std_msgs::PCLHead /** PCLImage <=> Image **/ -void fromPCLImageToImage(const pcl_sensor_msgs::PCLImage &pcl_image, sensor_msgs::Image &image) +void fromPCL(const pcl_sensor_msgs::PCLImage &pcl_image, sensor_msgs::Image &image) { - fromPCLHeaderToHeader(pcl_image.header, image.header); + fromPCL(pcl_image.header, image.header); image.height = pcl_image.height; image.width = pcl_image.width; image.encoding = pcl_image.encoding; @@ -78,9 +78,9 @@ void fromPCLImageToImage(const pcl_sensor_msgs::PCLImage &pcl_image, sensor_msgs image.data = pcl_image.data; } -void fromImageToPCLImage(const sensor_msgs::Image &image, pcl_sensor_msgs::PCLImage &pcl_image) +void toPCL(const sensor_msgs::Image &image, pcl_sensor_msgs::PCLImage &pcl_image) { - fromHeaderToPCLHeader(image.header, pcl_image.header); + toPCL(image.header, pcl_image.header); pcl_image.height = image.height; pcl_image.width = image.width; pcl_image.encoding = image.encoding; @@ -91,7 +91,7 @@ void fromImageToPCLImage(const sensor_msgs::Image &image, pcl_sensor_msgs::PCLIm /** PCLPointCloud2 <=> PointCloud2 **/ -void fromPCLPointFieldToPointField(const pcl_sensor_msgs::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) +void fromPCL(const pcl_sensor_msgs::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) { pf.name = pcl_pf.name; pf.offset = pcl_pf.offset; @@ -99,7 +99,7 @@ void fromPCLPointFieldToPointField(const pcl_sensor_msgs::PCLPointField &pcl_pf, pf.count = pcl_pf.count; } -void fromPointFieldToPCLPointField(const sensor_msgs::PointField &pf, pcl_sensor_msgs::PCLPointField &pcl_pf) +void toPCL(const sensor_msgs::PointField &pf, pcl_sensor_msgs::PCLPointField &pcl_pf) { pcl_pf.name = pf.name; pcl_pf.offset = pf.offset; @@ -109,16 +109,16 @@ void fromPointFieldToPCLPointField(const sensor_msgs::PointField &pf, pcl_sensor /** PCLPointCloud2 <=> PointCloud2 **/ -void fromPCLPointCloud2ToPointCloud2(const pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) +void fromPCL(const pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) { - fromPCLHeaderToHeader(pcl_pc2.header, pc2.header); + fromPCL(pcl_pc2.header, pc2.header); pc2.height = pcl_pc2.height; pc2.width = pcl_pc2.width; pc2.fields.resize(pcl_pc2.fields.size()); std::vector::const_iterator it = pcl_pc2.fields.begin(); int i = 0; for(; it != pcl_pc2.fields.end(); ++it, ++i) { - fromPCLPointFieldToPointField(*(it), pc2.fields[i]); + fromPCL(*(it), pc2.fields[i]); } pc2.is_bigendian = pcl_pc2.is_bigendian; pc2.point_step = pcl_pc2.point_step; @@ -127,16 +127,16 @@ void fromPCLPointCloud2ToPointCloud2(const pcl_sensor_msgs::PCLPointCloud2 &pcl_ pc2.is_dense = pcl_pc2.is_dense; } -void fromPointCloud2ToPCLPointCloud2(const sensor_msgs::PointCloud2 &pc2, pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2) +void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2) { - fromHeaderToPCLHeader(pc2.header, pcl_pc2.header); + toPCL(pc2.header, pcl_pc2.header); pcl_pc2.height = pc2.height; pcl_pc2.width = pc2.width; pcl_pc2.fields.resize(pc2.fields.size()); std::vector::const_iterator it = pc2.fields.begin(); int i = 0; for(; it != pc2.fields.end(); ++it, ++i) { - fromPointFieldToPCLPointField(*(it), pcl_pc2.fields[i]); + toPCL(*(it), pcl_pc2.fields[i]); } pcl_pc2.is_bigendian = pc2.is_bigendian; pcl_pc2.point_step = pc2.point_step; diff --git a/test/test_pcl_conversions.cpp b/test/test_pcl_conversions.cpp index df647af2..e7cd99f2 100644 --- a/test/test_pcl_conversions.cpp +++ b/test/test_pcl_conversions.cpp @@ -61,10 +61,10 @@ void test_image(T &image) { } TEST_F(PCLConversionTests, imageConversion) { - pcl_conversions::fromPCLImageToImage(pcl_image, image); + pcl_conversions::fromPCL(pcl_image, image); test_image(image); pcl_sensor_msgs::PCLImage pcl_image2; - pcl_conversions::fromImageToPCLImage(image, pcl_image2); + pcl_conversions::toPCL(image, pcl_image2); test_image(pcl_image2); } @@ -91,10 +91,10 @@ void test_pc(T &pc) { } TEST_F(PCLConversionTests, pointcloud2Conversion) { - pcl_conversions::fromPCLPointCloud2ToPointCloud2(pcl_pc2, pc2); + pcl_conversions::fromPCL(pcl_pc2, pc2); test_pc(pc2); pcl_sensor_msgs::PCLPointCloud2 pcl_pc2_2; - pcl_conversions::fromPointCloud2ToPCLPointCloud2(pc2, pcl_pc2_2); + pcl_conversions::toPCL(pc2, pcl_pc2_2); test_pc(pcl_pc2_2); } From 9dc59ae6ec34816776d587bd6cbcbdc93c6b966a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 16:29:37 -0700 Subject: [PATCH 009/405] Adding include guard --- include/pcl_conversions/pcl_conversions.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index f142372e..1887c989 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -33,6 +33,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#ifndef PCL_CONVERSIONS_H__ +#define PCL_CONVERSIONS_H__ + #include #include @@ -146,3 +149,5 @@ void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl_sensor_msgs::PCLPointCloud2 } } // namespace pcl_conversions + +#endif \ No newline at end of file From 2b131448db9ba5d3f9ccd2b2213f9b86ade0f544 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 16:32:39 -0700 Subject: [PATCH 010/405] updating pcl namespaces --- include/pcl_conversions/pcl_conversions.h | 28 +++++++++++++---------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 1887c989..63a3e1ef 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -38,30 +38,34 @@ #include -#include +#include + +#include + +#include #include -#include +#include #include -#include +#include #include -#include +#include #include namespace pcl_conversions { /** PCLHeader <=> Header **/ -void fromPCL(const pcl_std_msgs::PCLHeader &pcl_header, std_msgs::Header &header) +void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header) { header.stamp.fromNSec(pcl_header.stamp * 1e3); // Convert from us to ns header.seq = pcl_header.seq; header.frame_id = pcl_header.frame_id; } -void toPCL(const std_msgs::Header &header, pcl_std_msgs::PCLHeader &pcl_header) +void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) { pcl_header.stamp = header.stamp.toNSec() / 1e3; // Convert from ns to us pcl_header.seq = header.seq; @@ -70,7 +74,7 @@ void toPCL(const std_msgs::Header &header, pcl_std_msgs::PCLHeader &pcl_header) /** PCLImage <=> Image **/ -void fromPCL(const pcl_sensor_msgs::PCLImage &pcl_image, sensor_msgs::Image &image) +void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) { fromPCL(pcl_image.header, image.header); image.height = pcl_image.height; @@ -81,7 +85,7 @@ void fromPCL(const pcl_sensor_msgs::PCLImage &pcl_image, sensor_msgs::Image &ima image.data = pcl_image.data; } -void toPCL(const sensor_msgs::Image &image, pcl_sensor_msgs::PCLImage &pcl_image) +void toPCL(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image) { toPCL(image.header, pcl_image.header); pcl_image.height = image.height; @@ -94,7 +98,7 @@ void toPCL(const sensor_msgs::Image &image, pcl_sensor_msgs::PCLImage &pcl_image /** PCLPointCloud2 <=> PointCloud2 **/ -void fromPCL(const pcl_sensor_msgs::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) +void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) { pf.name = pcl_pf.name; pf.offset = pcl_pf.offset; @@ -102,7 +106,7 @@ void fromPCL(const pcl_sensor_msgs::PCLPointField &pcl_pf, sensor_msgs::PointFie pf.count = pcl_pf.count; } -void toPCL(const sensor_msgs::PointField &pf, pcl_sensor_msgs::PCLPointField &pcl_pf) +void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf) { pcl_pf.name = pf.name; pcl_pf.offset = pf.offset; @@ -112,7 +116,7 @@ void toPCL(const sensor_msgs::PointField &pf, pcl_sensor_msgs::PCLPointField &pc /** PCLPointCloud2 <=> PointCloud2 **/ -void fromPCL(const pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) +void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) { fromPCL(pcl_pc2.header, pc2.header); pc2.height = pcl_pc2.height; @@ -130,7 +134,7 @@ void fromPCL(const pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointC pc2.is_dense = pcl_pc2.is_dense; } -void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl_sensor_msgs::PCLPointCloud2 &pcl_pc2) +void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) { toPCL(pc2.header, pcl_pc2.header); pcl_pc2.height = pc2.height; From 5c7d7c3f868ed94f21818340624a6383da1aefcc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 16:33:56 -0700 Subject: [PATCH 011/405] New toPCL and fromPCL functions --- include/pcl_conversions/pcl_conversions.h | 48 +++++++++++++++++------ 1 file changed, 36 insertions(+), 12 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 63a3e1ef..ae9e1b3c 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -72,6 +72,20 @@ void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) pcl_header.frame_id = header.frame_id; } +std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) +{ + std_msgs::Header header; + fromPCL(pcl_header, header); + return header; +} + +pcl::PCLHeader toPCL(const std_msgs::Header &header) +{ + pcl::PCLHeader pcl_header; + toPCL(header, pcl_header); + return pcl_header; +} + /** PCLImage <=> Image **/ void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) @@ -106,6 +120,16 @@ void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) pf.count = pcl_pf.count; } +void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) +{ + pfs.resize(pcl_pfs.size()); + std::vector::const_iterator it = pcl_pfs.begin(); + int i = 0; + for(; it != pcl_pfs.end(); ++it, ++i) { + fromPCL(*(it), pfs[i]); + } +} + void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf) { pcl_pf.name = pf.name; @@ -114,6 +138,16 @@ void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf) pcl_pf.count = pf.count; } +void toPCL(const std::vector &pfs, std::vector &pcl_pfs) +{ + pcl_pfs.resize(pfs.size()); + std::vector::const_iterator it = pfs.begin(); + int i = 0; + for(; it != pfs.end(); ++it, ++i) { + toPCL(*(it), pcl_pfs[i]); + } +} + /** PCLPointCloud2 <=> PointCloud2 **/ void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) @@ -121,12 +155,7 @@ void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) fromPCL(pcl_pc2.header, pc2.header); pc2.height = pcl_pc2.height; pc2.width = pcl_pc2.width; - pc2.fields.resize(pcl_pc2.fields.size()); - std::vector::const_iterator it = pcl_pc2.fields.begin(); - int i = 0; - for(; it != pcl_pc2.fields.end(); ++it, ++i) { - fromPCL(*(it), pc2.fields[i]); - } + fromPCL(pcl_pc2.fields, pc2.fields); pc2.is_bigendian = pcl_pc2.is_bigendian; pc2.point_step = pcl_pc2.point_step; pc2.row_step = pcl_pc2.row_step; @@ -139,12 +168,7 @@ void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) toPCL(pc2.header, pcl_pc2.header); pcl_pc2.height = pc2.height; pcl_pc2.width = pc2.width; - pcl_pc2.fields.resize(pc2.fields.size()); - std::vector::const_iterator it = pc2.fields.begin(); - int i = 0; - for(; it != pc2.fields.end(); ++it, ++i) { - toPCL(*(it), pcl_pc2.fields[i]); - } + toPCL(pc2.fields, pcl_pc2.fields); pcl_pc2.is_bigendian = pc2.is_bigendian; pcl_pc2.point_step = pc2.point_step; pcl_pc2.row_step = pc2.row_step; From 2e89da5996faa826d8ed3355b55a11d818990200 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 16:34:15 -0700 Subject: [PATCH 012/405] Custom serializer for PCLPointCloud2 and PCLHeader --- include/pcl_conversions/pcl_conversions.h | 231 ++++++++++++++++++++++ 1 file changed, 231 insertions(+) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index ae9e1b3c..6ad5e27a 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -54,6 +54,11 @@ #include #include +#include + +#include +#include + namespace pcl_conversions { /** PCLHeader <=> Header **/ @@ -178,4 +183,230 @@ void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) } // namespace pcl_conversions +namespace pcl { + +/** Overload pcl::getFieldIndex **/ +inline int getFieldIndex(const sensor_msgs::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 (-1); + +} + +/** Overload pcl::getFieldsList **/ +inline std::string getFieldsList(const sensor_msgs::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); +} + +/** Provide pcl::toROSMsg **/ +void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) +{ + pcl::PCLPointCloud2 pcl_cloud; + pcl_conversions::toPCL(cloud, pcl_cloud); + pcl::PCLImage pcl_image; + pcl::toPCLPointCloud2(pcl_cloud, pcl_image); + pcl_conversions::fromPCL(pcl_image, image); +} + +/** Overload pcl::createMapping **/ +template +void createMapping(const std::vector& msg_fields, MsgFieldMap& field_map) +{ + std::vector pcl_msg_fields; + pcl_conversions::toPCL(msg_fields, pcl_msg_fields); + return createMapping(pcl_msg_fields, field_map); +} + +namespace io { + +/** Overload pcl::io::savePCDFile **/ +inline int +savePCDFile(const std::string &file_name, const sensor_msgs::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); + return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode); +} + +/** Overload pcl::io::loadPCDFile **/ +inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud) +{ + pcl::PCLPointCloud2 pcl_cloud; + int ret = pcl::io::loadPCDFile(file_name, pcl_cloud); + pcl_conversions::fromPCL(pcl_cloud, cloud); + return ret; +} + +} // namespace io + +} // namespace pcl + +/* + * Provide a custom serialization for pcl::PCLPointCloud2 + */ + +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 ROS_VERSION_MINIMUM(1, 3, 1) + template<> + struct DefaultMessageCreator + { + boost::shared_ptr operator() () + { + boost::shared_ptr msg(new pcl::PCLPointCloud2()); + return msg; + } + }; +#endif + + namespace message_traits + { + template<> + struct MD5Sum + { + static const char* value() { return MD5Sum::value(); } + static const char* value(const pcl::PCLPointCloud2&) { return value(); } + + static const uint64_t static_value1 = MD5Sum::static_value1; + static const uint64_t static_value2 = MD5Sum::static_value2; + + // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here. + ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL); + ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL); + }; + + template<> + struct DataType + { + static const char* value() { return DataType::value(); } + static const char* value(const pcl::PCLPointCloud2&) { return value(); } + }; + + template<> + struct Definition + { + static const char* value() { return Definition::value(); } + static const char* value(const pcl::PCLPointCloud2&) { return value(); } + }; + + template<> struct HasHeader : TrueType {}; + } // namespace ros::message_traits + + namespace serialization + { + template<> + struct Serializer + { + template + inline static void write(Stream& stream, const pcl::PCLPointCloud2& m) + { + std_msgs::Header header; + pcl_conversions::fromPCL(m.header, header); + stream.next(header); + stream.next(m.height); + stream.next(m.width); + std::vector pfs; + pcl_conversions::fromPCL(m.fields, pfs); + stream.next(pfs); + stream.next(m.is_bigendian); + stream.next(m.point_step); + stream.next(m.row_step); + stream.next(m.data); + stream.next(m.is_dense); + } + + template + inline static void read(Stream& stream, pcl::PCLPointCloud2& m) + { + std_msgs::Header header; + stream.next(header); + pcl_conversions::toPCL(header, m.header); + stream.next(m.height); + stream.next(m.width); + std::vector pfs; + stream.next(pfs); + pcl_conversions::toPCL(pfs, m.fields); + stream.next(m.fields); + stream.next(m.is_bigendian); + stream.next(m.point_step); + stream.next(m.row_step); + stream.next(m.data); + stream.next(m.is_dense); + } + + inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m) + { + uint32_t length = 0; + + std_msgs::Header header; + pcl_conversions::fromPCL(m.header, header); + length += serializationLength(header); + length += 4; // height + length += 4; // width + std::vector pfs; + pcl_conversions::fromPCL(m.fields, pfs); + length += serializationLength(pfs); // fields + length += 1; // is_bigendian + length += 4; // point_step + length += 4; // row_step + length += 4; // data's size + length += m.data.size() * sizeof(pcl::uint8_t); + length += 1; // is_dense + + return length; + } + }; + + template<> + struct Serializer + { + template + inline static void write(Stream& stream, const pcl::PCLHeader& m) + { + std_msgs::Header header; + pcl_conversions::fromPCL(m, header); + stream.next(header); + } + + template + inline static void read(Stream& stream, pcl::PCLHeader& m) + { + std_msgs::Header header; + stream.next(header); + pcl_conversions::toPCL(header, m); + } + + inline static uint32_t serializedLength(const pcl::PCLHeader& m) + { + uint32_t length = 0; + + std_msgs::Header header; + pcl_conversions::fromPCL(m, header); + length += serializationLength(header); + + return length; + } + }; + } // namespace ros::serialization + +} // namespace ros + + #endif \ No newline at end of file From 6b77c9163f8eac76eb2b35834acb114148c2e203 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 17:36:58 -0700 Subject: [PATCH 013/405] add conversions for PointIndices --- include/pcl_conversions/pcl_conversions.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 6ad5e27a..c1b523e9 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -54,6 +54,9 @@ #include #include +#include +#include + #include #include @@ -181,6 +184,19 @@ void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) pcl_pc2.is_dense = pc2.is_dense; } +/** pcl::PointIndices <=> pcl_msgs::PointIndices **/ +void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi) +{ + fromPCL(pcl_pi.header, pi.header); + pi.indices = pcl_pi.indices; +} + +void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi) +{ + toPCL(pi.header, pcl_pi.header); + pcl_pi.indices = pi.indices; +} + } // namespace pcl_conversions namespace pcl { From 9f685befcbd59fad4662622c88a5d74095bbc4b0 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 18:50:55 -0700 Subject: [PATCH 014/405] Inlined functions to prevent duplicate symbols --- include/pcl_conversions/pcl_conversions.h | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index c1b523e9..b9098b37 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -66,6 +66,7 @@ namespace pcl_conversions { /** PCLHeader <=> Header **/ +inline void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header) { header.stamp.fromNSec(pcl_header.stamp * 1e3); // Convert from us to ns @@ -73,6 +74,7 @@ void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header) header.frame_id = pcl_header.frame_id; } +inline void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) { pcl_header.stamp = header.stamp.toNSec() / 1e3; // Convert from ns to us @@ -80,6 +82,7 @@ void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) pcl_header.frame_id = header.frame_id; } +inline std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) { std_msgs::Header header; @@ -87,6 +90,7 @@ std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) return header; } +inline pcl::PCLHeader toPCL(const std_msgs::Header &header) { pcl::PCLHeader pcl_header; @@ -96,6 +100,7 @@ pcl::PCLHeader toPCL(const std_msgs::Header &header) /** PCLImage <=> Image **/ +inline void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) { fromPCL(pcl_image.header, image.header); @@ -107,6 +112,7 @@ void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) image.data = pcl_image.data; } +inline void toPCL(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image) { toPCL(image.header, pcl_image.header); @@ -120,6 +126,7 @@ void toPCL(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image) /** PCLPointCloud2 <=> PointCloud2 **/ +inline void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) { pf.name = pcl_pf.name; @@ -128,6 +135,7 @@ void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) pf.count = pcl_pf.count; } +inline void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) { pfs.resize(pcl_pfs.size()); @@ -138,6 +146,7 @@ void fromPCL(const std::vector &pcl_pfs, std::vector &pfs, std::vector &pcl_pfs) { pcl_pfs.resize(pfs.size()); @@ -158,6 +168,7 @@ void toPCL(const std::vector &pfs, std::vector PointCloud2 **/ +inline void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) { fromPCL(pcl_pc2.header, pc2.header); @@ -171,6 +182,7 @@ void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) pc2.is_dense = pcl_pc2.is_dense; } +inline void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) { toPCL(pc2.header, pcl_pc2.header); @@ -185,12 +197,15 @@ void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) } /** pcl::PointIndices <=> pcl_msgs::PointIndices **/ + +inline void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi) { fromPCL(pcl_pi.header, pi.header); pi.indices = pcl_pi.indices; } +inline void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi) { toPCL(pi.header, pcl_pi.header); @@ -226,6 +241,8 @@ inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud) } /** Provide pcl::toROSMsg **/ + +inline void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) { pcl::PCLPointCloud2 pcl_cloud; @@ -425,4 +442,4 @@ namespace ros } // namespace ros -#endif \ No newline at end of file +#endif /* PCL_CONVERSIONS_H__ */ \ No newline at end of file From 4b6240e9248c57ba134698b2a3104d812acc2f61 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Jul 2013 18:55:42 -0700 Subject: [PATCH 015/405] Added converters and destructive move functions --- include/pcl_conversions/pcl_conversions.h | 622 +++++++++++++++++----- 1 file changed, 498 insertions(+), 124 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index b9098b37..23dc3d43 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -57,6 +57,15 @@ #include #include +#include +#include + +#include +#include + +#include +#include + #include #include @@ -64,113 +73,139 @@ namespace pcl_conversions { -/** PCLHeader <=> Header **/ + /** PCLHeader <=> Header **/ -inline -void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header) -{ + inline + void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header) + { header.stamp.fromNSec(pcl_header.stamp * 1e3); // Convert from us to ns header.seq = pcl_header.seq; header.frame_id = pcl_header.frame_id; -} + } -inline -void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) -{ + inline + void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) + { pcl_header.stamp = header.stamp.toNSec() / 1e3; // Convert from ns to us pcl_header.seq = header.seq; pcl_header.frame_id = header.frame_id; -} + } -inline -std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) -{ + inline + std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) + { std_msgs::Header header; fromPCL(pcl_header, header); return header; -} + } -inline -pcl::PCLHeader toPCL(const std_msgs::Header &header) -{ + inline + pcl::PCLHeader toPCL(const std_msgs::Header &header) + { pcl::PCLHeader pcl_header; toPCL(header, pcl_header); return pcl_header; -} + } -/** PCLImage <=> Image **/ + /** PCLImage <=> Image **/ -inline -void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) -{ + inline + void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) + { fromPCL(pcl_image.header, image.header); image.height = pcl_image.height; image.width = pcl_image.width; image.encoding = pcl_image.encoding; image.is_bigendian = pcl_image.is_bigendian; image.step = pcl_image.step; - image.data = pcl_image.data; -} + } -inline -void toPCL(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image) -{ + inline + void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) + { + copyPCLImageMetaData(pcl_image, image); + image.data = pcl_image.data; + } + + inline + void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image) + { + copyPCLImageMetaData(pcl_image, image); + image.data.swap(pcl_image.data); + } + + inline + void copyImageMetaData(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image) + { toPCL(image.header, pcl_image.header); pcl_image.height = image.height; pcl_image.width = image.width; pcl_image.encoding = image.encoding; pcl_image.is_bigendian = image.is_bigendian; pcl_image.step = image.step; + } + + inline + void toPCL(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image) + { + copyImageMetaData(image, pcl_image); pcl_image.data = image.data; -} + } -/** PCLPointCloud2 <=> PointCloud2 **/ + inline + void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image) + { + copyImageMetaData(image, pcl_image); + pcl_image.data.swap(image.data); + } -inline -void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) -{ + /** PCLPointField <=> PointField **/ + + inline + void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) + { pf.name = pcl_pf.name; pf.offset = pcl_pf.offset; pf.datatype = pcl_pf.datatype; pf.count = pcl_pf.count; -} + } -inline -void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) -{ + inline + void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) + { pfs.resize(pcl_pfs.size()); std::vector::const_iterator it = pcl_pfs.begin(); int i = 0; for(; it != pcl_pfs.end(); ++it, ++i) { - fromPCL(*(it), pfs[i]); + fromPCL(*(it), pfs[i]); } -} + } -inline -void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf) -{ + inline + void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf) + { pcl_pf.name = pf.name; pcl_pf.offset = pf.offset; pcl_pf.datatype = pf.datatype; pcl_pf.count = pf.count; -} + } -inline -void toPCL(const std::vector &pfs, std::vector &pcl_pfs) -{ + inline + void toPCL(const std::vector &pfs, std::vector &pcl_pfs) + { pcl_pfs.resize(pfs.size()); std::vector::const_iterator it = pfs.begin(); int i = 0; for(; it != pfs.end(); ++it, ++i) { - toPCL(*(it), pcl_pfs[i]); + toPCL(*(it), pcl_pfs[i]); } -} + } -/** PCLPointCloud2 <=> PointCloud2 **/ + /** PCLPointCloud2 <=> PointCloud2 **/ -inline -void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) -{ + inline + void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) + { fromPCL(pcl_pc2.header, pc2.header); pc2.height = pcl_pc2.height; pc2.width = pcl_pc2.width; @@ -178,13 +213,26 @@ void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) pc2.is_bigendian = pcl_pc2.is_bigendian; pc2.point_step = pcl_pc2.point_step; pc2.row_step = pcl_pc2.row_step; - pc2.data = pcl_pc2.data; pc2.is_dense = pcl_pc2.is_dense; -} + } -inline -void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) -{ + inline + void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) + { + copyPCLPointCloud2MetaData(pcl_pc2, pc2); + pc2.data = pcl_pc2.data; + } + + inline + void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) + { + copyPCLPointCloud2MetaData(pcl_pc2, pc2); + pc2.data.swap(pcl_pc2.data); + } + + inline + void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { toPCL(pc2.header, pcl_pc2.header); pcl_pc2.height = pc2.height; pcl_pc2.width = pc2.width; @@ -192,112 +240,397 @@ void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) pcl_pc2.is_bigendian = pc2.is_bigendian; pcl_pc2.point_step = pc2.point_step; pcl_pc2.row_step = pc2.row_step; - pcl_pc2.data = pc2.data; pcl_pc2.is_dense = pc2.is_dense; -} + } -/** pcl::PointIndices <=> pcl_msgs::PointIndices **/ + inline + void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { + copyPointCloud2MetaData(pc2, pcl_pc2); + pcl_pc2.data = pc2.data; + } -inline -void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi) -{ - fromPCL(pcl_pi.header, pi.header); - pi.indices = pcl_pi.indices; -} + inline + void moveToPCL(sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { + copyPointCloud2MetaData(pc2, pcl_pc2); + pcl_pc2.data.swap(pc2.data); + } -inline -void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi) -{ - toPCL(pi.header, pcl_pi.header); - pcl_pi.indices = pi.indices; -} + /** pcl::PointIndices <=> pcl_msgs::PointIndices **/ + + inline + void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi) + { + fromPCL(pcl_pi.header, pi.header); + pi.indices = pcl_pi.indices; + } + + inline + void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi) + { + fromPCL(pcl_pi.header, pi.header); + pi.indices.swap(pcl_pi.indices); + } + + inline + void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi) + { + toPCL(pi.header, pcl_pi.header); + pcl_pi.indices = pi.indices; + } + + inline + void moveToPCL(pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi) + { + toPCL(pi.header, pcl_pi.header); + pcl_pi.indices.swap(pi.indices); + } + + /** pcl::ModelCoefficients <=> pcl_msgs::ModelCoefficients **/ + + inline + void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc) + { + fromPCL(pcl_mc.header, mc.header); + mc.values = pcl_mc.values; + } + + inline + void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc) + { + fromPCL(pcl_mc.header, mc.header); + mc.values.swap(pcl_mc.values); + } + + inline + void toPCL(const pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc) + { + toPCL(mc.header, pcl_mc.header); + pcl_mc.values = mc.values; + } + + inline + void moveToPCL(pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc) + { + toPCL(mc.header, pcl_mc.header); + pcl_mc.values.swap(mc.values); + } + + /** pcl::Vertices <=> pcl_msgs::Vertices **/ + + inline + void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert) + { + vert.vertices = pcl_vert.vertices; + } + + inline + void fromPCL(const std::vector &pcl_verts, std::vector &verts) + { + verts.resize(pcl_verts.size()); + std::vector::const_iterator it = pcl_verts.begin(); + std::vector::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::Vertices &vert) + { + vert.vertices.swap(pcl_vert.vertices); + } + + inline + void fromPCL(std::vector &pcl_verts, std::vector &verts) + { + verts.resize(pcl_verts.size()); + std::vector::iterator it = pcl_verts.begin(); + std::vector::iterator jt = verts.begin(); + for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) { + moveFromPCL(*(it), *(jt)); + } + } + + inline + void toPCL(const pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert) + { + pcl_vert.vertices = vert.vertices; + } + + inline + void toPCL(const std::vector &verts, std::vector &pcl_verts) + { + pcl_verts.resize(verts.size()); + std::vector::const_iterator it = verts.begin(); + std::vector::iterator jt = pcl_verts.begin(); + for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) { + toPCL(*(it), *(jt)); + } + } + + inline + void moveToPCL(pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert) + { + pcl_vert.vertices.swap(vert.vertices); + } + + inline + void moveToPCL(std::vector &verts, std::vector &pcl_verts) + { + pcl_verts.resize(verts.size()); + std::vector::iterator it = verts.begin(); + std::vector::iterator jt = pcl_verts.begin(); + for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) { + moveToPCL(*(it), *(jt)); + } + } + + /** pcl::PolygonMesh <=> pcl_msgs::PolygonMesh **/ + + inline + void fromPCL(const pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh) + { + fromPCL(pcl_mesh.header, mesh.header); + fromPCL(pcl_mesh.cloud, mesh.cloud); + fromPCL(pcl_mesh.polygons, mesh.polygons); + } + + inline + void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh) + { + fromPCL(pcl_mesh.header, mesh.header); + moveFromPCL(pcl_mesh.cloud, mesh.cloud); + moveFromPCL(pcl_mesh.cloud, mesh.cloud); + } + + inline + void toPCL(const pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh) + { + toPCL(mesh.header, pcl_mesh.header); + toPCL(mesh.cloud, pcl_mesh.cloud); + toPCL(mesh.polygons, pcl_mesh.polygons); + } + + inline + void moveToPCL(pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh) + { + toPCL(mesh.header, pcl_mesh.header); + moveToPCL(mesh.cloud, pcl_mesh.cloud); + moveToPCL(mesh.polygons, pcl_mesh.polygons); + } } // namespace pcl_conversions namespace pcl { -/** Overload pcl::getFieldIndex **/ -inline int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name) -{ + /** Overload pcl::getFieldIndex **/ + + inline int getFieldIndex(const sensor_msgs::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)); - } + if (cloud.fields[d].name == field_name) { + return (static_cast(d)); + } } return (-1); + } -} + /** Overload pcl::getFieldsList **/ -/** Overload pcl::getFieldsList **/ -inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud) -{ + inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud) + { std::string result; for (size_t i = 0; i < cloud.fields.size () - 1; ++i) { - result += cloud.fields[i].name + " "; + result += cloud.fields[i].name + " "; } result += cloud.fields[cloud.fields.size () - 1].name; return (result); -} + } -/** Provide pcl::toROSMsg **/ + /** Provide pcl::toROSMsg **/ -inline -void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) -{ + inline + void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) + { pcl::PCLPointCloud2 pcl_cloud; pcl_conversions::toPCL(cloud, pcl_cloud); pcl::PCLImage pcl_image; pcl::toPCLPointCloud2(pcl_cloud, pcl_image); - pcl_conversions::fromPCL(pcl_image, image); -} + pcl_conversions::moveFromPCL(pcl_image, image); + } -/** Overload pcl::createMapping **/ -template -void createMapping(const std::vector& msg_fields, MsgFieldMap& field_map) -{ + inline + void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) + { + pcl::PCLPointCloud2 pcl_cloud; + pcl_conversions::moveToPCL(cloud, pcl_cloud); + pcl::PCLImage pcl_image; + pcl::toPCLPointCloud2(pcl_cloud, pcl_image); + pcl_conversions::moveFromPCL(pcl_image, image); + } + + /** Overload pcl::createMapping **/ + template + void createMapping(const std::vector& msg_fields, MsgFieldMap& field_map) + { std::vector pcl_msg_fields; pcl_conversions::toPCL(msg_fields, pcl_msg_fields); return createMapping(pcl_msg_fields, field_map); -} + } -namespace io { + namespace io { -/** Overload pcl::io::savePCDFile **/ -inline int -savePCDFile(const std::string &file_name, const sensor_msgs::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); - return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode); -} + /** Overload pcl::io::savePCDFile **/ + inline int + savePCDFile(const std::string &file_name, const sensor_msgs::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); + return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode); + } -/** Overload pcl::io::loadPCDFile **/ -inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud) -{ - pcl::PCLPointCloud2 pcl_cloud; - int ret = pcl::io::loadPCDFile(file_name, pcl_cloud); - pcl_conversions::fromPCL(pcl_cloud, cloud); - return ret; -} + inline int + destructiveSavePCDFile(const std::string &file_name, sensor_msgs::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); + return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode); + } -} // namespace io + /** Overload pcl::io::loadPCDFile **/ + inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud) + { + pcl::PCLPointCloud2 pcl_cloud; + int ret = pcl::io::loadPCDFile(file_name, pcl_cloud); + pcl_conversions::moveFromPCL(pcl_cloud, cloud); + return ret; + } + + } // namespace io + + /** Overload asdf **/ + + inline + bool concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1, + const sensor_msgs::PointCloud2 &cloud2, + sensor_msgs::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) + { + cloud_out = cloud2; + return (true); + } + else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0) + { + cloud_out = cloud1; + return (true); + } + + bool strip = false; + 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 == "_") + 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); + } + + // 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.is_dense = false; + else + cloud_out.is_dense = true; + + // We need to strip the extra padding fields + 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 == "_") + continue; + + fields2_sizes.push_back (cloud2.fields[j].count * + 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) + { + int i = 0; + 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)) + { + 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]), + fields2_sizes[j]); + ++i; // increment the field 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")) + 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); + } + } + + cloud_out.data.resize (nrpts + cloud2.data.size ()); + memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ()); + } + return (true); + } } // namespace pcl -/* - * Provide a custom serialization for pcl::PCLPointCloud2 - */ - 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 ROS_VERSION_MINIMUM(1, 3, 1) template<> struct DefaultMessageCreator { @@ -307,7 +640,6 @@ namespace ros return msg; } }; -#endif namespace message_traits { @@ -344,6 +676,9 @@ namespace ros namespace serialization { + /* + * Provide a custom serialization for pcl::PCLPointCloud2 + */ template<> struct Serializer { @@ -376,7 +711,6 @@ namespace ros std::vector pfs; stream.next(pfs); pcl_conversions::toPCL(pfs, m.fields); - stream.next(m.fields); stream.next(m.is_bigendian); stream.next(m.point_step); stream.next(m.row_step); @@ -407,6 +741,46 @@ namespace ros } }; + /* + * Provide a custom serialization for pcl::PCLPointField + */ + template<> + struct Serializer + { + template + inline static void write(Stream& stream, const pcl::PCLPointField& m) + { + stream.next(m.name); + stream.next(m.offset); + stream.next(m.datatype); + stream.next(m.count); + } + + template + inline static void read(Stream& stream, pcl::PCLPointField& m) + { + stream.next(m.name); + stream.next(m.offset); + stream.next(m.datatype); + stream.next(m.count); + } + + inline static uint32_t serializedLength(const pcl::PCLPointField& m) + { + uint32_t length = 0; + + length += serializationLength(m.name); + length += serializationLength(m.offset); + length += serializationLength(m.datatype); + length += serializationLength(m.count); + + return length; + } + }; + + /* + * Provide a custom serialization for pcl::PCLHeader + */ template<> struct Serializer { From 85d9828cd8455ff77d943e99fee8a8966a30f97c Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Jul 2013 21:49:26 -0700 Subject: [PATCH 016/405] Added more pcl::to/fromROSMsg which went missing in 1.7 --- include/pcl_conversions/pcl_conversions.h | 29 +++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 23dc3d43..7c9df674 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -472,7 +472,34 @@ namespace pcl { pcl_conversions::moveFromPCL(pcl_image, image); } + /** Provide to/fromROSMsg for sensor_msgs::PointCloud2 <=> pcl::PointCloud **/ + + template + void toROSMsg(const pcl::PointCloud &pcl_cloud, sensor_msgs::PointCloud2 &cloud) + { + pcl::PCLPointCloud2 pcl_pc2; + pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2); + pcl_conversions::moveFromPCL(pcl_pc2, cloud); + } + + template + void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + { + pcl::PCLPointCloud2 pcl_pc2; + pcl_conversions::toPCL(cloud, pcl_pc2); + pcl::toPCLPointCloud2(pcl_cloud, cloud); + } + + template + void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + { + pcl::PCLPointCloud2 pcl_pc2; + pcl_conversions::moveToPCL(cloud, pcl_pc2); + pcl::toPCLPointCloud2(pcl_cloud, cloud); + } + /** Overload pcl::createMapping **/ + template void createMapping(const std::vector& msg_fields, MsgFieldMap& field_map) { @@ -484,6 +511,7 @@ namespace pcl { namespace io { /** Overload pcl::io::savePCDFile **/ + inline int savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), @@ -507,6 +535,7 @@ namespace pcl { } /** Overload pcl::io::loadPCDFile **/ + inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud) { pcl::PCLPointCloud2 pcl_cloud; From c8e85dbe99fdcb613083b42f70fdd5ee5d350463 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Jul 2013 22:09:44 -0700 Subject: [PATCH 017/405] Adding a basic changelog --- CHANGELOG.rst | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 CHANGELOG.rst diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 00000000..5965c8a4 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pcl_conversions +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2013-07-09 21:49:26 -0700) +--------------------------------- +- Initial release +- This package is designed to allow users to more easily convert between pcl-1.7+ types and ROS message types From 2103cd06af4b14a01e176556a50069eb76ac2e37 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 10 Jul 2013 13:35:11 -0700 Subject: [PATCH 019/405] Fix find_package bug with pcl --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index d5083c25..5e151216 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,9 @@ project(pcl_conversions) find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs) +# This line can be removed once this is addressed: +# https://github.com/PointCloudLibrary/pcl/issues/184 +find_package(PCL REQUIRED) find_package(PCL REQUIRED COMPONENTS COMMON) include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) From fe71108db7887b593199a61033b54a0152fe59a7 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 10 Jul 2013 13:35:23 -0700 Subject: [PATCH 020/405] Ignore build folders --- .gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..378eac25 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build From fa4f5876d4c6cdd1b5f6794fdc944b5ced1639d4 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 10 Jul 2013 13:36:04 -0700 Subject: [PATCH 021/405] Updating changelog --- CHANGELOG.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 5965c8a4..30c3da07 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2013-07-10) +------------------ +* Fix find_package bug with pcl + 0.1.0 (2013-07-09 21:49:26 -0700) --------------------------------- - Initial release From b2eea447810004c4cbe8bc8bd4c0b25b5bbb92e6 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 10 Jul 2013 13:36:19 -0700 Subject: [PATCH 022/405] 0.1.1 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index d68fe7b0..b53d727d 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 0.1.0 + 0.1.1 Provides conversions from PCL data types and ROS message types William Woodall From a369efcaa8a375c4145419ed448390d8aa4d80b9 Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Fri, 12 Jul 2013 15:12:04 +0200 Subject: [PATCH 023/405] small fix for conversion functions --- include/pcl_conversions/pcl_conversions.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 7c9df674..b8d90a9c 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -487,7 +487,7 @@ namespace pcl { { pcl::PCLPointCloud2 pcl_pc2; pcl_conversions::toPCL(cloud, pcl_pc2); - pcl::toPCLPointCloud2(pcl_cloud, cloud); + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); } template @@ -495,7 +495,7 @@ namespace pcl { { pcl::PCLPointCloud2 pcl_pc2; pcl_conversions::moveToPCL(cloud, pcl_pc2); - pcl::toPCLPointCloud2(pcl_cloud, cloud); + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); } /** Overload pcl::createMapping **/ @@ -845,4 +845,4 @@ namespace ros } // namespace ros -#endif /* PCL_CONVERSIONS_H__ */ \ No newline at end of file +#endif /* PCL_CONVERSIONS_H__ */ From bd587dc94a3416ec1109a7d336f7e17763311ee2 Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Fri, 12 Jul 2013 15:16:17 +0200 Subject: [PATCH 024/405] update changelog --- CHANGELOG.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 30c3da07..260a05e9 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2013-07-12) +------------------ +* small fix for conversion functions + 0.1.1 (2013-07-10) ------------------ * Fix find_package bug with pcl From d4bc987feddbe47d89764a576b3c053f7d1373bd Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Fri, 12 Jul 2013 15:16:20 +0200 Subject: [PATCH 025/405] 0.1.2 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index b53d727d..3a0fa6ba 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 0.1.1 + 0.1.2 Provides conversions from PCL data types and ROS message types William Woodall From 2f1a58d4425179e6ebb5da4f949fd91d37181302 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 12 Jul 2013 13:59:30 -0700 Subject: [PATCH 026/405] Fixup tests and pcl usage in CMakeList.txt --- .gitignore | 1 + CMakeLists.txt | 10 +++------- test/test_pcl_conversions.cpp | 16 ++++++++-------- 3 files changed, 12 insertions(+), 15 deletions(-) diff --git a/.gitignore b/.gitignore index 378eac25..d0adf262 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ build +*.sublime-* diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e151216..b31ca302 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,17 +3,13 @@ project(pcl_conversions) find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs) -# This line can be removed once this is addressed: -# https://github.com/PointCloudLibrary/pcl/issues/184 -find_package(PCL REQUIRED) -find_package(PCL REQUIRED COMPONENTS COMMON) +find_package(PCL REQUIRED QUIET COMPONENTS common) -include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS}) catkin_package( - INCLUDE_DIRS include + INCLUDE_DIRS include ${PCL_COMMON_INCLUDE_DIRS} CATKIN_DEPENDS sensor_msgs std_msgs - DEPENDS pcl ) # Mark cpp header files for installation diff --git a/test/test_pcl_conversions.cpp b/test/test_pcl_conversions.cpp index e7cd99f2..ba048763 100644 --- a/test/test_pcl_conversions.cpp +++ b/test/test_pcl_conversions.cpp @@ -28,11 +28,11 @@ protected: pcl_pc2.is_dense = true; pcl_pc2.fields.resize(2); pcl_pc2.fields[0].name = "XYZ"; - pcl_pc2.fields[0].datatype = pcl_sensor_msgs::PCLPointField::INT8; + 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].datatype = pcl_sensor_msgs::PCLPointField::INT8; + pcl_pc2.fields[1].datatype = pcl::PCLPointField::INT8; pcl_pc2.fields[1].count = 3; pcl_pc2.fields[1].offset = 8 * 3; pcl_pc2.data.resize(2); @@ -40,10 +40,10 @@ protected: pcl_pc2.data[1] = 0x43; } - pcl_sensor_msgs::PCLImage pcl_image; + pcl::PCLImage pcl_image; sensor_msgs::Image image; - pcl_sensor_msgs::PCLPointCloud2 pcl_pc2; + pcl::PCLPointCloud2 pcl_pc2; sensor_msgs::PointCloud2 pc2; }; @@ -63,7 +63,7 @@ void test_image(T &image) { TEST_F(PCLConversionTests, imageConversion) { pcl_conversions::fromPCL(pcl_image, image); test_image(image); - pcl_sensor_msgs::PCLImage pcl_image2; + pcl::PCLImage pcl_image2; pcl_conversions::toPCL(image, pcl_image2); test_image(pcl_image2); } @@ -78,11 +78,11 @@ void test_pc(T &pc) { EXPECT_TRUE(pc.is_bigendian); EXPECT_TRUE(pc.is_dense); EXPECT_EQ("XYZ", pc.fields[0].name); - EXPECT_EQ(pcl_sensor_msgs::PCLPointField::INT8, pc.fields[0].datatype); + EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[0].datatype); EXPECT_EQ(3, pc.fields[0].count); EXPECT_EQ(0, pc.fields[0].offset); EXPECT_EQ("RGB", pc.fields[1].name); - EXPECT_EQ(pcl_sensor_msgs::PCLPointField::INT8, pc.fields[1].datatype); + EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[1].datatype); EXPECT_EQ(3, pc.fields[1].count); EXPECT_EQ(8 * 3, pc.fields[1].offset); EXPECT_EQ(2, pc.data.size()); @@ -93,7 +93,7 @@ void test_pc(T &pc) { TEST_F(PCLConversionTests, pointcloud2Conversion) { pcl_conversions::fromPCL(pcl_pc2, pc2); test_pc(pc2); - pcl_sensor_msgs::PCLPointCloud2 pcl_pc2_2; + pcl::PCLPointCloud2 pcl_pc2_2; pcl_conversions::toPCL(pc2, pcl_pc2_2); test_pc(pcl_pc2_2); } From 7a46755d94c3c0f416b0507431a933cb69bf2779 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 12 Jul 2013 14:10:06 -0700 Subject: [PATCH 027/405] Add missing dependency on roscpp --- CMakeLists.txt | 2 +- include/pcl_conversions/pcl_conversions.h | 29 +++++++++++------------ package.xml | 6 +++-- 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b31ca302..ee3b6e54 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(pcl_conversions) -find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs) +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs) find_package(PCL REQUIRED QUIET COMPONENTS common) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index b8d90a9c..68718175 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -512,9 +512,9 @@ namespace pcl { /** Overload pcl::io::savePCDFile **/ - inline int + inline int savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), const bool binary_mode = false) { @@ -523,9 +523,9 @@ namespace pcl { return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode); } - inline int + inline int destructiveSavePCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), const bool binary_mode = false) { @@ -579,7 +579,7 @@ namespace pcl { 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 (); @@ -602,7 +602,7 @@ namespace pcl { if (cloud2.fields[j].name == "_") continue; - fields2_sizes.push_back (cloud2.fields[j].count * + fields2_sizes.push_back (cloud2.fields[j].count * pcl::getFieldSize (cloud2.fields[j].datatype)); fields2.push_back (cloud2.fields[j]); } @@ -626,8 +626,8 @@ namespace pcl { (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 (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]), + reinterpret_cast (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]), fields2_sizes[j]); ++i; // increment the field size i } @@ -645,11 +645,10 @@ namespace pcl { // 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 ()); + 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 ()); } @@ -658,11 +657,11 @@ namespace pcl { } // namespace pcl -namespace ros +namespace ros { template<> struct DefaultMessageCreator - { + { boost::shared_ptr operator() () { boost::shared_ptr msg(new pcl::PCLPointCloud2()); @@ -670,7 +669,7 @@ namespace ros } }; - namespace message_traits + namespace message_traits { template<> struct MD5Sum @@ -680,7 +679,7 @@ namespace ros static const uint64_t static_value1 = MD5Sum::static_value1; static const uint64_t static_value2 = MD5Sum::static_value2; - + // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here. ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL); ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL); @@ -703,7 +702,7 @@ namespace ros template<> struct HasHeader : TrueType {}; } // namespace ros::message_traits - namespace serialization + namespace serialization { /* * Provide a custom serialization for pcl::PCLPointCloud2 diff --git a/package.xml b/package.xml index 3a0fa6ba..f6acbef8 100644 --- a/package.xml +++ b/package.xml @@ -14,12 +14,14 @@ catkin + pcl + roscpp sensor_msgs std_msgs - pcl + pcl + roscpp sensor_msgs std_msgs - pcl \ No newline at end of file From 93c7b52b82934582d89a6810c38fb52d93a3d82a Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Sat, 13 Jul 2013 09:21:02 +0200 Subject: [PATCH 028/405] update changelog --- CHANGELOG.rst | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 260a05e9..bd35f773 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.3 (2013-07-13) +------------------ +* Add missing dependency on roscpp +* Fixup tests and pcl usage in CMakeList.txt + 0.1.2 (2013-07-12) ------------------ * small fix for conversion functions From cb88b4f66e395a62ad941a62bd45c67be2cefae5 Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Sat, 13 Jul 2013 09:21:06 +0200 Subject: [PATCH 029/405] 0.1.3 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index f6acbef8..aaebd1bc 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 0.1.2 + 0.1.3 Provides conversions from PCL data types and ROS message types William Woodall From b4071f4aa65e9a32908b2dacbdc8e15362d1209b Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sat, 13 Jul 2013 13:19:08 -0700 Subject: [PATCH 030/405] Fixup dependencies and CMakeLists.txt I added a versioned dependency on pcl, fixes #1, and I added a dependency on pcl_msgs, fixes #2, and I wrapped the test target in a CATKIN_ENABLE_TESTING check. --- CMakeLists.txt | 10 ++++++---- package.xml | 6 ++++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ee3b6e54..1da9af75 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(pcl_conversions) -find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs) +find_package(catkin REQUIRED COMPONENTS pcl_msgs roscpp sensor_msgs std_msgs) find_package(PCL REQUIRED QUIET COMPONENTS common) @@ -9,7 +9,7 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include ${PCL_COMMON_INCLUDE_DIRS} - CATKIN_DEPENDS sensor_msgs std_msgs + CATKIN_DEPENDS pcl_msgs roscpp sensor_msgs std_msgs ) # Mark cpp header files for installation @@ -20,5 +20,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) # Add gtest based cpp test target -catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp) -target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES}) +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp) + target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES}) +endif() diff --git a/package.xml b/package.xml index aaebd1bc..fae4db33 100644 --- a/package.xml +++ b/package.xml @@ -14,12 +14,14 @@ catkin - pcl + pcl + pcl_msgs roscpp sensor_msgs std_msgs - pcl + pcl + pcl_msgs roscpp sensor_msgs std_msgs From 95c63f99803ea61ad50b969cdc88874af54c8ecd Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sat, 13 Jul 2013 13:23:04 -0700 Subject: [PATCH 031/405] Updating changelog --- CHANGELOG.rst | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index bd35f773..9aae0c06 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.4 (2013-07-13) +------------------ +* Fixup dependencies and CMakeLists.txt: + + * Added a versioned dependency on pcl, fixes `#1 `_ + * Added a dependency on pcl_msgs, fixes `#2 `_ + * Wrapped the test target in a CATKIN_ENABLE_TESTING check + 0.1.3 (2013-07-13) ------------------ * Add missing dependency on roscpp From 9e568f8d8be5998115d1c5bdc7d8f06e183deb5f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sat, 13 Jul 2013 13:23:32 -0700 Subject: [PATCH 032/405] 0.1.4 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index fae4db33..82458232 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 0.1.3 + 0.1.4 Provides conversions from PCL data types and ROS message types William Woodall From a868e1a16e442c135f66a9738619d290bc4ee896 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 27 Aug 2013 12:19:38 -0700 Subject: [PATCH 033/405] Use new pcl keys --- package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 82458232..e2e8b39a 100644 --- a/package.xml +++ b/package.xml @@ -14,13 +14,13 @@ catkin - pcl + libpcl-all-dev pcl_msgs roscpp sensor_msgs std_msgs - pcl + libpcl-all pcl_msgs roscpp sensor_msgs From 85eeb2944d79ce9dd1c1f01087f2404214dad852 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 27 Aug 2013 12:20:44 -0700 Subject: [PATCH 034/405] Updating changelog --- CHANGELOG.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 9aae0c06..ed1d9478 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.5 (2013-08-27) +------------------ +* Use new pcl rosdep keys (libpcl-all and libpcl-all-dev) + 0.1.4 (2013-07-13) ------------------ * Fixup dependencies and CMakeLists.txt: From 5b52fedea20b383f19e82e1fcc892e7187782476 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 27 Aug 2013 12:20:49 -0700 Subject: [PATCH 035/405] 0.1.5 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index e2e8b39a..d2fc1bd7 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 0.1.4 + 0.1.5 Provides conversions from PCL data types and ROS message types William Woodall From ad560f262661a74a576e019546d15eb3173fad62 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Fri, 28 Feb 2014 19:03:17 -0600 Subject: [PATCH 036/405] Make pcl_conversions run_depend on libpcl-all-dev When downstream projects build against pcl_conversions, they need the pcl headers provided by libpcl-all-dev. --- package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index d2fc1bd7..4e5f7e50 100644 --- a/package.xml +++ b/package.xml @@ -21,9 +21,10 @@ std_msgs libpcl-all + libpcl-all-dev pcl_msgs roscpp sensor_msgs std_msgs - \ No newline at end of file + From 388c0dc0ff1c0fa5bfed38ae98a72d3a90d57fa4 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 8 Apr 2014 22:34:30 +0000 Subject: [PATCH 037/405] fix Eigen dependency --- CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1da9af75..5538c471 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,11 +1,12 @@ cmake_minimum_required(VERSION 2.8.3) project(pcl_conversions) -find_package(catkin REQUIRED COMPONENTS pcl_msgs roscpp sensor_msgs std_msgs) +find_package(catkin REQUIRED COMPONENTS pcl_msgs roscpp sensor_msgs std_msgs cmake_modules) find_package(PCL REQUIRED QUIET COMPONENTS common) +find_package(Eigen REQUIRED) -include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include ${PCL_COMMON_INCLUDE_DIRS} From e1ba69bfcaabfa7e964d883d1dc37d8078d04bf0 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 8 Apr 2014 22:34:55 +0000 Subject: [PATCH 038/405] update maintainer info, add eigen dependency --- package.xml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/package.xml b/package.xml index 4e5f7e50..eaf1dfc0 100644 --- a/package.xml +++ b/package.xml @@ -3,17 +3,19 @@ pcl_conversions 0.1.5 Provides conversions from PCL data types and ROS message types - William Woodall + + William Woodall + Paul Bovbel BSD + http://wiki.ros.org/pcl_conversions https://github.com/ros-perception/pcl_conversions https://github.com/ros-perception/pcl_conversions/issues - William Woodall - catkin + cmake_modules libpcl-all-dev pcl_msgs roscpp From 7dc1ae60b0af203ec5d2ccbc1e70541d3ca898ba Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Thu, 27 Feb 2014 15:22:42 -0800 Subject: [PATCH 039/405] Added conversions for stamp types --- include/pcl_conversions/pcl_conversions.h | 34 +++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 68718175..6834cae8 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -75,10 +75,40 @@ namespace pcl_conversions { /** PCLHeader <=> Header **/ + inline + void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp) + { + stamp.fromNSec(pcl_stamp * 1e3); // Convert from us to ns + } + + inline + void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp) + { + pcl_stamp = stamp.toNSec() / 1e3; // Convert from ns to us + } + + inline + ros::Time fromPCL(const pcl::uint64_t &pcl_stamp) + { + ros::Time stamp; + fromPCL(pcl_stamp, stamp); + return stamp; + } + + inline + pcl::uint64_t toPCL(const ros::Time &stamp) + { + pcl::uint64_t pcl_stamp; + toPCL(stamp, pcl_stamp); + return pcl_stamp; + } + + /** PCLHeader <=> Header **/ + inline void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header) { - header.stamp.fromNSec(pcl_header.stamp * 1e3); // Convert from us to ns + fromPCL(pcl_header.stamp, header.stamp); header.seq = pcl_header.seq; header.frame_id = pcl_header.frame_id; } @@ -86,7 +116,7 @@ namespace pcl_conversions { inline void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) { - pcl_header.stamp = header.stamp.toNSec() / 1e3; // Convert from ns to us + toPCL(header.stamp, pcl_header.stamp); pcl_header.seq = header.seq; pcl_header.frame_id = header.frame_id; } From 6619fbde8fdb346c2a7607b66a186672053e0c9e Mon Sep 17 00:00:00 2001 From: agentx3r Date: Thu, 10 Apr 2014 14:37:09 +0000 Subject: [PATCH 040/405] update maintainer info and changelog --- CHANGELOG.rst | 8 ++++++++ package.xml | 1 + 2 files changed, 9 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index ed1d9478..26cdf29b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added conversions for stamp types +* update maintainer info, add eigen dependency +* fix Eigen dependency +* Make pcl_conversions run_depend on libpcl-all-dev +* Contributors: Brice Rebsamen, Paul Bovbel, Scott K Logan, William Woodall + 0.1.5 (2013-08-27) ------------------ * Use new pcl rosdep keys (libpcl-all and libpcl-all-dev) diff --git a/package.xml b/package.xml index eaf1dfc0..16b0f931 100644 --- a/package.xml +++ b/package.xml @@ -6,6 +6,7 @@ William Woodall Paul Bovbel + Bill Morris BSD From 04d919f5bddaa419da8053cce6109947affdcf50 Mon Sep 17 00:00:00 2001 From: agentx3r Date: Thu, 10 Apr 2014 14:37:18 +0000 Subject: [PATCH 041/405] 0.2.0 --- CHANGELOG.rst | 4 ++-- package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 26cdf29b..b5ad8d3e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2014-04-10) +------------------ * Added conversions for stamp types * update maintainer info, add eigen dependency * fix Eigen dependency diff --git a/package.xml b/package.xml index 16b0f931..1eedfa98 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 0.1.5 + 0.2.0 Provides conversions from PCL data types and ROS message types William Woodall From d080cd78ac14481f580a6361a6afccbeb3c4a6b8 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Tue, 27 May 2014 12:53:08 -0700 Subject: [PATCH 042/405] add pcl::PointCloud to Image msg converter for extracting the rgb component of a cloud --- include/pcl_conversions/pcl_conversions.h | 31 +++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 6834cae8..3da01eaf 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2013, Open Source Robotics Foundation, Inc. + * Copyright (c) 2010-2012, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -502,6 +503,36 @@ namespace pcl { pcl_conversions::moveFromPCL(pcl_image, image); } + template void + toROSMsg (const pcl::PointCloud &cloud, sensor_msgs::Image& msg) + { + // Ease the user's burden on specifying width/height for unorganized datasets + if (cloud.width == 0 && cloud.height == 0) + { + throw std::runtime_error("Needs to be a dense like cloud!!"); + } + else + { + if (cloud.points.size () != cloud.width * cloud.height) + throw std::runtime_error("The width and height do not match the cloud size!"); + msg.height = cloud.height; + msg.width = cloud.width; + } + + // sensor_msgs::image_encodings::BGR8; + msg.encoding = "bgr8"; + msg.step = msg.width * sizeof (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++) + { + uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); + memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t)); + } + } + } + /** Provide to/fromROSMsg for sensor_msgs::PointCloud2 <=> pcl::PointCloud **/ template From 2c572253700a4353efbe33b9982cb12affd888c7 Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Wed, 11 Feb 2015 11:29:33 -0800 Subject: [PATCH 043/405] Added a test for rounding errors in stamp conversion for some values the test fails. This is because of rounding errors. See #14. --- test/test_pcl_conversions.cpp | 43 +++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/test/test_pcl_conversions.cpp b/test/test_pcl_conversions.cpp index ba048763..c07720a7 100644 --- a/test/test_pcl_conversions.cpp +++ b/test/test_pcl_conversions.cpp @@ -100,6 +100,49 @@ TEST_F(PCLConversionTests, pointcloud2Conversion) { } // namespace + +struct StampTestData +{ + const ros::Time stamp_; + ros::Time stamp2_; + + explicit StampTestData(const ros::Time &stamp) + : stamp_(stamp) + { + pcl::uint64_t pcl_stamp; + pcl_conversions::toPCL(stamp_, pcl_stamp); + pcl_conversions::fromPCL(pcl_stamp, stamp2_); + } +}; + +TEST(PCLConversionStamp, Stamps) +{ + { + const StampTestData d(ros::Time(1.000001)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } + + { + const StampTestData d(ros::Time(1.999999)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } + + { + const StampTestData d(ros::Time(1.999)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } + + { + const StampTestData d(ros::Time(1423680574, 746000000)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } + + { + const StampTestData d(ros::Time(1423680629, 901000000)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } +} + int main(int argc, char **argv) { try { ::testing::InitGoogleTest(&argc, argv); From d1b27af512256176d1a50564dfe41976f9eefb4e Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Wed, 11 Feb 2015 11:36:56 -0800 Subject: [PATCH 044/405] Conversion in integral precision fixes #14 --- include/pcl_conversions/pcl_conversions.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 3da01eaf..44c4381b 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -79,13 +79,13 @@ namespace pcl_conversions { inline void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp) { - stamp.fromNSec(pcl_stamp * 1e3); // Convert from us to ns + stamp.fromNSec(pcl_stamp * 1000ull); // Convert from us to ns } inline void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp) { - pcl_stamp = stamp.toNSec() / 1e3; // Convert from ns to us + pcl_stamp = stamp.toNSec() / 1000ull; // Convert from ns to us } inline From 6eef12333ffb458bc2ca179fac81b0c0091e96fb Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 8 Jun 2015 09:02:46 -0400 Subject: [PATCH 045/405] Update changelogs --- CHANGELOG.rst | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b5ad8d3e..8e81f97a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added a test for rounding errors in stamp conversion + for some values the test fails. +* add pcl::PointCloud to Image msg converter for extracting the rgb component of a cloud +* Contributors: Brice Rebsamen, Lucid One, Michael Ferguson, Paul Bovbel + 0.2.0 (2014-04-10) ------------------ * Added conversions for stamp types From 98d99aa91c371d9f09fbe224ee527fc8b6affa63 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 8 Jun 2015 09:03:46 -0400 Subject: [PATCH 046/405] 0.2.1 --- CHANGELOG.rst | 4 ++-- package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 8e81f97a..1bce5a49 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2015-06-08) +------------------ * Added a test for rounding errors in stamp conversion for some values the test fails. * add pcl::PointCloud to Image msg converter for extracting the rgb component of a cloud diff --git a/package.xml b/package.xml index 1eedfa98..9ba4f0ed 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 0.2.0 + 0.2.1 Provides conversions from PCL data types and ROS message types William Woodall From 3f250963987424fad9be4196f6b71aace198fd7b Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 27 Oct 2016 11:00:13 -0400 Subject: [PATCH 047/405] Remove duplicated line --- include/pcl_conversions/pcl_conversions.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/pcl_conversions/pcl_conversions.h b/include/pcl_conversions/pcl_conversions.h index 44c4381b..9fdf988a 100644 --- a/include/pcl_conversions/pcl_conversions.h +++ b/include/pcl_conversions/pcl_conversions.h @@ -433,7 +433,6 @@ namespace pcl_conversions { { fromPCL(pcl_mesh.header, mesh.header); moveFromPCL(pcl_mesh.cloud, mesh.cloud); - moveFromPCL(pcl_mesh.cloud, mesh.cloud); } inline From 49ccc0599a082d38be527be1071174ed9098eb88 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 30 Apr 2018 10:58:35 -0400 Subject: [PATCH 048/405] Drop deprecated Eigen cmake module --- CMakeLists.txt | 6 +++--- package.xml | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5538c471..60a6ee14 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 2.8.3) project(pcl_conversions) -find_package(catkin REQUIRED COMPONENTS pcl_msgs roscpp sensor_msgs std_msgs cmake_modules) +find_package(catkin REQUIRED COMPONENTS pcl_msgs roscpp sensor_msgs std_msgs) find_package(PCL REQUIRED QUIET COMPONENTS common) -find_package(Eigen REQUIRED) +find_package(Eigen3 REQUIRED) -include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include ${PCL_COMMON_INCLUDE_DIRS} diff --git a/package.xml b/package.xml index 9ba4f0ed..79227a0a 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + pcl_conversions 0.2.1 Provides conversions from PCL data types and ROS message types @@ -16,7 +16,8 @@ catkin - cmake_modules + eigen + libpcl-all-dev pcl_msgs roscpp From 4bf3478d04feac315cbf0103908d26171af35d2e Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 30 Apr 2018 11:10:34 -0400 Subject: [PATCH 049/405] Fix up PCL libraries --- CMakeLists.txt | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 60a6ee14..61e72446 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,25 +3,26 @@ project(pcl_conversions) find_package(catkin REQUIRED COMPONENTS pcl_msgs roscpp sensor_msgs std_msgs) -find_package(PCL REQUIRED QUIET COMPONENTS common) +find_package(PCL REQUIRED COMPONENTS common io) find_package(Eigen3 REQUIRED) -include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS}) - catkin_package( - INCLUDE_DIRS include ${PCL_COMMON_INCLUDE_DIRS} + INCLUDE_DIRS include CATKIN_DEPENDS pcl_msgs roscpp sensor_msgs std_msgs + DEPENDS Eigen3 PCL ) -# Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE ) -# Add gtest based cpp test target if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp) - target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES}) + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS}) + + catkin_add_gtest(test_pcl_conversions test/test_pcl_conversions.cpp) + target_link_libraries(test_pcl_conversions ${catkin_LIBRARIES}) endif() From bdecc4af0d333224523316fc4a5314ea30b2d744 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 30 Apr 2018 11:11:51 -0400 Subject: [PATCH 050/405] Make dependencies transitive and test dependencies --- package.xml | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/package.xml b/package.xml index 79227a0a..d33d5175 100644 --- a/package.xml +++ b/package.xml @@ -16,19 +16,18 @@ catkin - eigen + eigen + libpcl-all-dev + pcl_msgs + roscpp + sensor_msgs + std_msgs - libpcl-all-dev - pcl_msgs - roscpp - sensor_msgs - std_msgs - - libpcl-all - libpcl-all-dev - pcl_msgs - roscpp - sensor_msgs - std_msgs + eigen + libpcl-all-dev + pcl_msgs + roscpp + sensor_msgs + std_msgs From 693a18feb0db4cd1640df2fec5295bb51e1a4a23 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 30 Apr 2018 11:34:36 -0400 Subject: [PATCH 051/405] Move to subdirectory --- .gitignore | 2 -- CHANGELOG.rst => pcl_conversions/CHANGELOG.rst | 0 CMakeLists.txt => pcl_conversions/CMakeLists.txt | 0 README.rst => pcl_conversions/README.rst | 0 .../include}/pcl_conversions/pcl_conversions.h | 0 package.xml => pcl_conversions/package.xml | 0 {test => pcl_conversions/test}/test_pcl_conversions.cpp | 0 7 files changed, 2 deletions(-) delete mode 100644 .gitignore rename CHANGELOG.rst => pcl_conversions/CHANGELOG.rst (100%) rename CMakeLists.txt => pcl_conversions/CMakeLists.txt (100%) rename README.rst => pcl_conversions/README.rst (100%) rename {include => pcl_conversions/include}/pcl_conversions/pcl_conversions.h (100%) rename package.xml => pcl_conversions/package.xml (100%) rename {test => pcl_conversions/test}/test_pcl_conversions.cpp (100%) diff --git a/.gitignore b/.gitignore deleted file mode 100644 index d0adf262..00000000 --- a/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -build -*.sublime-* diff --git a/CHANGELOG.rst b/pcl_conversions/CHANGELOG.rst similarity index 100% rename from CHANGELOG.rst rename to pcl_conversions/CHANGELOG.rst diff --git a/CMakeLists.txt b/pcl_conversions/CMakeLists.txt similarity index 100% rename from CMakeLists.txt rename to pcl_conversions/CMakeLists.txt diff --git a/README.rst b/pcl_conversions/README.rst similarity index 100% rename from README.rst rename to pcl_conversions/README.rst diff --git a/include/pcl_conversions/pcl_conversions.h b/pcl_conversions/include/pcl_conversions/pcl_conversions.h similarity index 100% rename from include/pcl_conversions/pcl_conversions.h rename to pcl_conversions/include/pcl_conversions/pcl_conversions.h diff --git a/package.xml b/pcl_conversions/package.xml similarity index 100% rename from package.xml rename to pcl_conversions/package.xml diff --git a/test/test_pcl_conversions.cpp b/pcl_conversions/test/test_pcl_conversions.cpp similarity index 100% rename from test/test_pcl_conversions.cpp rename to pcl_conversions/test/test_pcl_conversions.cpp From adf647f14dd7826a08680c4e8bd543101cc1d8db Mon Sep 17 00:00:00 2001 From: rusu Date: Wed, 5 Sep 2012 23:29:36 +0000 Subject: [PATCH 052/405] added PCL 1.6 stac for Groovy --- CMakeLists.txt | 19 ++ Makefile | 1 + pcl/CMakeLists.txt | 8 + pcl/Makefile | 57 ++++ pcl/cmake/FindEigen.cmake | 81 ++++++ pcl/manifest.xml | 33 +++ pcl/msg/ModelCoefficients.msg | 3 + pcl/msg/PointIndices.msg | 3 + pcl/msg/PolygonMesh.msg | 6 + pcl/msg/Vertices.msg | 2 + pcl_ros/CMakeLists.txt | 21 ++ pcl_ros/Makefile | 1 + pcl_ros/cmake/FindEigen.cmake | 81 ++++++ pcl_ros/include/pcl_ros/impl/transforms.hpp | 216 +++++++++++++++ pcl_ros/include/pcl_ros/point_cloud.h | 256 ++++++++++++++++++ pcl_ros/include/pcl_ros/publisher.h | 146 ++++++++++ pcl_ros/include/pcl_ros/transforms.h | 167 ++++++++++++ pcl_ros/manifest.xml | 37 +++ pcl_ros/src/pcl_ros/transforms.cpp | 280 ++++++++++++++++++++ rosdep.yaml | 32 +++ stack.xml | 17 ++ 21 files changed, 1467 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 Makefile create mode 100644 pcl/CMakeLists.txt create mode 100644 pcl/Makefile create mode 100644 pcl/cmake/FindEigen.cmake create mode 100644 pcl/manifest.xml create mode 100644 pcl/msg/ModelCoefficients.msg create mode 100644 pcl/msg/PointIndices.msg create mode 100644 pcl/msg/PolygonMesh.msg create mode 100644 pcl/msg/Vertices.msg create mode 100644 pcl_ros/CMakeLists.txt create mode 100644 pcl_ros/Makefile create mode 100644 pcl_ros/cmake/FindEigen.cmake create mode 100644 pcl_ros/include/pcl_ros/impl/transforms.hpp create mode 100644 pcl_ros/include/pcl_ros/point_cloud.h create mode 100644 pcl_ros/include/pcl_ros/publisher.h create mode 100644 pcl_ros/include/pcl_ros/transforms.h create mode 100644 pcl_ros/manifest.xml create mode 100644 pcl_ros/src/pcl_ros/transforms.cpp create mode 100644 rosdep.yaml create mode 100644 stack.xml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..15d597ea --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +set(ROSPACK_MAKEDIST true) + +# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of +# directories (or patterns, but directories should suffice) that should +# be excluded from the distro. This is not the place to put things that +# should be ignored everywhere, like "build" directories; that happens in +# rosbuild/rosbuild.cmake. Here should be listed packages that aren't +# ready for inclusion in a distro. +# +# This list is combined with the list in rosbuild/rosbuild.cmake. Note +# that CMake 2.6 may be required to ensure that the two lists are combined +# properly. CMake 2.4 seems to have unpredictable scoping rules for such +# variables. +#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) + +rosbuild_make_distribution(1.6.0) diff --git a/Makefile b/Makefile new file mode 100644 index 00000000..a818ccad --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file diff --git a/pcl/CMakeLists.txt b/pcl/CMakeLists.txt new file mode 100644 index 00000000..0853d403 --- /dev/null +++ b/pcl/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +find_package(Eigen REQUIRED) +include_directories(${Eigen_INCLUDE_DIRS}) +rosbuild_init() +rosbuild_genmsg() + diff --git a/pcl/Makefile b/pcl/Makefile new file mode 100644 index 00000000..f42463cf --- /dev/null +++ b/pcl/Makefile @@ -0,0 +1,57 @@ +all: installed + +SVN_DIR = build/pcl_1.6.0 +# Developers, please use this URL: +SVN_URL = http://svn.pointclouds.org/pcl/tags/pcl-1.6.0 # For the very latest version + +ifeq ($(strip $(SVN_CMDLINE)),) +SVN_CMDLINE = svn +endif + +$(SVN_DIR): + $(SVN_CMDLINE) co $(SVN_REVISION) $(SVN_URL) $(SVN_DIR) +ifneq ($(strip $(SVN_PATCH)),) + cd $(SVN_DIR) && patch -p0 < ../$(SVN_PATCH) +endif + cd $(SVN_DIR) && \ + sed -Ei 's/-Wold-style-cast/-Wno-old-style-cast -Wno-unused-parameter -Wno-conversion/g' ./CMakeLists.txt && \ + sed -Ei 's/${INCLUDE_INSTALL_ROOT}\/pcl/${INCLUDE_INSTALL_ROOT}\/pcl$(PCL_VERSION)/g' ./cmake/pcl_utils.cmake + +SVN_UP: $(SVN_DIR) + cd $(SVN_DIR) && $(SVN_CMDLINE) up $(SVN_REVISION) + +download: $(SVN_UP) + +installed: $(SVN_DIR) cleaned + mkdir -p msg/build && cd msg/build && cmake ../.. && make && cd - + cd $(SVN_DIR) && mkdir -p build && cd build && \ + rm -rf ../common/include/sensor_msgs ../common/include/std_msgs \ + ../common/include/pcl$(PCL_VERSION)/ModelCoefficients.h ../common/include/pcl$(PCL_VERSION)/PointIndices.h ../common/include/pcl$(PCL_VERSION)/PolygonMesh.h ../common/include/pcl$(PCL_VERSION)/Vertices.h && \ + export CPATH="`rospack cflags-only-I sensor_msgs`:`rospack cflags-only-I roscpp_serialization`:`rospack cflags-only-I cpp_common`:`rospack cflags-only-I rostime`:`rospack cflags-only-I roscpp_traits`:`rospack cflags-only-I roscpp`:`rospack cflags-only-I rosconsole`:`rospack cflags-only-I std_msgs`:`rospack cflags-only-I sensor_msgs`:`rospack find pcl$(PCL_VERSION)`/msg_gen/cpp/include:$$CPATH" && \ + export LD_LIBRARY_PATH="`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:`rospack libs-only-L roscpp_serialization`:`rospack libs-only-L cpp_common`:`rospack libs-only-L rostime`:`rospack libs-only-L roscpp_traits`:`rospack libs-only-L roscpp`:`rospack libs-only-L rosconsole`:`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:$$LD_LIBRARY_PATH" && \ + export LIBRARY_PATH="`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:`rospack libs-only-L roscpp_serialization`:`rospack libs-only-L cpp_common`:`rospack libs-only-L rostime`:`rospack libs-only-L roscpp_traits`:`rospack libs-only-L roscpp`:`rospack libs-only-L rosconsole`:`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:$$LIBRARY_PATH" && \ + cmake -DCMAKE_INSTALL_PREFIX=`pwd`/../../.. \ + -DCMAKE_BUILD_TYPE=Release \ + -DUSE_ROS=ON \ + -DBUILD_OPENNI=ON \ + -DBUILD_TESTS=OFF \ + -DBUILD_apps=OFF \ + -DBUILD_examples=OFF \ + -DCMAKE_VERBOSE_MAKEFILE=OFF \ + .. && \ + make ${ROS_PARALLEL_JOBS} install + touch installed + +cleaned: Makefile + make clean + touch cleaned + +clean: + -rm -rf $(SVN_DIR)/build rospack_nosubdirs patched installed include bin lib64 msg_gen src *~ + +wiped: Makefile + make wipe + touch wiped + +wipe: clean + rm -rf build cleaned msg/build diff --git a/pcl/cmake/FindEigen.cmake b/pcl/cmake/FindEigen.cmake new file mode 100644 index 00000000..2666481c --- /dev/null +++ b/pcl/cmake/FindEigen.cmake @@ -0,0 +1,81 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN_FOUND - system has eigen lib with correct version +# EIGEN_INCLUDE_DIR - the eigen include directory +# EIGEN_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen_FIND_VERSION) + if(NOT Eigen_FIND_VERSION_MAJOR) + set(Eigen_FIND_VERSION_MAJOR 2) + endif(NOT Eigen_FIND_VERSION_MAJOR) + if(NOT Eigen_FIND_VERSION_MINOR) + set(Eigen_FIND_VERSION_MINOR 91) + endif(NOT Eigen_FIND_VERSION_MINOR) + if(NOT Eigen_FIND_VERSION_PATCH) + set(Eigen_FIND_VERSION_PATCH 0) + endif(NOT Eigen_FIND_VERSION_PATCH) + + set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") +endif(NOT Eigen_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) + if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + set(EIGEN_VERSION_OK FALSE) + else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + set(EIGEN_VERSION_OK TRUE) + endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + + if(NOT EIGEN_VERSION_OK) + + message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " + "but at least version ${Eigen_FIND_VERSION} is required") + endif(NOT EIGEN_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN_INCLUDE_DIRS) + + # in cache already + _eigen3_check_version() + set(EIGEN_FOUND ${EIGEN_VERSION_OK}) + +else () + + find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) + + mark_as_advanced(EIGEN_INCLUDE_DIR) + SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") + +endif() diff --git a/pcl/manifest.xml b/pcl/manifest.xml new file mode 100644 index 00000000..1f1ea64d --- /dev/null +++ b/pcl/manifest.xml @@ -0,0 +1,33 @@ + + + +

+ PCL - Point Cloud Library: a comprehensive open + source library for n-D Point Clouds and 3D geometry processing. + The library contains numerous state-of-the art algorithms for: filtering, + feature estimation, surface reconstruction, registration, model fitting and + segmentation, etc. +

+ + + + See http://pcl.ros.org/authors for the complete list of authors. + BSD + http://pointclouds.org + + + + + + + + + + + + + + + + + diff --git a/pcl/msg/ModelCoefficients.msg b/pcl/msg/ModelCoefficients.msg new file mode 100644 index 00000000..8d3f9b89 --- /dev/null +++ b/pcl/msg/ModelCoefficients.msg @@ -0,0 +1,3 @@ +Header header +float32[] values + diff --git a/pcl/msg/PointIndices.msg b/pcl/msg/PointIndices.msg new file mode 100644 index 00000000..007c2900 --- /dev/null +++ b/pcl/msg/PointIndices.msg @@ -0,0 +1,3 @@ +Header header +int32[] indices + diff --git a/pcl/msg/PolygonMesh.msg b/pcl/msg/PolygonMesh.msg new file mode 100644 index 00000000..8eeb5a4c --- /dev/null +++ b/pcl/msg/PolygonMesh.msg @@ -0,0 +1,6 @@ +# Separate header for the polygonal surface +Header header +# Vertices of the mesh as a point cloud +sensor_msgs/PointCloud2 cloud +# List of polygons +Vertices[] polygons diff --git a/pcl/msg/Vertices.msg b/pcl/msg/Vertices.msg new file mode 100644 index 00000000..6b7c72a0 --- /dev/null +++ b/pcl/msg/Vertices.msg @@ -0,0 +1,2 @@ +# List of point indices +uint32[] vertices diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt new file mode 100644 index 00000000..400c76e4 --- /dev/null +++ b/pcl_ros/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required (VERSION 2.4.6) + +include ($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +rosbuild_init () +rosbuild_add_boost_directories () +add_definitions (-DEIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET) +rosbuild_check_for_sse () + +set (EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +include_directories (${CMAKE_CURRENT_BINARY_DIR}) +include_directories (src) + +# Uses Eigen +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +find_package(Eigen REQUIRED) +include_directories(${Eigen_INCLUDE_DIRS}) +include_directories(${EIGEN_INCLUDE_DIRS}) + +# ---[ Point Cloud Library - Transforms +rosbuild_add_library (pcl_ros_tf src/pcl_ros/transforms.cpp) +rosbuild_add_compile_flags (pcl_ros_tf ${SSE_FLAGS}) diff --git a/pcl_ros/Makefile b/pcl_ros/Makefile new file mode 100644 index 00000000..bbd3fc60 --- /dev/null +++ b/pcl_ros/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk diff --git a/pcl_ros/cmake/FindEigen.cmake b/pcl_ros/cmake/FindEigen.cmake new file mode 100644 index 00000000..2666481c --- /dev/null +++ b/pcl_ros/cmake/FindEigen.cmake @@ -0,0 +1,81 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN_FOUND - system has eigen lib with correct version +# EIGEN_INCLUDE_DIR - the eigen include directory +# EIGEN_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen_FIND_VERSION) + if(NOT Eigen_FIND_VERSION_MAJOR) + set(Eigen_FIND_VERSION_MAJOR 2) + endif(NOT Eigen_FIND_VERSION_MAJOR) + if(NOT Eigen_FIND_VERSION_MINOR) + set(Eigen_FIND_VERSION_MINOR 91) + endif(NOT Eigen_FIND_VERSION_MINOR) + if(NOT Eigen_FIND_VERSION_PATCH) + set(Eigen_FIND_VERSION_PATCH 0) + endif(NOT Eigen_FIND_VERSION_PATCH) + + set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") +endif(NOT Eigen_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) + if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + set(EIGEN_VERSION_OK FALSE) + else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + set(EIGEN_VERSION_OK TRUE) + endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + + if(NOT EIGEN_VERSION_OK) + + message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " + "but at least version ${Eigen_FIND_VERSION} is required") + endif(NOT EIGEN_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN_INCLUDE_DIRS) + + # in cache already + _eigen3_check_version() + set(EIGEN_FOUND ${EIGEN_VERSION_OK}) + +else () + + find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) + + mark_as_advanced(EIGEN_INCLUDE_DIR) + SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") + +endif() diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp new file mode 100644 index 00000000..26fcbc3b --- /dev/null +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -0,0 +1,216 @@ +/* + * 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. + * + * + */ + +#ifndef pcl_ros_IMPL_TRANSFORMS_H_ +#define pcl_ros_IMPL_TRANSFORMS_H_ + +#include "pcl_ros/transforms.h" + +namespace pcl_ros +{ +////////////////////////////////////////////////////////////////////////////////////////////// +template void +transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, const tf::Transform &transform) +{ + // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering + // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but + // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. + // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to + // the conversion of the point cloud anyway. Idem for the origin. + tf::Quaternion q = transform.getRotation (); + Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w) + tf::Vector3 v = transform.getOrigin (); + Eigen::Vector3f origin (v.x (), v.y (), v.z ()); + // Eigen::Translation3f translation(v); + // Assemble an Eigen Transform + //Eigen::Transform3f t; + //t = translation * rotation; + transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, const tf::Transform &transform) +{ + // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering + // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but + // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. + // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to + // the conversion of the point cloud anyway. Idem for the origin. + tf::Quaternion q = transform.getRotation (); + Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w) + tf::Vector3 v = transform.getOrigin (); + Eigen::Vector3f origin (v.x (), v.y (), v.z ()); + // Eigen::Translation3f translation(v); + // Assemble an Eigen Transform + //Eigen::Transform3f t; + //t = translation * rotation; + transformPointCloud (cloud_in, cloud_out, origin, rotation); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +transformPointCloudWithNormals (const std::string &target_frame, + const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const tf::TransformListener &tf_listener) +{ + if (cloud_in.header.frame_id == target_frame) + { + cloud_out = cloud_in; + return (true); + } + + tf::StampedTransform transform; + try + { + tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, cloud_in.header.stamp, transform); + } + catch (tf::LookupException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + catch (tf::ExtrapolationException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + + transformPointCloudWithNormals (cloud_in, cloud_out, transform); + cloud_out.header.frame_id = target_frame; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +transformPointCloud (const std::string &target_frame, + const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const tf::TransformListener &tf_listener) +{ + if (cloud_in.header.frame_id == target_frame) + { + cloud_out = cloud_in; + return (true); + } + + tf::StampedTransform transform; + try + { + tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, cloud_in.header.stamp, transform); + } + catch (tf::LookupException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + catch (tf::ExtrapolationException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + transformPointCloud (cloud_in, cloud_out, transform); + cloud_out.header.frame_id = target_frame; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +transformPointCloud (const std::string &target_frame, + const ros::Time & target_time, + const pcl::PointCloud &cloud_in, + const std::string &fixed_frame, + pcl::PointCloud &cloud_out, + const tf::TransformListener &tf_listener) +{ + tf::StampedTransform transform; + try + { + tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, cloud_in.header.stamp, fixed_frame, transform); + } + catch (tf::LookupException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + catch (tf::ExtrapolationException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + + transformPointCloud (cloud_in, cloud_out, transform); + cloud_out.header.frame_id = target_frame; + cloud_out.header.stamp = target_time; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +transformPointCloudWithNormals (const std::string &target_frame, + const ros::Time & target_time, + const pcl::PointCloud &cloud_in, + const std::string &fixed_frame, + pcl::PointCloud &cloud_out, + const tf::TransformListener &tf_listener) +{ + tf::StampedTransform transform; + try + { + tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, cloud_in.header.stamp, fixed_frame, transform); + } + catch (tf::LookupException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + catch (tf::ExtrapolationException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + + transformPointCloudWithNormals (cloud_in, cloud_out, transform); + cloud_out.header.frame_id = target_frame; + cloud_out.header.stamp = target_time; + return (true); +} + +} // namespace pcl_ros +#endif diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h new file mode 100644 index 00000000..b0dfe433 --- /dev/null +++ b/pcl_ros/include/pcl_ros/point_cloud.h @@ -0,0 +1,256 @@ +#ifndef pcl_ROS_POINT_CLOUD_H_ +#define pcl_ROS_POINT_CLOUD_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace detail + { + template + struct FieldStreamer + { + FieldStreamer(Stream& stream) : stream_(stream) {} + + template void operator() () + { + const char* name = traits::name::value; + uint32_t name_length = strlen(name); + stream_.next(name_length); + if (name_length > 0) + memcpy(stream_.advance(name_length), name, name_length); + + uint32_t offset = traits::offset::value; + stream_.next(offset); + + uint8_t datatype = traits::datatype::value; + stream_.next(datatype); + + uint32_t count = traits::datatype::size; + stream_.next(count); + } + + Stream& stream_; + }; + + template + struct FieldsLength + { + FieldsLength() : length(0) {} + + template void operator() () + { + uint32_t name_length = strlen(traits::name::value); + length += name_length + 13; + } + + uint32_t length; + }; + } // namespace pcl::detail +} // namespace pcl + +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 ROS_VERSION_MINIMUM(1, 3, 1) + template + struct DefaultMessageCreator > + { + boost::shared_ptr mapping_; + + DefaultMessageCreator() + : mapping_( boost::make_shared() ) + { + } + + boost::shared_ptr > operator() () + { + boost::shared_ptr > msg (new pcl::PointCloud ()); + pcl::detail::getMapping(*msg) = mapping_; + return msg; + } + }; +#endif + + namespace message_traits + { + template struct MD5Sum > + { + static const char* value() { return MD5Sum::value(); } + static const char* value(const pcl::PointCloud&) { return value(); } + + static const uint64_t static_value1 = MD5Sum::static_value1; + static const uint64_t static_value2 = MD5Sum::static_value2; + + // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here. + ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL); + ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL); + }; + + template struct DataType > + { + static const char* value() { return DataType::value(); } + static const char* value(const pcl::PointCloud&) { return value(); } + }; + + template struct Definition > + { + static const char* value() { return Definition::value(); } + static const char* value(const pcl::PointCloud&) { return value(); } + }; + + template struct HasHeader > : TrueType {}; + } // namespace ros::message_traits + + namespace serialization + { + template + struct Serializer > + { + template + inline static void write(Stream& stream, const pcl::PointCloud& m) + { + stream.next(m.header); + + // Ease the user's burden on specifying width/height for unorganized datasets + uint32_t height = m.height, width = m.width; + if (height == 0 && width == 0) { + width = m.points.size(); + height = 1; + } + stream.next(height); + stream.next(width); + + // Stream out point field metadata + typedef typename pcl::traits::fieldList::type FieldList; + uint32_t fields_size = boost::mpl::size::value; + stream.next(fields_size); + pcl::for_each_type(pcl::detail::FieldStreamer(stream)); + + // Assume little-endian... + uint8_t is_bigendian = false; + stream.next(is_bigendian); + + // Write out point data as binary blob + uint32_t point_step = sizeof(T); + stream.next(point_step); + uint32_t row_step = point_step * width; + stream.next(row_step); + uint32_t data_size = row_step * height; + stream.next(data_size); + memcpy(stream.advance(data_size), &m.points[0], data_size); + + uint8_t is_dense = m.is_dense; + stream.next(is_dense); + } + + template + inline static void read(Stream& stream, pcl::PointCloud& m) + { + stream.next(m.header); + stream.next(m.height); + stream.next(m.width); + + /// @todo Check that fields haven't changed! + std::vector fields; + stream.next(fields); + + // Construct field mapping if deserializing for the first time + boost::shared_ptr& mapping_ptr = pcl::detail::getMapping(m); + if (!mapping_ptr) + { + // This normally should get allocated by DefaultMessageCreator, but just in case + mapping_ptr = boost::make_shared(); + } + pcl::MsgFieldMap& mapping = *mapping_ptr; + if (mapping.empty()) + pcl::createMapping (fields, mapping); + + uint8_t is_bigendian; + stream.next(is_bigendian); // ignoring... + uint32_t point_step, row_step; + stream.next(point_step); + stream.next(row_step); + + // Copy point data + uint32_t data_size; + stream.next(data_size); + assert(data_size == m.height * m.width * point_step); + m.points.resize(m.height * m.width); + uint8_t* m_data = reinterpret_cast(&m.points[0]); + // If the data layouts match, can copy a whole row in one memcpy + if (mapping.size() == 1 && + mapping[0].serialized_offset == 0 && + mapping[0].struct_offset == 0 && + point_step == sizeof(T)) + { + uint32_t m_row_step = sizeof(T) * m.width; + // And if the row steps match, can copy whole point cloud in one memcpy + if (m_row_step == row_step) + { + memcpy (m_data, stream.advance(data_size), data_size); + } + else + { + for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step) + memcpy (m_data, stream.advance(row_step), m_row_step); + } + } + else + { + // If not, do a lot of memcpys to copy over the fields + for (uint32_t row = 0; row < m.height; ++row) { + const uint8_t* stream_data = stream.advance(row_step); + for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) { + BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) { + memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size); + } + m_data += sizeof(T); + } + } + } + + uint8_t is_dense; + stream.next(is_dense); + m.is_dense = is_dense; + } + + inline static uint32_t serializedLength(const pcl::PointCloud& m) + { + uint32_t length = 0; + + length += serializationLength(m.header); + length += 8; // height/width + + pcl::detail::FieldsLength fl; + typedef typename pcl::traits::fieldList::type FieldList; + pcl::for_each_type(boost::ref(fl)); + length += 4; // size of 'fields' + length += fl.length; + + length += 1; // is_bigendian + length += 4; // point_step + length += 4; // row_step + length += 4; // size of 'data' + length += m.points.size() * sizeof(T); // data + length += 1; // is_dense + + return length; + } + }; + } // namespace ros::serialization + + /// @todo Printer specialization in message_operations + +} // namespace ros + +#endif diff --git a/pcl_ros/include/pcl_ros/publisher.h b/pcl_ros/include/pcl_ros/publisher.h new file mode 100644 index 00000000..e97d2ea1 --- /dev/null +++ b/pcl_ros/include/pcl_ros/publisher.h @@ -0,0 +1,146 @@ +/* + * 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: publisher.h 33238 2010-03-11 00:46:58Z rusu $ + * + */ +/** + +\author Patrick Mihelich + +@b Publisher represents a ROS publisher for the templated PointCloud implementation. + +**/ + +#ifndef pcl_ros_PUBLISHER_H_ +#define pcl_ros_PUBLISHER_H_ + +#include +#include "pcl/ros/conversions.h" + +namespace pcl_ros +{ + class BasePublisher + { + public: + void + advertise (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) + { + pub_ = nh.advertise(topic, queue_size); + } + + std::string + getTopic () + { + return (pub_.getTopic ()); + } + + uint32_t + getNumSubscribers () const + { + return (pub_.getNumSubscribers ()); + } + + void + shutdown () + { + pub_.shutdown (); + } + + operator void*() const + { + return (pub_) ? (void*)1 : (void*)0; + } + + protected: + ros::Publisher pub_; + }; + + template + class Publisher : public BasePublisher + { + public: + Publisher () {} + + Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) + { + advertise (nh, topic, queue_size); + } + + ~Publisher () {} + + inline void + publish (const boost::shared_ptr > &point_cloud) const + { + publish (*point_cloud); + } + + inline void + publish (const pcl::PointCloud& point_cloud) const + { + // Fill point cloud binary data + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg (point_cloud, msg); + pub_.publish (boost::make_shared (msg)); + } + }; + + template <> + class Publisher : public BasePublisher + { + public: + Publisher () {} + + Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) + { + advertise (nh, topic, queue_size); + } + + ~Publisher () {} + + void + publish (const sensor_msgs::PointCloud2Ptr& point_cloud) const + { + pub_.publish (point_cloud); + //pub_.publish (*point_cloud); + } + + void + publish (const sensor_msgs::PointCloud2& point_cloud) const + { + pub_.publish (boost::make_shared (point_cloud)); + //pub_.publish (point_cloud); + } + }; +} +#endif //#ifndef PCL_ROS_PUBLISHER_H_ diff --git a/pcl_ros/include/pcl_ros/transforms.h b/pcl_ros/include/pcl_ros/transforms.h new file mode 100644 index 00000000..36e5d805 --- /dev/null +++ b/pcl_ros/include/pcl_ros/transforms.h @@ -0,0 +1,167 @@ +/* + * 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. + * + * + */ + +#ifndef pcl_ROS_TRANSFORMS_H_ +#define pcl_ROS_TRANSFORMS_H_ + +#include +#include +#include +#include + +namespace pcl_ros +{ + /** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param transform a rigid transformation from tf + * \note calls the Eigen version + */ + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const tf::Transform &transform); + + /** \brief Transforms a point cloud in a given target TF frame using a TransformListener + * \param target_frame the target TF frame the point cloud should be transformed to + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param tf_listener a TF listener object + */ + template bool + transformPointCloudWithNormals (const std::string &target_frame, + const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const tf::TransformListener &tf_listener); + + /** \brief Transforms a point cloud in a given target TF frame using a TransformListener + * \param target_frame the target TF frame the point cloud should be transformed to + * \param target_time the target timestamp + * \param cloud_in the input point cloud + * \param fixed_frame fixed TF frame + * \param cloud_out the input point cloud + * \param tf_listener a TF listener object + */ + template bool + transformPointCloudWithNormals (const std::string &target_frame, + const ros::Time & target_time, + const pcl::PointCloud &cloud_in, + const std::string &fixed_frame, + pcl::PointCloud &cloud_out, + const tf::TransformListener &tf_listener); + + /** \brief Apply a rigid transform defined by a 3D offset and a quaternion + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param transform a rigid transformation from tf + * \note calls the Eigen version + */ + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const tf::Transform &transform); + + /** \brief Transforms a point cloud in a given target TF frame using a TransformListener + * \param target_frame the target TF frame the point cloud should be transformed to + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param tf_listener a TF listener object + */ + template bool + transformPointCloud (const std::string &target_frame, + const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const tf::TransformListener &tf_listener); + + /** \brief Transforms a point cloud in a given target TF frame using a TransformListener + * \param target_frame the target TF frame the point cloud should be transformed to + * \param target_time the target timestamp + * \param cloud_in the input point cloud + * \param fixed_frame fixed TF frame + * \param cloud_out the input point cloud + * \param tf_listener a TF listener object + */ + template bool + transformPointCloud (const std::string &target_frame, const ros::Time & target_time, + const pcl::PointCloud &cloud_in, + const std::string &fixed_frame, + pcl::PointCloud &cloud_out, + const tf::TransformListener &tf_listener); + + /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. + * \param target_frame the target TF frame + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + * \param tf_listener a TF listener object + */ + bool + transformPointCloud (const std::string &target_frame, + const sensor_msgs::PointCloud2 &in, + sensor_msgs::PointCloud2 &out, + const tf::TransformListener &tf_listener); + + /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. + * \param target_frame the target TF frame + * \param net_transform the TF transformer object + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + */ + void + transformPointCloud (const std::string &target_frame, + const tf::Transform &net_transform, + const sensor_msgs::PointCloud2 &in, + sensor_msgs::PointCloud2 &out); + + /** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix. + * \param transform the transformation to use on the points + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + */ + void + transformPointCloud (const Eigen::Matrix4f &transform, + const sensor_msgs::PointCloud2 &in, + sensor_msgs::PointCloud2 &out); + + /** \brief Obtain the transformation matrix from TF into an Eigen form + * \param bt the TF transformation + * \param out_mat the Eigen transformation + */ + void + transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat); +} + +#endif // PCL_ROS_TRANSFORMS_H_ + diff --git a/pcl_ros/manifest.xml b/pcl_ros/manifest.xml new file mode 100644 index 00000000..1c4e8b1d --- /dev/null +++ b/pcl_ros/manifest.xml @@ -0,0 +1,37 @@ + + + +

+ PCL (Point Cloud Library) ROS interface package. PCL-ROS is the preferred + bridge for 3D applications involving n-D Point Clouds and 3D geometry + processing in ROS. The package contains powerful nodelet interfaces for PCL + algorithms, accepts dynamic reconfiguration of parameters, and supports + multiple threading natively for large scale PPG (Perception Processing + Graphs) construction and usage. +

+ + + + Maintained by Open Perception + BSD + http://ros.org/wiki/pcl_ros + + + + + + + + + + + + + + + + + + + + diff --git a/pcl_ros/src/pcl_ros/transforms.cpp b/pcl_ros/src/pcl_ros/transforms.cpp new file mode 100644 index 00000000..d35e2366 --- /dev/null +++ b/pcl_ros/src/pcl_ros/transforms.cpp @@ -0,0 +1,280 @@ +/* + * 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. + * + * + */ + +#include +#include +#include +#include "pcl_ros/transforms.h" +#include "pcl_ros/impl/transforms.hpp" + +namespace pcl_ros +{ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool +transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, + sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener) +{ + if (in.header.frame_id == target_frame) + { + out = in; + return (true); + } + + // Get the TF transform + tf::StampedTransform transform; + try + { + tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform); + } + catch (tf::LookupException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + catch (tf::ExtrapolationException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + + // Convert the TF transform to Eigen format + Eigen::Matrix4f eigen_transform; + transformAsMatrix (transform, eigen_transform); + + transformPointCloud (eigen_transform, in, out); + + out.header.frame_id = target_frame; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, + const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) +{ + if (in.header.frame_id == target_frame) + { + out = in; + return; + } + + // Get the transformation + Eigen::Matrix4f transform; + transformAsMatrix (net_transform, transform); + + transformPointCloud (transform, in, out); + + out.header.frame_id = target_frame; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, + sensor_msgs::PointCloud2 &out) +{ + // Get X-Y-Z indices + int x_idx = pcl::getFieldIndex (in, "x"); + int y_idx = pcl::getFieldIndex (in, "y"); + int z_idx = pcl::getFieldIndex (in, "z"); + + if (x_idx == -1 || y_idx == -1 || z_idx == -1) + { + ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); + return; + } + + if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || + in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || + in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) + { + ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported."); + return; + } + + // Check if distance is available + int dist_idx = pcl::getFieldIndex (in, "distance"); + + // Copy the other data + if (&in != &out) + { + out.header = in.header; + out.height = in.height; + out.width = in.width; + out.fields = in.fields; + out.is_bigendian = in.is_bigendian; + out.point_step = in.point_step; + out.row_step = in.row_step; + out.is_dense = in.is_dense; + out.data.resize (in.data.size ()); + // Copy everything as it's faster than copying individual elements + memcpy (&out.data[0], &in.data[0], in.data.size ()); + } + + Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0); + + for (size_t i = 0; i < in.width * in.height; ++i) + { + Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1); + Eigen::Vector4f pt_out; + + bool max_range_point = false; + int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; + float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); + if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) + { + if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point + { + pt_out = pt; + } + else // max range point + { + pt[0] = *distance_ptr; // Replace x with the x value saved in distance + pt_out = transform * pt; + max_range_point = true; + //std::cout << pt[0]<<","< "<::quiet_NaN(); + } + + memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float)); + memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float)); + memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float)); + + + xyz_offset += in.point_step; + } + + // Check if the viewpoint information is present + int vp_idx = pcl::getFieldIndex (in, "vp_x"); + if (vp_idx != -1) + { + // Transform the viewpoint info too + for (size_t i = 0; i < out.width * out.height; ++i) + { + float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset]; + // Assume vp_x, vp_y, vp_z are consecutive + Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1); + Eigen::Vector4f vp_out = transform * vp_in; + + pstep[0] = vp_out[0]; + pstep[1] = vp_out[1]; + pstep[2] = vp_out[2]; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat) +{ + double mv[12]; + bt.getBasis ().getOpenGLSubMatrix (mv); + + tf::Vector3 origin = bt.getOrigin (); + + out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; + out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; + out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; + + out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1; + out_mat (0, 3) = origin.x (); + out_mat (1, 3) = origin.y (); + out_mat (2, 3) = origin.z (); +} + +} // namespace pcl_ros + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); + diff --git a/rosdep.yaml b/rosdep.yaml new file mode 100644 index 00000000..4a9d45d6 --- /dev/null +++ b/rosdep.yaml @@ -0,0 +1,32 @@ +libtbb: + ubuntu: libtbb-dev + debian: libtbb-dev + fedora: tbb + macports: tbb +libvtk: + ubuntu: libvtk5-dev + debian: libvtk5-dev + fedora: vtk-devel + macports: vtk-devel +unzip: + ubuntu: unzip + debian: unzip + fedora: unzip + macports: unzip +hdf5: + ubuntu: libhdf5-serial-dev + debian: libhdf5-serial-dev + fedora: hdf5-devel + macports: hdf5-18 +libqhull: + ubuntu: libqhull-dev + debian: libqhull-dev + fedora: qhull-devel + macports: qhull +cmake: + ubuntu: cmake + debian: cmake + fedora: cmake + macports: cmake +libvtk-qt: + ubuntu: libvtk5-qt4-dev \ No newline at end of file diff --git a/stack.xml b/stack.xml new file mode 100644 index 00000000..9d5c90c1 --- /dev/null +++ b/stack.xml @@ -0,0 +1,17 @@ + + + This contains the Point Cloud Library (PCL), its + 3rd party dependencies, and a ROS interface for PCL nodelets. + + Maintained by Open Perception + BSD + + http://ros.org/wiki/perception_pcl + + + + + + + + From 6d993c0282d795b43849bd142f10c782de086589 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Mon, 10 Sep 2012 15:20:50 +0000 Subject: [PATCH 053/405] removed pcl release from perception_pcl groovy stack --- pcl/CMakeLists.txt | 8 ---- pcl/Makefile | 57 ------------------------ pcl/cmake/FindEigen.cmake | 81 ----------------------------------- pcl/manifest.xml | 33 -------------- pcl/msg/ModelCoefficients.msg | 3 -- pcl/msg/PointIndices.msg | 3 -- pcl/msg/PolygonMesh.msg | 6 --- pcl/msg/Vertices.msg | 2 - 8 files changed, 193 deletions(-) delete mode 100644 pcl/CMakeLists.txt delete mode 100644 pcl/Makefile delete mode 100644 pcl/cmake/FindEigen.cmake delete mode 100644 pcl/manifest.xml delete mode 100644 pcl/msg/ModelCoefficients.msg delete mode 100644 pcl/msg/PointIndices.msg delete mode 100644 pcl/msg/PolygonMesh.msg delete mode 100644 pcl/msg/Vertices.msg diff --git a/pcl/CMakeLists.txt b/pcl/CMakeLists.txt deleted file mode 100644 index 0853d403..00000000 --- a/pcl/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -find_package(Eigen REQUIRED) -include_directories(${Eigen_INCLUDE_DIRS}) -rosbuild_init() -rosbuild_genmsg() - diff --git a/pcl/Makefile b/pcl/Makefile deleted file mode 100644 index f42463cf..00000000 --- a/pcl/Makefile +++ /dev/null @@ -1,57 +0,0 @@ -all: installed - -SVN_DIR = build/pcl_1.6.0 -# Developers, please use this URL: -SVN_URL = http://svn.pointclouds.org/pcl/tags/pcl-1.6.0 # For the very latest version - -ifeq ($(strip $(SVN_CMDLINE)),) -SVN_CMDLINE = svn -endif - -$(SVN_DIR): - $(SVN_CMDLINE) co $(SVN_REVISION) $(SVN_URL) $(SVN_DIR) -ifneq ($(strip $(SVN_PATCH)),) - cd $(SVN_DIR) && patch -p0 < ../$(SVN_PATCH) -endif - cd $(SVN_DIR) && \ - sed -Ei 's/-Wold-style-cast/-Wno-old-style-cast -Wno-unused-parameter -Wno-conversion/g' ./CMakeLists.txt && \ - sed -Ei 's/${INCLUDE_INSTALL_ROOT}\/pcl/${INCLUDE_INSTALL_ROOT}\/pcl$(PCL_VERSION)/g' ./cmake/pcl_utils.cmake - -SVN_UP: $(SVN_DIR) - cd $(SVN_DIR) && $(SVN_CMDLINE) up $(SVN_REVISION) - -download: $(SVN_UP) - -installed: $(SVN_DIR) cleaned - mkdir -p msg/build && cd msg/build && cmake ../.. && make && cd - - cd $(SVN_DIR) && mkdir -p build && cd build && \ - rm -rf ../common/include/sensor_msgs ../common/include/std_msgs \ - ../common/include/pcl$(PCL_VERSION)/ModelCoefficients.h ../common/include/pcl$(PCL_VERSION)/PointIndices.h ../common/include/pcl$(PCL_VERSION)/PolygonMesh.h ../common/include/pcl$(PCL_VERSION)/Vertices.h && \ - export CPATH="`rospack cflags-only-I sensor_msgs`:`rospack cflags-only-I roscpp_serialization`:`rospack cflags-only-I cpp_common`:`rospack cflags-only-I rostime`:`rospack cflags-only-I roscpp_traits`:`rospack cflags-only-I roscpp`:`rospack cflags-only-I rosconsole`:`rospack cflags-only-I std_msgs`:`rospack cflags-only-I sensor_msgs`:`rospack find pcl$(PCL_VERSION)`/msg_gen/cpp/include:$$CPATH" && \ - export LD_LIBRARY_PATH="`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:`rospack libs-only-L roscpp_serialization`:`rospack libs-only-L cpp_common`:`rospack libs-only-L rostime`:`rospack libs-only-L roscpp_traits`:`rospack libs-only-L roscpp`:`rospack libs-only-L rosconsole`:`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:$$LD_LIBRARY_PATH" && \ - export LIBRARY_PATH="`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:`rospack libs-only-L roscpp_serialization`:`rospack libs-only-L cpp_common`:`rospack libs-only-L rostime`:`rospack libs-only-L roscpp_traits`:`rospack libs-only-L roscpp`:`rospack libs-only-L rosconsole`:`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:$$LIBRARY_PATH" && \ - cmake -DCMAKE_INSTALL_PREFIX=`pwd`/../../.. \ - -DCMAKE_BUILD_TYPE=Release \ - -DUSE_ROS=ON \ - -DBUILD_OPENNI=ON \ - -DBUILD_TESTS=OFF \ - -DBUILD_apps=OFF \ - -DBUILD_examples=OFF \ - -DCMAKE_VERBOSE_MAKEFILE=OFF \ - .. && \ - make ${ROS_PARALLEL_JOBS} install - touch installed - -cleaned: Makefile - make clean - touch cleaned - -clean: - -rm -rf $(SVN_DIR)/build rospack_nosubdirs patched installed include bin lib64 msg_gen src *~ - -wiped: Makefile - make wipe - touch wiped - -wipe: clean - rm -rf build cleaned msg/build diff --git a/pcl/cmake/FindEigen.cmake b/pcl/cmake/FindEigen.cmake deleted file mode 100644 index 2666481c..00000000 --- a/pcl/cmake/FindEigen.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN_FOUND - system has eigen lib with correct version -# EIGEN_INCLUDE_DIR - the eigen include directory -# EIGEN_VERSION - eigen version - -# Copyright (c) 2006, 2007 Montel Laurent, -# Copyright (c) 2008, 2009 Gael Guennebaud, -# Copyright (c) 2009 Benoit Jacob -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen_FIND_VERSION) - if(NOT Eigen_FIND_VERSION_MAJOR) - set(Eigen_FIND_VERSION_MAJOR 2) - endif(NOT Eigen_FIND_VERSION_MAJOR) - if(NOT Eigen_FIND_VERSION_MINOR) - set(Eigen_FIND_VERSION_MINOR 91) - endif(NOT Eigen_FIND_VERSION_MINOR) - if(NOT Eigen_FIND_VERSION_PATCH) - set(Eigen_FIND_VERSION_PATCH 0) - endif(NOT Eigen_FIND_VERSION_PATCH) - - set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") -endif(NOT Eigen_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) - if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) - set(EIGEN_VERSION_OK FALSE) - else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) - set(EIGEN_VERSION_OK TRUE) - endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) - - if(NOT EIGEN_VERSION_OK) - - message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " - "but at least version ${Eigen_FIND_VERSION} is required") - endif(NOT EIGEN_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN_INCLUDE_DIRS) - - # in cache already - _eigen3_check_version() - set(EIGEN_FOUND ${EIGEN_VERSION_OK}) - -else () - - find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - - if(EIGEN_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) - - mark_as_advanced(EIGEN_INCLUDE_DIR) - SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") - -endif() diff --git a/pcl/manifest.xml b/pcl/manifest.xml deleted file mode 100644 index 1f1ea64d..00000000 --- a/pcl/manifest.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - -

- PCL - Point Cloud Library: a comprehensive open - source library for n-D Point Clouds and 3D geometry processing. - The library contains numerous state-of-the art algorithms for: filtering, - feature estimation, surface reconstruction, registration, model fitting and - segmentation, etc. -

- - - - See http://pcl.ros.org/authors for the complete list of authors. - BSD - http://pointclouds.org - - - - - - - - - - - - - - - - - diff --git a/pcl/msg/ModelCoefficients.msg b/pcl/msg/ModelCoefficients.msg deleted file mode 100644 index 8d3f9b89..00000000 --- a/pcl/msg/ModelCoefficients.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -float32[] values - diff --git a/pcl/msg/PointIndices.msg b/pcl/msg/PointIndices.msg deleted file mode 100644 index 007c2900..00000000 --- a/pcl/msg/PointIndices.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -int32[] indices - diff --git a/pcl/msg/PolygonMesh.msg b/pcl/msg/PolygonMesh.msg deleted file mode 100644 index 8eeb5a4c..00000000 --- a/pcl/msg/PolygonMesh.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Separate header for the polygonal surface -Header header -# Vertices of the mesh as a point cloud -sensor_msgs/PointCloud2 cloud -# List of polygons -Vertices[] polygons diff --git a/pcl/msg/Vertices.msg b/pcl/msg/Vertices.msg deleted file mode 100644 index 6b7c72a0..00000000 --- a/pcl/msg/Vertices.msg +++ /dev/null @@ -1,2 +0,0 @@ -# List of point indices -uint32[] vertices From 73e8aa93acde33b84170c51725f9ab39d78b4e0e Mon Sep 17 00:00:00 2001 From: jkammerl Date: Thu, 13 Sep 2012 11:30:39 +0000 Subject: [PATCH 054/405] added perception_pcl_unstable to groovy branch --- Makefile | 123 ++++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 122 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index a818ccad..74a837e9 100644 --- a/Makefile +++ b/Makefile @@ -1 +1,122 @@ -include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# The program to use to edit the cache. +CMAKE_EDIT_COMMAND = /usr/bin/ccmake + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake cache editor..." + /usr/bin/ccmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# The main all target +all: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl/CMakeFiles /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl/CMakeFiles/progress.marks + $(MAKE) -f CMakeFiles/Makefile2 all + $(CMAKE_COMMAND) -E cmake_progress_start /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + $(MAKE) -f CMakeFiles/Makefile2 clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... edit_cache" + @echo "... rebuild_cache" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + From 33ae605cd16bba4da4f558da9c0834cb82d3409b Mon Sep 17 00:00:00 2001 From: jkammerl Date: Thu, 13 Sep 2012 13:26:12 +0000 Subject: [PATCH 055/405] catkinized version of perception_pcl for groovy --- CMakeLists.txt | 40 +++--- Makefile | 123 +----------------- {pcl_ros/cmake => cmake}/FindEigen.cmake | 0 .../pcl_ros/impl/transforms.hpp | 0 .../include => include}/pcl_ros/point_cloud.h | 0 .../include => include}/pcl_ros/publisher.h | 0 .../include => include}/pcl_ros/transforms.h | 0 pcl_ros/manifest.xml => manifest.xml | 0 pcl_ros/CMakeLists.txt | 21 --- pcl_ros/Makefile | 1 - {pcl_ros/src => src}/pcl_ros/transforms.cpp | 0 stack.xml | 17 --- 12 files changed, 25 insertions(+), 177 deletions(-) rename {pcl_ros/cmake => cmake}/FindEigen.cmake (100%) rename {pcl_ros/include => include}/pcl_ros/impl/transforms.hpp (100%) rename {pcl_ros/include => include}/pcl_ros/point_cloud.h (100%) rename {pcl_ros/include => include}/pcl_ros/publisher.h (100%) rename {pcl_ros/include => include}/pcl_ros/transforms.h (100%) rename pcl_ros/manifest.xml => manifest.xml (100%) delete mode 100644 pcl_ros/CMakeLists.txt delete mode 100644 pcl_ros/Makefile rename {pcl_ros/src => src}/pcl_ros/transforms.cpp (100%) delete mode 100644 stack.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 15d597ea..c531939b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,19 +1,27 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +cmake_minimum_required(VERSION 2.8) -set(ROSPACK_MAKEDIST true) +project(perception_pcl) -# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of -# directories (or patterns, but directories should suffice) that should -# be excluded from the distro. This is not the place to put things that -# should be ignored everywhere, like "build" directories; that happens in -# rosbuild/rosbuild.cmake. Here should be listed packages that aren't -# ready for inclusion in a distro. -# -# This list is combined with the list in rosbuild/rosbuild.cmake. Note -# that CMake 2.6 may be required to ensure that the two lists are combined -# properly. CMake 2.4 seems to have unpredictable scoping rules for such -# variables. -#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) +# Deal with catkin +find_package(catkin REQUIRED) +catkin_stack() + +# deal with ROS +include_directories(${catkin_INCLUDE_DIRS}) +link_directories(${catkin_LIBRARY_DIRS}) + +find_package(PCL) +find_package(eigen) + +include_directories(SYSTEM ${PCL_INCLUDE_DIRS} ${eigen_INCLUDE_DIRS}) +include_directories(include) + +catkin_project(${PROJECT_NAME} + INCLUDE_DIRS include + LIBRARIES pcl_ros_tf + DEPENDS PCL eigen +) + +# ---[ Point Cloud Library - Transforms +add_library (pcl_ros_tf src/transforms.cpp) -rosbuild_make_distribution(1.6.0) diff --git a/Makefile b/Makefile index 74a837e9..bbd3fc60 100644 --- a/Makefile +++ b/Makefile @@ -1,122 +1 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 - -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# The program to use to edit the cache. -CMAKE_EDIT_COMMAND = /usr/bin/ccmake - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake cache editor..." - /usr/bin/ccmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache -.PHONY : edit_cache/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache -.PHONY : rebuild_cache/fast - -# The main all target -all: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl/CMakeFiles /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl/CMakeFiles/progress.marks - $(MAKE) -f CMakeFiles/Makefile2 all - $(CMAKE_COMMAND) -E cmake_progress_start /home/kammerl/Documents/PCL/ros/branches/groovy/perception_pcl/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - $(MAKE) -f CMakeFiles/Makefile2 clean -.PHONY : clean - -# The main clean target -clean/fast: clean -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... edit_cache" - @echo "... rebuild_cache" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - +include $(shell rospack find mk)/cmake.mk diff --git a/pcl_ros/cmake/FindEigen.cmake b/cmake/FindEigen.cmake similarity index 100% rename from pcl_ros/cmake/FindEigen.cmake rename to cmake/FindEigen.cmake diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/include/pcl_ros/impl/transforms.hpp similarity index 100% rename from pcl_ros/include/pcl_ros/impl/transforms.hpp rename to include/pcl_ros/impl/transforms.hpp diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/include/pcl_ros/point_cloud.h similarity index 100% rename from pcl_ros/include/pcl_ros/point_cloud.h rename to include/pcl_ros/point_cloud.h diff --git a/pcl_ros/include/pcl_ros/publisher.h b/include/pcl_ros/publisher.h similarity index 100% rename from pcl_ros/include/pcl_ros/publisher.h rename to include/pcl_ros/publisher.h diff --git a/pcl_ros/include/pcl_ros/transforms.h b/include/pcl_ros/transforms.h similarity index 100% rename from pcl_ros/include/pcl_ros/transforms.h rename to include/pcl_ros/transforms.h diff --git a/pcl_ros/manifest.xml b/manifest.xml similarity index 100% rename from pcl_ros/manifest.xml rename to manifest.xml diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt deleted file mode 100644 index 400c76e4..00000000 --- a/pcl_ros/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required (VERSION 2.4.6) - -include ($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) -rosbuild_init () -rosbuild_add_boost_directories () -add_definitions (-DEIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET) -rosbuild_check_for_sse () - -set (EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -include_directories (${CMAKE_CURRENT_BINARY_DIR}) -include_directories (src) - -# Uses Eigen -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -find_package(Eigen REQUIRED) -include_directories(${Eigen_INCLUDE_DIRS}) -include_directories(${EIGEN_INCLUDE_DIRS}) - -# ---[ Point Cloud Library - Transforms -rosbuild_add_library (pcl_ros_tf src/pcl_ros/transforms.cpp) -rosbuild_add_compile_flags (pcl_ros_tf ${SSE_FLAGS}) diff --git a/pcl_ros/Makefile b/pcl_ros/Makefile deleted file mode 100644 index bbd3fc60..00000000 --- a/pcl_ros/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk diff --git a/pcl_ros/src/pcl_ros/transforms.cpp b/src/pcl_ros/transforms.cpp similarity index 100% rename from pcl_ros/src/pcl_ros/transforms.cpp rename to src/pcl_ros/transforms.cpp diff --git a/stack.xml b/stack.xml deleted file mode 100644 index 9d5c90c1..00000000 --- a/stack.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - This contains the Point Cloud Library (PCL), its - 3rd party dependencies, and a ROS interface for PCL nodelets. - - Maintained by Open Perception - BSD - - http://ros.org/wiki/perception_pcl - - - - - - - - From dfa5be4f9dacde426d6f7a3e454ab4197e454ddb Mon Sep 17 00:00:00 2001 From: jkammerl Date: Thu, 13 Sep 2012 13:27:05 +0000 Subject: [PATCH 056/405] added new stack.xml, removed manifest.xml --- manifest.xml | 37 ------------------------------------- stack.xml | 25 +++++++++++++++++++++++++ 2 files changed, 25 insertions(+), 37 deletions(-) delete mode 100644 manifest.xml create mode 100644 stack.xml diff --git a/manifest.xml b/manifest.xml deleted file mode 100644 index 1c4e8b1d..00000000 --- a/manifest.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - -

- PCL (Point Cloud Library) ROS interface package. PCL-ROS is the preferred - bridge for 3D applications involving n-D Point Clouds and 3D geometry - processing in ROS. The package contains powerful nodelet interfaces for PCL - algorithms, accepts dynamic reconfiguration of parameters, and supports - multiple threading natively for large scale PPG (Perception Processing - Graphs) construction and usage. -

- - - - Maintained by Open Perception - BSD - http://ros.org/wiki/pcl_ros - - - - - - - - - - - - - - - - - - - - diff --git a/stack.xml b/stack.xml new file mode 100644 index 00000000..8841eca1 --- /dev/null +++ b/stack.xml @@ -0,0 +1,25 @@ + + perception_pcl + 1.0.4 + +

+ PCL (Point Cloud Library) ROS interface package. PCL-ROS is the preferred + bridge for 3D applications involving n-D Point Clouds and 3D geometry + processing in ROS. +

+ + Open Perception + Julius Kammerl + BSD + + http://ros.org/wiki/perception_pcl + Open Perception + + cmake + catkin + eigen + pcl + + pcl + eigen + From a57e6ab5792f13eaf475c53548bc0125e17c2c25 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Thu, 13 Sep 2012 13:27:56 +0000 Subject: [PATCH 057/405] moved transforms.cpp to /src --- src/{pcl_ros => }/transforms.cpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/{pcl_ros => }/transforms.cpp (100%) diff --git a/src/pcl_ros/transforms.cpp b/src/transforms.cpp similarity index 100% rename from src/pcl_ros/transforms.cpp rename to src/transforms.cpp From ffb3e9d7eba436aad5006093fa44673703c2c001 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Thu, 13 Sep 2012 13:40:21 +0000 Subject: [PATCH 058/405] minor --- CMakeLists.txt | 29 ++++++++++++++++++++--------- Makefile | 1 - 2 files changed, 20 insertions(+), 10 deletions(-) delete mode 100644 Makefile diff --git a/CMakeLists.txt b/CMakeLists.txt index c531939b..b9fb420a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,18 +3,15 @@ cmake_minimum_required(VERSION 2.8) project(perception_pcl) # Deal with catkin -find_package(catkin REQUIRED) -catkin_stack() +find_package(catkin REQUIRED roscpp sensor_msgs) +find_package(PCL) +find_package(eigen) # deal with ROS include_directories(${catkin_INCLUDE_DIRS}) link_directories(${catkin_LIBRARY_DIRS}) -find_package(PCL) -find_package(eigen) - -include_directories(SYSTEM ${PCL_INCLUDE_DIRS} ${eigen_INCLUDE_DIRS}) -include_directories(include) +catkin_stack() catkin_project(${PROJECT_NAME} INCLUDE_DIRS include @@ -22,6 +19,20 @@ catkin_project(${PROJECT_NAME} DEPENDS PCL eigen ) -# ---[ Point Cloud Library - Transforms -add_library (pcl_ros_tf src/transforms.cpp) +include_directories(SYSTEM ${PCL_INCLUDE_DIRS} ${eigen_INCLUDE_DIRS}) +include_directories(include) + +# ---[ Point Cloud Library - Transforms +add_library (pcl_ros_tf SHARED src/transforms.cpp) +target_link_libraries(pcl_ros_tf ${PCL_LIBS}) + +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) + +install(TARGETS pcl_ros_tf + RUNTIME DESTINATION ${CATKIN_PROJECT_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PROJECT_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PROJECT_LIB_DESTINATION} +) diff --git a/Makefile b/Makefile deleted file mode 100644 index bbd3fc60..00000000 --- a/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk From a9f97a06065582b5da0efbc87d2214e291cffdbe Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 14:30:42 +0000 Subject: [PATCH 059/405] added missing tf stack to CMakeList --- CMakeLists.txt | 21 ++++++++++----------- stack.xml | 15 +++++++++++---- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b9fb420a..5b3e9eae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,29 +3,28 @@ cmake_minimum_required(VERSION 2.8) project(perception_pcl) # Deal with catkin -find_package(catkin REQUIRED roscpp sensor_msgs) -find_package(PCL) -find_package(eigen) +find_package(catkin REQUIRED roscpp sensor_msgs tf) +find_package(Boost COMPONENTS system filesystem thread REQUIRED) +find_package(Eigen) +find_package(PCL) # deal with ROS -include_directories(${catkin_INCLUDE_DIRS}) -link_directories(${catkin_LIBRARY_DIRS}) +include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +include_directories(include) + +link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) catkin_stack() catkin_project(${PROJECT_NAME} INCLUDE_DIRS include LIBRARIES pcl_ros_tf - DEPENDS PCL eigen + DEPENDS roscpp common_msgs sensor_msgs tf ) -include_directories(SYSTEM ${PCL_INCLUDE_DIRS} ${eigen_INCLUDE_DIRS}) -include_directories(include) - - # ---[ Point Cloud Library - Transforms add_library (pcl_ros_tf SHARED src/transforms.cpp) -target_link_libraries(pcl_ros_tf ${PCL_LIBS}) +target_link_libraries(pcl_ros_tf ${PCL_LIBS} ${Boost_LIBS} ${catkin_LIBS}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} diff --git a/stack.xml b/stack.xml index 8841eca1..69b35e73 100644 --- a/stack.xml +++ b/stack.xml @@ -17,9 +17,16 @@ cmake catkin - eigen - pcl + Eigen + PCL + tf + sensor_msgs + roscpp_core + + PCL + Eigen + tf + sensor_msgs + roscpp_core - pcl - eigen From 6dcca0ae2e43958af336d98182dadebde2e03a11 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 14:34:14 +0000 Subject: [PATCH 060/405] increased stack version to 1.0.5 --- stack.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stack.xml b/stack.xml index 69b35e73..42421b2a 100644 --- a/stack.xml +++ b/stack.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.4 + 1.0.5

PCL (Point Cloud Library) ROS interface package. PCL-ROS is the preferred From 8e042cf9d5bc8c030cc8f1b65d9cb54985056a6f Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 14:57:11 +0000 Subject: [PATCH 061/405] tf->geometry in stack.xml --- stack.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stack.xml b/stack.xml index 42421b2a..5e4e4573 100644 --- a/stack.xml +++ b/stack.xml @@ -19,13 +19,13 @@ catkin Eigen PCL - tf + geometry sensor_msgs roscpp_core PCL Eigen - tf + geometry sensor_msgs roscpp_core From eb0ea004b705696cc705be830beb748060218233 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 15:04:01 +0000 Subject: [PATCH 062/405] fixed stack.xml - common_msgs --- stack.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stack.xml b/stack.xml index 5e4e4573..59f9a503 100644 --- a/stack.xml +++ b/stack.xml @@ -20,13 +20,13 @@ Eigen PCL geometry - sensor_msgs + common_msgs roscpp_core PCL Eigen geometry - sensor_msgs + common_msgs roscpp_core From 4043ed1cde01342266f313d3d65e868284f12389 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 15:09:17 +0000 Subject: [PATCH 063/405] stack.xml: Eigen->eigen --- stack.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stack.xml b/stack.xml index 59f9a503..803710d2 100644 --- a/stack.xml +++ b/stack.xml @@ -24,7 +24,7 @@ roscpp_core PCL - Eigen + eigen geometry common_msgs roscpp_core From 441319bcee6d98753a2eecfb24033f8d6e81d87a Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 15:12:06 +0000 Subject: [PATCH 064/405] stack.xml: PCL->pcl --- stack.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stack.xml b/stack.xml index 803710d2..f3f557a5 100644 --- a/stack.xml +++ b/stack.xml @@ -23,7 +23,7 @@ common_msgs roscpp_core - PCL + pcl eigen geometry common_msgs From cb6e2b052929db480879e597a57b0fb5cf560b7c Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 15:13:30 +0000 Subject: [PATCH 065/405] minor --- stack.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stack.xml b/stack.xml index f3f557a5..d50269b4 100644 --- a/stack.xml +++ b/stack.xml @@ -17,8 +17,8 @@ cmake catkin - Eigen - PCL + eigen + pcl geometry common_msgs roscpp_core From bcda3321ad7d443a746f4c5d7f5428d4bf9d8038 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 17:30:20 +0000 Subject: [PATCH 066/405] restructuring perception_pcl in groovy branch --- CMakeLists.txt => pcl_ros/CMakeLists.txt | 0 {cmake => pcl_ros/cmake}/FindEigen.cmake | 0 {include => pcl_ros/include}/pcl_ros/impl/transforms.hpp | 0 {include => pcl_ros/include}/pcl_ros/point_cloud.h | 0 {include => pcl_ros/include}/pcl_ros/publisher.h | 0 {include => pcl_ros/include}/pcl_ros/transforms.h | 0 {src => pcl_ros/src}/transforms.cpp | 0 7 files changed, 0 insertions(+), 0 deletions(-) rename CMakeLists.txt => pcl_ros/CMakeLists.txt (100%) rename {cmake => pcl_ros/cmake}/FindEigen.cmake (100%) rename {include => pcl_ros/include}/pcl_ros/impl/transforms.hpp (100%) rename {include => pcl_ros/include}/pcl_ros/point_cloud.h (100%) rename {include => pcl_ros/include}/pcl_ros/publisher.h (100%) rename {include => pcl_ros/include}/pcl_ros/transforms.h (100%) rename {src => pcl_ros/src}/transforms.cpp (100%) diff --git a/CMakeLists.txt b/pcl_ros/CMakeLists.txt similarity index 100% rename from CMakeLists.txt rename to pcl_ros/CMakeLists.txt diff --git a/cmake/FindEigen.cmake b/pcl_ros/cmake/FindEigen.cmake similarity index 100% rename from cmake/FindEigen.cmake rename to pcl_ros/cmake/FindEigen.cmake diff --git a/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp similarity index 100% rename from include/pcl_ros/impl/transforms.hpp rename to pcl_ros/include/pcl_ros/impl/transforms.hpp diff --git a/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h similarity index 100% rename from include/pcl_ros/point_cloud.h rename to pcl_ros/include/pcl_ros/point_cloud.h diff --git a/include/pcl_ros/publisher.h b/pcl_ros/include/pcl_ros/publisher.h similarity index 100% rename from include/pcl_ros/publisher.h rename to pcl_ros/include/pcl_ros/publisher.h diff --git a/include/pcl_ros/transforms.h b/pcl_ros/include/pcl_ros/transforms.h similarity index 100% rename from include/pcl_ros/transforms.h rename to pcl_ros/include/pcl_ros/transforms.h diff --git a/src/transforms.cpp b/pcl_ros/src/transforms.cpp similarity index 100% rename from src/transforms.cpp rename to pcl_ros/src/transforms.cpp From 3c3c86fc37d6d8dd2a115f77912daffaf111cdba Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 18:17:12 +0000 Subject: [PATCH 067/405] perception_pcl restructuring in groovy branch --- CMakeLists.txt | 6 ++++++ pcl_ros/CMakeLists.txt | 6 ++---- pcl_ros/manifest.xml | 33 +++++++++++++++++++++++++++++++++ stack.xml | 2 +- 4 files changed, 42 insertions(+), 5 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 pcl_ros/manifest.xml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..d7faf9f9 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 2.8) +project(perception_pcl) +find_package(catkin) +catkin_stack() + +add_subdirectory(pcl_ros) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 5b3e9eae..cd674aa7 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 2.8) -project(perception_pcl) +project(pcl_ros) # Deal with catkin find_package(catkin REQUIRED roscpp sensor_msgs tf) @@ -14,12 +14,10 @@ include_directories(include) link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) -catkin_stack() - catkin_project(${PROJECT_NAME} INCLUDE_DIRS include LIBRARIES pcl_ros_tf - DEPENDS roscpp common_msgs sensor_msgs tf + DEPENDS roscpp sensor_msgs tf ) # ---[ Point Cloud Library - Transforms diff --git a/pcl_ros/manifest.xml b/pcl_ros/manifest.xml new file mode 100644 index 00000000..09c720d4 --- /dev/null +++ b/pcl_ros/manifest.xml @@ -0,0 +1,33 @@ + + +

+ PCL (Point Cloud Library) ROS interface package. PCL-ROS is the preferred + bridge for 3D applications involving n-D Point Clouds and 3D geometry + processing in ROS. +

+ + Open Perception + Julius Kammerl + BSD + http://ros.org/wiki/pcl_ros + + + + + + + + + + + + + + + + + + + + + diff --git a/stack.xml b/stack.xml index d50269b4..ce3193b4 100644 --- a/stack.xml +++ b/stack.xml @@ -3,7 +3,7 @@ 1.0.5

- PCL (Point Cloud Library) ROS interface package. PCL-ROS is the preferred + PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry processing in ROS.

From e6846d72da1e3b2c3d1711fdc88a258ff8c46076 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Fri, 14 Sep 2012 18:18:07 +0000 Subject: [PATCH 068/405] change perception_pcl version to 1.0.6 in stack.xml (groovy branch) --- stack.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stack.xml b/stack.xml index ce3193b4..ed39d158 100644 --- a/stack.xml +++ b/stack.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.5 + 1.0.6

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From e40bfe91f0b464719136e9044b2c5df063303d67 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Sat, 15 Sep 2012 09:31:47 +0000 Subject: [PATCH 069/405] removing common_rosdeps from manifest.xml --- pcl_ros/manifest.xml | 1 - stack.xml | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/pcl_ros/manifest.xml b/pcl_ros/manifest.xml index 09c720d4..f1bf3a18 100644 --- a/pcl_ros/manifest.xml +++ b/pcl_ros/manifest.xml @@ -19,7 +19,6 @@ - diff --git a/stack.xml b/stack.xml index ed39d158..b7c27d3e 100644 --- a/stack.xml +++ b/stack.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.6 + 1.0.7

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From e7176a53559de8b169f68ac9d3dd528f802ec7a1 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Sat, 15 Sep 2012 13:26:08 +0000 Subject: [PATCH 070/405] fixed rosdeb pcl in pcl_ros/manifest.xml --- pcl_ros/manifest.xml | 2 +- stack.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/manifest.xml b/pcl_ros/manifest.xml index f1bf3a18..555cdc2c 100644 --- a/pcl_ros/manifest.xml +++ b/pcl_ros/manifest.xml @@ -22,7 +22,7 @@ - + diff --git a/stack.xml b/stack.xml index b7c27d3e..9714f151 100644 --- a/stack.xml +++ b/stack.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.7 + 1.0.8

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From fb513120e3bed384ee8a53c3a2eceb6c73786ebf Mon Sep 17 00:00:00 2001 From: jkammerl Date: Sat, 15 Sep 2012 23:52:26 +0000 Subject: [PATCH 071/405] added pcl exports in manifest.xml --- pcl_ros/manifest.xml | 5 ++++- stack.xml | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/pcl_ros/manifest.xml b/pcl_ros/manifest.xml index 555cdc2c..db3cb5e1 100644 --- a/pcl_ros/manifest.xml +++ b/pcl_ros/manifest.xml @@ -25,7 +25,10 @@ - + diff --git a/stack.xml b/stack.xml index 9714f151..fa543d51 100644 --- a/stack.xml +++ b/stack.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.8 + 1.0.9

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From a29653e9aa89135dd7d27ffc5430514331383da7 Mon Sep 17 00:00:00 2001 From: jkammerl Date: Tue, 18 Sep 2012 13:11:22 +0000 Subject: [PATCH 072/405] fixed pcl_ros manifest --- pcl_ros/manifest.xml | 2 +- stack.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/manifest.xml b/pcl_ros/manifest.xml index db3cb5e1..d7319bc6 100644 --- a/pcl_ros/manifest.xml +++ b/pcl_ros/manifest.xml @@ -22,7 +22,7 @@ - +

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From a1b32d4fb63c273b53fd503fa79c2a53ec8bfd07 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Thu, 4 Oct 2012 16:58:35 +0200 Subject: [PATCH 073/405] comply to the new catkin API --- CMakeLists.txt | 6 ------ pcl_ros/CMakeLists.txt | 14 ++++++-------- stack.xml => pcl_ros/package.xml | 31 +++++++++++++++---------------- perception_pcl/package.xml | 20 ++++++++++++++++++++ rosdep.yaml | 32 -------------------------------- 5 files changed, 41 insertions(+), 62 deletions(-) delete mode 100644 CMakeLists.txt rename stack.xml => pcl_ros/package.xml (52%) create mode 100644 perception_pcl/package.xml delete mode 100644 rosdep.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index d7faf9f9..00000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -cmake_minimum_required(VERSION 2.8) -project(perception_pcl) -find_package(catkin) -catkin_stack() - -add_subdirectory(pcl_ros) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index cd674aa7..54ee3f90 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -1,12 +1,11 @@ cmake_minimum_required(VERSION 2.8) - project(pcl_ros) # Deal with catkin find_package(catkin REQUIRED roscpp sensor_msgs tf) find_package(Boost COMPONENTS system filesystem thread REQUIRED) find_package(Eigen) -find_package(PCL) +find_package(PCL) # deal with ROS include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) @@ -14,18 +13,17 @@ include_directories(include) link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) -catkin_project(${PROJECT_NAME} - INCLUDE_DIRS include - LIBRARIES pcl_ros_tf - DEPENDS roscpp sensor_msgs tf +catkin_package(DEPENDS Eigen pcl roscpp sensor_msgs tf + INCLUDE_DIRS include + LIBRARIES pcl_ros_tf ) # ---[ Point Cloud Library - Transforms add_library (pcl_ros_tf SHARED src/transforms.cpp) target_link_libraries(pcl_ros_tf ${PCL_LIBS} ${Boost_LIBS} ${catkin_LIBS}) -install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) install(TARGETS pcl_ros_tf diff --git a/stack.xml b/pcl_ros/package.xml similarity index 52% rename from stack.xml rename to pcl_ros/package.xml index 3009da9f..0e16821c 100644 --- a/stack.xml +++ b/pcl_ros/package.xml @@ -1,5 +1,5 @@ - - perception_pcl + + pcl_ros 1.0.10

@@ -15,18 +15,17 @@ http://ros.org/wiki/perception_pcl Open Perception - cmake - catkin - eigen - pcl - geometry - common_msgs - roscpp_core + catkin + cmake + common_msgs + eigen + geometry + pcl + roscpp_core - pcl - eigen - geometry - common_msgs - roscpp_core - - + common_msgs + eigen + geometry + pcl + roscpp_core + diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml new file mode 100644 index 00000000..b33d05bd --- /dev/null +++ b/perception_pcl/package.xml @@ -0,0 +1,20 @@ + + perception_pcl + 1.0.10 + +

+ PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred + bridge for 3D applications involving n-D Point Clouds and 3D geometry + processing in ROS. +

+ + Open Perception + Julius Kammerl + BSD + http://ros.org/wiki/perception_pcl + Open Perception + + catkin + + pcl_ros + diff --git a/rosdep.yaml b/rosdep.yaml deleted file mode 100644 index 4a9d45d6..00000000 --- a/rosdep.yaml +++ /dev/null @@ -1,32 +0,0 @@ -libtbb: - ubuntu: libtbb-dev - debian: libtbb-dev - fedora: tbb - macports: tbb -libvtk: - ubuntu: libvtk5-dev - debian: libvtk5-dev - fedora: vtk-devel - macports: vtk-devel -unzip: - ubuntu: unzip - debian: unzip - fedora: unzip - macports: unzip -hdf5: - ubuntu: libhdf5-serial-dev - debian: libhdf5-serial-dev - fedora: hdf5-devel - macports: hdf5-18 -libqhull: - ubuntu: libqhull-dev - debian: libqhull-dev - fedora: qhull-devel - macports: qhull -cmake: - ubuntu: cmake - debian: cmake - fedora: cmake - macports: cmake -libvtk-qt: - ubuntu: libvtk5-qt4-dev \ No newline at end of file From 03cde871a21e48c467cc84f37a8e5f9adbc560d8 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Wed, 10 Oct 2012 11:43:26 +0200 Subject: [PATCH 074/405] fix a few dependencies --- pcl_ros/CMakeLists.txt | 2 +- pcl_ros/package.xml | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 54ee3f90..8ce8f594 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -13,7 +13,7 @@ include_directories(include) link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) -catkin_package(DEPENDS Eigen pcl roscpp sensor_msgs tf +catkin_package(DEPENDS Eigen PCL roscpp sensor_msgs tf INCLUDE_DIRS include LIBRARIES pcl_ros_tf ) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 0e16821c..b0f67751 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -16,16 +16,19 @@ Open Perception catkin - cmake common_msgs eigen geometry pcl - roscpp_core + roscpp + sensor_msgs + tf common_msgs eigen geometry pcl - roscpp_core + roscpp + sensor_msgs + tf From 70d156a8b1b9b8a6d40c4f24d0ac0d397ea4c3b0 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Wed, 10 Oct 2012 11:43:37 +0200 Subject: [PATCH 075/405] 1.0.11 --- pcl_ros/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index b0f67751..d25f277c 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.10 + 1.0.11

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 52dd24a4af622b1833ebb20099b3bee3a30846e6 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Wed, 10 Oct 2012 11:45:18 +0200 Subject: [PATCH 076/405] 1.0.11 --- perception_pcl/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index b33d05bd..5c67167c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.10 + 1.0.11

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From c0b5f1030d6551b149921f613571675c197e6351 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Thu, 11 Oct 2012 17:24:34 +0200 Subject: [PATCH 077/405] make sure perception_pcl is a meta package --- perception_pcl/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 5c67167c..d98b46c3 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -17,4 +17,8 @@ catkin pcl_ros + + + + From a3726b72cd857a68684001e8ac9d5b831a477490 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Thu, 11 Oct 2012 17:25:16 +0200 Subject: [PATCH 078/405] 1.0.12 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index d25f277c..fe923c38 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.11 + 1.0.12

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index d98b46c3..11ece6b9 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.11 + 1.0.12

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From a54a5188377e59145d3bb9e6146701bef562daf4 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Thu, 11 Oct 2012 17:45:51 +0200 Subject: [PATCH 079/405] install library to the right place --- pcl_ros/CMakeLists.txt | 6 +++--- pcl_ros/manifest.xml | 35 ----------------------------------- 2 files changed, 3 insertions(+), 38 deletions(-) delete mode 100644 pcl_ros/manifest.xml diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 8ce8f594..cc56897e 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -27,7 +27,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) install(TARGETS pcl_ros_tf - RUNTIME DESTINATION ${CATKIN_PROJECT_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PROJECT_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PROJECT_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) diff --git a/pcl_ros/manifest.xml b/pcl_ros/manifest.xml deleted file mode 100644 index d7319bc6..00000000 --- a/pcl_ros/manifest.xml +++ /dev/null @@ -1,35 +0,0 @@ - - -

- PCL (Point Cloud Library) ROS interface package. PCL-ROS is the preferred - bridge for 3D applications involving n-D Point Clouds and 3D geometry - processing in ROS. -

- - Open Perception - Julius Kammerl - BSD - http://ros.org/wiki/pcl_ros - - - - - - - - - - - - - - - - - - - - From a5c158904de13cc7d8559e2738cdd7b718ac3469 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Thu, 11 Oct 2012 17:46:17 +0200 Subject: [PATCH 080/405] 1.0.13 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index fe923c38..8e18aa15 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.12 + 1.0.13

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 11ece6b9..de50e20d 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.12 + 1.0.13

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From e677ddf7945c17c12cf059fba0f64dc793e4c053 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Tue, 23 Oct 2012 19:45:04 -0700 Subject: [PATCH 081/405] bring back the PCL msgs --- pcl_ros/CMakeLists.txt | 20 ++++++++++++++++++-- pcl_ros/msg/ModelCoefficients.msg | 3 +++ pcl_ros/msg/PointIndices.msg | 3 +++ pcl_ros/msg/PolygonMesh.msg | 6 ++++++ pcl_ros/msg/Vertices.msg | 2 ++ pcl_ros/package.xml | 2 ++ 6 files changed, 34 insertions(+), 2 deletions(-) create mode 100644 pcl_ros/msg/ModelCoefficients.msg create mode 100644 pcl_ros/msg/PointIndices.msg create mode 100644 pcl_ros/msg/PolygonMesh.msg create mode 100644 pcl_ros/msg/Vertices.msg diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index cc56897e..001e74ca 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -2,13 +2,17 @@ cmake_minimum_required(VERSION 2.8) project(pcl_ros) # Deal with catkin -find_package(catkin REQUIRED roscpp sensor_msgs tf) find_package(Boost COMPONENTS system filesystem thread REQUIRED) +find_package(catkin REQUIRED genmsg roscpp sensor_msgs std_msgs tf) find_package(Eigen) find_package(PCL) # deal with ROS -include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +include_directories(SYSTEM ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) include_directories(include) link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) @@ -18,9 +22,21 @@ catkin_package(DEPENDS Eigen PCL roscpp sensor_msgs tf LIBRARIES pcl_ros_tf ) +# create messages +project(pcl) +add_message_files(DIRECTORY msg + FILES ModelCoefficients.msg + PointIndices.msg + PolygonMesh.msg + Vertices.msg +) +generate_messages(DEPENDENCIES sensor_msgs std_msgs) + +project(pcl_ros) # ---[ Point Cloud Library - Transforms add_library (pcl_ros_tf SHARED src/transforms.cpp) target_link_libraries(pcl_ros_tf ${PCL_LIBS} ${Boost_LIBS} ${catkin_LIBS}) +add_dependencies(pcl_ros_tf ros_gencpp pcl_ros_copy) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} diff --git a/pcl_ros/msg/ModelCoefficients.msg b/pcl_ros/msg/ModelCoefficients.msg new file mode 100644 index 00000000..8d3f9b89 --- /dev/null +++ b/pcl_ros/msg/ModelCoefficients.msg @@ -0,0 +1,3 @@ +Header header +float32[] values + diff --git a/pcl_ros/msg/PointIndices.msg b/pcl_ros/msg/PointIndices.msg new file mode 100644 index 00000000..007c2900 --- /dev/null +++ b/pcl_ros/msg/PointIndices.msg @@ -0,0 +1,3 @@ +Header header +int32[] indices + diff --git a/pcl_ros/msg/PolygonMesh.msg b/pcl_ros/msg/PolygonMesh.msg new file mode 100644 index 00000000..8eeb5a4c --- /dev/null +++ b/pcl_ros/msg/PolygonMesh.msg @@ -0,0 +1,6 @@ +# Separate header for the polygonal surface +Header header +# Vertices of the mesh as a point cloud +sensor_msgs/PointCloud2 cloud +# List of polygons +Vertices[] polygons diff --git a/pcl_ros/msg/Vertices.msg b/pcl_ros/msg/Vertices.msg new file mode 100644 index 00000000..6b7c72a0 --- /dev/null +++ b/pcl_ros/msg/Vertices.msg @@ -0,0 +1,2 @@ +# List of point indices +uint32[] vertices diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 8e18aa15..bd297c08 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -22,6 +22,7 @@ pcl roscpp sensor_msgs + std_msgs tf common_msgs @@ -30,5 +31,6 @@ pcl roscpp sensor_msgs + std_msgs tf From 6a8696b61f2ca3bdefdc55020eeca7a6a60edfa7 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Tue, 23 Oct 2012 19:45:40 -0700 Subject: [PATCH 082/405] 1.0.14 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index bd297c08..0ef89764 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.13 + 1.0.14

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index de50e20d..ce4684a5 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.13 + 1.0.14

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 9370244dca7df255706b2a65ed2a224f408b63bb Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Wed, 24 Oct 2012 08:12:38 -0700 Subject: [PATCH 083/405] do not generrate messages automatically --- pcl_ros/CMakeLists.txt | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 001e74ca..3f4aac2e 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -23,16 +23,17 @@ catkin_package(DEPENDS Eigen PCL roscpp sensor_msgs tf ) # create messages -project(pcl) -add_message_files(DIRECTORY msg - FILES ModelCoefficients.msg - PointIndices.msg - PolygonMesh.msg - Vertices.msg -) -generate_messages(DEPENDENCIES sensor_msgs std_msgs) +### what's below is just to generate the messages that are manually patched into PCL +###project(pcl) +###add_message_files(DIRECTORY msg +### FILES ModelCoefficients.msg +### PointIndices.msg +### PolygonMesh.msg +### Vertices.msg +###) +###generate_messages(DEPENDENCIES sensor_msgs std_msgs) +###project(pcl_ros) -project(pcl_ros) # ---[ Point Cloud Library - Transforms add_library (pcl_ros_tf SHARED src/transforms.cpp) target_link_libraries(pcl_ros_tf ${PCL_LIBS} ${Boost_LIBS} ${catkin_LIBS}) From 68459d68d2c4f78914e1fd20e78e57b19e6567f7 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Wed, 24 Oct 2012 08:13:09 -0700 Subject: [PATCH 084/405] 1.0.15 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 0ef89764..6eb60fd8 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.14 + 1.0.15

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index ce4684a5..34becd4d 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.14 + 1.0.15

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From f21e5b679affec35520f5a6b6b90bab0b617082f Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Fri, 26 Oct 2012 08:52:17 -0700 Subject: [PATCH 085/405] no need to depend on a meta-package --- pcl_ros/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 6eb60fd8..02003a44 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -16,7 +16,6 @@ Open Perception catkin - common_msgs eigen geometry pcl @@ -25,7 +24,6 @@ std_msgs tf - common_msgs eigen geometry pcl From 9694594f7ce15c6c2fc564694feddb964c786db5 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Fri, 26 Oct 2012 08:53:21 -0700 Subject: [PATCH 086/405] 1.0.16 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 02003a44..05355e30 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.15 + 1.0.16

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 34becd4d..f7601f8d 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.15 + 1.0.16

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From fe018e31741442200984b20553f85df6d72d7273 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Fri, 26 Oct 2012 08:56:20 -0700 Subject: [PATCH 087/405] remove useless tags --- pcl_ros/package.xml | 4 +--- perception_pcl/package.xml | 3 +-- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 05355e30..f2235910 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros 1.0.16 - +

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry @@ -11,9 +11,7 @@ Open Perception Julius Kammerl BSD - http://ros.org/wiki/perception_pcl - Open Perception catkin eigen diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index f7601f8d..2011d090 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,7 +1,7 @@ perception_pcl 1.0.16 - +

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry @@ -12,7 +12,6 @@ Julius Kammerl BSD http://ros.org/wiki/perception_pcl - Open Perception catkin From 25e6d566f1eca650987cf4ae3a21062579d536ae Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Fri, 26 Oct 2012 09:28:03 -0700 Subject: [PATCH 088/405] 1.0.17 --- pcl_ros/package.xml | 4 +--- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index f2235910..85c86090 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.16 + 1.0.17

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred @@ -15,7 +15,6 @@ catkin eigen - geometry pcl roscpp sensor_msgs @@ -23,7 +22,6 @@ tf eigen - geometry pcl roscpp sensor_msgs diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 2011d090..0ed20b87 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.16 + 1.0.17

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From d5d9e3816a2372efca4b207cc94a595e7a1aad67 Mon Sep 17 00:00:00 2001 From: mirzashah Date: Fri, 14 Dec 2012 15:02:54 -0800 Subject: [PATCH 089/405] Updated for new catkin<...> catkin rule --- pcl_ros/package.xml | 3 ++- perception_pcl/package.xml | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 85c86090..4f92ec85 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -13,7 +13,8 @@ BSD http://ros.org/wiki/perception_pcl - catkin + catkin + eigen pcl roscpp diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 0ed20b87..c31c549a 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -13,7 +13,7 @@ BSD http://ros.org/wiki/perception_pcl - catkin + catkin pcl_ros From a3701bb3df14ba08df641fd6967264b73e15c060 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 17 Dec 2012 18:09:15 -0800 Subject: [PATCH 090/405] migrating nodelets and tools from fuerte release to pcl_ros --- pcl_ros/CMakeLists.txt | 72 ++- pcl_ros/cfg/ExtractIndices.cfg | 14 + pcl_ros/cfg/Filter.cfg | 15 + pcl_ros/cfg/Filter_common.py | 19 + pcl_ros/cfg/Filter_common.pyc | Bin 0 -> 1614 bytes pcl_ros/cfg/StatisticalOutlierRemoval.cfg | 18 + pcl_ros/cfg/VoxelGrid.cfg | 16 + .../include/pcl_ros/filters/extract_indices.h | 94 ++++ pcl_ros/include/pcl_ros/filters/filter.h | 149 ++++++ pcl_ros/include/pcl_ros/filters/passthrough.h | 94 ++++ .../include/pcl_ros/filters/project_inliers.h | 104 ++++ .../pcl_ros/filters/radius_outlier_removal.h | 82 ++++ .../filters/statistical_outlier_removal.h | 102 ++++ pcl_ros/include/pcl_ros/filters/voxel_grid.h | 89 ++++ pcl_ros/include/pcl_ros/io/bag_io.h | 130 +++++ pcl_ros/include/pcl_ros/io/concatenate_data.h | 134 ++++++ .../include/pcl_ros/io/concatenate_fields.h | 100 ++++ pcl_ros/include/pcl_ros/io/pcd_io.h | 132 ++++++ pcl_ros/include/pcl_ros/pcl_nodelet.h | 228 +++++++++ pcl_ros/include/pcl_ros/publisher.h | 4 +- pcl_ros/package.xml | 12 +- pcl_ros/pcl_nodelets.xml | 86 ++++ pcl_ros/src/pcl_ros/__init__.py | 0 .../src/pcl_ros/filters/extract_indices.cpp | 70 +++ .../src/pcl_ros/filters/features/boundary.cpp | 77 +++ .../src/pcl_ros/filters/features/feature.cpp | 443 ++++++++++++++++++ pcl_ros/src/pcl_ros/filters/features/fpfh.cpp | 79 ++++ .../src/pcl_ros/filters/features/fpfh_omp.cpp | 79 ++++ .../filters/features/moment_invariants.cpp | 77 +++ .../pcl_ros/filters/features/normal_3d.cpp | 77 +++ .../filters/features/normal_3d_omp.cpp | 77 +++ .../filters/features/normal_3d_tbb.cpp | 81 ++++ pcl_ros/src/pcl_ros/filters/features/pfh.cpp | 79 ++++ .../filters/features/principal_curvatures.cpp | 79 ++++ pcl_ros/src/pcl_ros/filters/features/vfh.cpp | 79 ++++ pcl_ros/src/pcl_ros/filters/filter.cpp | 230 +++++++++ pcl_ros/src/pcl_ros/filters/passthrough.cpp | 120 +++++ .../src/pcl_ros/filters/project_inliers.cpp | 144 ++++++ .../filters/radius_outlier_removal.cpp | 43 ++ .../filters/statistical_outlier_removal.cpp | 81 ++++ pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 124 +++++ pcl_ros/tools/bag_to_pcd.cpp | 149 ++++++ pcl_ros/tools/convert_pcd_to_image.cpp | 94 ++++ pcl_ros/tools/convert_pointcloud_to_image.cpp | 97 ++++ pcl_ros/tools/pcd_to_pointcloud.cpp | 156 ++++++ pcl_ros/tools/pointcloud_to_pcd.cpp | 122 +++++ 46 files changed, 4331 insertions(+), 20 deletions(-) create mode 100755 pcl_ros/cfg/ExtractIndices.cfg create mode 100755 pcl_ros/cfg/Filter.cfg create mode 100644 pcl_ros/cfg/Filter_common.py create mode 100644 pcl_ros/cfg/Filter_common.pyc create mode 100755 pcl_ros/cfg/StatisticalOutlierRemoval.cfg create mode 100755 pcl_ros/cfg/VoxelGrid.cfg create mode 100644 pcl_ros/include/pcl_ros/filters/extract_indices.h create mode 100644 pcl_ros/include/pcl_ros/filters/filter.h create mode 100644 pcl_ros/include/pcl_ros/filters/passthrough.h create mode 100644 pcl_ros/include/pcl_ros/filters/project_inliers.h create mode 100644 pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h create mode 100644 pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h create mode 100644 pcl_ros/include/pcl_ros/filters/voxel_grid.h create mode 100644 pcl_ros/include/pcl_ros/io/bag_io.h create mode 100644 pcl_ros/include/pcl_ros/io/concatenate_data.h create mode 100644 pcl_ros/include/pcl_ros/io/concatenate_fields.h create mode 100644 pcl_ros/include/pcl_ros/io/pcd_io.h create mode 100644 pcl_ros/include/pcl_ros/pcl_nodelet.h create mode 100644 pcl_ros/pcl_nodelets.xml create mode 100644 pcl_ros/src/pcl_ros/__init__.py create mode 100644 pcl_ros/src/pcl_ros/filters/extract_indices.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/boundary.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/feature.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/fpfh.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/pfh.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/vfh.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/filter.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/passthrough.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/project_inliers.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/voxel_grid.cpp create mode 100644 pcl_ros/tools/bag_to_pcd.cpp create mode 100644 pcl_ros/tools/convert_pcd_to_image.cpp create mode 100644 pcl_ros/tools/convert_pointcloud_to_image.cpp create mode 100644 pcl_ros/tools/pcd_to_pointcloud.cpp create mode 100644 pcl_ros/tools/pointcloud_to_pcd.cpp diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 3f4aac2e..53275398 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -3,7 +3,7 @@ project(pcl_ros) # Deal with catkin find_package(Boost COMPONENTS system filesystem thread REQUIRED) -find_package(catkin REQUIRED genmsg roscpp sensor_msgs std_msgs tf) +find_package(catkin REQUIRED dynamic_reconfigure genmsg roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib) find_package(Eigen) find_package(PCL) @@ -17,28 +17,66 @@ include_directories(include) link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) -catkin_package(DEPENDS Eigen PCL roscpp sensor_msgs tf - INCLUDE_DIRS include - LIBRARIES pcl_ros_tf -) +# generate the dynamic_reconfigure config file +generate_dynamic_reconfigure_options(cfg/Filter.cfg + cfg/ExtractIndices.cfg + cfg/StatisticalOutlierRemoval.cfg + cfg/VoxelGrid.cfg -# create messages -### what's below is just to generate the messages that are manually patched into PCL -###project(pcl) -###add_message_files(DIRECTORY msg -### FILES ModelCoefficients.msg -### PointIndices.msg -### PolygonMesh.msg -### Vertices.msg -###) -###generate_messages(DEPENDENCIES sensor_msgs std_msgs) -###project(pcl_ros) +) +include_directories(include cfg/cpp) + +catkin_package() # ---[ Point Cloud Library - Transforms add_library (pcl_ros_tf SHARED src/transforms.cpp) -target_link_libraries(pcl_ros_tf ${PCL_LIBS} ${Boost_LIBS} ${catkin_LIBS}) +target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) add_dependencies(pcl_ros_tf ros_gencpp pcl_ros_copy) +############ NODELETS + +# ---[ Point Cloud Library - IO +add_library (pcl_ros_io + src/pcl_ros/io/io.cpp + src/pcl_ros/io/concatenate_fields.cpp + src/pcl_ros/io/concatenate_data.cpp + src/pcl_ros/io/pcd_io.cpp + src/pcl_ros/io/bag_io.cpp + ) +#rosbuild_add_compile_flags (pcl_ros_io ${SSE_FLAGS}) +target_link_libraries (pcl_ros_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +# ---[ PCL ROS - Filters +add_library (pcl_ros_filters + src/pcl_ros/filters/filter.cpp + src/pcl_ros/filters/passthrough.cpp + src/pcl_ros/filters/project_inliers.cpp + src/pcl_ros/filters/extract_indices.cpp + src/pcl_ros/filters/radius_outlier_removal.cpp + src/pcl_ros/filters/statistical_outlier_removal.cpp + src/pcl_ros/filters/voxel_grid.cpp + ) +#add_compile_flags (pcl_ros_filters ${SSE_FLAGS}) +target_link_libraries (pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +############ TOOLS + +add_executable (pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) +target_link_libraries (pcd_to_pointcloud pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +add_executable (pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) +target_link_libraries (pointcloud_to_pcd pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +add_executable (bag_to_pcd tools/bag_to_pcd.cpp) +target_link_libraries (bag_to_pcd pcl_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +add_executable (convert_pcd_to_image tools/convert_pcd_to_image.cpp) +target_link_libraries (convert_pcd_to_image pcl_io pcl_common ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +add_executable (convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) +target_link_libraries (convert_pointcloud_to_image pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) diff --git a/pcl_ros/cfg/ExtractIndices.cfg b/pcl_ros/cfg/ExtractIndices.cfg new file mode 100755 index 00000000..b3ed9cde --- /dev/null +++ b/pcl_ros/cfg/ExtractIndices.cfg @@ -0,0 +1,14 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +import roslib.packages + +gen = ParameterGenerator () +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("negative", bool_t, 0, "Extract indices or the negative (all-indices)", False) + +exit (gen.generate (PACKAGE, "pcl_ros", "ExtractIndices")) diff --git a/pcl_ros/cfg/Filter.cfg b/pcl_ros/cfg/Filter.cfg new file mode 100755 index 00000000..bcda6baf --- /dev/null +++ b/pcl_ros/cfg/Filter.cfg @@ -0,0 +1,15 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; +roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +import Filter_common as common + +gen = ParameterGenerator () +common.add_common_parameters (gen) + +exit (gen.generate (PACKAGE, "pcl_ros", "Filter")) + diff --git a/pcl_ros/cfg/Filter_common.py b/pcl_ros/cfg/Filter_common.py new file mode 100644 index 00000000..b414c378 --- /dev/null +++ b/pcl_ros/cfg/Filter_common.py @@ -0,0 +1,19 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; +roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; + +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", 0.0, -5.0, 5.0) + gen.add ("filter_limit_max", double_t, 0, "The maximum allowed field value a point will be considered from", 1.0, -5.0, 5.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].", 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.", 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.", "") + diff --git a/pcl_ros/cfg/Filter_common.pyc b/pcl_ros/cfg/Filter_common.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9ccbd7322de5f0d9972c36eb357be0a1b44e81e8 GIT binary patch literal 1614 zcmcIkOK%i85OzWq(!mxw>XPtSr5J`))fI-P6PWJ&bS%LJRQ> zG7-8bY$Bc|f)cUc6C}iGB9dsB5q3uG>RIsuduQXuoz*!UOJZmdR&@A*-`W zm1Lk4;TFX19?4cOqh2$}ux03kFHK>rDGg_(slwe30CQ6|rBv3sQ3QyN ziR5e5#|VPj&Q*KHuNvYjE9> zGtR1_p<;7L;VCRT#FCw4k2FjnMa;ZD2RRw zmKhufQk$Ycja1xc_!PVh0g}HBWvu+MSX6=1{)te=jbsL)D6Khm6>ya?$F8AWGq8lk zJQf4zET(bCrm}&?ttK8EQniXd(Tea0vvf2}13P~^-9J!eN#3SU9`B^NV%$v{e4f_i zb6mJ_RfYzZc18wPR78IEOghn5uQ + +#include "pcl_ros/filters/filter.h" +#include "pcl_ros/ExtractIndicesConfig.h" + +namespace pcl_ros +{ + /** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ + 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 + * \param output the resultant filtered dataset + */ + inline void + filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, + PointCloud2 &output) + { + boost::mutex::scoped_lock lock (mutex_); + impl_.setInputCloud (input); + impl_.setIndices (indices); + impl_.filter (output); + } + + /** \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); + + /** \brief Dynamic reconfigure service callback. */ + void + config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level); + + private: + /** \brief The PCL filter implementation used. */ + pcl::ExtractIndices impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ + diff --git a/pcl_ros/include/pcl_ros/filters/filter.h b/pcl_ros/include/pcl_ros/filters/filter.h new file mode 100644 index 00000000..24fca0b0 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/filter.h @@ -0,0 +1,149 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: filter.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef PCL_ROS_FILTER_H_ +#define PCL_ROS_FILTER_H_ + +// PCL includes +#include +#include "pcl_ros/pcl_nodelet.h" + +// Dynamic reconfigure +#include +#include "pcl_ros/FilterConfig.h" + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + /** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters + * are defined here as static methods. + * \author Radu Bogdan Rusu + */ + class Filter : public PCLNodelet + { + public: + typedef sensor_msgs::PointCloud2 PointCloud2; + + typedef boost::shared_ptr > IndicesPtr; + typedef boost::shared_ptr > IndicesConstPtr; + + Filter () {} + + protected: + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + message_filters::Subscriber sub_input_filter_; + + /** \brief The desired user filter field name. */ + std::string filter_field_name_; + + /** \brief The minimum allowed filter value a point will be considered from. */ + double filter_limit_min_; + + /** \brief The maximum allowed filter value a point will be considered from. */ + double filter_limit_max_; + + /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ + bool filter_limit_negative_; + + /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_input_frame_; + + /** \brief The original data input TF frame. */ + std::string tf_input_orig_frame_; + + /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + 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); + } + + /** \brief Virtual abstract filter method. To be implemented by every child. + * \param input the input point cloud dataset. + * \param indices a pointer to the vector of point indices to use. + * \param output the resultant filtered PointCloud2 + */ + virtual void + filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, + PointCloud2 &output) = 0; + + /** \brief Nodelet initialization routine. */ + virtual void + onInit (); + + /** \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); + + private: + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \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 PointCloud2 + Indices data callback. */ + void + input_indices_callback (const PointCloud2::ConstPtr &cloud, + const PointIndicesConstPtr &indices); + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FILTER_H_ diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.h b/pcl_ros/include/pcl_ros/filters/passthrough.h new file mode 100644 index 00000000..c21a7d1b --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/passthrough.h @@ -0,0 +1,94 @@ +/* + * 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: passthrough.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ +#define PCL_ROS_FILTERS_PASSTHROUGH_H_ + +// PCL includes +#include +#include "pcl_ros/filters/filter.h" + +namespace pcl_ros +{ + /** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given + * constraints. + * \author Radu Bogdan Rusu + */ + 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 + * \param output the resultant filtered dataset + */ + inline void + filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, + PointCloud2 &output) + { + boost::mutex::scoped_lock lock (mutex_); + impl_.setInputCloud (input); + impl_.setIndices (indices); + impl_.filter (output); + } + + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + bool + child_init (ros::NodeHandle &nh, bool &has_service); + + /** \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); + + private: + /** \brief The PCL filter implementation used. */ + pcl::PassThrough impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.h b/pcl_ros/include/pcl_ros/filters/project_inliers.h new file mode 100644 index 00000000..200ef10e --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.h @@ -0,0 +1,104 @@ +/* + * 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: project_inliers.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef PCL_ROS_FILTERS_PROJECT_INLIERS_H_ +#define PCL_ROS_FILTERS_PROJECT_INLIERS_H_ + +// PCL includes +#include +#include "pcl_ros/filters/filter.h" + +#include + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + /** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a + * separate PointCloud. + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ + class ProjectInliers : public Filter + { + public: + ProjectInliers () : model_ () {} + + protected: + /** \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 + */ + inline void + filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, + PointCloud2 &output) + { + impl_.setInputCloud (input); + impl_.setIndices (indices); + impl_.setModelCoefficients (model_); + impl_.filter (output); + } + + private: + /** \brief A pointer to the vector of model coefficients. */ + ModelCoefficientsConstPtr model_; + + /** \brief The message filter subscriber for model coefficients. */ + 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 PointCloud2 + Indices + Model data callback. */ + void + input_indices_model_callback (const PointCloud2::ConstPtr &cloud, + const PointIndicesConstPtr &indices, + const ModelCoefficientsConstPtr &model); + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_ diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h new file mode 100644 index 00000000..fc316a83 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h @@ -0,0 +1,82 @@ +/* + * 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: radius_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_ +#define PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_ + +// PCL includes +#include +#include "pcl_ros/filters/filter.h" + +namespace pcl_ros +{ + /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain + * search radius is smaller than a given K. + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ + class RadiusOutlierRemoval : public Filter + { + protected: + /** \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 + */ + inline void + filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, + PointCloud2 &output) + { + impl_.setInputCloud (input); + impl_.setIndices (indices); + impl_.filter (output); + } + + /** \brief Child initialization routine. + * \param nh ROS node handle + */ + virtual inline bool child_init (ros::NodeHandle &nh) { return (true); } + + private: + /** \brief The PCL filter implementation used. */ + pcl::RadiusOutlierRemoval impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_ diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h new file mode 100644 index 00000000..e97735d5 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h @@ -0,0 +1,102 @@ +/* + * 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: statistical_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_ +#define PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_ + +// PCL includes +#include +#include "pcl_ros/filters/filter.h" + +// Dynamic reconfigure +#include "pcl_ros/StatisticalOutlierRemovalConfig.h" + +namespace pcl_ros +{ + /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more + * information check: + *

    + *
  • R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. + * Towards 3D Point Cloud Based Object Maps for Household Environments + * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. + *
+ * + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ + 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 + * \param output the resultant filtered dataset + */ + inline void + filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, + PointCloud2 &output) + { + boost::mutex::scoped_lock lock (mutex_); + impl_.setInputCloud (input); + impl_.setIndices (indices); + impl_.filter (output); + } + + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + bool child_init (ros::NodeHandle &nh, bool &has_service); + + /** \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); + + private: + /** \brief The PCL filter implementation used. */ + pcl::StatisticalOutlierRemoval impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_ diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.h b/pcl_ros/include/pcl_ros/filters/voxel_grid.h new file mode 100644 index 00000000..95942259 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.h @@ -0,0 +1,89 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: voxel_grid.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef PCL_ROS_FILTERS_VOXEL_H_ +#define PCL_ROS_FILTERS_VOXEL_H_ + +// PCL includes +#include +#include "pcl_ros/filters/filter.h" + +// Dynamic reconfigure +#include "pcl_ros/VoxelGridConfig.h" + +namespace pcl_ros +{ + /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. + * \author Radu Bogdan Rusu + */ + 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 + filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, + PointCloud2 &output); + + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + bool + child_init (ros::NodeHandle &nh, bool &has_service); + + /** \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); + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_ diff --git a/pcl_ros/include/pcl_ros/io/bag_io.h b/pcl_ros/include/pcl_ros/io/bag_io.h new file mode 100644 index 00000000..18f53de2 --- /dev/null +++ b/pcl_ros/include/pcl_ros/io/bag_io.h @@ -0,0 +1,130 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: bag_io.h 35471 2011-01-25 06:50:00Z rusu $ + * + */ + +#ifndef PCL_ROS_IO_BAG_IO_H_ +#define PCL_ROS_IO_BAG_IO_H_ + +#include +#include +#include +#include + +namespace pcl_ros +{ + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief BAG PointCloud file format reader. + * \author Radu Bogdan Rusu + */ + class BAGReader: public nodelet::Nodelet + { + public: + typedef sensor_msgs::PointCloud2 PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + /** \brief Empty constructor. */ + BAGReader () : publish_rate_ (0), output_ ()/*, cloud_received_ (false)*/ {}; + + /** \brief Set the publishing rate in seconds. + * \param publish_rate the publishing rate in seconds + */ + inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; } + + /** \brief Get the publishing rate in seconds. */ + inline double getPublishRate () { return (publish_rate_); } + + /** \brief Get the next point cloud dataset in the BAG file. + * \return the next point cloud dataset as read from the file + */ + inline PointCloudConstPtr + getNextCloud () + { + if (it_ != view_.end ()) + { + output_ = it_->instantiate (); + ++it_; + } + return (output_); + } + + /** \brief Open a BAG file for reading and select a specified topic + * \param file_name the BAG file to open + * \param topic_name the topic that we want to read data from + */ + bool open (const std::string &file_name, const std::string &topic_name); + + /** \brief Close an open BAG file. */ + inline void + close () + { + bag_.close (); + } + + /** \brief Nodelet initialization routine. */ + virtual void onInit (); + + private: + /** \brief The publishing interval in seconds. Set to 0 to publish once (default). */ + double publish_rate_; + + /** \brief The BAG object. */ + rosbag::Bag bag_; + + /** \brief The BAG view object. */ + rosbag::View view_; + + /** \brief The BAG view iterator object. */ + rosbag::View::iterator it_; + + /** \brief The name of the topic that contains the PointCloud data. */ + std::string topic_name_; + + /** \brief The name of the BAG file that contains the PointCloud data. */ + std::string file_name_; + + /** \brief The output point cloud dataset containing the points loaded from the file. */ + PointCloudPtr output_; + + /** \brief Signals that a new PointCloud2 message has been read from the file. */ + //bool cloud_received_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +} + +#endif //#ifndef PCL_ROS_IO_BAG_IO_H_ diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.h b/pcl_ros/include/pcl_ros/io/concatenate_data.h new file mode 100644 index 00000000..7f00c6d5 --- /dev/null +++ b/pcl_ros/include/pcl_ros/io/concatenate_data.h @@ -0,0 +1,134 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: concatenate_data.h 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#ifndef PCL_IO_CONCATENATE_DATA_H_ +#define PCL_IO_CONCATENATE_DATA_H_ + +// ROS includes +#include +#include +#include +#include +#include +#include +#include + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data + * synchronizer: it listens for a set of input PointCloud messages on the same topic, + * checks their timestamps, and concatenates their fields together into a single + * PointCloud output message. + * \author Radu Bogdan Rusu + */ + class PointCloudConcatenateDataSynchronizer: public nodelet::Nodelet + { + public: + typedef sensor_msgs::PointCloud2 PointCloud2; + typedef PointCloud2::Ptr PointCloud2Ptr; + typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + + /** \brief Empty constructor. */ + PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {}; + + /** \brief Empty constructor. + * \param queue_size the maximum queue size + */ + PointCloudConcatenateDataSynchronizer (int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {}; + + /** \brief Empty destructor. */ + virtual ~PointCloudConcatenateDataSynchronizer () {}; + + void onInit (); + + private: + /** \brief ROS local node handle. */ + ros::NodeHandle private_nh_; + + /** \brief The output PointCloud publisher. */ + ros::Publisher pub_output_; + + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_; + + /** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ + bool approximate_sync_; + + /** \brief A vector of message filters. */ + std::vector > > filters_; + + /** \brief Output TF frame the concatenated points should be transformed to. */ + std::string output_frame_; + + /** \brief TF listener object. */ + tf::TransformListener tf_; + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_; + + /** \brief Synchronizer. + * \note This will most likely be rewritten soon using the DynamicTimeSynchronizer. + */ + boost::shared_ptr > > ts_a_; + boost::shared_ptr > > ts_e_; + + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. + */ + inline void + input_callback (const PointCloud2ConstPtr &input) + { + PointCloud2 cloud; + cloud.header.stamp = input->header.stamp; + nf_.add (boost::make_shared (cloud)); + } + + /** \brief Input callback for 8 synchronized topics. */ + void input (const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, + const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, + const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, + const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8); + + void combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out); + }; +} + +#endif //#ifndef PCL_ROS_IO_CONCATENATE_H_ diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.h b/pcl_ros/include/pcl_ros/io/concatenate_fields.h new file mode 100644 index 00000000..f6252b68 --- /dev/null +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.h @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: concatenate_fields.h 35052 2011-01-03 21:04:57Z rusu $ + * + */ + +#ifndef PCL_IO_CONCATENATE_FIELDS_H_ +#define PCL_IO_CONCATENATE_FIELDS_H_ + +// ROS includes +#include +#include +#include +#include +#include + +namespace pcl_ros +{ + /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of + * input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into + * a single PointCloud output message. + * \author Radu Bogdan Rusu + */ + class PointCloudConcatenateFieldsSynchronizer: public nodelet::Nodelet + { + public: + typedef sensor_msgs::PointCloud2 PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + /** \brief Empty constructor. */ + PointCloudConcatenateFieldsSynchronizer () : maximum_queue_size_ (3), maximum_seconds_ (0) {}; + + /** \brief Empty constructor. + * \param queue_size the maximum queue size + */ + PointCloudConcatenateFieldsSynchronizer (int queue_size) : maximum_queue_size_ (queue_size), maximum_seconds_ (0) {}; + + /** \brief Empty destructor. */ + virtual ~PointCloudConcatenateFieldsSynchronizer () {}; + + void onInit (); + void input_callback (const PointCloudConstPtr &cloud); + + private: + /** \brief ROS local node handle. */ + ros::NodeHandle private_nh_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief The output PointCloud publisher. */ + ros::Publisher pub_output_; + + /** \brief The number of input messages that we expect on the input topic. */ + int input_messages_; + + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_; + + /** \brief The maximum number of seconds to wait until we drop the synchronization. */ + double maximum_seconds_; + + /** \brief A queue for messages. */ + std::map > queue_; + }; +} + +#endif //#ifndef PCL_IO_CONCATENATE_H_ diff --git a/pcl_ros/include/pcl_ros/io/pcd_io.h b/pcl_ros/include/pcl_ros/io/pcd_io.h new file mode 100644 index 00000000..89663451 --- /dev/null +++ b/pcl_ros/include/pcl_ros/io/pcd_io.h @@ -0,0 +1,132 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: pcd_io.h 35054 2011-01-03 21:16:49Z rusu $ + * + */ + +#ifndef PCL_ROS_IO_PCD_IO_H_ +#define PCL_ROS_IO_PCD_IO_H_ + +#include +#include "pcl_ros/pcl_nodelet.h" + +namespace pcl_ros +{ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Point Cloud Data (PCD) file format reader. + * \author Radu Bogdan Rusu + */ + class PCDReader : public PCLNodelet + { + public: + typedef sensor_msgs::PointCloud2 PointCloud2; + typedef PointCloud2::Ptr PointCloud2Ptr; + typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + + /** \brief Empty constructor. */ + PCDReader () : publish_rate_ (0), tf_frame_ ("/base_link") {}; + + virtual void onInit (); + + /** \brief Set the publishing rate in seconds. + * \param publish_rate the publishing rate in seconds + */ + inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; } + + /** \brief Get the publishing rate in seconds. */ + inline double getPublishRate () { return (publish_rate_); } + + /** \brief Set the TF frame the PointCloud will be published in. + * \param tf_frame the TF frame the PointCloud will be published in + */ + inline void setTFframe (std::string tf_frame) { tf_frame_ = tf_frame; } + + /** \brief Get the TF frame the PointCloud is published in. */ + inline std::string getTFframe () { return (tf_frame_); } + + protected: + /** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */ + double publish_rate_; + + /** \brief The TF frame the data should be published in ("/base_link" by default). */ + std::string tf_frame_; + + /** \brief The name of the file that contains the PointCloud data. */ + std::string file_name_; + + /** \brief The output point cloud dataset containing the points loaded from the file. */ + PointCloud2Ptr output_; + + private: + /** \brief The PCL implementation used. */ + pcl::PCDReader impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Point Cloud Data (PCD) file format writer. + * \author Radu Bogdan Rusu + */ + class PCDWriter : public PCLNodelet + { + public: + PCDWriter () : file_name_ (""), binary_mode_ (true) {} + + typedef sensor_msgs::PointCloud2 PointCloud2; + typedef PointCloud2::Ptr PointCloud2Ptr; + typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + + virtual void onInit (); + void input_callback (const PointCloud2ConstPtr &cloud); + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + protected: + /** \brief The name of the file that contains the PointCloud data. */ + std::string file_name_; + + /** \brief Set to true if the output files should be saved in binary mode (true). */ + bool binary_mode_; + + private: + /** \brief The PCL implementation used. */ + pcl::PCDWriter impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_IO_PCD_IO_H_ diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.h b/pcl_ros/include/pcl_ros/pcl_nodelet.h new file mode 100644 index 00000000..9fef5ba7 --- /dev/null +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.h @@ -0,0 +1,228 @@ +/* + * 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_NODELET_H_ +#define PCL_NODELET_H_ + +#include +// PCL includes +#include +#include +#include +#include "pcl_ros/point_cloud.h" +// ROS Nodelet includes +#include +#include +#include +#include +#include + +// Include TF +#include + +namespace pcl_ros +{ + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */ + class PCLNodelet : public nodelet::Nodelet + { + public: + typedef sensor_msgs::PointCloud2 PointCloud2; + + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + typedef pcl::PointIndices PointIndices; + typedef PointIndices::Ptr PointIndicesPtr; + typedef PointIndices::ConstPtr PointIndicesConstPtr; + + typedef pcl::ModelCoefficients ModelCoefficients; + typedef ModelCoefficients::Ptr ModelCoefficientsPtr; + typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; + + typedef boost::shared_ptr > IndicesPtr; + typedef boost::shared_ptr > IndicesConstPtr; + + /** \brief Empty constructor. */ + PCLNodelet () : use_indices_ (false), latched_indices_ (false), + max_queue_size_ (3), approximate_sync_ (false) {}; + + protected: + /** \brief The ROS NodeHandle used for parameters, publish/subscribe, etc. */ + boost::shared_ptr pnh_; + + /** \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, 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 Nodelet initialization routine. Reads in global parameters used by all nodelets. */ + virtual void + onInit () + { + pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); + + // 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 + }; +} + +#endif //#ifndef PCL_NODELET_H_ diff --git a/pcl_ros/include/pcl_ros/publisher.h b/pcl_ros/include/pcl_ros/publisher.h index e97d2ea1..7ebc0984 100644 --- a/pcl_ros/include/pcl_ros/publisher.h +++ b/pcl_ros/include/pcl_ros/publisher.h @@ -42,8 +42,8 @@ **/ -#ifndef pcl_ros_PUBLISHER_H_ -#define pcl_ros_PUBLISHER_H_ +#ifndef PCL_ROS_PUBLISHER_H_ +#define PCL_ROS_PUBLISHER_H_ #include #include "pcl/ros/conversions.h" diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 4f92ec85..86be1cdc 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -14,18 +14,28 @@ http://ros.org/wiki/perception_pcl catkin - + + dynamic_reconfigure eigen pcl roscpp sensor_msgs std_msgs + rosbag tf + dynamic_reconfigure eigen pcl roscpp sensor_msgs std_msgs + rosbag tf + + + + + + diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml new file mode 100644 index 00000000..e6cd0f90 --- /dev/null +++ b/pcl_ros/pcl_nodelets.xml @@ -0,0 +1,86 @@ + + + + + + + NodeletMUX represent a mux nodelet for PointCloud topics: it takes N (up + to 8) input topics, and publishes all of them on one output topic. + + + + + + NodeletDEMUX represent a demux nodelet for PointCloud topics: it + publishes 1 input topic to N output topics. + + + + + + PCDReader reads in a PCD (Point Cloud Data) v.5 file from disk and converts it to a PointCloud message. + + + + + + BAGReader reads in sensor_msgs/PointCloud2 messages from BAG files. + + + + + + PCDWriter writes a PointCloud message to disk in a PCD (Point Cloud Data) v.5 file format. + + + + + + PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of input PointCloud messages on the + same topic, checks their timestamps, and concatenates their fields together into a single PointCloud output message. + + + + + PointCloudConcatenateDataSynchronizer is a special form of data + synchronizer: it listens for a set of input PointCloud messages on + different topics, and concatenates them together into a single PointCloud + output message. + + + + + + + + + PassThrough is a filter that uses the basic Filter class mechanisms for passing data around. + + + + + + VoxelGrid assembles a local 3D grid over a given PointCloud, and uses that to downsample the data. + + + + + + ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a separate PointCloud. + + + + + + ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. + + + + + + StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. + + + + + diff --git a/pcl_ros/src/pcl_ros/__init__.py b/pcl_ros/src/pcl_ros/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp new file mode 100644 index 00000000..2d596cd9 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -0,0 +1,70 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: extract_indices.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#include +#include "pcl_ros/filters/extract_indices.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl_ros::ExtractIndices::child_init (ros::NodeHandle &nh, bool &has_service) +{ + has_service = true; + + srv_ = boost::make_shared > (nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&ExtractIndices::config_callback, this, _1, _2); + srv_->setCallback (f); + + use_indices_ = true; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ExtractIndices::config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock lock (mutex_); + + if (impl_.getNegative () != config.negative) + { + impl_.setNegative (config.negative); + NODELET_DEBUG ("[%s::config_callback] Setting the extraction to: %s.", getName ().c_str (), (config.negative ? "indices" : "everything but the indices")); + } +} + +typedef pcl_ros::ExtractIndices ExtractIndices; +PLUGINLIB_DECLARE_CLASS (pcl, ExtractIndices, ExtractIndices, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp new file mode 100644 index 00000000..0e17c5fd --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/boundary.h" + +void +pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::BoundaryEstimation BoundaryEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, BoundaryEstimation, BoundaryEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/feature.cpp b/pcl_ros/src/pcl_ros/filters/features/feature.cpp new file mode 100644 index 00000000..434707ee --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/feature.cpp @@ -0,0 +1,443 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: feature.cpp 35422 2011-01-24 20:04:44Z rusu $ + * + */ + +//#include +// Include the implementations here instead of compiling them separately to speed up compile time +//#include "normal_3d.cpp" +//#include "boundary.cpp" +//#include "fpfh.cpp" +//#include "fpfh_omp.cpp" +//#include "moment_invariants.cpp" +//#include "normal_3d_omp.cpp" +//#include "normal_3d_tbb.cpp" +//#include "pfh.cpp" +//#include "principal_curvatures.cpp" +//#include "vfh.cpp" +#include +#include "pcl_ros/features/feature.h" +#include + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // Call the child init + childInit (*pnh_); + + // Allow each individual class that inherits from us to declare their own Publisher + // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc + //pub_output_ = pnh_->template advertise ("output", max_queue_size_); + + // ---[ Mandatory parameters + if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) + { + NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + return; + } + if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) + { + NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + return; + } + + // ---[ Optional parameters + pnh_->getParam ("use_surface", use_surface_); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); + srv_->setCallback (f); + + // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages + if (use_indices_ || use_surface_) + { + // Create the objects here + if (approximate_sync_) + sync_input_surface_indices_a_ = boost::make_shared > >(max_queue_size_); + else + sync_input_surface_indices_e_ = boost::make_shared > >(max_queue_size_); + + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + if (use_indices_) + { + // If indices are enabled, subscribe to the indices + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + if (use_surface_) // Use both indices and surface + { + // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register + sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + if (approximate_sync_) + sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); + else + sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); + } + else // Use only indices + { + sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); + // surface not enabled, connect the input-indices duo and register + if (approximate_sync_) + sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); + else + sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); + } + } + else // Use only surface + { + sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); + // indices not enabled, connect the input-surface duo and register + sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + if (approximate_sync_) + sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); + else + sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); + } + // Register callbacks + if (approximate_sync_) + sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); + else + sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); + } + else + // Subscribe in an old fashion to input only (no filters) + sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName ().c_str (), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level) +{ + if (k_ != config.k_search) + { + k_ = config.k_search; + NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_); + } + if (search_radius_ != config.radius_search) + { + search_radius_ = config.radius_search; + NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0) + return; + + // If cloud is given, check if it's valid + if (!isValid (cloud)) + { + NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + // If surface is given, check if it's valid + if (cloud_surface && !isValid (cloud_surface, "surface")) + { + NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + /// DEBUG + if (cloud_surface) + if (indices) + NODELET_DEBUG ("[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ()); + + else + if (indices) + NODELET_DEBUG ("[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + /// + + + if ((int)(cloud->width * cloud->height) < k_) + { + NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height)); + emptyPublish (cloud); + return; + } + + // If indices given... + IndicesPtr vindices; + if (indices && !indices->header.frame_id.empty ()) + vindices.reset (new std::vector (indices->indices)); + + computePublish (cloud, cloud_surface, vindices); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // Call the child init + childInit (*pnh_); + + // Allow each individual class that inherits from us to declare their own Publisher + // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc + //pub_output_ = pnh_->template advertise ("output", max_queue_size_); + + // ---[ Mandatory parameters + if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) + { + NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + return; + } + if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) + { + NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + return; + } + // ---[ Optional parameters + pnh_->getParam ("use_surface", use_surface_); + + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); + srv_->setCallback (f); + + // Create the objects here + if (approximate_sync_) + sync_input_normals_surface_indices_a_ = boost::make_shared > > (max_queue_size_); + else + sync_input_normals_surface_indices_e_ = boost::make_shared > > (max_queue_size_); + + // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages + if (use_indices_ || use_surface_) + { + if (use_indices_) + { + // If indices are enabled, subscribe to the indices + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + if (use_surface_) // Use both indices and surface + { + // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register + sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + if (approximate_sync_) + sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); + else + sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); + } + else // Use only indices + { + sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); + if (approximate_sync_) + // surface not enabled, connect the input-indices duo and register + sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); + else + // surface not enabled, connect the input-indices duo and register + sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); + } + } + else // Use only surface + { + // indices not enabled, connect the input-surface duo and register + sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + + sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); + if (approximate_sync_) + sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); + else + sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); + } + } + else + { + sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); + + if (approximate_sync_) + sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); + else + sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); + } + + // Register callbacks + if (approximate_sync_) + sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); + else + sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName ().c_str (), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( + const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, + const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0) + return; + + // If cloud+normals is given, check if it's valid + if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) + { + NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + // If surface is given, check if it's valid + if (cloud_surface && !isValid (cloud_surface, "surface")) + { + NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + /// DEBUG + if (cloud_surface) + if (indices) + NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + else + if (indices) + NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + /// + + if ((int)(cloud->width * cloud->height) < k_) + { + NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height)); + emptyPublish (cloud); + return; + } + + // If indices given... + IndicesPtr vindices; + if (indices && !indices->header.frame_id.empty ()) + vindices.reset (new std::vector (indices->indices)); + + computePublish (cloud, cloud_normals, cloud_surface, vindices); +} + diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp new file mode 100644 index 00000000..7782fde9 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/fpfh.h" + +void +pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::FPFHEstimation FPFHEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimation, FPFHEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp new file mode 100644 index 00000000..51880a9f --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/fpfh_omp.h" + +void +pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; +PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimationOMP, FPFHEstimationOMP, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp new file mode 100644 index 00000000..85bd209a --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/moment_invariants.h" + +void +pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, MomentInvariantsEstimation, MomentInvariantsEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp new file mode 100644 index 00000000..a512c41f --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/normal_3d.h" + +void +pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::NormalEstimation NormalEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimation, NormalEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp new file mode 100644 index 00000000..e53fe9c7 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/normal_3d_omp.h" + +void +pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; +PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationOMP, NormalEstimationOMP, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp new file mode 100644 index 00000000..d6636f73 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: normal_3d_tbb.cpp 35625 2011-01-31 07:56:13Z gbiggs $ + * + */ + +#include +#include "pcl_ros/features/normal_3d_tbb.h" + +#if defined HAVE_TBB + +void +pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloud output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; +PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationTBB, NormalEstimationTBB, nodelet::Nodelet); + +#endif // HAVE_TBB + diff --git a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp new file mode 100644 index 00000000..f40e0bab --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: pfh.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/pfh.h" + +void +pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::PFHEstimation PFHEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, PFHEstimation, PFHEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp new file mode 100644 index 00000000..d6431160 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: principal_curvatures.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/principal_curvatures.h" + +void +pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, PrincipalCurvaturesEstimation, PrincipalCurvaturesEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp new file mode 100644 index 00000000..f13028e3 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: vfh.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/vfh.h" + +void +pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::VFHEstimation VFHEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, VFHEstimation, VFHEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp new file mode 100644 index 00000000..9fcf844c --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -0,0 +1,230 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: filter.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#include +#include "pcl_ros/transforms.h" +#include "pcl_ros/filters/filter.h" + +/*//#include +//#include +*/ + +/*//typedef pcl::PixelGrid PixelGrid; +//typedef pcl::FilterDimension FilterDimension; +*/ + +// Include the implementations instead of compiling them separately to speed up compile time +//#include "extract_indices.cpp" +//#include "passthrough.cpp" +//#include "project_inliers.cpp" +//#include "radius_outlier_removal.cpp" +//#include "statistical_outlier_removal.cpp" +//#include "voxel_grid.cpp" + +/*//PLUGINLIB_DECLARE_CLASS (pcl, PixelGrid, PixelGrid, nodelet::Nodelet); +//PLUGINLIB_DECLARE_CLASS (pcl, FilterDimension, FilterDimension, nodelet::Nodelet); +*/ + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices) +{ + PointCloud2 output; + // Call the virtual method in the child + filter (input, indices, output); + + PointCloud2::Ptr cloud_tf (new PointCloud2 (output)); // set the output by default + // Check whether the user has given a different output TF frame + if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_) + { + NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ()); + // Convert the cloud into the different frame + PointCloud2 cloud_transformed; + if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_)) + { + NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ()); + return; + } + cloud_tf.reset (new PointCloud2 (cloud_transformed)); + } + if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_) + // no tf_output_frame given, transform the dataset to its original frame + { + NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ()); + // Convert the cloud into the different frame + PointCloud2 cloud_transformed; + if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_)) + { + NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ()); + return; + } + cloud_tf.reset (new PointCloud2 (cloud_transformed)); + } + + // Publish a boost shared ptr + pub_output_.publish (cloud_tf); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // Call the child's local init + bool has_service = false; + if (!child_init (*pnh_, has_service)) + { + NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ()); + return; + } + + pub_output_ = pnh_->advertise ("output", max_queue_size_); + + // Enable the dynamic reconfigure service + if (!has_service) + { + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2); + srv_->setCallback (f); + } + + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + + if (approximate_sync_) + { + sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); + sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); + } + else + { + sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); + sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); + } + } + else + // Subscribe in an old fashion to input only (no filters) + sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl::PointIndicesConstPtr ())); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level) +{ + // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL + if (tf_input_frame_ != config.input_frame) + { + tf_input_frame_ = config.input_frame; + NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); + } + if (tf_output_frame_ != config.output_frame) + { + tf_output_frame_ = config.output_frame; + NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0) + return; + + // If cloud is given, check if it's valid + if (!isValid (cloud)) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + return; + } + + /// DEBUG + if (indices) + NODELET_DEBUG ("[%s::input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + /// + + // Check whether the user has given a different input TF frame + tf_input_orig_frame_ = cloud->header.frame_id; + PointCloud2::ConstPtr cloud_tf; + if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) + { + NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); + // Save the original frame ID + // Convert the cloud into the different frame + PointCloud2 cloud_transformed; + if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) + { + NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); + return; + } + cloud_tf = boost::make_shared (cloud_transformed); + } + else + cloud_tf = cloud; + + // Need setInputCloud () here because we have to extract x/y/z + IndicesPtr vindices; + if (indices) + vindices.reset (new std::vector (indices->indices)); + + computePublish (cloud_tf, vindices); +} + diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp new file mode 100644 index 00000000..e4f54f4b --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -0,0 +1,120 @@ +/* + * 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: passthrough.cpp 36194 2011-02-23 07:49:21Z rusu $ + * + */ + +#include +#include "pcl_ros/filters/passthrough.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl_ros::PassThrough::child_init (ros::NodeHandle &nh, bool &has_service) +{ + // Enable the dynamic reconfigure service + has_service = true; + srv_ = boost::make_shared > (nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2); + srv_->setCallback (f); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock lock (mutex_); + + double filter_min, filter_max; + impl_.getFilterLimits (filter_min, filter_max); + + // Check the current values for filter min-max + if (filter_min != config.filter_limit_min) + { + filter_min = config.filter_limit_min; + NODELET_DEBUG ("[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_min); + // Set the filter min-max if different + impl_.setFilterLimits (filter_min, filter_max); + } + // Check the current values for filter min-max + if (filter_max != config.filter_limit_max) + { + filter_max = config.filter_limit_max; + NODELET_DEBUG ("[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_max); + // Set the filter min-max if different + impl_.setFilterLimits (filter_min, filter_max); + } + + // Check the current value for the filter field + //std::string filter_field = impl_.getFilterFieldName (); + if (impl_.getFilterFieldName () != config.filter_field_name) + { + // Set the filter field if different + impl_.setFilterFieldName (config.filter_field_name); + NODELET_DEBUG ("[%s::config_callback] Setting the filter field name to: %s.", getName ().c_str (), config.filter_field_name.c_str ()); + } + + // Check the current value for keep_organized + if (impl_.getKeepOrganized () != config.keep_organized) + { + NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); + // Call the virtual method in the child + impl_.setKeepOrganized (config.keep_organized); + } + + // Check the current value for the negative flag + if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) + { + NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); + // Call the virtual method in the child + impl_.setFilterLimitsNegative (config.filter_limit_negative); + } + + // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL + if (tf_input_frame_ != config.input_frame) + { + tf_input_frame_ = config.input_frame; + NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); + } + if (tf_output_frame_ != config.output_frame) + { + tf_output_frame_ = config.output_frame; + NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); + } +} + +typedef pcl_ros::PassThrough PassThrough; +PLUGINLIB_DECLARE_CLASS (pcl, PassThrough, PassThrough, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp new file mode 100644 index 00000000..f498ba65 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -0,0 +1,144 @@ +/* + * 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: project_inliers.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#include +#include "pcl_ros/filters/project_inliers.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ProjectInliers::onInit () +{ + // No need to call the super onInit as we are overwriting everything + //Filter::onInit (); + PCLNodelet::onInit (); + + // ---[ Mandatory parameters + // The type of model to use (user given parameter). + int model_type; + if (!pnh_->getParam ("model_type", model_type)) + { + NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ()); + return; + } + // ---[ Optional parameters + // True if all data will be returned, false if only the projected inliers. Default: false. + bool copy_all_data = false; + + // True if all fields will be returned, false if only XYZ. Default: true. + bool copy_all_fields = true; + + pnh_->getParam ("copy_all_data", copy_all_data); + pnh_->getParam ("copy_all_fields", copy_all_fields); + + pub_output_ = pnh_->advertise ("output", max_queue_size_); + + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + +/* + TODO : implement use_indices_ + if (use_indices_) + {*/ + + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + + sub_model_.subscribe (*pnh_, "model", max_queue_size_); + + if (approximate_sync_) + { + sync_input_indices_model_a_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); + sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); + } + else + { + sync_input_indices_model_e_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); + sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); + } + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - copy_all_data : %s\n" + " - copy_all_fields : %s", + getName ().c_str (), + model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); + + // Set given parameters here + impl_.setModelType (model_type); + impl_.setCopyAllFields (copy_all_fields); + impl_.setCopyAllData (copy_all_data); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud, + const PointIndicesConstPtr &indices, + const ModelCoefficientsConstPtr &model) +{ + if (pub_output_.getNumSubscribers () <= 0) + return; + + if (!isValid (model) || !isValid (indices) || !isValid (cloud)) + { + NODELET_ERROR ("[%s::input_indices_model_callback] Invalid input!", getName ().c_str ()); + return; + } + + NODELET_DEBUG ("[%s::input_indices_model_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" + " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (), + model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ()); + + tf_input_orig_frame_ = cloud->header.frame_id; + + IndicesPtr vindices; + if (indices) + vindices.reset (new std::vector (indices->indices)); + + model_ = model; + computePublish (cloud, vindices); +} + +typedef pcl_ros::ProjectInliers ProjectInliers; +PLUGINLIB_DECLARE_CLASS (pcl, ProjectInliers, ProjectInliers, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp new file mode 100644 index 00000000..6aaf2fbb --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -0,0 +1,43 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: radius_outlier_removal.cpp 33319 2010-10-15 04:49:28Z rusu $ + * + */ + +#include +#include "pcl_ros/filters/radius_outlier_removal.h" + +typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; +PLUGINLIB_DECLARE_CLASS (pcl, RadiusOutlierRemoval, RadiusOutlierRemoval, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp new file mode 100644 index 00000000..74288f56 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: statistical_outlier_removal.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#include +#include "pcl_ros/filters/statistical_outlier_removal.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl_ros::StatisticalOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) +{ + // Enable the dynamic reconfigure service + has_service = true; + srv_ = boost::make_shared > (nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&StatisticalOutlierRemoval::config_callback, this, _1, _2); + srv_->setCallback (f); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock lock (mutex_); + + if (impl_.getMeanK () != config.mean_k) + { + impl_.setMeanK (config.mean_k); + NODELET_DEBUG ("[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", getName ().c_str (), config.mean_k); + } + + if (impl_.getStddevMulThresh () != config.stddev) + { + impl_.setStddevMulThresh (config.stddev); + NODELET_DEBUG ("[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", getName ().c_str (), config.stddev); + } + + if (impl_.getNegative () != config.negative) + { + impl_.setNegative (config.negative); + NODELET_DEBUG ("[%s::config_callback] Returning only inliers: %s.", getName ().c_str (), config.negative ? "false" : "true"); + } +} + +typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval; +PLUGINLIB_DECLARE_CLASS (pcl, StatisticalOutlierRemoval, StatisticalOutlierRemoval, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp new file mode 100644 index 00000000..55181f75 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#include +#include "pcl_ros/filters/voxel_grid.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl_ros::VoxelGrid::child_init (ros::NodeHandle &nh, bool &has_service) +{ + // Enable the dynamic reconfigure service + has_service = true; + srv_ = boost::make_shared > (nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2); + srv_->setCallback (f); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, + const IndicesPtr &indices, + PointCloud2 &output) +{ + boost::mutex::scoped_lock lock (mutex_); + + impl_.setInputCloud (input); + impl_.setIndices (indices); + impl_.filter (output); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock lock (mutex_); + + Eigen::Vector3f leaf_size = impl_.getLeafSize (); + + if (leaf_size[0] != config.leaf_size) + { + leaf_size.setConstant (config.leaf_size); + NODELET_DEBUG ("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]); + impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]); + } + + double filter_min, filter_max; + impl_.getFilterLimits (filter_min, filter_max); + if (filter_min != config.filter_limit_min) + { + filter_min = config.filter_limit_min; + NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min); + } + if (filter_max != config.filter_limit_max) + { + filter_max = config.filter_limit_max; + NODELET_DEBUG ("[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max); + } + impl_.setFilterLimits (filter_min, filter_max); + + if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) + { + impl_.setFilterLimitsNegative (config.filter_limit_negative); + NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); + } + + if (impl_.getFilterFieldName () != config.filter_field_name) + { + impl_.setFilterFieldName (config.filter_field_name); + NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ()); + } + + // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter + if (tf_input_frame_ != config.input_frame) + { + tf_input_frame_ = config.input_frame; + NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ()); + } + if (tf_output_frame_ != config.output_frame) + { + tf_output_frame_ = config.output_frame; + NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ()); + } + // ]--- +} + +typedef pcl_ros::VoxelGrid VoxelGrid; +PLUGINLIB_DECLARE_CLASS (pcl, VoxelGrid, VoxelGrid, nodelet::Nodelet); + diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp new file mode 100644 index 00000000..0a753d3a --- /dev/null +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -0,0 +1,149 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: bag_to_pcd.cpp 35812 2011-02-08 00:05:03Z rusu $ + * + */ + +/** + +\author Radu Bogdan Rusu + +@b bag_to_pcd is a simple node that reads in a BAG file and saves all the PointCloud messages to disk in PCD (Point +Cloud Data) format. + + **/ + +#include +#include +#include +#include +#include "pcl/io/io.h" +#include "pcl/io/pcd_io.h" +#include "pcl_ros/transforms.h" +#include +#include + +typedef sensor_msgs::PointCloud2 PointCloud; +typedef PointCloud::Ptr PointCloudPtr; +typedef PointCloud::ConstPtr PointCloudConstPtr; + +/* ---[ */ +int + main (int argc, char** argv) +{ + ros::init (argc, argv, "bag_to_pcd"); + if (argc < 4) + { + std::cerr << "Syntax is: " << argv[0] << " " << std::endl; + std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds" << std::endl; + return (-1); + } + + // TF + tf::TransformListener tf_listener; + tf::TransformBroadcaster tf_broadcaster; + + rosbag::Bag bag; + rosbag::View view; + rosbag::View::iterator view_it; + + try + { + bag.open (argv[1], rosbag::bagmode::Read); + } + catch (rosbag::BagException) + { + std::cerr << "Error opening file " << argv[1] << std::endl; + return (-1); + } + + view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2")); + view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage")); + view_it = view.begin (); + + std::string output_dir = std::string (argv[3]); + boost::filesystem::path outpath (output_dir); + if (!boost::filesystem::exists (outpath)) + { + if (!boost::filesystem::create_directories (outpath)) + { + std::cerr << "Error creating directory " << output_dir << std::endl; + return (-1); + } + std::cerr << "Creating directory " << output_dir << std::endl; + } + + // Add the PointCloud2 handler + std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl; + + PointCloud cloud_t; + ros::Duration r (0.001); + // Loop over the whole bag file + while (view_it != view.end ()) + { + // Handle TF messages first + tf::tfMessage::ConstPtr tf = view_it->instantiate (); + if (tf != NULL) + { + tf_broadcaster.sendTransform (tf->transforms); + ros::spinOnce (); + r.sleep (); + } + else + { + // Get the PointCloud2 message + PointCloudConstPtr cloud = view_it->instantiate (); + if (cloud == NULL) + { + ++view_it; + continue; + } + // Transform it + pcl_ros::transformPointCloud ("/base_link", *cloud, cloud_t, tf_listener); + + std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl; + + std::stringstream ss; + ss << output_dir << "/" << cloud_t.header.stamp << ".pcd"; + std::cerr << "Data saved to " << ss.str () << std::endl; + pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (), + Eigen::Quaternionf::Identity (), true); + } + // Increment the iterator + ++view_it; + } + + return (0); +} +/* ]--- */ diff --git a/pcl_ros/tools/convert_pcd_to_image.cpp b/pcl_ros/tools/convert_pcd_to_image.cpp new file mode 100644 index 00000000..2325f513 --- /dev/null +++ b/pcl_ros/tools/convert_pcd_to_image.cpp @@ -0,0 +1,94 @@ +/* + * 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: surface_convex_hull.cpp 34612 2010-12-08 01:06:27Z rusu $ + * + */ + +/** + \author Ethan Rublee + + @b convert a pcd to an image file + run with: + rosrun pcl convert_pcd_image cloud_00042.pcd + It will publish a ros image message on /pcd/image + View the image with: + rosrun image_view image_view image:=/pcd/image + **/ + +#include +#include +#include +#include +#include + +/* ---[ */ +int +main (int argc, char **argv) +{ + ros::init (argc, argv, "image_publisher"); + ros::NodeHandle nh; + ros::Publisher image_pub = nh.advertise ("output", 1); + + if (argc != 2) + { + std::cout << "usage:\n" << argv[0] << " cloud.pcd" << std::endl; + return 1; + } + + sensor_msgs::Image image; + sensor_msgs::PointCloud2 cloud; + pcl::io::loadPCDFile (std::string (argv[1]), cloud); + + try + { + pcl::toROSMsg (cloud, image); //convert the cloud + } + catch (std::runtime_error e) + { + ROS_ERROR_STREAM("Error in converting cloud to image message: " + << e.what()); + return 1; //fail! + } + ros::Rate loop_rate (5); + while (nh.ok ()) + { + image_pub.publish (image); + ros::spinOnce (); + loop_rate.sleep (); + } + + return (0); +} + +/* ]--- */ diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp new file mode 100644 index 00000000..ebadd99d --- /dev/null +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -0,0 +1,97 @@ +/* + * 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: surface_convex_hull.cpp 34612 2010-12-08 01:06:27Z rusu $ + * + */ + +/** + \author Ethan Rublee + **/ +// ROS core +#include +//Image message +#include +//pcl::toROSMsg +#include +//stl stuff +#include + +class PointCloudToImage +{ +public: + void + cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud) + { + if ((cloud->width * cloud->height) == 0) + return; //return if the cloud is not dense! + try + { + pcl::toROSMsg (*cloud, image_); //convert the cloud + } + catch (std::runtime_error e) + { + ROS_ERROR_STREAM("Error in converting cloud to image message: " + << e.what()); + } + image_pub_.publish (image_); //publish our cloud image + } + PointCloudToImage () : cloud_topic_("input"),image_topic_("output") + { + sub_ = nh_.subscribe (cloud_topic_, 30, + &PointCloudToImage::cloud_cb, this); + image_pub_ = nh_.advertise (image_topic_, 30); + + //print some info about the node + std::string r_ct = nh_.resolveName (cloud_topic_); + std::string r_it = nh_.resolveName (image_topic_); + ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct ); + ROS_INFO_STREAM("Publishing image on topic " << r_it ); + } +private: + ros::NodeHandle nh_; + sensor_msgs::Image image_; //cache the image message + std::string cloud_topic_; //default input + std::string image_topic_; //default output + ros::Subscriber sub_; //cloud subscriber + ros::Publisher image_pub_; //image message publisher +}; + +int +main (int argc, char **argv) +{ + ros::init (argc, argv, "convert_pointcloud_to_image"); + PointCloudToImage pci; //this loads up the node + ros::spin (); //where she stops nobody knows + return 0; +} diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp new file mode 100644 index 00000000..b707557b --- /dev/null +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -0,0 +1,156 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: pcd_to_pointcloud.cpp 33238 2010-03-11 00:46:58Z rusu $ + * + */ + +/** + +\author Radu Bogdan Rusu + +@b pcd_to_pointcloud is a simple node that loads PCD (Point Cloud Data) files from disk and publishes them as ROS messages on the network. + + **/ + +// ROS core +#include +#include +#include +#include + +#include "pcl_ros/publisher.h" + +using namespace std; + +class PCDGenerator +{ + protected: + string tf_frame_; + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + public: + + // ROS messages + sensor_msgs::PointCloud2 cloud_; + + string file_name_, cloud_topic_; + double rate_; + + pcl_ros::Publisher pub_; + + //////////////////////////////////////////////////////////////////////////////// + PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~") + { + // Maximum number of outgoing messages to be queued for delivery to subscribers = 1 + + cloud_topic_ = "cloud_pcd"; + pub_.advertise (nh_, cloud_topic_.c_str (), 1); + private_nh_.param("frame_id", tf_frame_, std::string("/base_link")); + ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str()); + } + + //////////////////////////////////////////////////////////////////////////////// + // Start + int + start () + { + if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1) + return (-1); + cloud_.header.frame_id = tf_frame_; + return (0); + } + + //////////////////////////////////////////////////////////////////////////////// + // Spin (!) + bool spin () + { + int nr_points = cloud_.width * cloud_.height; + string fields_list = pcl::getFieldsList (cloud_); + double interval = rate_ * 1e+6; + while (nh_.ok ()) + { + ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ()); + cloud_.header.stamp = ros::Time::now (); + + if (pub_.getNumSubscribers () > 0) + { + ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ()); + pub_.publish (cloud_); + } + else + { + ros::Duration (0.001).sleep (); + continue; + } + + usleep (interval); + + if (interval == 0) // We only publish once if a 0 seconds interval is given + break; + } + + ros::Duration (3.0).sleep (); + return (true); + } + + +}; + +/* ---[ */ +int + main (int argc, char** argv) +{ + if (argc < 3) + { + std::cerr << "Syntax is: " << argv[0] << " [publishing_interval (in seconds)]" << std::endl; + return (-1); + } + + ros::init (argc, argv, "pcd_to_pointcloud"); + + PCDGenerator c; + c.file_name_ = string (argv[1]); + c.rate_ = atof (argv[2]); + + if (c.start () == -1) + { + ROS_ERROR ("Could not load file %s. Exiting.", argv[1]); + return (-1); + } + ROS_INFO ("Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.", c.cloud_.width * c.cloud_.height, c.cloud_.data.size (), pcl::getFieldsList (c.cloud_).c_str ()); + c.spin (); + + return (0); +} +/* ]--- */ diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp new file mode 100644 index 00000000..9b52113c --- /dev/null +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -0,0 +1,122 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: pointcloud_to_pcd.cpp 33238 2010-03-11 00:46:58Z rusu $ + * + */ + +// ROS core +#include +// PCL includes +#include +#include +#include + +using namespace std; + +/** +\author Radu Bogdan Rusu + +@b pointcloud_to_pcd is a simple node that retrieves a ROS point cloud message and saves it to disk into a PCD (Point +Cloud Data) file format. + +**/ +class PointCloudToPCD +{ + protected: + ros::NodeHandle nh_; + + private: + std::string prefix_; + + public: + string cloud_topic_; + + ros::Subscriber sub_; + + //////////////////////////////////////////////////////////////////////////////// + // Callback + void + cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud) + { + if ((cloud->width * cloud->height) == 0) + return; + + ROS_INFO ("Received %d data points in frame %s with the following fields: %s", + (int)cloud->width * cloud->height, + cloud->header.frame_id.c_str (), + pcl::getFieldsList (*cloud).c_str ()); + + std::stringstream ss; + ss << prefix_ << cloud->header.stamp << ".pcd"; + ROS_INFO ("Data saved to %s", ss.str ().c_str ()); + + pcl::io::savePCDFile (ss.str (), *cloud, Eigen::Vector4f::Zero (), + Eigen::Quaternionf::Identity (), false); + } + + //////////////////////////////////////////////////////////////////////////////// + PointCloudToPCD () + { + // Check if a prefix parameter is defined for output file names. + ros::NodeHandle priv_nh("~"); + if (priv_nh.getParam ("prefix", prefix_)) + { + ROS_INFO_STREAM ("PCD file prefix is: " << prefix_); + } + else if (nh_.getParam ("prefix", prefix_)) + { + ROS_WARN_STREAM ("Non-private PCD prefix parameter is DEPRECATED: " + << prefix_); + } + + cloud_topic_ = "input"; + + sub_ = nh_.subscribe (cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this); + ROS_INFO ("Listening for incoming data on topic %s", + nh_.resolveName (cloud_topic_).c_str ()); + } +}; + +/* ---[ */ +int + main (int argc, char** argv) +{ + ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName); + + PointCloudToPCD b; + ros::spin (); + + return (0); +} +/* ]--- */ From baa59c0d00806e6a5ca8c39a636724add2f66340 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 17 Dec 2012 18:10:33 -0800 Subject: [PATCH 091/405] increasing version to 1.0.18 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 86be1cdc..3336f539 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.17 + 1.0.18

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index c31c549a..3841197b 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.17 + 1.0.18

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 540fcd06046246100ff6271aff52aff51738451a Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 17 Dec 2012 18:54:03 -0800 Subject: [PATCH 092/405] CMake install fixes --- pcl_ros/CMakeLists.txt | 11 +- pcl_ros/src/pcl_ros/io/bag_io.cpp | 113 ++++++++ pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 257 ++++++++++++++++++ pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 152 +++++++++++ pcl_ros/src/pcl_ros/io/io.cpp | 57 ++++ pcl_ros/src/pcl_ros/io/pcd_io.cpp | 175 ++++++++++++ 6 files changed, 763 insertions(+), 2 deletions(-) create mode 100644 pcl_ros/src/pcl_ros/io/bag_io.cpp create mode 100644 pcl_ros/src/pcl_ros/io/concatenate_data.cpp create mode 100644 pcl_ros/src/pcl_ros/io/concatenate_fields.cpp create mode 100644 pcl_ros/src/pcl_ros/io/io.cpp create mode 100644 pcl_ros/src/pcl_ros/io/pcd_io.cpp diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 53275398..86f532a2 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -67,7 +67,7 @@ target_link_libraries (pcd_to_pointcloud pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARI add_executable (pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) target_link_libraries (pointcloud_to_pcd pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) -add_executable (bag_to_pcd tools/bag_to_pcd.cpp) +add_executable ( bag_to_pcd tools/bag_to_pcd.cpp) target_link_libraries (bag_to_pcd pcl_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) add_executable (convert_pcd_to_image tools/convert_pcd_to_image.cpp) @@ -81,8 +81,15 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(TARGETS pcl_ros_tf +install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) + +install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image + DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install(FILES pcl_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + + diff --git a/pcl_ros/src/pcl_ros/io/bag_io.cpp b/pcl_ros/src/pcl_ros/io/bag_io.cpp new file mode 100644 index 00000000..cc370da9 --- /dev/null +++ b/pcl_ros/src/pcl_ros/io/bag_io.cpp @@ -0,0 +1,113 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: bag_io.cpp 34896 2010-12-19 06:21:42Z rusu $ + * + */ + +#include +#include "pcl_ros/io/bag_io.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name) +{ + try + { + bag_.open (file_name, rosbag::bagmode::Read); + view_.addQuery (bag_, rosbag::TopicQuery (topic_name)); + + if (view_.size () == 0) + return (false); + + it_ = view_.begin (); + } + catch (rosbag::BagException &e) + { + return (false); + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::BAGReader::onInit () +{ + boost::shared_ptr pnh_; + pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); + // ---[ Mandatory parameters + if (!pnh_->getParam ("file_name", file_name_)) + { + NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!"); + return; + } + if (!pnh_->getParam ("topic_name", topic_name_)) + { + NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!"); + return; + } + // ---[ Optional parameters + int max_queue_size = 1; + pnh_->getParam ("publish_rate", publish_rate_); + pnh_->getParam ("max_queue_size", max_queue_size); + + ros::Publisher pub_output = pnh_->advertise ("output", max_queue_size); + + NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n" + " - file_name : %s\n" + " - topic_name : %s", + file_name_.c_str (), topic_name_.c_str ()); + + if (!open (file_name_, topic_name_)) + return; + PointCloud output; + output_ = boost::make_shared (output); + output_->header.stamp = ros::Time::now (); + + // Continous publishing enabled? + while (pnh_->ok ()) + { + PointCloudConstPtr cloud = getNextCloud (); + NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ()); + output_->header.stamp = ros::Time::now (); + + pub_output.publish (output_); + + ros::Duration (publish_rate_).sleep (); + ros::spinOnce (); + } +} + +typedef pcl_ros::BAGReader BAGReader; +PLUGINLIB_DECLARE_CLASS (pcl, BAGReader, BAGReader, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp new file mode 100644 index 00000000..3055f4cd --- /dev/null +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -0,0 +1,257 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#include +#include +#include "pcl_ros/transforms.h" +#include "pcl_ros/io/concatenate_data.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () +{ + private_nh_ = getMTPrivateNodeHandle (); + // ---[ Mandatory parameters + private_nh_.getParam ("output_frame", output_frame_); + private_nh_.getParam ("approximate_sync", approximate_sync_); + + if (output_frame_.empty ()) + { + NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!"); + return; + } + + // ---[ Optional parameters + private_nh_.getParam ("max_queue_size", maximum_queue_size_); + + // Output + pub_output_ = private_nh_.advertise ("output", maximum_queue_size_); + + XmlRpc::XmlRpcValue input_topics; + if (!private_nh_.getParam ("input_topics", input_topics)) + { + NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!"); + return; + } + // Check the type + switch (input_topics.getType ()) + { + case XmlRpc::XmlRpcValue::TypeArray: + { + if (input_topics.size () == 1) + { + NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue."); + return; + } + + if (input_topics.size () > 8) + { + NODELET_ERROR ("[onInit] More than 8 topics passed!"); + return; + } + + ROS_INFO_STREAM ("[onInit] Subscribing to " << input_topics.size () << " user given topics as inputs:"); + for (int d = 0; d < input_topics.size (); ++d) + ROS_INFO_STREAM (" - " << (std::string)(input_topics[d])); + + // Subscribe to the filters + filters_.resize (input_topics.size ()); + + // 8 topics + if (approximate_sync_) + ts_a_.reset (new message_filters::Synchronizer + > (maximum_queue_size_)); + else + ts_e_.reset (new message_filters::Synchronizer + > (maximum_queue_size_)); + + // First input_topics.size () filters are valid + for (int d = 0; d < input_topics.size (); ++d) + { + filters_[d].reset (new message_filters::Subscriber ()); + filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), maximum_queue_size_); + } + + // Bogus null filter + filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1)); + + switch (input_topics.size ()) + { + case 2: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); + break; + } + case 3: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); + break; + } + case 4: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); + break; + } + case 5: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); + break; + } + case 6: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); + break; + } + case 7: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); + break; + } + case 8: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); + break; + } + default: + { + NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!"); + return; + } + } + break; + } + default: + { + NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!"); + return; + } + } + + if (approximate_sync_) + ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8)); + else + ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out) +{ + //ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ()); + PointCloud2::Ptr in1_t (new PointCloud2 ()); + PointCloud2::Ptr in2_t (new PointCloud2 ()); + + // Transform the point clouds into the specified output frame + if (output_frame_ != in1.header.frame_id) + pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_); + else + in1_t = boost::make_shared (in1); + + if (output_frame_ != in2.header.frame_id) + pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_); + else + in2_t = boost::make_shared (in2); + + // Concatenate the results + pcl::concatenatePointCloud (*in1_t, *in2_t, out); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateDataSynchronizer::input ( + const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, + const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, + const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, + const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8) +{ + PointCloud2::Ptr out1 (new PointCloud2 ()); + PointCloud2::Ptr out2 (new PointCloud2 ()); + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1); + if (in3) + { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2); + out1 = out2; + if (in4) + { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1); + if (in5) + { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2); + out1 = out2; + if (in6) + { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1); + if (in7) + { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2); + out1 = out2; + } + } + } + } + } + pub_output_.publish (boost::make_shared (*out1)); +} + +typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer; +PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateDataSynchronizer, PointCloudConcatenateDataSynchronizer, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp new file mode 100644 index 00000000..e9a907c5 --- /dev/null +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: concatenate_fields.cpp 35052 2011-01-03 21:04:57Z rusu $ + * + */ + +/** \author Radu Bogdan Rusu */ + +#include +#include +#include "pcl_ros/io/concatenate_fields.h" + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () +{ + private_nh_ = getMTPrivateNodeHandle (); + // ---[ Mandatory parameters + if (!private_nh_.getParam ("input_messages", input_messages_)) + { + NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!"); + return; + } + if (input_messages_ < 2) + { + NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!"); + return; + } + // ---[ Optional parameters + private_nh_.getParam ("max_queue_size", maximum_queue_size_); + private_nh_.getParam ("maximum_seconds", maximum_seconds_); + sub_input_ = private_nh_.subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this); + pub_output_ = private_nh_.advertise ("output", maximum_queue_size_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud) +{ + NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), private_nh_.resolveName ("input").c_str ()); + + // Erase old data in the queue + if (maximum_seconds_ > 0 && queue_.size () > 0) + { + while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0) + { + NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_, + (*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () )); + queue_.erase (queue_.begin ()); + } + } + + // Push back new data + queue_[cloud->header.stamp].push_back (cloud); + if ((int)queue_[cloud->header.stamp].size () >= input_messages_) + { + // Concatenate together and publish + std::vector &clouds = queue_[cloud->header.stamp]; + PointCloud cloud_out = *clouds[0]; + + // Resize the output dataset + int data_size = cloud_out.data.size (); + int nr_fields = cloud_out.fields.size (); + int nr_points = cloud_out.width * cloud_out.height; + for (size_t i = 1; i < clouds.size (); ++i) + { + assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step); + + if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) + { + NODELET_ERROR ("[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!", + i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height); + break; + } + // Point step must increase with the length of each new field + cloud_out.point_step += clouds[i]->point_step; + // Resize data to hold all clouds + data_size += clouds[i]->data.size (); + + // Concatenate fields + cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ()); + int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype); + for (size_t d = 0; d < clouds[i]->fields.size (); ++d) + { + cloud_out.fields[nr_fields + d] = clouds[i]->fields[d]; + cloud_out.fields[nr_fields + d].offset += delta_offset; + } + nr_fields += clouds[i]->fields.size (); + } + // Recalculate row_step + cloud_out.row_step = cloud_out.point_step * cloud_out.width; + cloud_out.data.resize (data_size); + + // Iterate over each point and perform the appropriate memcpys + int point_offset = 0; + for (int cp = 0; cp < nr_points; ++cp) + { + for (size_t i = 0; i < clouds.size (); ++i) + { + // Copy each individual point + memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step); + point_offset += clouds[i]->point_step; + } + } + pub_output_.publish (boost::make_shared (cloud_out)); + queue_.erase (cloud->header.stamp); + } + + // Clean the queue to avoid overflowing + if (maximum_queue_size_ > 0) + { + while ((int)queue_.size () > maximum_queue_size_) + queue_.erase (queue_.begin ()); + } + +} + +typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer; +PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateFieldsSynchronizer, PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/io/io.cpp b/pcl_ros/src/pcl_ros/io/io.cpp new file mode 100644 index 00000000..41139a6b --- /dev/null +++ b/pcl_ros/src/pcl_ros/io/io.cpp @@ -0,0 +1,57 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: io.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include +#include +//#include +#include +#include + +typedef nodelet::NodeletMUX > NodeletMUX; +//typedef nodelet::NodeletDEMUX > NodeletDEMUX; +typedef nodelet::NodeletDEMUX NodeletDEMUX; + +//#include "pcd_io.cpp" +//#include "bag_io.cpp" +//#include "concatenate_fields.cpp" +//#include "concatenate_data.cpp" + +PLUGINLIB_DECLARE_CLASS (pcl, NodeletMUX, NodeletMUX, nodelet::Nodelet); +PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX, NodeletDEMUX, nodelet::Nodelet); +//PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX_ROS, NodeletDEMUX_ROS, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp new file mode 100644 index 00000000..5d4e24e4 --- /dev/null +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -0,0 +1,175 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: pcd_io.cpp 35812 2011-02-08 00:05:03Z rusu $ + * + */ + +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PCDReader::onInit () +{ + ros::NodeHandle private_nh = getMTPrivateNodeHandle (); + // Provide a latched topic + ros::Publisher pub_output = private_nh.advertise ("output", max_queue_size_, true); + + private_nh.getParam ("publish_rate", publish_rate_); + private_nh.getParam ("tf_frame", tf_frame_); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - publish_rate : %f\n" + " - tf_frame : %s", + getName ().c_str (), + publish_rate_, tf_frame_.c_str ()); + + PointCloud2Ptr output_new; + output_ = boost::make_shared (); + output_new = boost::make_shared (); + + // Wait in a loop until someone connects + do + { + ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ()); + ros::spinOnce (); + ros::Duration (0.01).sleep (); + } + while (private_nh.ok () && pub_output.getNumSubscribers () == 0); + + std::string file_name; + + while (private_nh.ok ()) + { + // Get the current filename parameter. If no filename set, loop + if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ()) + { + ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ()); + ros::Duration (0.01).sleep (); + ros::spinOnce (); + continue; + } + + // If the filename parameter holds a different value than the last one we read + if (file_name_.compare (file_name) != 0 && !file_name_.empty ()) + { + NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ()); + file_name = file_name_; + PointCloud2 cloud; + if (impl_.read (file_name_, cloud) < 0) + { + NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ()); + return; + } + output_ = boost::make_shared (cloud); + output_->header.stamp = ros::Time::now (); + output_->header.frame_id = tf_frame_; + } + + // We do not publish empty data + if (output_->data.size () == 0) + continue; + + if (publish_rate_ == 0) + { + if (output_ != output_new) + { + NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); + pub_output.publish (output_); + output_new = output_; + } + ros::Duration (0.01).sleep (); + } + else + { + NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); + output_->header.stamp = ros::Time::now (); + pub_output.publish (output_); + + ros::Duration (publish_rate_).sleep (); + } + + ros::spinOnce (); + // Update parameters from server + private_nh.getParam ("publish_rate", publish_rate_); + private_nh.getParam ("tf_frame", tf_frame_); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PCDWriter::onInit () +{ + ros::NodeHandle pnh = getMTPrivateNodeHandle (); + sub_input_ = pnh.subscribe ("input", 1, &PCDWriter::input_callback, this); + // ---[ Optional parameters + pnh.getParam ("filename", file_name_); + pnh.getParam ("binary_mode", binary_mode_); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - filename : %s\n" + " - binary_mode : %s", + getName ().c_str (), + file_name_.c_str (), (binary_mode_) ? "true" : "false"); + +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud) +{ + if (!isValid (cloud)) + return; + + getMTPrivateNodeHandle ().getParam ("filename", file_name_); + + NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + + std::string fname; + if (file_name_.empty ()) + fname = boost::lexical_cast (cloud->header.stamp.toSec ()) + ".pcd"; + else + fname = file_name_; + impl_.write (fname, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_); + + NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ()); +} + +typedef pcl_ros::PCDReader PCDReader; +typedef pcl_ros::PCDWriter PCDWriter; +PLUGINLIB_DECLARE_CLASS (pcl, PCDReader, PCDReader, nodelet::Nodelet); +PLUGINLIB_DECLARE_CLASS (pcl, PCDWriter, PCDWriter, nodelet::Nodelet); + From 993afab40b8f161b2b601280129fd414e52e49e8 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 17 Dec 2012 20:43:08 -0800 Subject: [PATCH 093/405] adding nodelet dependencies --- pcl_ros/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 3336f539..3a928554 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -23,6 +23,7 @@ std_msgs rosbag tf + nodelet dynamic_reconfigure eigen @@ -32,7 +33,7 @@ std_msgs rosbag tf - + nodelet From 077b4f3dea4a716269d081dbf2643c0effa7db14 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 17 Dec 2012 21:17:31 -0800 Subject: [PATCH 094/405] adding pluginlib dependency --- pcl_ros/CMakeLists.txt | 6 +++++- pcl_ros/package.xml | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 86f532a2..0ae2556c 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -3,7 +3,7 @@ project(pcl_ros) # Deal with catkin find_package(Boost COMPONENTS system filesystem thread REQUIRED) -find_package(catkin REQUIRED dynamic_reconfigure genmsg roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib) +find_package(catkin REQUIRED dynamic_reconfigure genmsg roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib pluginlib) find_package(Eigen) find_package(PCL) @@ -46,6 +46,8 @@ add_library (pcl_ros_io #rosbuild_add_compile_flags (pcl_ros_io ${SSE_FLAGS}) target_link_libraries (pcl_ros_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +class_loader_hide_library_symbols(pcl_ros_io) + # ---[ PCL ROS - Filters add_library (pcl_ros_filters src/pcl_ros/filters/filter.cpp @@ -59,6 +61,8 @@ add_library (pcl_ros_filters #add_compile_flags (pcl_ros_filters ${SSE_FLAGS}) target_link_libraries (pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +class_loader_hide_library_symbols(pcl_ros_filters) + ############ TOOLS add_executable (pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 3a928554..1131212a 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -24,6 +24,7 @@ rosbag tf nodelet + pluginlib dynamic_reconfigure eigen @@ -34,6 +35,7 @@ rosbag tf nodelet + pluginlib From 140a4a3bf9281973b9b3ebb233982c6f44adc0e8 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 17 Dec 2012 21:46:22 -0800 Subject: [PATCH 095/405] adding nodelet_topic_tools dependency --- pcl_ros/package.xml | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 1131212a..d87707b6 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -17,25 +17,27 @@ dynamic_reconfigure eigen + nodelet + nodelet_topic_tools pcl + pluginlib + rosbag roscpp sensor_msgs std_msgs - rosbag tf - nodelet - pluginlib dynamic_reconfigure eigen + nodelet + nodelet_topic_tools pcl + pluginlib + rosbag roscpp sensor_msgs std_msgs - rosbag tf - nodelet - pluginlib From a2617ff8265c601a4cf71f98e9839ba697a5b206 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 17 Dec 2012 21:47:19 -0800 Subject: [PATCH 096/405] switching to version 1.0.19 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index d87707b6..ecbff386 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.18 + 1.0.19

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 3841197b..3845050e 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.18 + 1.0.19

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 19906dc32347234859b5e2017a92cfb99fbfded9 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 18 Dec 2012 14:20:45 -0800 Subject: [PATCH 097/405] adding catkin_project dependencies --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 0ae2556c..5b190bc5 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -26,7 +26,7 @@ generate_dynamic_reconfigure_options(cfg/Filter.cfg ) include_directories(include cfg/cpp) -catkin_package() +catkin_package(DEPENDS Eigen PCL roscpp sensor_msgs tf) # ---[ Point Cloud Library - Transforms add_library (pcl_ros_tf SHARED src/transforms.cpp) From e0f9f36fec5b2f0fc14f68038e09a606e4a406f9 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 18 Dec 2012 14:21:42 -0800 Subject: [PATCH 098/405] switching to version 1.0.20 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index ecbff386..a19b9e85 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.19 + 1.0.20

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 3845050e..7b0a9212 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.19 + 1.0.20

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From a0464e15b07339696fe9babd1a9e0f68eed561c7 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 18 Dec 2012 17:40:49 -0800 Subject: [PATCH 099/405] fixing catkin_package debs --- pcl_ros/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 5b190bc5..e706d87f 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -26,7 +26,11 @@ generate_dynamic_reconfigure_options(cfg/Filter.cfg ) include_directories(include cfg/cpp) -catkin_package(DEPENDS Eigen PCL roscpp sensor_msgs tf) +catkin_package( + INCLUDE_DIRS include + LIBRARIES pcl_ros_tf pcl_ros_io pcl_ros_filters + DEPENDS Eigen PCL roscpp sensor_msgs tf sensor_msgs std_msgs +) # ---[ Point Cloud Library - Transforms add_library (pcl_ros_tf SHARED src/transforms.cpp) From 54f95ea10f259f2475be53cbefe0180e650c85e0 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 18 Dec 2012 17:42:19 -0800 Subject: [PATCH 100/405] switching to version 1.0.21 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index a19b9e85..f2c00907 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.20 + 1.0.21

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 7b0a9212..4e834bbf 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.20 + 1.0.21

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 2da1700056bfb2361eb6ed18314937316a27d6cf Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 19 Dec 2012 15:22:05 -0800 Subject: [PATCH 101/405] fix dyn reconf files --- pcl_ros/cfg/ExtractIndices.cfg | 4 +--- pcl_ros/cfg/Filter.cfg | 2 +- pcl_ros/cfg/Filter_common.py | 2 +- pcl_ros/cfg/Filter_common.pyc | Bin 1614 -> 0 bytes pcl_ros/cfg/StatisticalOutlierRemoval.cfg | 5 ++--- pcl_ros/cfg/VoxelGrid.cfg | 3 +-- 6 files changed, 6 insertions(+), 10 deletions(-) delete mode 100644 pcl_ros/cfg/Filter_common.pyc diff --git a/pcl_ros/cfg/ExtractIndices.cfg b/pcl_ros/cfg/ExtractIndices.cfg index b3ed9cde..debcb65d 100755 --- a/pcl_ros/cfg/ExtractIndices.cfg +++ b/pcl_ros/cfg/ExtractIndices.cfg @@ -3,9 +3,7 @@ # set up parameters that we care about PACKAGE = 'pcl_ros' -import roslib; roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; -import roslib.packages +from dynamic_reconfigure.parameter_generator_catkin import *; gen = ParameterGenerator () # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): diff --git a/pcl_ros/cfg/Filter.cfg b/pcl_ros/cfg/Filter.cfg index bcda6baf..c6b29abb 100755 --- a/pcl_ros/cfg/Filter.cfg +++ b/pcl_ros/cfg/Filter.cfg @@ -5,7 +5,7 @@ PACKAGE = 'pcl_ros' import roslib; roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; +from dynamic_reconfigure.parameter_generator_catkin import *; import Filter_common as common gen = ParameterGenerator () diff --git a/pcl_ros/cfg/Filter_common.py b/pcl_ros/cfg/Filter_common.py index b414c378..42327390 100644 --- a/pcl_ros/cfg/Filter_common.py +++ b/pcl_ros/cfg/Filter_common.py @@ -5,7 +5,7 @@ PACKAGE = 'pcl_ros' import roslib; roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; +from dynamic_reconfigure.parameter_generator_catkin import *; def add_common_parameters (gen): # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): diff --git a/pcl_ros/cfg/Filter_common.pyc b/pcl_ros/cfg/Filter_common.pyc deleted file mode 100644 index 9ccbd7322de5f0d9972c36eb357be0a1b44e81e8..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1614 zcmcIkOK%i85OzWq(!mxw>XPtSr5J`))fI-P6PWJ&bS%LJRQ> zG7-8bY$Bc|f)cUc6C}iGB9dsB5q3uG>RIsuduQXuoz*!UOJZmdR&@A*-`W zm1Lk4;TFX19?4cOqh2$}ux03kFHK>rDGg_(slwe30CQ6|rBv3sQ3QyN ziR5e5#|VPj&Q*KHuNvYjE9> zGtR1_p<;7L;VCRT#FCw4k2FjnMa;ZD2RRw zmKhufQk$Ycja1xc_!PVh0g}HBWvu+MSX6=1{)te=jbsL)D6Khm6>ya?$F8AWGq8lk zJQf4zET(bCrm}&?ttK8EQniXd(Tea0vvf2}13P~^-9J!eN#3SU9`B^NV%$v{e4f_i zb6mJ_RfYzZc18wPR78IEOghn5uQ Date: Wed, 19 Dec 2012 15:22:23 -0800 Subject: [PATCH 102/405] 1.0.22 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index f2c00907..3b7a9006 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.21 + 1.0.22

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 4e834bbf..7c87ea90 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.21 + 1.0.22

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From c4aed1cb0cf86e2c216b7c710eab59f61fc6bfcd Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 19 Dec 2012 16:52:09 -0800 Subject: [PATCH 103/405] clean up shared parameters --- pcl_ros/cfg/Filter.cfg | 12 +++++++----- pcl_ros/cfg/VoxelGrid.cfg | 11 +++++++---- pcl_ros/cfg/{Filter_common.py => common.py} | 0 3 files changed, 14 insertions(+), 9 deletions(-) rename pcl_ros/cfg/{Filter_common.py => common.py} (100%) diff --git a/pcl_ros/cfg/Filter.cfg b/pcl_ros/cfg/Filter.cfg index c6b29abb..398af1aa 100755 --- a/pcl_ros/cfg/Filter.cfg +++ b/pcl_ros/cfg/Filter.cfg @@ -3,13 +3,15 @@ # set up parameters that we care about PACKAGE = 'pcl_ros' -import roslib; -roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator_catkin import *; -import Filter_common as common +import os +import sys +sys.path.insert(0, os.path.dirname(__file__)) +from common import add_common_parameters + +from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator () -common.add_common_parameters (gen) +add_common_parameters (gen) exit (gen.generate (PACKAGE, "pcl_ros", "Filter")) diff --git a/pcl_ros/cfg/VoxelGrid.cfg b/pcl_ros/cfg/VoxelGrid.cfg index 71db1e89..332f1f0a 100755 --- a/pcl_ros/cfg/VoxelGrid.cfg +++ b/pcl_ros/cfg/VoxelGrid.cfg @@ -3,13 +3,16 @@ # set up parameters that we care about PACKAGE = 'pcl_ros' -import roslib; roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator_catkin import *; -import Filter_common as common +import os +import sys +sys.path.insert(0, os.path.dirname(__file__)) +from common import add_common_parameters + +from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator () # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): gen.add ("leaf_size", double_t, 0, "The size of a leaf (on x,y,z) used for downsampling.", 0.01, 0, 1.0) -common.add_common_parameters (gen) +add_common_parameters (gen) exit (gen.generate (PACKAGE, "pcl_ros", "VoxelGrid")) diff --git a/pcl_ros/cfg/Filter_common.py b/pcl_ros/cfg/common.py similarity index 100% rename from pcl_ros/cfg/Filter_common.py rename to pcl_ros/cfg/common.py From 95ec2c717193455682d1b12e80375afb67b08a5c Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 19 Dec 2012 16:52:16 -0800 Subject: [PATCH 104/405] 1.0.23 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 3b7a9006..029628a7 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.22 + 1.0.23

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 7c87ea90..e87b815f 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.22 + 1.0.23

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From c7c4a1fa56ba3a6de0f349fdd3974018753c137b Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Thu, 20 Dec 2012 15:27:51 -0800 Subject: [PATCH 105/405] remove obsolete roslib import --- pcl_ros/cfg/common.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/pcl_ros/cfg/common.py b/pcl_ros/cfg/common.py index 42327390..296951da 100644 --- a/pcl_ros/cfg/common.py +++ b/pcl_ros/cfg/common.py @@ -3,8 +3,6 @@ # set up parameters that we care about PACKAGE = 'pcl_ros' -import roslib; -roslib.load_manifest (PACKAGE); from dynamic_reconfigure.parameter_generator_catkin import *; def add_common_parameters (gen): From 20bed7483f63fc59f5e483095e31b58c53e4ed20 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 21 Dec 2012 12:34:07 -0800 Subject: [PATCH 106/405] 1.0.24 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 029628a7..6ed4895c 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.23 + 1.0.24

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index e87b815f..a5770e29 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.23 + 1.0.24

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 3fc3fa400ad821ba3c4f2ded779db42402024f66 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Tue, 1 Jan 2013 14:52:37 +0100 Subject: [PATCH 107/405] fixes #1 --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index e706d87f..73901c81 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -3,7 +3,7 @@ project(pcl_ros) # Deal with catkin find_package(Boost COMPONENTS system filesystem thread REQUIRED) -find_package(catkin REQUIRED dynamic_reconfigure genmsg roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib pluginlib) +find_package(catkin REQUIRED dynamic_reconfigure genmsg nodelet nodelet_topic_tools roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib pluginlib) find_package(Eigen) find_package(PCL) From 7bd503852018e3cfafa8b841b75d9abffc0fd6d7 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Tue, 1 Jan 2013 14:58:36 +0100 Subject: [PATCH 108/405] 1.0.25 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 6ed4895c..48ab4077 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.24 + 1.0.25

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index a5770e29..5cd4da8c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.24 + 1.0.25

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From ae3aff7b544a282285a4ab964653a3194440a78c Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Thu, 17 Jan 2013 15:45:57 -0800 Subject: [PATCH 109/405] removing build_tool dependency from package.xml --- perception_pcl/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 5cd4da8c..79d38562 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -13,8 +13,6 @@ BSD http://ros.org/wiki/perception_pcl - catkin - pcl_ros From 23d80101494671f955995f25016ab1814a0ed617 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Thu, 17 Jan 2013 15:49:30 -0800 Subject: [PATCH 110/405] fixing catkin export --- pcl_ros/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 73901c81..d0d1b2f5 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -29,7 +29,8 @@ include_directories(include cfg/cpp) catkin_package( INCLUDE_DIRS include LIBRARIES pcl_ros_tf pcl_ros_io pcl_ros_filters - DEPENDS Eigen PCL roscpp sensor_msgs tf sensor_msgs std_msgs + CATKIN_DEPENDS roscpp sensor_msgs tf sensor_msgs std_msgs + DEPENDS Eigen PCL ) # ---[ Point Cloud Library - Transforms From 8133001c657d4ad9d4801e72a62b43a5cbde5fa7 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Thu, 17 Jan 2013 15:50:14 -0800 Subject: [PATCH 111/405] 1.0.25 -> 1.0.26 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 48ab4077..60284cfb 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.25 + 1.0.26

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 79d38562..ef9f4ad3 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.25 + 1.0.26

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 758f23264dfcdc71be852d7b85cca77f4cb822bc Mon Sep 17 00:00:00 2001 From: jon-weisz Date: Sun, 20 Jan 2013 20:22:03 -0800 Subject: [PATCH 112/405] update pluginlib macro --- pcl_ros/src/pcl_ros/filters/extract_indices.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/boundary.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/fpfh.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/pfh.cpp | 2 +- .../src/pcl_ros/filters/features/principal_curvatures.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/vfh.cpp | 2 +- pcl_ros/src/pcl_ros/filters/filter.cpp | 4 ++-- pcl_ros/src/pcl_ros/filters/passthrough.cpp | 2 +- pcl_ros/src/pcl_ros/filters/project_inliers.cpp | 2 +- pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp | 2 +- pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp | 2 +- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 2 +- pcl_ros/src/pcl_ros/io/bag_io.cpp | 2 +- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 2 +- pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 2 +- pcl_ros/src/pcl_ros/io/io.cpp | 6 +++--- pcl_ros/src/pcl_ros/io/pcd_io.cpp | 4 ++-- 22 files changed, 26 insertions(+), 26 deletions(-) diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp index 2d596cd9..98419b09 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -66,5 +66,5 @@ pcl_ros::ExtractIndices::config_callback (pcl_ros::ExtractIndicesConfig &config, } typedef pcl_ros::ExtractIndices ExtractIndices; -PLUGINLIB_DECLARE_CLASS (pcl, ExtractIndices, ExtractIndices, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(ExtractIndices,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp index 0e17c5fd..d90ac374 100644 --- a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp @@ -74,4 +74,4 @@ pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::BoundaryEstimation BoundaryEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, BoundaryEstimation, BoundaryEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(BoundaryEstimation,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp index 7782fde9..c7f98f1d 100644 --- a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp @@ -75,5 +75,5 @@ pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::FPFHEstimation FPFHEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimation, FPFHEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(FPFHEstimation,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp index 51880a9f..57c508c0 100644 --- a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp @@ -75,5 +75,5 @@ pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; -PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimationOMP, FPFHEstimationOMP, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp index 85bd209a..0d1c039a 100644 --- a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp @@ -73,5 +73,5 @@ pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr } typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, MomentInvariantsEstimation, MomentInvariantsEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp index a512c41f..fbc81abc 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp @@ -73,5 +73,5 @@ pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::NormalEstimation NormalEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimation, NormalEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NormalEstimation,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp index e53fe9c7..0a257afa 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp @@ -73,5 +73,5 @@ pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; -PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationOMP, NormalEstimationOMP, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp index d6636f73..330e2de7 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp @@ -75,7 +75,7 @@ pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; -PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationTBB, NormalEstimationTBB, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB,nodelet::Nodelet); #endif // HAVE_TBB diff --git a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp index f40e0bab..b8e3d8d2 100644 --- a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp @@ -75,5 +75,5 @@ pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::PFHEstimation PFHEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, PFHEstimation, PFHEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PFHEstimation,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp index d6431160..5e8c10f7 100644 --- a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp @@ -75,5 +75,5 @@ pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstP } typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, PrincipalCurvaturesEstimation, PrincipalCurvaturesEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp index f13028e3..351a8af7 100644 --- a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp @@ -75,5 +75,5 @@ pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::VFHEstimation VFHEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, VFHEstimation, VFHEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(VFHEstimation,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 9fcf844c..6257301a 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -55,8 +55,8 @@ //#include "statistical_outlier_removal.cpp" //#include "voxel_grid.cpp" -/*//PLUGINLIB_DECLARE_CLASS (pcl, PixelGrid, PixelGrid, nodelet::Nodelet); -//PLUGINLIB_DECLARE_CLASS (pcl, FilterDimension, FilterDimension, nodelet::Nodelet); +/*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet); +//PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet); */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index e4f54f4b..e0de5af3 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -116,5 +116,5 @@ pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t l } typedef pcl_ros::PassThrough PassThrough; -PLUGINLIB_DECLARE_CLASS (pcl, PassThrough, PassThrough, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PassThrough,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index f498ba65..77349c46 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -140,5 +140,5 @@ pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstP } typedef pcl_ros::ProjectInliers ProjectInliers; -PLUGINLIB_DECLARE_CLASS (pcl, ProjectInliers, ProjectInliers, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(ProjectInliers,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp index 6aaf2fbb..da965c69 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -39,5 +39,5 @@ #include "pcl_ros/filters/radius_outlier_removal.h" typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; -PLUGINLIB_DECLARE_CLASS (pcl, RadiusOutlierRemoval, RadiusOutlierRemoval, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp index 74288f56..16535b7f 100644 --- a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -77,5 +77,5 @@ pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlier } typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval; -PLUGINLIB_DECLARE_CLASS (pcl, StatisticalOutlierRemoval, StatisticalOutlierRemoval, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 55181f75..87c6ea43 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -120,5 +120,5 @@ pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t } typedef pcl_ros::VoxelGrid VoxelGrid; -PLUGINLIB_DECLARE_CLASS (pcl, VoxelGrid, VoxelGrid, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(VoxelGrid,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/bag_io.cpp b/pcl_ros/src/pcl_ros/io/bag_io.cpp index cc370da9..a17edd14 100644 --- a/pcl_ros/src/pcl_ros/io/bag_io.cpp +++ b/pcl_ros/src/pcl_ros/io/bag_io.cpp @@ -109,5 +109,5 @@ pcl_ros::BAGReader::onInit () } typedef pcl_ros::BAGReader BAGReader; -PLUGINLIB_DECLARE_CLASS (pcl, BAGReader, BAGReader, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(BAGReader,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index 3055f4cd..e4ccba9c 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -253,5 +253,5 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::input ( } typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer; -PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateDataSynchronizer, PointCloudConcatenateDataSynchronizer, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index e9a907c5..52b17edd 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -148,5 +148,5 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointClo } typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer; -PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateFieldsSynchronizer, PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/io.cpp b/pcl_ros/src/pcl_ros/io/io.cpp index 41139a6b..c9887581 100644 --- a/pcl_ros/src/pcl_ros/io/io.cpp +++ b/pcl_ros/src/pcl_ros/io/io.cpp @@ -51,7 +51,7 @@ typedef nodelet::NodeletDEMUX NodeletDEMUX; //#include "concatenate_fields.cpp" //#include "concatenate_data.cpp" -PLUGINLIB_DECLARE_CLASS (pcl, NodeletMUX, NodeletMUX, nodelet::Nodelet); -PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX, NodeletDEMUX, nodelet::Nodelet); -//PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX_ROS, NodeletDEMUX_ROS, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NodeletMUX,nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NodeletDEMUX,nodelet::Nodelet); +//PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index 5d4e24e4..026c0d20 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -170,6 +170,6 @@ pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud) typedef pcl_ros::PCDReader PCDReader; typedef pcl_ros::PCDWriter PCDWriter; -PLUGINLIB_DECLARE_CLASS (pcl, PCDReader, PCDReader, nodelet::Nodelet); -PLUGINLIB_DECLARE_CLASS (pcl, PCDWriter, PCDWriter, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PCDReader,nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PCDWriter,nodelet::Nodelet); From 750dc76fe43c7053833e78ba95a5e1425ef34ebf Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 5 Feb 2013 11:59:03 -0800 Subject: [PATCH 113/405] Fixing target install directory for pcl tools --- pcl_ros/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index d0d1b2f5..3ba11a4d 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -90,14 +90,14 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters +install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image - DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) +#install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image +# DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(FILES pcl_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) From 418a15560d50de194711e2199c4c406b6227d13e Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 5 Feb 2013 11:59:23 -0800 Subject: [PATCH 114/405] Update pcl_ros/package.xml --- pcl_ros/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 60284cfb..6e3c335f 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.26 + 1.0.27

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 058bed54e1bc434295ba10730a23e759c7fa2e05 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 5 Feb 2013 12:10:19 -0800 Subject: [PATCH 115/405] switching to 1.0.27 --- perception_pcl/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index ef9f4ad3..6dcd566f 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.26 + 1.0.27

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 604047b18d05fb069eaa968757159ef4e8908849 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 5 Feb 2013 12:28:39 -0800 Subject: [PATCH 116/405] reenabling deprecated install targets - comment added --- pcl_ros/CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 3ba11a4d..5aff6d2f 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -96,8 +96,9 @@ install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters pcd_to_pointcloud pointclo LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -#install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image -# DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) +# these targets locations are deprecated and will be dropped in Hydro! +install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image + DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(FILES pcl_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) From 941066555644ac426ec58fa9ab0c6e1827b7ed9b Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 5 Feb 2013 12:29:23 -0800 Subject: [PATCH 117/405] bumping version to 1.0.28 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 6e3c335f..59f67f07 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.27 + 1.0.28

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 6dcd566f..12547c1a 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.27 + 1.0.28

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From f7e8a659a91370f22e27ebbb03737a9b42bc0f98 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 5 Feb 2013 12:31:26 -0800 Subject: [PATCH 118/405] typo --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 5aff6d2f..939479ed 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -96,7 +96,7 @@ install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters pcd_to_pointcloud pointclo LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -# these targets locations are deprecated and will be dropped in Hydro! +# these target locations are deprecated and will be dropped in Hydro! install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) From c6a48a583f1b93eedbcc3d8d33290fb8a9f8b7fa Mon Sep 17 00:00:00 2001 From: Kai Franke Date: Wed, 20 Feb 2013 08:59:27 -0800 Subject: [PATCH 119/405] now also works without specifying publishing interval like described in the wiki. Also renamed rate_ to time_ since it is no rate --- pcl_ros/tools/pcd_to_pointcloud.cpp | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index b707557b..bbb3e769 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -65,7 +65,7 @@ class PCDGenerator sensor_msgs::PointCloud2 cloud_; string file_name_, cloud_topic_; - double rate_; + double wait_; pcl_ros::Publisher pub_; @@ -97,7 +97,7 @@ class PCDGenerator { int nr_points = cloud_.width * cloud_.height; string fields_list = pcl::getFieldsList (cloud_); - double interval = rate_ * 1e+6; + double interval = wait_ * 1e+6; while (nh_.ok ()) { ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ()); @@ -110,17 +110,20 @@ class PCDGenerator } else { - ros::Duration (0.001).sleep (); + // check once a second if there is any subscriber + ros::Duration (1).sleep (); continue; } usleep (interval); - if (interval == 0) // We only publish once if a 0 seconds interval is given + if (interval == 0) // We only publish once if a 0 seconds interval is given + { + // Give subscribers 3 seconds until point cloud decays... a little ugly! + ros::Duration (3.0).sleep (); break; + } } - - ros::Duration (3.0).sleep (); return (true); } @@ -131,7 +134,7 @@ class PCDGenerator int main (int argc, char** argv) { - if (argc < 3) + if (argc < 2) { std::cerr << "Syntax is: " << argv[0] << " [publishing_interval (in seconds)]" << std::endl; return (-1); @@ -141,7 +144,15 @@ int PCDGenerator c; c.file_name_ = string (argv[1]); - c.rate_ = atof (argv[2]); + // check if publishing interval is given + if (argc == 2) + { + c.wait_ = 0; + } + else + { + c.wait_ = atof (argv[2]); + } if (c.start () == -1) { From 12eee07fdafd63c5051c4a0144e41cf370ed0ed2 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Mon, 4 Mar 2013 10:19:09 -0800 Subject: [PATCH 120/405] Fixes #7 --- pcl_ros/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 59f67f07..c62e4949 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -40,7 +40,7 @@ tf - + From 3b5234cb7f517d3712de32e5bc8275315a45b219 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Mon, 4 Mar 2013 10:20:12 -0800 Subject: [PATCH 121/405] 1.0.29 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index c62e4949..4c0c6707 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.28 + 1.0.29

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 12547c1a..6f28ce42 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.28 + 1.0.29

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 570269f2e51b1ced9eaf899e77055c7ff52a32e4 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 22 Apr 2013 11:32:02 -0700 Subject: [PATCH 122/405] deprecating bin install targets --- pcl_ros/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 939479ed..0054a16e 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -97,8 +97,8 @@ install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters pcd_to_pointcloud pointclo ) # these target locations are deprecated and will be dropped in Hydro! -install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image - DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) +#install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image +# DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(FILES pcl_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) From 1ee537f320e5da3c47528780069289c40078a1ed Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 22 Apr 2013 11:34:11 -0700 Subject: [PATCH 123/405] adding buildtool_depend to meta package --- perception_pcl/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 6f28ce42..8847064c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.29 + 1.0.30

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred @@ -13,6 +13,8 @@ BSD http://ros.org/wiki/perception_pcl + catkin + pcl_ros From 16d630573afd116cb85a7884a69d861ed4b80bf5 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 22 Apr 2013 11:34:42 -0700 Subject: [PATCH 124/405] bumping version to 1.0.30 --- pcl_ros/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 4c0c6707..1987adbf 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.29 + 1.0.30

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 46ceee9f80750e084b7fde4e7b1cbf2ab5751dca Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 22 Apr 2013 11:47:37 -0700 Subject: [PATCH 125/405] adding CMakeLists.txt file for metapackage --- perception_pcl/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 perception_pcl/CMakeLists.txt diff --git a/perception_pcl/CMakeLists.txt b/perception_pcl/CMakeLists.txt new file mode 100644 index 00000000..91a482aa --- /dev/null +++ b/perception_pcl/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(perception_pcl) +find_package(catkin REQUIRED) +catkin_metapackage() \ No newline at end of file From 7848d6fbec748e8dd95d2341ed4632912bc3872a Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 22 Apr 2013 11:57:57 -0700 Subject: [PATCH 126/405] minor --- perception_pcl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception_pcl/CMakeLists.txt b/perception_pcl/CMakeLists.txt index 91a482aa..b74e3f0b 100644 --- a/perception_pcl/CMakeLists.txt +++ b/perception_pcl/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 2.8.3) project(perception_pcl) find_package(catkin REQUIRED) -catkin_metapackage() \ No newline at end of file +catkin_metapackage() From 669c2d2b04121d46d1af98016ce94f075664f80f Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 22 Apr 2013 11:58:30 -0700 Subject: [PATCH 127/405] 1.0.30 -> 1.0.31 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 1987adbf..4844c7d4 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.30 + 1.0.31

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 8847064c..e59b0efd 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.30 + 1.0.31

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From f2553aac6c3abe764580eda80dfd2490ca52820d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 11 May 2013 03:29:09 +0900 Subject: [PATCH 128/405] copy features/segmentation/surface from fuerte-devel --- pcl_ros/CMakeLists.txt | 56 +- pcl_ros/cfg/EuclideanClusterExtraction.cfg | 19 + pcl_ros/cfg/ExtractPolygonalPrismData.cfg | 15 + pcl_ros/cfg/Feature.cfg | 19 + pcl_ros/cfg/MLS.cfg | 22 + pcl_ros/cfg/SACSegmentation.cfg | 14 + pcl_ros/cfg/SACSegmentationFromNormals.cfg | 16 + pcl_ros/cfg/SACSegmentation_common.py | 21 + pcl_ros/cfg/SegmentDifferences.cfg | 16 + pcl_ros/include/pcl_ros/features/boundary.h | 86 +++ pcl_ros/include/pcl_ros/features/feature.h | 240 +++++++ pcl_ros/include/pcl_ros/features/fpfh.h | 100 +++ pcl_ros/include/pcl_ros/features/fpfh_omp.h | 96 +++ .../pcl_ros/features/moment_invariants.h | 83 +++ pcl_ros/include/pcl_ros/features/normal_3d.h | 86 +++ .../include/pcl_ros/features/normal_3d_omp.h | 79 +++ .../include/pcl_ros/features/normal_3d_tbb.h | 86 +++ pcl_ros/include/pcl_ros/features/pfh.h | 100 +++ .../pcl_ros/features/principal_curvatures.h | 83 +++ pcl_ros/include/pcl_ros/features/vfh.h | 83 +++ .../pcl_ros/segmentation/extract_clusters.h | 105 +++ .../extract_polygonal_prism_data.h | 126 ++++ .../pcl_ros/segmentation/sac_segmentation.h | 277 ++++++++ .../segmentation/segment_differences.h | 106 +++ pcl_ros/include/pcl_ros/surface/convex_hull.h | 92 +++ .../pcl_ros/surface/moving_least_squares.h | 145 ++++ pcl_ros/src/pcl_ros/features/boundary.cpp | 77 +++ pcl_ros/src/pcl_ros/features/feature.cpp | 443 ++++++++++++ pcl_ros/src/pcl_ros/features/fpfh.cpp | 79 +++ pcl_ros/src/pcl_ros/features/fpfh_omp.cpp | 79 +++ .../pcl_ros/features/moment_invariants.cpp | 77 +++ pcl_ros/src/pcl_ros/features/normal_3d.cpp | 77 +++ .../src/pcl_ros/features/normal_3d_omp.cpp | 77 +++ .../src/pcl_ros/features/normal_3d_tbb.cpp | 81 +++ pcl_ros/src/pcl_ros/features/pfh.cpp | 79 +++ .../pcl_ros/features/principal_curvatures.cpp | 79 +++ pcl_ros/src/pcl_ros/features/vfh.cpp | 79 +++ .../pcl_ros/segmentation/extract_clusters.cpp | 220 ++++++ .../extract_polygonal_prism_data.cpp | 193 ++++++ .../pcl_ros/segmentation/sac_segmentation.cpp | 645 ++++++++++++++++++ .../segmentation/segment_differences.cpp | 128 ++++ .../src/pcl_ros/segmentation/segmentation.cpp | 44 ++ pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 182 +++++ .../pcl_ros/surface/moving_least_squares.cpp | 217 ++++++ pcl_ros/src/pcl_ros/surface/surface.cpp | 48 ++ 45 files changed, 5073 insertions(+), 2 deletions(-) create mode 100755 pcl_ros/cfg/EuclideanClusterExtraction.cfg create mode 100755 pcl_ros/cfg/ExtractPolygonalPrismData.cfg create mode 100755 pcl_ros/cfg/Feature.cfg create mode 100755 pcl_ros/cfg/MLS.cfg create mode 100755 pcl_ros/cfg/SACSegmentation.cfg create mode 100755 pcl_ros/cfg/SACSegmentationFromNormals.cfg create mode 100644 pcl_ros/cfg/SACSegmentation_common.py create mode 100755 pcl_ros/cfg/SegmentDifferences.cfg create mode 100644 pcl_ros/include/pcl_ros/features/boundary.h create mode 100644 pcl_ros/include/pcl_ros/features/feature.h create mode 100644 pcl_ros/include/pcl_ros/features/fpfh.h create mode 100644 pcl_ros/include/pcl_ros/features/fpfh_omp.h create mode 100644 pcl_ros/include/pcl_ros/features/moment_invariants.h create mode 100644 pcl_ros/include/pcl_ros/features/normal_3d.h create mode 100644 pcl_ros/include/pcl_ros/features/normal_3d_omp.h create mode 100644 pcl_ros/include/pcl_ros/features/normal_3d_tbb.h create mode 100644 pcl_ros/include/pcl_ros/features/pfh.h create mode 100644 pcl_ros/include/pcl_ros/features/principal_curvatures.h create mode 100644 pcl_ros/include/pcl_ros/features/vfh.h create mode 100644 pcl_ros/include/pcl_ros/segmentation/extract_clusters.h create mode 100644 pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h create mode 100644 pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h create mode 100644 pcl_ros/include/pcl_ros/segmentation/segment_differences.h create mode 100644 pcl_ros/include/pcl_ros/surface/convex_hull.h create mode 100644 pcl_ros/include/pcl_ros/surface/moving_least_squares.h create mode 100644 pcl_ros/src/pcl_ros/features/boundary.cpp create mode 100644 pcl_ros/src/pcl_ros/features/feature.cpp create mode 100644 pcl_ros/src/pcl_ros/features/fpfh.cpp create mode 100644 pcl_ros/src/pcl_ros/features/fpfh_omp.cpp create mode 100644 pcl_ros/src/pcl_ros/features/moment_invariants.cpp create mode 100644 pcl_ros/src/pcl_ros/features/normal_3d.cpp create mode 100644 pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp create mode 100644 pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp create mode 100644 pcl_ros/src/pcl_ros/features/pfh.cpp create mode 100644 pcl_ros/src/pcl_ros/features/principal_curvatures.cpp create mode 100644 pcl_ros/src/pcl_ros/features/vfh.cpp create mode 100644 pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp create mode 100644 pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp create mode 100644 pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp create mode 100644 pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp create mode 100644 pcl_ros/src/pcl_ros/segmentation/segmentation.cpp create mode 100644 pcl_ros/src/pcl_ros/surface/convex_hull.cpp create mode 100644 pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp create mode 100644 pcl_ros/src/pcl_ros/surface/surface.cpp diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 0054a16e..24f506ae 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -18,8 +18,15 @@ include_directories(include) link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) # generate the dynamic_reconfigure config file -generate_dynamic_reconfigure_options(cfg/Filter.cfg +generate_dynamic_reconfigure_options(cfg/Feature.cfg + cfg/Filter.cfg + cfg/EuclideanClusterExtraction.cfg cfg/ExtractIndices.cfg + cfg/ExtractPolygonalPrismData.cfg + cfg/MLS.cfg + cfg/SACSegmentation.cfg + cfg/SACSegmentationFromNormals.cfg + cfg/SegmentDifferences.cfg cfg/StatisticalOutlierRemoval.cfg cfg/VoxelGrid.cfg @@ -53,6 +60,26 @@ target_link_libraries (pcl_ros_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} class_loader_hide_library_symbols(pcl_ros_io) +# ---[ PCL ROS - Features +add_library (pcl_ros_features + src/pcl_ros/features/feature.cpp + # Compilation is much faster if we include all the following CPP files in feature.cpp +# src/pcl_ros/features/boundary.cpp +# src/pcl_ros/features/fpfh.cpp +# src/pcl_ros/features/fpfh_omp.cpp +# src/pcl_ros/features/moment_invariants.cpp +# src/pcl_ros/features/normal_3d.cpp +# src/pcl_ros/features/normal_3d_omp.cpp +# src/pcl_ros/features/pfh.cpp +# src/pcl_ros/features/principal_curvatures.cpp +# src/pcl_ros/features/vfh.cpp + ) +#rosbuild_add_compile_flags (pcl_ros_features ${SSE_FLAGS}) +target_link_libraries (pcl_ros_features ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +class_loader_hide_library_symbols(pcl_ros_features) + + # ---[ PCL ROS - Filters add_library (pcl_ros_filters src/pcl_ros/filters/filter.cpp @@ -68,6 +95,31 @@ target_link_libraries (pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRA class_loader_hide_library_symbols(pcl_ros_filters) +# ---[ Point Cloud Library - Segmentation +add_library (pcl_ros_segmentation + src/pcl_ros/segmentation/segmentation.cpp + src/pcl_ros/segmentation/segment_differences.cpp +# src/pcl_ros/segmentation/extract_clusters.cpp + src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp + src/pcl_ros/segmentation/sac_segmentation.cpp + ) +#rosbuild_add_compile_flags (pcl_ros_segmentation ${SSE_FLAGS}) +target_link_libraries (pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +class_loader_hide_library_symbols(pcl_ros_segmentation) + +# ---[ Point Cloud Library - Surface +add_library (pcl_ros_surface + src/pcl_ros/surface/surface.cpp + # Compilation is much faster if we include all the following CPP files in surface.cpp + src/pcl_ros/surface/convex_hull.cpp + #src/pcl_ros/surface/moving_least_squares.cpp + ) +#rosbuild_add_compile_flags (pcl_ros_surface ${SSE_FLAGS}) +target_link_libraries (pcl_ros_surface ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +class_loader_hide_library_symbols(pcl_ros_surface) + ############ TOOLS add_executable (pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) @@ -90,7 +142,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image +install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_features pcl_ros_filters pcl_ros_surface pcl_ros_segmentation pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/pcl_ros/cfg/EuclideanClusterExtraction.cfg b/pcl_ros/cfg/EuclideanClusterExtraction.cfg new file mode 100755 index 00000000..50ae3729 --- /dev/null +++ b/pcl_ros/cfg/EuclideanClusterExtraction.cfg @@ -0,0 +1,19 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; +roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +gen = ParameterGenerator () + +# enabling/disabling the unit limits +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("cluster_tolerance", double_t, 0, "The spatial tolerance as a measure in the L2 Euclidean space", 0.05, 0.0, 2.0) +gen.add ("cluster_min_size", int_t, 0, "The minimum number of points that a cluster must contain in order to be accepted", 1, 0, 1000) +gen.add ("cluster_max_size", int_t, 0, "The maximum number of points that a cluster must contain in order to be accepted", 2147483647, 0, 2147483647) +gen.add ("max_clusters", int_t, 0, "The maximum number of clusters to extract.", 2147483647, 1, 2147483647) + +exit (gen.generate (PACKAGE, "pcl_ros", "EuclideanClusterExtraction")) + diff --git a/pcl_ros/cfg/ExtractPolygonalPrismData.cfg b/pcl_ros/cfg/ExtractPolygonalPrismData.cfg new file mode 100755 index 00000000..109770a9 --- /dev/null +++ b/pcl_ros/cfg/ExtractPolygonalPrismData.cfg @@ -0,0 +1,15 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +import roslib.packages + +gen = ParameterGenerator () +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("height_min", double_t, 0, "The minimum allowed distance to the plane model value a point will be considered from", 0.0, -10.0, 10.0) +gen.add ("height_max", double_t, 0, "The maximum allowed distance to the plane model value a point will be considered from", 0.5, -10.0, 10.0) + +exit (gen.generate (PACKAGE, "pcl_ros", "ExtractPolygonalPrismData")) diff --git a/pcl_ros/cfg/Feature.cfg b/pcl_ros/cfg/Feature.cfg new file mode 100755 index 00000000..21fe0b03 --- /dev/null +++ b/pcl_ros/cfg/Feature.cfg @@ -0,0 +1,19 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; +roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +gen = ParameterGenerator () + +#enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator") +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000) +gen.add ("radius_search", double_t, 0, "Sphere radius for nearest neighbor search", 0.0, 0.0, 0.5) +#gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum) +#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN") + +exit (gen.generate (PACKAGE, "pcl_ros", "Feature")) + diff --git a/pcl_ros/cfg/MLS.cfg b/pcl_ros/cfg/MLS.cfg new file mode 100755 index 00000000..db6c2bdf --- /dev/null +++ b/pcl_ros/cfg/MLS.cfg @@ -0,0 +1,22 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; +roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +gen = ParameterGenerator () + +enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator") +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum) +gen.add ("search_radius", double_t, 0, "Sphere radius for nearest neighbor search", 0.02, 0.0, 0.5) +#gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000) +gen.add ("use_polynomial_fit", bool_t, 0, "Whether to use polynomial fit", True) +gen.add ("polynomial_order", int_t, 0, "Set the spatial locator", 2, 0, 5) +gen.add ("gaussian_parameter", double_t, 0, "How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit)", 0.02, 0.0, 0.5) +#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN") + +exit (gen.generate (PACKAGE, "pcl_ros", "MLS")) + diff --git a/pcl_ros/cfg/SACSegmentation.cfg b/pcl_ros/cfg/SACSegmentation.cfg new file mode 100755 index 00000000..8068e5c6 --- /dev/null +++ b/pcl_ros/cfg/SACSegmentation.cfg @@ -0,0 +1,14 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +import SACSegmentation_common as common + +gen = ParameterGenerator () +common.add_common_parameters (gen) + +exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentation")) + diff --git a/pcl_ros/cfg/SACSegmentationFromNormals.cfg b/pcl_ros/cfg/SACSegmentationFromNormals.cfg new file mode 100755 index 00000000..d9c3a854 --- /dev/null +++ b/pcl_ros/cfg/SACSegmentationFromNormals.cfg @@ -0,0 +1,16 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +import roslib.packages +import SACSegmentation_common as common + +gen = ParameterGenerator () +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("normal_distance_weight", double_t, 0, "The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point normals and the plane normal.", 0.1, 0, 1.0) +common.add_common_parameters (gen) + +exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentationFromNormals")) diff --git a/pcl_ros/cfg/SACSegmentation_common.py b/pcl_ros/cfg/SACSegmentation_common.py new file mode 100644 index 00000000..8b2c6668 --- /dev/null +++ b/pcl_ros/cfg/SACSegmentation_common.py @@ -0,0 +1,21 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; + +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", 50, 0, 100000) + 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", 0.02, 0, 1.0) + 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)", 0.0, 0, 1.0) + 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.", 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.", 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.", "") + diff --git a/pcl_ros/cfg/SegmentDifferences.cfg b/pcl_ros/cfg/SegmentDifferences.cfg new file mode 100755 index 00000000..0cc5172d --- /dev/null +++ b/pcl_ros/cfg/SegmentDifferences.cfg @@ -0,0 +1,16 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import roslib; +roslib.load_manifest (PACKAGE); +from dynamic_reconfigure.parameter_generator import *; +gen = ParameterGenerator () + +# enabling/disabling the unit limits +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("distance_threshold", double_t, 0, "The distance tolerance as a measure in the L2 Euclidean space between corresponding points.", 0.0, 0.0, 2.0) + +exit (gen.generate (PACKAGE, "pcl_ros", "SegmentDifferences")) + diff --git a/pcl_ros/include/pcl_ros/features/boundary.h b/pcl_ros/include/pcl_ros/features/boundary.h new file mode 100644 index 00000000..bb91cd61 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/boundary.h @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: boundary.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_BOUNDARY_H_ +#define PCL_ROS_BOUNDARY_H_ + +#include +#include "pcl_ros/features/feature.h" + +namespace pcl_ros +{ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle + * criterion. The code makes use of the estimated surface normals at each point in the input dataset. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. + * \author Radu Bogdan Rusu + */ + class BoundaryEstimation: public FeatureFromNormals + { + private: + pcl::BoundaryEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_BOUNDARY_H_ + + diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.h new file mode 100644 index 00000000..7ef9d644 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/feature.h @@ -0,0 +1,240 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: feature.h 35422 2011-01-24 20:04:44Z rusu $ + * + */ + +#ifndef PCL_ROS_FEATURE_H_ +#define PCL_ROS_FEATURE_H_ + +// PCL includes +#include +#include + +#include "pcl_ros/pcl_nodelet.h" +#include + +// Dynamic reconfigure +#include +#include "pcl_ros/FeatureConfig.h" + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + /////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b Feature represents the base feature class. Some generic 3D operations that + * are applicable to all features are defined here as static methods. + * \author Radu Bogdan Rusu + */ + class Feature : public PCLNodelet + { + public: + typedef pcl::KdTree KdTree; + typedef pcl::KdTree::Ptr KdTreePtr; + + typedef pcl::PointCloud PointCloudIn; + typedef PointCloudIn::Ptr PointCloudInPtr; + typedef PointCloudIn::ConstPtr PointCloudInConstPtr; + + typedef boost::shared_ptr > IndicesPtr; + typedef boost::shared_ptr > IndicesConstPtr; + + /** \brief Empty constructor. */ + Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0), + use_surface_(false), spatial_locator_type_(-1) + {}; + + protected: + /** \brief The input point cloud dataset. */ + //PointCloudInConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + //IndicesConstPtr indices_; + + /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ + //PointCloudInConstPtr surface_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The number of K nearest neighbors to use for each point. */ + int k_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + // ROS nodelet attributes + /** \brief The surface PointCloud subscriber filter. */ + message_filters::Subscriber sub_surface_filter_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */ + bool use_surface_; + + /** \brief Parameter for the spatial locator tree. By convention, the values represent: + * 0: ANN (Approximate Nearest Neigbor library) kd-tree + * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree + * 2: Organized spatial dataset index + */ + int spatial_locator_type_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \brief Child initialization routine. Internal method. */ + virtual bool childInit (ros::NodeHandle &nh) = 0; + + /** \brief Publish an empty point cloud of the feature output type. */ + virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0; + + /** \brief Compute the feature and publish it. Internal method. */ + virtual void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) = 0; + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback (FeatureConfig &config, uint32_t level); + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_pi_; + message_filters::PassThrough nf_pc_; + + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. + */ + inline void + input_callback (const PointCloudInConstPtr &input) + { + PointIndices indices; + indices.header.stamp = input->header.stamp; + PointCloudIn cloud; + cloud.header.stamp = input->header.stamp; + nf_pc_.add (cloud.makeShared ()); + nf_pi_.add (boost::make_shared (indices)); + } + + private: + /** \brief Synchronized input, surface, and point indices.*/ + boost::shared_ptr > > sync_input_surface_indices_a_; + boost::shared_ptr > > sync_input_surface_indices_e_; + + /** \brief Nodelet initialization routine. */ + virtual void onInit (); + + /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. + * \param cloud the pointer to the input point cloud + * \param cloud_surface the pointer to the surface point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_surface_indices_callback (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &cloud_surface, + const PointIndicesConstPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////// + class FeatureFromNormals : public Feature + { + public: + typedef sensor_msgs::PointCloud2 PointCloud2; + + typedef pcl::PointCloud PointCloudN; + typedef PointCloudN::Ptr PointCloudNPtr; + typedef PointCloudN::ConstPtr PointCloudNConstPtr; + + FeatureFromNormals () : normals_() {}; + + protected: + /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */ + PointCloudNConstPtr normals_; + + /** \brief Child initialization routine. Internal method. */ + virtual bool childInit (ros::NodeHandle &nh) = 0; + + /** \brief Publish an empty point cloud of the feature output type. */ + virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0; + + /** \brief Compute the feature and publish it. */ + virtual void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) = 0; + + private: + /** \brief The normals PointCloud subscriber filter. */ + message_filters::Subscriber sub_normals_filter_; + + /** \brief Synchronized input, normals, surface, and point indices.*/ + boost::shared_ptr > > sync_input_normals_surface_indices_a_; + boost::shared_ptr > > sync_input_normals_surface_indices_e_; + + /** \brief Internal method. */ + void computePublish (const PointCloudInConstPtr &, + const PointCloudInConstPtr &, + const IndicesPtr &) {} // This should never be called + + /** \brief Nodelet initialization routine. */ + virtual void onInit (); + + /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. + * \param cloud the pointer to the input point cloud + * \param cloud_normals the pointer to the input point cloud normals + * \param cloud_surface the pointer to the surface point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &cloud_normals, + const PointCloudInConstPtr &cloud_surface, + const PointIndicesConstPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FEATURE_H_ diff --git a/pcl_ros/include/pcl_ros/features/fpfh.h b/pcl_ros/include/pcl_ros/features/fpfh.h new file mode 100644 index 00000000..aca59cd3 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/fpfh.h @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: fpfh.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_FPFH_H_ +#define PCL_ROS_FPFH_H_ + +#include +#include "pcl_ros/features/pfh.h" + +namespace pcl_ros +{ + /** \brief @b FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud + * dataset containing points and normals. + * + * @note If you use this code in any academic work, please cite: + * + *

    + *
  • R.B. Rusu, N. Blodow, M. Beetz. + * Fast Point Feature Histograms (FPFH) for 3D Registration. + * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), + * Kobe, Japan, May 12-17 2009. + *
    • + *
  • R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. + * Fast Geometric Point Labeling using Conditional Random Fields. + * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * St. Louis, MO, USA, October 11-15 2009. + *
    • + *
+ * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * \author Radu Bogdan Rusu + */ + class FPFHEstimation : public FeatureFromNormals + { + private: + pcl::FPFHEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_FPFH_H_ + + diff --git a/pcl_ros/include/pcl_ros/features/fpfh_omp.h b/pcl_ros/include/pcl_ros/features/fpfh_omp.h new file mode 100644 index 00000000..2db8eb18 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/fpfh_omp.h @@ -0,0 +1,96 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: fpfh_omp.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_FPFH_OMP_H_ +#define PCL_ROS_FPFH_OMP_H_ + +#include +#include "pcl_ros/features/fpfh.h" + +namespace pcl_ros +{ + /** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud + * dataset containing points and normals, in parallel, using the OpenMP standard. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • R.B. Rusu, N. Blodow, M. Beetz. + * Fast Point Feature Histograms (FPFH) for 3D Registration. + * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), + * Kobe, Japan, May 12-17 2009. + *
    • + *
  • R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. + * Fast Geometric Point Labeling using Conditional Random Fields. + * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * St. Louis, MO, USA, October 11-15 2009. + *
    • + *
+ * \author Radu Bogdan Rusu + */ + class FPFHEstimationOMP : public FeatureFromNormals + { + private: + pcl::FPFHEstimationOMP impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FPFH_OMP_H_ + diff --git a/pcl_ros/include/pcl_ros/features/moment_invariants.h b/pcl_ros/include/pcl_ros/features/moment_invariants.h new file mode 100644 index 00000000..ef6df20a --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/moment_invariants.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: moment_invariants.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ +#define PCL_ROS_MOMENT_INVARIANTS_H_ + +#include +#include "pcl_ros/features/feature.h" + +namespace pcl_ros +{ + /** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. + * \author Radu Bogdan Rusu + */ + class MomentInvariantsEstimation: public Feature + { + private: + pcl::MomentInvariantsEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ + + diff --git a/pcl_ros/include/pcl_ros/features/normal_3d.h b/pcl_ros/include/pcl_ros/features/normal_3d.h new file mode 100644 index 00000000..7a1506e4 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/normal_3d.h @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: normal_3d.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_NORMAL_3D_H_ +#define PCL_ROS_NORMAL_3D_H_ + +#include +#include "pcl_ros/features/feature.h" + +namespace pcl_ros +{ + /** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and + * curvatures. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations. + * \author Radu Bogdan Rusu + */ + class NormalEstimation: public Feature + { + private: + /** \brief PCL implementation object. */ + pcl::NormalEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. + * \param cloud the input point cloud to copy the header from. + */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_NORMAL_3D_H_ + diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_omp.h b/pcl_ros/include/pcl_ros/features/normal_3d_omp.h new file mode 100644 index 00000000..ab265162 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/normal_3d_omp.h @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: normal_3d_omp.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_NORMAL_3D_OMP_H_ +#define PCL_ROS_NORMAL_3D_OMP_H_ + +#include +#include "pcl_ros/features/normal_3d.h" + +namespace pcl_ros +{ + /** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and + * curvatures, in parallel, using the OpenMP standard. + * \author Radu Bogdan Rusu + */ + class NormalEstimationOMP: public Feature + { + private: + pcl::NormalEstimationOMP impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_ diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h new file mode 100644 index 00000000..ec570f75 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: normal_3d_tbb.h 35661 2011-02-01 06:04:14Z rusu $ + * + */ + +#ifndef PCL_ROS_NORMAL_3D_TBB_H_ +#define PCL_ROS_NORMAL_3D_TBB_H_ + +//#include "pcl_ros/pcl_ros_config.h" +//#if defined(HAVE_TBB) + +#include +#include "pcl_ros/features/normal_3d.h" + +namespace pcl_ros +{ + /** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and + * curvatures, in parallel, using Intel's Threading Building Blocks library. + * \author Radu Bogdan Rusu + */ + class NormalEstimationTBB: public Feature + { + private: + pcl::NormalEstimationTBB impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +//#endif // HAVE_TBB + +#endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_ + + diff --git a/pcl_ros/include/pcl_ros/features/pfh.h b/pcl_ros/include/pcl_ros/features/pfh.h new file mode 100644 index 00000000..262ce37c --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/pfh.h @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: pfh.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_PFH_H_ +#define PCL_ROS_PFH_H_ + +#include +#include "pcl_ros/features/feature.h" + +namespace pcl_ros +{ + /** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset + * containing points and normals. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz. + * Aligning Point Cloud Views using Persistent Feature Histograms. + * In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * Nice, France, September 22-26 2008. + *
    • + *
  • R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz. + * Learning Informative Point Classes for the Acquisition of Object Model Maps. + * In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV), + * Hanoi, Vietnam, December 17-20 2008. + *
    • + *
+ * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * \author Radu Bogdan Rusu + */ + class PFHEstimation : public FeatureFromNormals + { + private: + pcl::PFHEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_PFH_H_ + + diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.h b/pcl_ros/include/pcl_ros/features/principal_curvatures.h new file mode 100644 index 00000000..7a0ddfb6 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/principal_curvatures.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: principal_curvatures.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ +#define PCL_ROS_PRINCIPAL_CURVATURES_H_ + +#include +#include "pcl_ros/features/feature.h" + +namespace pcl_ros +{ + /** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of + * principal surface curvatures for a given point cloud dataset containing points and normals. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. + * \author Radu Bogdan Rusu, Jared Glover + */ + class PrincipalCurvaturesEstimation : public FeatureFromNormals + { + private: + pcl::PrincipalCurvaturesEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ diff --git a/pcl_ros/include/pcl_ros/features/vfh.h b/pcl_ros/include/pcl_ros/features/vfh.h new file mode 100644 index 00000000..90e588f1 --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/vfh.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: vfh.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_FEATURES_VFH_H_ +#define PCL_ROS_FEATURES_VFH_H_ + +#include +#include "pcl_ros/features/fpfh.h" + +namespace pcl_ros +{ + /** \brief @b VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud + * dataset containing points and normals. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * \author Radu Bogdan Rusu + */ + class VFHEstimation : public FeatureFromNormals + { + private: + pcl::VFHEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FEATURES_VFH_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h new file mode 100644 index 00000000..38a6d379 --- /dev/null +++ b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h @@ -0,0 +1,105 @@ +/* + * 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: extract_clusters.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ +#define PCL_ROS_EXTRACT_CLUSTERS_H_ + +#include +#include "pcl_ros/pcl_nodelet.h" + +// Dynamic reconfigure +#include +#include "pcl_ros/EuclideanClusterExtractionConfig.h" + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. + * \author Radu Bogdan Rusu + */ + class EuclideanClusterExtraction : public PCLNodelet + { + public: + /** \brief Empty constructor. */ + EuclideanClusterExtraction () : publish_indices_ (false), max_clusters_ (std::numeric_limits::max ()) {}; + + protected: + // ROS nodelet attributes + /** \brief Publish indices or convert to PointCloud clusters. Default: false */ + bool publish_indices_; + + /** \brief Maximum number of clusters to publish. */ + int max_clusters_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \brief Nodelet initialization routine. */ + void onInit (); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback (EuclideanClusterExtractionConfig &config, uint32_t level); + + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices); + + private: + /** \brief The PCL implementation used. */ + pcl::EuclideanClusterExtraction impl_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr > > sync_input_indices_e_; + boost::shared_ptr > > sync_input_indices_a_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h new file mode 100644 index 00000000..33050a41 --- /dev/null +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h @@ -0,0 +1,126 @@ +/* + * 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: extract_polygonal_prism_data.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ +#define PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ + +#include "pcl_ros/pcl_nodelet.h" +#include +#include +#include + +// PCL includes +#include + +// Dynamic reconfigure +#include +#include "pcl_ros/ExtractPolygonalPrismDataConfig.h" + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + /** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with + * a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying + * inside it. + * + * An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane). + * \author Radu Bogdan Rusu + */ + class ExtractPolygonalPrismData : public PCLNodelet + { + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + protected: + /** \brief The output PointIndices publisher. */ + ros::Publisher pub_output_; + + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_hull_filter_; + + /** \brief Synchronized input, planar hull, and indices.*/ + boost::shared_ptr > > sync_input_hull_indices_e_; + boost::shared_ptr > > sync_input_hull_indices_a_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_; + + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. + */ + inline void + input_callback (const PointCloudConstPtr &input) + { + PointIndices cloud; + cloud.header.stamp = input->header.stamp; + nf_.add (boost::make_shared (cloud)); + } + + /** \brief Nodelet initialization routine. */ + void onInit (); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level); + + /** \brief Input point cloud callback. Used when \a use_indices is set. + * \param cloud the pointer to the input point cloud + * \param hull the pointer to the planar hull point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_hull_indices_callback (const PointCloudConstPtr &cloud, + const PointCloudConstPtr &hull, + const PointIndicesConstPtr &indices); + + private: + /** \brief The PCL implementation used. */ + pcl::ExtractPolygonalPrismData impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h new file mode 100644 index 00000000..7ff369b6 --- /dev/null +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h @@ -0,0 +1,277 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: sac_segmentation.h 35564 2011-01-27 07:32:12Z rusu $ + * + */ + +#ifndef PCL_ROS_SAC_SEGMENTATION_H_ +#define PCL_ROS_SAC_SEGMENTATION_H_ + +#include "pcl_ros/pcl_nodelet.h" +#include + +// PCL includes +#include + +// Dynamic reconfigure +#include +#include "pcl_ros/SACSegmentationConfig.h" +#include "pcl_ros/SACSegmentationFromNormalsConfig.h" + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in + * the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. + * \author Radu Bogdan Rusu + */ + class SACSegmentation : public PCLNodelet + { + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + public: + /** \brief Constructor. */ + SACSegmentation () : min_inliers_(0) {} + + /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. + * \param tf_frame the TF frame the input PointCloud should be transformed into before processing + */ + inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; } + + /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ + inline std::string getInputTFframe () { return (tf_input_frame_); } + + /** \brief Set the output TF frame the data should be transformed into after processing. + * \param tf_frame the TF frame the PointCloud should be transformed into after processing + */ + inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; } + + /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ + inline std::string getOutputTFframe () { return (tf_output_frame_); } + + protected: + // The minimum number of inliers a model must have in order to be considered valid. + int min_inliers_; + + // ROS nodelet attributes + /** \brief The output PointIndices publisher. */ + ros::Publisher pub_indices_; + + /** \brief The output ModelCoefficients publisher. */ + ros::Publisher pub_model_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_input_frame_; + + /** \brief The original data input TF frame. */ + std::string tf_input_orig_frame_; + + /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_output_frame_; + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_pi_; + + /** \brief Nodelet initialization routine. */ + virtual void onInit (); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback (SACSegmentationConfig &config, uint32_t level); + + /** \brief Input point cloud callback. Used when \a use_indices is set. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices); + + /** \brief Pointer to a set of indices stored internally. + * (used when \a latched_indices_ is set). + */ + PointIndices indices_; + + /** \brief Indices callback. Used when \a latched_indices_ is set. + * \param indices the pointer to the input point cloud indices + */ + inline void + indices_callback (const PointIndicesConstPtr &indices) + { + indices_ = *indices; + } + + /** \brief Input callback. Used when \a latched_indices_ is set. + * \param input the pointer to the input point cloud + */ + inline void + input_callback (const PointCloudConstPtr &input) + { + indices_.header = input->header; + PointIndicesConstPtr indices; + indices.reset (new PointIndices (indices_)); + nf_pi_.add (indices); + } + + private: + /** \brief Internal mutex. */ + boost::mutex mutex_; + + /** \brief The PCL implementation used. */ + pcl::SACSegmentation impl_; + + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr > > sync_input_indices_e_; + boost::shared_ptr > > sync_input_indices_a_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and + * models that require the use of surface normals for estimation. + */ + class SACSegmentationFromNormals: public SACSegmentation + { + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + typedef pcl::PointCloud PointCloudN; + typedef PointCloudN::Ptr PointCloudNPtr; + typedef PointCloudN::ConstPtr PointCloudNConstPtr; + + public: + /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. + * \param tf_frame the TF frame the input PointCloud should be transformed into before processing + */ + inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; } + + /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ + inline std::string getInputTFframe () { return (tf_input_frame_); } + + /** \brief Set the output TF frame the data should be transformed into after processing. + * \param tf_frame the TF frame the PointCloud should be transformed into after processing + */ + inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; } + + /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ + inline std::string getOutputTFframe () { return (tf_output_frame_); } + + protected: + // ROS nodelet attributes + /** \brief The normals PointCloud subscriber filter. */ + message_filters::Subscriber sub_normals_filter_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_axis_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. + */ + inline void + input_callback (const PointCloudConstPtr &cloud) + { + PointIndices indices; + indices.header.stamp = cloud->header.stamp; + nf_.add (boost::make_shared (indices)); + } + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_; + + /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_input_frame_; + /** \brief The original data input TF frame. */ + std::string tf_input_orig_frame_; + /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_output_frame_; + + /** \brief Nodelet initialization routine. */ + virtual void onInit (); + + /** \brief Model callback + * \param model the sample consensus model found + */ + void axis_callback (const pcl::ModelCoefficientsConstPtr &model); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level); + + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param cloud_normals the pointer to the input point cloud normals + * \param indices the pointer to the input point cloud indices + */ + void input_normals_indices_callback (const PointCloudConstPtr &cloud, + const PointCloudNConstPtr &cloud_normals, + const PointIndicesConstPtr &indices); + + private: + /** \brief Internal mutex. */ + boost::mutex mutex_; + + /** \brief The PCL implementation used. */ + pcl::SACSegmentationFromNormals impl_; + + /** \brief Synchronized input, normals, and indices.*/ + boost::shared_ptr > > sync_input_normals_indices_a_; + boost::shared_ptr > > sync_input_normals_indices_e_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.h b/pcl_ros/include/pcl_ros/segmentation/segment_differences.h new file mode 100644 index 00000000..287d8fb2 --- /dev/null +++ b/pcl_ros/include/pcl_ros/segmentation/segment_differences.h @@ -0,0 +1,106 @@ +/* + * 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: segment_differences.h 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ +#define PCL_ROS_SEGMENT_DIFFERENCES_H_ + +#include +#include "pcl_ros/pcl_nodelet.h" + +// Dynamic reconfigure +#include +#include "pcl_ros/SegmentDifferencesConfig.h" + + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the + * difference between them for a maximum given distance threshold. + * \author Radu Bogdan Rusu + */ + class SegmentDifferences : public PCLNodelet + { + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + public: + /** \brief Empty constructor. */ + SegmentDifferences () {}; + + protected: + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_target_filter_; + + /** \brief Synchronized input, and planar hull.*/ + boost::shared_ptr > > sync_input_target_e_; + boost::shared_ptr > > sync_input_target_a_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \brief Nodelet initialization routine. */ + void onInit (); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback (SegmentDifferencesConfig &config, uint32_t level); + + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param cloud_target the pointcloud that we want to segment \a cloud from + */ + void input_target_callback (const PointCloudConstPtr &cloud, + const PointCloudConstPtr &cloud_target); + + private: + /** \brief The PCL implementation used. */ + pcl::SegmentDifferences impl_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.h b/pcl_ros/include/pcl_ros/surface/convex_hull.h new file mode 100644 index 00000000..59fa4f6f --- /dev/null +++ b/pcl_ros/include/pcl_ros/surface/convex_hull.h @@ -0,0 +1,92 @@ +/* + * 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: convex_hull.h 36116 2011-02-22 00:05:23Z rusu $ + * + */ + +#ifndef PCL_ROS_CONVEX_HULL_2D_H_ +#define PCL_ROS_CONVEX_HULL_2D_H_ + +#include "pcl_ros/pcl_nodelet.h" + +// PCL includes +#include + +// Dynamic reconfigure +#include + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + /** \brief @b ConvexHull2D represents a 2D ConvexHull implementation. + * \author Radu Bogdan Rusu + */ + class ConvexHull2D : public PCLNodelet + { + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + private: + /** \brief Nodelet initialization routine. */ + virtual void onInit (); + + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_indices_callback (const PointCloudConstPtr &cloud, + const PointIndicesConstPtr &indices); + + private: + /** \brief The PCL implementation used. */ + pcl::ConvexHull impl_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief Publisher for PolygonStamped. */ + ros::Publisher pub_plane_; + + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr > > sync_input_indices_e_; + boost::shared_ptr > > sync_input_indices_a_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_ diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h new file mode 100644 index 00000000..87aa2662 --- /dev/null +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h @@ -0,0 +1,145 @@ +/* + * 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: moving_least_squares.h 36097 2011-02-20 14:18:58Z marton $ + * + */ + +#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ +#define PCL_ROS_MOVING_LEAST_SQUARES_H_ + +#include "pcl_ros/pcl_nodelet.h" + +// PCL includes +#include + +// Dynamic reconfigure +#include +#include "pcl_ros/MLSConfig.h" + +namespace pcl_ros +{ + namespace sync_policies = message_filters::sync_policies; + + /** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation. + * The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters. + * Normals are estimated at each point as well and published on a separate topic. + * \author Radu Bogdan Rusu, Zoltan-Csaba Marton + */ + class MovingLeastSquares : public PCLNodelet + { + typedef pcl::PointXYZ PointIn; + typedef pcl::Normal NormalOut; + + typedef pcl::PointCloud PointCloudIn; + typedef PointCloudIn::Ptr PointCloudInPtr; + typedef PointCloudIn::ConstPtr PointCloudInConstPtr; + typedef pcl::PointCloud NormalCloudOut; + + typedef pcl::KdTree KdTree; + typedef pcl::KdTree::Ptr KdTreePtr; + + protected: + /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ + PointCloudInConstPtr surface_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief The number of K nearest neighbors to use for each point. */ + //int k_; + + /** \brief Whether to use a polynomial fit. */ + bool use_polynomial_fit_; + + /** \brief The order of the polynomial to be fit. */ + int polynomial_order_; + + /** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */ + double gaussian_parameter_; + + // ROS nodelet attributes + /** \brief The surface PointCloud subscriber filter. */ + message_filters::Subscriber sub_surface_filter_; + + /** \brief Parameter for the spatial locator tree. By convention, the values represent: + * 0: ANN (Approximate Nearest Neigbor library) kd-tree + * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree + * 2: Organized spatial dataset index + */ + int spatial_locator_type_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback (MLSConfig &config, uint32_t level); + + private: + /** \brief Nodelet initialization routine. */ + virtual void onInit (); + + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_indices_callback (const PointCloudInConstPtr &cloud, + const PointIndicesConstPtr &indices); + + + private: + /** \brief The PCL implementation used. */ + pcl::MovingLeastSquares impl_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief The output PointCloud (containing the normals) publisher. */ + ros::Publisher pub_normals_; + + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr > > sync_input_indices_e_; + boost::shared_ptr > > sync_input_indices_a_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp new file mode 100644 index 00000000..0e17c5fd --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/boundary.h" + +void +pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::BoundaryEstimation BoundaryEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, BoundaryEstimation, BoundaryEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp new file mode 100644 index 00000000..434707ee --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -0,0 +1,443 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: feature.cpp 35422 2011-01-24 20:04:44Z rusu $ + * + */ + +//#include +// Include the implementations here instead of compiling them separately to speed up compile time +//#include "normal_3d.cpp" +//#include "boundary.cpp" +//#include "fpfh.cpp" +//#include "fpfh_omp.cpp" +//#include "moment_invariants.cpp" +//#include "normal_3d_omp.cpp" +//#include "normal_3d_tbb.cpp" +//#include "pfh.cpp" +//#include "principal_curvatures.cpp" +//#include "vfh.cpp" +#include +#include "pcl_ros/features/feature.h" +#include + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // Call the child init + childInit (*pnh_); + + // Allow each individual class that inherits from us to declare their own Publisher + // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc + //pub_output_ = pnh_->template advertise ("output", max_queue_size_); + + // ---[ Mandatory parameters + if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) + { + NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + return; + } + if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) + { + NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + return; + } + + // ---[ Optional parameters + pnh_->getParam ("use_surface", use_surface_); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); + srv_->setCallback (f); + + // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages + if (use_indices_ || use_surface_) + { + // Create the objects here + if (approximate_sync_) + sync_input_surface_indices_a_ = boost::make_shared > >(max_queue_size_); + else + sync_input_surface_indices_e_ = boost::make_shared > >(max_queue_size_); + + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + if (use_indices_) + { + // If indices are enabled, subscribe to the indices + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + if (use_surface_) // Use both indices and surface + { + // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register + sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + if (approximate_sync_) + sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); + else + sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); + } + else // Use only indices + { + sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); + // surface not enabled, connect the input-indices duo and register + if (approximate_sync_) + sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); + else + sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); + } + } + else // Use only surface + { + sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); + // indices not enabled, connect the input-surface duo and register + sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + if (approximate_sync_) + sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); + else + sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); + } + // Register callbacks + if (approximate_sync_) + sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); + else + sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); + } + else + // Subscribe in an old fashion to input only (no filters) + sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName ().c_str (), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level) +{ + if (k_ != config.k_search) + { + k_ = config.k_search; + NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_); + } + if (search_radius_ != config.radius_search) + { + search_radius_ = config.radius_search; + NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0) + return; + + // If cloud is given, check if it's valid + if (!isValid (cloud)) + { + NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + // If surface is given, check if it's valid + if (cloud_surface && !isValid (cloud_surface, "surface")) + { + NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + /// DEBUG + if (cloud_surface) + if (indices) + NODELET_DEBUG ("[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ()); + + else + if (indices) + NODELET_DEBUG ("[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + /// + + + if ((int)(cloud->width * cloud->height) < k_) + { + NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height)); + emptyPublish (cloud); + return; + } + + // If indices given... + IndicesPtr vindices; + if (indices && !indices->header.frame_id.empty ()) + vindices.reset (new std::vector (indices->indices)); + + computePublish (cloud, cloud_surface, vindices); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // Call the child init + childInit (*pnh_); + + // Allow each individual class that inherits from us to declare their own Publisher + // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc + //pub_output_ = pnh_->template advertise ("output", max_queue_size_); + + // ---[ Mandatory parameters + if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) + { + NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + return; + } + if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) + { + NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + return; + } + // ---[ Optional parameters + pnh_->getParam ("use_surface", use_surface_); + + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); + srv_->setCallback (f); + + // Create the objects here + if (approximate_sync_) + sync_input_normals_surface_indices_a_ = boost::make_shared > > (max_queue_size_); + else + sync_input_normals_surface_indices_e_ = boost::make_shared > > (max_queue_size_); + + // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages + if (use_indices_ || use_surface_) + { + if (use_indices_) + { + // If indices are enabled, subscribe to the indices + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + if (use_surface_) // Use both indices and surface + { + // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register + sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + if (approximate_sync_) + sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); + else + sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); + } + else // Use only indices + { + sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); + if (approximate_sync_) + // surface not enabled, connect the input-indices duo and register + sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); + else + // surface not enabled, connect the input-indices duo and register + sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); + } + } + else // Use only surface + { + // indices not enabled, connect the input-surface duo and register + sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + + sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); + if (approximate_sync_) + sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); + else + sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); + } + } + else + { + sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); + + if (approximate_sync_) + sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); + else + sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); + } + + // Register callbacks + if (approximate_sync_) + sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); + else + sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName ().c_str (), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( + const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, + const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0) + return; + + // If cloud+normals is given, check if it's valid + if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) + { + NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + // If surface is given, check if it's valid + if (cloud_surface && !isValid (cloud_surface, "surface")) + { + NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ()); + emptyPublish (cloud); + return; + } + + /// DEBUG + if (cloud_surface) + if (indices) + NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + else + if (indices) + NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + /// + + if ((int)(cloud->width * cloud->height) < k_) + { + NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height)); + emptyPublish (cloud); + return; + } + + // If indices given... + IndicesPtr vindices; + if (indices && !indices->header.frame_id.empty ()) + vindices.reset (new std::vector (indices->indices)); + + computePublish (cloud, cloud_normals, cloud_surface, vindices); +} + diff --git a/pcl_ros/src/pcl_ros/features/fpfh.cpp b/pcl_ros/src/pcl_ros/features/fpfh.cpp new file mode 100644 index 00000000..7782fde9 --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/fpfh.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/fpfh.h" + +void +pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::FPFHEstimation FPFHEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimation, FPFHEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp new file mode 100644 index 00000000..51880a9f --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/fpfh_omp.h" + +void +pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; +PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimationOMP, FPFHEstimationOMP, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp new file mode 100644 index 00000000..85bd209a --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/moment_invariants.h" + +void +pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, MomentInvariantsEstimation, MomentInvariantsEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/features/normal_3d.cpp new file mode 100644 index 00000000..a512c41f --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/normal_3d.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/normal_3d.h" + +void +pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::NormalEstimation NormalEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimation, NormalEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp new file mode 100644 index 00000000..e53fe9c7 --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/normal_3d_omp.h" + +void +pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; +PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationOMP, NormalEstimationOMP, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp new file mode 100644 index 00000000..d6636f73 --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: normal_3d_tbb.cpp 35625 2011-01-31 07:56:13Z gbiggs $ + * + */ + +#include +#include "pcl_ros/features/normal_3d_tbb.h" + +#if defined HAVE_TBB + +void +pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloud output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; +PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationTBB, NormalEstimationTBB, nodelet::Nodelet); + +#endif // HAVE_TBB + diff --git a/pcl_ros/src/pcl_ros/features/pfh.cpp b/pcl_ros/src/pcl_ros/features/pfh.cpp new file mode 100644 index 00000000..f40e0bab --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/pfh.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: pfh.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/pfh.h" + +void +pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::PFHEstimation PFHEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, PFHEstimation, PFHEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp new file mode 100644 index 00000000..d6431160 --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: principal_curvatures.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/principal_curvatures.h" + +void +pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, PrincipalCurvaturesEstimation, PrincipalCurvaturesEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/vfh.cpp b/pcl_ros/src/pcl_ros/features/vfh.cpp new file mode 100644 index 00000000..f13028e3 --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/vfh.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: vfh.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/features/vfh.h" + +void +pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, k_); + impl_.setSearchMethod (tree_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::VFHEstimation VFHEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, VFHEstimation, VFHEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp new file mode 100644 index 00000000..3c067f0a --- /dev/null +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -0,0 +1,220 @@ +/* + * 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: extract_clusters.hpp 32052 2010-08-27 02:19:30Z rusu $ + * + */ + +#include +#include +#include "pcl_ros/segmentation/extract_clusters.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::EuclideanClusterExtraction::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // ---[ Mandatory parameters + double cluster_tolerance; + if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance)) + { + NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ()); + return; + } + int spatial_locator; + if (!pnh_->getParam ("spatial_locator", spatial_locator)) + { + NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + return; + } + + //private_nh.getParam ("use_indices", use_indices_); + pnh_->getParam ("publish_indices", publish_indices_); + + if (publish_indices_) + pub_output_ = pnh_->advertise ("output", max_queue_size_); + else + pub_output_ = pnh_->advertise ("output", max_queue_size_); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2); + srv_->setCallback (f); + + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + + if (approximate_sync_) + { + sync_input_indices_a_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2)); + } + else + { + sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2)); + } + } + else + // Subscribe in an old fashion to input only (no filters) + sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ())); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - max_queue_size : %d\n" + " - use_indices : %s\n" + " - cluster_tolerance : %f\n" + " - spatial_locator : %d", + getName ().c_str (), + max_queue_size_, + (use_indices_) ? "true" : "false", cluster_tolerance, spatial_locator); + + // Set given parameters here + impl_.setSpatialLocator (spatial_locator); + impl_.setClusterTolerance (cluster_tolerance); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level) +{ + if (impl_.getClusterTolerance () != config.cluster_tolerance) + { + impl_.setClusterTolerance (config.cluster_tolerance); + NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance); + } + if (impl_.getMinClusterSize () != config.cluster_min_size) + { + impl_.setMinClusterSize (config.cluster_min_size); + NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size); + } + if (impl_.getMaxClusterSize () != config.cluster_max_size) + { + impl_.setMaxClusterSize (config.cluster_max_size); + NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size); + } + if (max_clusters_ != config.max_clusters) + { + max_clusters_ = config.max_clusters; + NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::EuclideanClusterExtraction::input_indices_callback ( + const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0) + return; + + // If cloud is given, check if it's valid + if (!isValid (cloud)) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + return; + } + + /// DEBUG + if (indices) + NODELET_DEBUG ("[%s::input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + /// + + IndicesPtr indices_ptr; + if (indices) + indices_ptr.reset (new std::vector (indices->indices)); + + impl_.setInputCloud (cloud); + impl_.setIndices (indices_ptr); + + std::vector clusters; + impl_.extract (clusters); + + if (publish_indices_) + { + for (size_t i = 0; i < clusters.size (); ++i) + { + if ((int)i >= max_clusters_) + break; + // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. + clusters[i].header.stamp += ros::Duration (i * 0.001); + pub_output_.publish (boost::make_shared (clusters[i])); + } + + NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ()); + } + else + { + for (size_t i = 0; i < clusters.size (); ++i) + { + if ((int)i >= max_clusters_) + break; + PointCloud output; + copyPointCloud (*cloud, clusters[i].indices, output); + + //PointCloud output_blob; // Convert from the templated output to the PointCloud blob + //pcl::toROSMsg (output, output_blob); + // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. + output.header.stamp += ros::Duration (i * 0.001); + // Publish a Boost shared ptr const data + pub_output_.publish (output.makeShared ()); + NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", + i, clusters[i].indices.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); + } + } +} + +typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction; +PLUGINLIB_DECLARE_CLASS (pcl, EuclideanClusterExtraction, EuclideanClusterExtraction, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp new file mode 100644 index 00000000..23ab6d12 --- /dev/null +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -0,0 +1,193 @@ +/* + * 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: extract_polygonal_prism_data.hpp 32996 2010-09-30 23:42:11Z rusu $ + * + */ + +#include +#include "pcl_ros/transforms.h" +#include "pcl_ros/segmentation/extract_polygonal_prism_data.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ExtractPolygonalPrismData::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_); + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2); + srv_->setCallback (f); + + // Create the objects here + if (approximate_sync_) + sync_input_hull_indices_a_ = boost::make_shared > > (max_queue_size_); + else + sync_input_hull_indices_e_ = boost::make_shared > > (max_queue_size_); + + if (use_indices_) + { + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + if (approximate_sync_) + sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_); + else + sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_); + } + else + { + sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1)); + + if (approximate_sync_) + sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_); + else + sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_); + } + // Register callbacks + if (approximate_sync_) + sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3)); + else + sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3)); + + // Advertise the output topics + pub_output_ = pnh_->advertise ("output", max_queue_size_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level) +{ + double height_min, height_max; + impl_.getHeightLimits (height_min, height_max); + if (height_min != config.height_min) + { + height_min = config.height_min; + NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min); + impl_.setHeightLimits (height_min, height_max); + } + if (height_max != config.height_max) + { + height_max = config.height_max; + NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max); + impl_.setHeightLimits (height_min, height_max); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( + const PointCloudConstPtr &cloud, + const PointCloudConstPtr &hull, + const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0) + return; + + // Copy the header (stamp + frame_id) + pcl::PointIndices inliers; + inliers.header = cloud->header; + + // If cloud is given, check if it's valid + if (!isValid (cloud) || !isValid (hull, "planar_hull")) + { + NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ()); + pub_output_.publish (boost::make_shared (inliers)); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ()); + pub_output_.publish (boost::make_shared (inliers)); + return; + } + + /// DEBUG + if (indices) + NODELET_DEBUG ("[%s::input_indices_hull_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_indices_hull_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ()); + /// + + if (cloud->header.frame_id != hull->header.frame_id) + { + NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ()); + PointCloud planar_hull; + if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_)) + { + // Publish empty before return + pub_output_.publish (boost::make_shared (inliers)); + return; + } + impl_.setInputPlanarHull (planar_hull.makeShared ()); + } + else + impl_.setInputPlanarHull (hull); + + IndicesPtr indices_ptr; + if (indices && !indices->header.frame_id.empty ()) + indices_ptr.reset (new std::vector (indices->indices)); + + impl_.setInputCloud (cloud); + impl_.setIndices (indices_ptr); + + // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) + if (!cloud->points.empty ()) + impl_.segment (inliers); + // Enforce that the TF frame and the timestamp are copied + inliers.header = cloud->header; + pub_output_.publish (boost::make_shared (inliers)); + NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ()); +} + +typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData; +PLUGINLIB_DECLARE_CLASS (pcl, ExtractPolygonalPrismData, ExtractPolygonalPrismData, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp new file mode 100644 index 00000000..8478c71d --- /dev/null +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -0,0 +1,645 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: sac_segmentation.hpp 33195 2010-10-10 14:12:19Z marton $ + * + */ + +#include +#include "pcl_ros/segmentation/sac_segmentation.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + + // when "use_indices" is set to true, and "latched_indices" is set to true, + // we'll subscribe and get a separate callback for PointIndices that will + // save the indices internally, and a PointCloud + PointIndices callback + // will take care of meshing the new PointClouds with the old saved indices. + if (latched_indices_) + { + // Subscribe to a callback that saves the indices + sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1)); + // Subscribe to a callback that sets the header of the saved indices to the cloud header + sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1)); + + // Synchronize the two topics. No need for an approximate synchronizer here, as we'll + // match the timestamps exactly + sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_); + sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + } + // "latched_indices" not set, proceed with regular pairs + else + { + if (approximate_sync_) + { + sync_input_indices_a_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + } + else + { + sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + } + } + } + else + // Subscribe in an old fashion to input only (no filters) + sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ())); + + // Advertise the output topics + pub_indices_ = pnh_->advertise ("inliers", max_queue_size_); + pub_model_ = pnh_->advertise ("model", max_queue_size_); + + // ---[ Mandatory parameters + int model_type; + if (!pnh_->getParam ("model_type", model_type)) + { + NODELET_ERROR ("[onInit] Need a 'model_type' parameter to be set before continuing!"); + return; + } + double threshold; // unused - set via dynamic reconfigure in the callback + if (!pnh_->getParam ("distance_threshold", threshold)) + { + NODELET_ERROR ("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); + return; + } + + // ---[ Optional parameters + int method_type = 0; + pnh_->getParam ("method_type", method_type); + + XmlRpc::XmlRpcValue axis_param; + pnh_->getParam ("axis", axis_param); + Eigen::Vector3f axis = Eigen::Vector3f::Zero (); + + switch (axis_param.getType ()) + { + case XmlRpc::XmlRpcValue::TypeArray: + { + if (axis_param.size () != 3) + { + NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ()); + return; + } + for (int i = 0; i < 3; ++i) + { + if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble) + { + NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ()); + return; + } + double value = axis_param[i]; axis[i] = value; + } + break; + } + default: + { + break; + } + } + + // Initialize the random number generator + srand (time (0)); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared >(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentation::config_callback, this, _1, _2); + srv_->setCallback (f); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - method_type : %d\n" + " - model_threshold : %f\n" + " - axis : [%f, %f, %f]\n", + getName ().c_str (), model_type, method_type, threshold, + axis[0], axis[1], axis[2]); + + // Set given parameters here + impl_.setModelType (model_type); + impl_.setMethodType (method_type); + impl_.setAxis (axis); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock lock (mutex_); + + if (impl_.getDistanceThreshold () != config.distance_threshold) + { + //sac_->setDistanceThreshold (threshold_); - done in initSAC + impl_.setDistanceThreshold (config.distance_threshold); + NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold); + } + // The maximum allowed difference between the model normal and the given axis _in radians_ + if (impl_.getEpsAngle () != config.eps_angle) + { + impl_.setEpsAngle (config.eps_angle); + NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI); + } + + // Number of inliers + if (min_inliers_ != config.min_inliers) + { + min_inliers_ = config.min_inliers; + NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_); + } + + if (impl_.getMaxIterations () != config.max_iterations) + { + //sac_->setMaxIterations (max_iterations_); - done in initSAC + impl_.setMaxIterations (config.max_iterations); + NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations); + } + if (impl_.getProbability () != config.probability) + { + //sac_->setProbability (probability_); - done in initSAC + impl_.setProbability (config.probability); + NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability); + } + if (impl_.getOptimizeCoefficients () != config.optimize_coefficients) + { + impl_.setOptimizeCoefficients (config.optimize_coefficients); + NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false"); + } + + double radius_min, radius_max; + impl_.getRadiusLimits (radius_min, radius_max); + if (radius_min != config.radius_min) + { + radius_min = config.radius_min; + NODELET_DEBUG ("[config_callback] Setting minimum allowable model radius to: %f.", radius_min); + impl_.setRadiusLimits (radius_min, radius_max); + } + if (radius_max != config.radius_max) + { + radius_max = config.radius_max; + NODELET_DEBUG ("[config_callback] Setting maximum allowable model radius to: %f.", radius_max); + impl_.setRadiusLimits (radius_min, radius_max); + } + + if (tf_input_frame_ != config.input_frame) + { + tf_input_frame_ = config.input_frame; + NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ()); + NODELET_WARN ("input_frame TF not implemented yet!"); + } + if (tf_output_frame_ != config.output_frame) + { + tf_output_frame_ = config.output_frame; + NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ()); + NODELET_WARN ("output_frame TF not implemented yet!"); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &cloud, + const PointIndicesConstPtr &indices) +{ + boost::mutex::scoped_lock lock (mutex_); + + // No subscribers, no work + if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0) + return; + + PointIndices inliers; + ModelCoefficients model; + // Enforce that the TF frame and the timestamp are copied + inliers.header = model.header = cloud->header; + + // If cloud is given, check if it's valid + if (!isValid (cloud)) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + pub_indices_.publish (boost::make_shared (inliers)); + pub_model_.publish (boost::make_shared (model)); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + pub_indices_.publish (boost::make_shared (inliers)); + pub_model_.publish (boost::make_shared (model)); + return; + } + + /// DEBUG + if (indices && !indices->header.frame_id.empty ()) + NODELET_DEBUG ("[%s::input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + /// + + // Check whether the user has given a different input TF frame + tf_input_orig_frame_ = cloud->header.frame_id; + PointCloudConstPtr cloud_tf; +/* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) + { + NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); + // Save the original frame ID + // Convert the cloud into the different frame + PointCloud cloud_transformed; + if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, cloud_transformed, tf_listener_)) + return; + cloud_tf.reset (new PointCloud (cloud_transformed)); + } + else*/ + cloud_tf = cloud; + + IndicesPtr indices_ptr; + if (indices && !indices->header.frame_id.empty ()) + indices_ptr.reset (new std::vector (indices->indices)); + + impl_.setInputCloud (cloud_tf); + impl_.setIndices (indices_ptr); + + // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) + if (!cloud->points.empty ()) + impl_.segment (inliers, model); + + // Probably need to transform the model of the plane here + + // Check if we have enough inliers, clear inliers + model if not + if ((int)inliers.indices.size () <= min_inliers_) + { + inliers.indices.clear (); + model.values.clear (); + } + + // Publish + pub_indices_.publish (boost::make_shared (inliers)); + pub_model_.publish (boost::make_shared (model)); + NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", + getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (), + model.values.size (), pnh_->resolveName ("model").c_str ()); + + if (inliers.indices.empty ()) + NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2); + srv_->setCallback (f); + + // Subscribe to the input and normals using filters + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + + // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) + sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this); + + if (approximate_sync_) + sync_input_normals_indices_a_ = boost::make_shared > > (max_queue_size_); + else + sync_input_normals_indices_e_ = boost::make_shared > > (max_queue_size_); + + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + + if (approximate_sync_) + sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); + else + sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); + } + else + { + // Create a different callback for copying over the timestamp to fake indices + sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1)); + + if (approximate_sync_) + sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); + else + sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); + } + + if (approximate_sync_) + sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); + else + sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); + + // Advertise the output topics + pub_indices_ = pnh_->advertise ("inliers", max_queue_size_); + pub_model_ = pnh_->advertise ("model", max_queue_size_); + + // ---[ Mandatory parameters + int model_type; + if (!pnh_->getParam ("model_type", model_type)) + { + NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ()); + return; + } + double threshold; // unused - set via dynamic reconfigure in the callback + if (!pnh_->getParam ("distance_threshold", threshold)) + { + NODELET_ERROR ("[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", getName ().c_str ()); + return; + } + + // ---[ Optional parameters + int method_type = 0; + pnh_->getParam ("method_type", method_type); + + XmlRpc::XmlRpcValue axis_param; + pnh_->getParam ("axis", axis_param); + Eigen::Vector3f axis = Eigen::Vector3f::Zero (); + + switch (axis_param.getType ()) + { + case XmlRpc::XmlRpcValue::TypeArray: + { + if (axis_param.size () != 3) + { + NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ()); + return; + } + for (int i = 0; i < 3; ++i) + { + if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble) + { + NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ()); + return; + } + double value = axis_param[i]; axis[i] = value; + } + break; + } + default: + { + break; + } + } + + // Initialize the random number generator + srand (time (0)); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - method_type : %d\n" + " - model_threshold : %f\n" + " - axis : [%f, %f, %f]\n", + getName ().c_str (), model_type, method_type, threshold, + axis[0], axis[1], axis[2]); + + // Set given parameters here + impl_.setModelType (model_type); + impl_.setMethodType (method_type); + impl_.setAxis (axis); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl::ModelCoefficientsConstPtr &model) +{ + boost::mutex::scoped_lock lock (mutex_); + + if (model->values.size () < 3) + { + NODELET_ERROR ("[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", getName ().c_str (), model->values.size (), pnh_->resolveName ("axis").c_str ()); + return; + } + NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]); + + Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]); + impl_.setAxis (axis); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock lock (mutex_); + + if (impl_.getDistanceThreshold () != config.distance_threshold) + { + impl_.setDistanceThreshold (config.distance_threshold); + NODELET_DEBUG ("[%s::config_callback] Setting distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold); + } + // The maximum allowed difference between the model normal and the given axis _in radians_ + if (impl_.getEpsAngle () != config.eps_angle) + { + impl_.setEpsAngle (config.eps_angle); + NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI); + } + + if (impl_.getMaxIterations () != config.max_iterations) + { + impl_.setMaxIterations (config.max_iterations); + NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations); + } + + // Number of inliers + if (min_inliers_ != config.min_inliers) + { + min_inliers_ = config.min_inliers; + NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_); + } + + + if (impl_.getProbability () != config.probability) + { + impl_.setProbability (config.probability); + NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability); + } + + if (impl_.getOptimizeCoefficients () != config.optimize_coefficients) + { + impl_.setOptimizeCoefficients (config.optimize_coefficients); + NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false"); + } + + if (impl_.getNormalDistanceWeight () != config.normal_distance_weight) + { + impl_.setNormalDistanceWeight (config.normal_distance_weight); + NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight); + } + + double radius_min, radius_max; + impl_.getRadiusLimits (radius_min, radius_max); + if (radius_min != config.radius_min) + { + radius_min = config.radius_min; + NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min); + impl_.setRadiusLimits (radius_min, radius_max); + } + if (radius_max != config.radius_max) + { + radius_max = config.radius_max; + NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max); + impl_.setRadiusLimits (radius_min, radius_max); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( + const PointCloudConstPtr &cloud, + const PointCloudNConstPtr &cloud_normals, + const PointIndicesConstPtr &indices + ) +{ + boost::mutex::scoped_lock lock (mutex_); + + // No subscribers, no work + if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0) + return; + + PointIndices inliers; + ModelCoefficients model; + // Enforce that the TF frame and the timestamp are copied + inliers.header = model.header = cloud->header; + + if (impl_.getModelType () < 0) + { + NODELET_ERROR ("[%s::input_normals_indices_callback] Model type not set!", getName ().c_str ()); + pub_indices_.publish (boost::make_shared (inliers)); + pub_model_.publish (boost::make_shared (model)); + return; + } + + if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) + { + NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid input!", getName ().c_str ()); + pub_indices_.publish (boost::make_shared (inliers)); + pub_model_.publish (boost::make_shared (model)); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid (indices)) + { + NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid indices!", getName ().c_str ()); + pub_indices_.publish (boost::make_shared (inliers)); + pub_model_.publish (boost::make_shared (model)); + return; + } + + /// DEBUG + if (indices && !indices->header.frame_id.empty ()) + NODELET_DEBUG ("[%s::input_normals_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_normals_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + /// + + + // Extra checks for safety + int cloud_nr_points = cloud->width * cloud->height; + int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; + if (cloud_nr_points != cloud_normals_nr_points) + { + NODELET_ERROR ("[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", getName ().c_str (), cloud_nr_points, cloud_normals_nr_points); + pub_indices_.publish (boost::make_shared (inliers)); + pub_model_.publish (boost::make_shared (model)); + return; + } + + impl_.setInputCloud (cloud); + impl_.setInputNormals (cloud_normals); + + IndicesPtr indices_ptr; + if (indices && !indices->header.frame_id.empty ()) + indices_ptr.reset (new std::vector (indices->indices)); + + impl_.setIndices (indices_ptr); + + // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) + if (!cloud->points.empty ()) + impl_.segment (inliers, model); + + // Check if we have enough inliers, clear inliers + model if not + if ((int)inliers.indices.size () <= min_inliers_) + { + inliers.indices.clear (); + model.values.clear (); + } + + // Publish + pub_indices_.publish (boost::make_shared (inliers)); + pub_model_.publish (boost::make_shared (model)); + NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", + getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (), + model.values.size (), pnh_->resolveName ("model").c_str ()); + if (inliers.indices.empty ()) + NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ()); +} + +typedef pcl_ros::SACSegmentation SACSegmentation; +typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals; +PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentation, SACSegmentation, nodelet::Nodelet); +PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentationFromNormals, SACSegmentationFromNormals, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp new file mode 100644 index 00000000..f8fbae11 --- /dev/null +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -0,0 +1,128 @@ +/* + * 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: segment_differences.cpp 35361 2011-01-20 04:34:49Z rusu $ + * + */ + +#include +#include "pcl_ros/segmentation/segment_differences.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SegmentDifferences::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + pub_output_ = pnh_->advertise ("output", max_queue_size_); + + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2); + srv_->setCallback (f); + + if (approximate_sync_) + { + sync_input_target_a_ = boost::make_shared > > (max_queue_size_); + sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_); + sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2)); + } + else + { + sync_input_target_e_ = boost::make_shared > > (max_queue_size_); + sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_); + sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2)); + } + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - max_queue_size : %d", + getName ().c_str (), + max_queue_size_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level) +{ + if (impl_.getDistanceThreshold () != config.distance_threshold) + { + impl_.setDistanceThreshold (config.distance_threshold); + NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud, + const PointCloudConstPtr &cloud_target) +{ + if (pub_output_.getNumSubscribers () <= 0) + return; + + if (!isValid (cloud) || !isValid (cloud_target, "target")) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + PointCloud output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); + return; + } + + NODELET_DEBUG ("[%s::input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), cloud_target->header.stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ()); + + impl_.setInputCloud (cloud); + impl_.setTargetCloud (cloud_target); + + PointCloud output; + impl_.segment (output); + + pub_output_.publish (output.makeShared ()); + NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (), + output.points.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); +} + +typedef pcl_ros::SegmentDifferences SegmentDifferences; +PLUGINLIB_DECLARE_CLASS (pcl, SegmentDifferences, SegmentDifferences, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp new file mode 100644 index 00000000..8c261926 --- /dev/null +++ b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp @@ -0,0 +1,44 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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: segmentation.cpp 34600 2010-12-07 21:12:11Z rusu $ + * + */ + +// Include the implementations here instead of compiling them separately to speed up compile time +//#include "extract_clusters.cpp" +//#include "extract_polygonal_prism_data.cpp" +//#include "sac_segmentation.cpp" +//#include "segment_differences.cpp" + + diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp new file mode 100644 index 00000000..94dc5336 --- /dev/null +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -0,0 +1,182 @@ +/* + * 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: convex_hull.hpp 32993 2010-09-30 23:08:57Z rusu $ + * + */ + +#include +#include +#include "pcl_ros/surface/convex_hull.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ConvexHull2D::onInit () +{ + ros::NodeHandle pnh = getMTPrivateNodeHandle (); + pub_output_ = pnh.advertise ("output", max_queue_size_); + pub_plane_ = pnh.advertise("output_polygon", max_queue_size_); + + // ---[ Optional parameters + pnh.getParam ("use_indices", use_indices_); + + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_input_filter_.subscribe (pnh, "input", 1); + // If indices are enabled, subscribe to the indices + sub_indices_filter_.subscribe (pnh, "indices", 1); + + if (approximate_sync_) + { + sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); + // surface not enabled, connect the input-indices duo and register + sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2)); + } + else + { + sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); + // surface not enabled, connect the input-indices duo and register + sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2)); + } + } + else + // Subscribe in an old fashion to input only (no filters) + sub_input_ = pnh.subscribe ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ())); + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_indices : %s", + getName ().c_str (), + (use_indices_) ? "true" : "false"); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void + pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud, + const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0) + return; + + PointCloud output; + + // If cloud is given, check if it's valid + if (!isValid (cloud)) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + // Publish an empty message + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid (indices, "indices")) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + // Publish an empty message + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); + return; + } + + /// DEBUG + if (indices) + NODELET_DEBUG ("[%s::input_indices_model_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + + // Reset the indices and surface pointers + IndicesPtr indices_ptr; + if (indices) + indices_ptr.reset (new std::vector (indices->indices)); + + impl_.setInputCloud (cloud); + impl_.setIndices (indices_ptr); + + // Estimate the feature + impl_.reconstruct (output); + + // If more than 3 points are present, send a PolygonStamped hull too + if (output.points.size () >= 3) + { + geometry_msgs::PolygonStamped poly; + poly.header = output.header; + poly.polygon.points.resize (output.points.size ()); + // Get three consecutive points (without copying) + pcl::Vector4fMap O = output.points[1].getVector4fMap (); + pcl::Vector4fMap B = output.points[0].getVector4fMap (); + pcl::Vector4fMap A = output.points[2].getVector4fMap (); + // Check the direction of points -- polygon must have CCW direction + Eigen::Vector4f OA = A - O; + Eigen::Vector4f OB = B - O; + Eigen::Vector4f N = OA.cross3 (OB); + double theta = N.dot (O); + bool reversed = false; + if (theta < (M_PI / 2.0)) + reversed = true; + for (size_t i = 0; i < output.points.size (); ++i) + { + if (reversed) + { + size_t j = output.points.size () - i - 1; + poly.polygon.points[i].x = output.points[j].x; + poly.polygon.points[i].y = output.points[j].y; + poly.polygon.points[i].z = output.points[j].z; + } + else + { + poly.polygon.points[i].x = output.points[i].x; + poly.polygon.points[i].y = output.points[i].y; + poly.polygon.points[i].z = output.points[i].z; + } + } + pub_plane_.publish (boost::make_shared (poly)); + } + // Publish a Boost shared ptr const data + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::ConvexHull2D ConvexHull2D; +PLUGINLIB_DECLARE_CLASS (pcl, ConvexHull2D, ConvexHull2D, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp new file mode 100644 index 00000000..c79c0195 --- /dev/null +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -0,0 +1,217 @@ +/* + * 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: moving_least_squares.cpp 36097 2011-02-20 14:18:58Z marton $ + * + */ + +#include +#include "pcl_ros/surface/moving_least_squares.h" +#include +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::MovingLeastSquares::onInit () +{ + PCLNodelet::onInit (); + + //ros::NodeHandle private_nh = getMTPrivateNodeHandle (); + pub_output_ = pnh_->advertise ("output", max_queue_size_); + pub_normals_ = pnh_->advertise ("normals", max_queue_size_); + + //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) + if (!pnh_->getParam ("search_radius", search_radius_)) + { + //NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ()); + return; + } + if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) + { + NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + return; + } + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 ); + srv_->setCallback (f); + + // ---[ Optional parameters + pnh_->getParam ("use_indices", use_indices_); + + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", 1); + // If indices are enabled, subscribe to the indices + sub_indices_filter_.subscribe (*pnh_, "indices", 1); + + if (approximate_sync_) + { + sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); + // surface not enabled, connect the input-indices duo and register + sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2)); + } + else + { + sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); + // surface not enabled, connect the input-indices duo and register + sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2)); + } + } + else + // Subscribe in an old fashion to input only (no filters) + sub_input_ = pnh_->subscribe ("input", 1, bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ())); + + + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_indices : %s", + getName ().c_str (), + (use_indices_) ? "true" : "false"); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud, + const PointIndicesConstPtr &indices) +{ + // No subscribers, no work + if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0) + return; + + // Output points have the same type as the input, they are only smoothed + PointCloudIn output; + + // Normals are also estimated and published on a separate topic + NormalCloudOut::Ptr normals (new NormalCloudOut ()); + + // If cloud is given, check if it's valid + if (!isValid (cloud)) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid (indices, "indices")) + { + NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); + return; + } + + /// DEBUG + if (indices) + NODELET_DEBUG ("[%s::input_indices_model_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName ().c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), + indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); + else + NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + /// + + // Reset the indices and surface pointers + impl_.setInputCloud (cloud); + impl_.setOutputNormals (normals); + + IndicesPtr indices_ptr; + if (indices) + indices_ptr.reset (new std::vector (indices->indices)); + + impl_.setIndices (indices_ptr); + + // Initialize the spatial locator + initTree (spatial_locator_type_, tree_, 0); //k_); + impl_.setSearchMethod (tree_); + + // Do the reconstructon + impl_.reconstruct (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); + normals->header = cloud->header; + pub_normals_.publish (normals); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level) +{ + // \Note Zoli, shouldn't this be implemented in MLS too? + /*if (k_ != config.k_search) + { + k_ = config.k_search; + NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_); + }*/ + if (search_radius_ != config.search_radius) + { + search_radius_ = config.search_radius; + NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_); + impl_.setSearchRadius (search_radius_); + } + if (spatial_locator_type_ != config.spatial_locator) + { + spatial_locator_type_ = config.spatial_locator; + NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_); + } + if (use_polynomial_fit_ != config.use_polynomial_fit) + { + use_polynomial_fit_ = config.use_polynomial_fit; + NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_); + impl_.setPolynomialFit (use_polynomial_fit_); + } + if (polynomial_order_ != config.polynomial_order) + { + polynomial_order_ = config.polynomial_order; + NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_); + impl_.setPolynomialOrder (polynomial_order_); + } + if (gaussian_parameter_ != config.gaussian_parameter) + { + gaussian_parameter_ = config.gaussian_parameter; + NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_); + impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_); + } +} + +typedef pcl_ros::MovingLeastSquares MovingLeastSquares; +PLUGINLIB_DECLARE_CLASS (pcl, MovingLeastSquares, MovingLeastSquares, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/surface/surface.cpp b/pcl_ros/src/pcl_ros/surface/surface.cpp new file mode 100644 index 00000000..13976f90 --- /dev/null +++ b/pcl_ros/src/pcl_ros/surface/surface.cpp @@ -0,0 +1,48 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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 OWNERff 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: surface.cpp 34603 2010-12-07 22:42:10Z rusu $ + * + */ + +/** + +\author Radu Bogdan Rusu + +**/ + +// Include the implementations here instead of compiling them separately to speed up compile time +//#include "convex_hull.cpp" +//#include "moving_least_squares.cpp" + + From b2f9c6e898187f44b9495d7dd08839afae04a80c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 15 May 2013 12:57:28 +0900 Subject: [PATCH 129/405] fix to compileable --- pcl_ros/CMakeLists.txt | 22 +++++++++---------- .../pcl_ros/features/principal_curvatures.h | 1 + pcl_ros/src/pcl_ros/features/boundary.cpp | 3 --- pcl_ros/src/pcl_ros/features/fpfh.cpp | 3 --- pcl_ros/src/pcl_ros/features/fpfh_omp.cpp | 3 --- .../pcl_ros/features/moment_invariants.cpp | 3 --- pcl_ros/src/pcl_ros/features/normal_3d.cpp | 3 --- .../src/pcl_ros/features/normal_3d_omp.cpp | 3 --- pcl_ros/src/pcl_ros/features/pfh.cpp | 3 --- .../pcl_ros/features/principal_curvatures.cpp | 3 --- pcl_ros/src/pcl_ros/features/vfh.cpp | 3 --- .../pcl_ros/segmentation/extract_clusters.cpp | 6 ++--- .../pcl_ros/surface/moving_least_squares.cpp | 5 +---- 13 files changed, 15 insertions(+), 46 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 24f506ae..57c8ba61 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -64,15 +64,15 @@ class_loader_hide_library_symbols(pcl_ros_io) add_library (pcl_ros_features src/pcl_ros/features/feature.cpp # Compilation is much faster if we include all the following CPP files in feature.cpp -# src/pcl_ros/features/boundary.cpp -# src/pcl_ros/features/fpfh.cpp -# src/pcl_ros/features/fpfh_omp.cpp -# src/pcl_ros/features/moment_invariants.cpp -# src/pcl_ros/features/normal_3d.cpp -# src/pcl_ros/features/normal_3d_omp.cpp -# src/pcl_ros/features/pfh.cpp -# src/pcl_ros/features/principal_curvatures.cpp -# src/pcl_ros/features/vfh.cpp + src/pcl_ros/features/boundary.cpp + src/pcl_ros/features/fpfh.cpp + src/pcl_ros/features/fpfh_omp.cpp + src/pcl_ros/features/moment_invariants.cpp + src/pcl_ros/features/normal_3d.cpp + src/pcl_ros/features/normal_3d_omp.cpp + src/pcl_ros/features/pfh.cpp + src/pcl_ros/features/principal_curvatures.cpp + src/pcl_ros/features/vfh.cpp ) #rosbuild_add_compile_flags (pcl_ros_features ${SSE_FLAGS}) target_link_libraries (pcl_ros_features ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) @@ -99,7 +99,7 @@ class_loader_hide_library_symbols(pcl_ros_filters) add_library (pcl_ros_segmentation src/pcl_ros/segmentation/segmentation.cpp src/pcl_ros/segmentation/segment_differences.cpp -# src/pcl_ros/segmentation/extract_clusters.cpp + src/pcl_ros/segmentation/extract_clusters.cpp src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp src/pcl_ros/segmentation/sac_segmentation.cpp ) @@ -113,7 +113,7 @@ add_library (pcl_ros_surface src/pcl_ros/surface/surface.cpp # Compilation is much faster if we include all the following CPP files in surface.cpp src/pcl_ros/surface/convex_hull.cpp - #src/pcl_ros/surface/moving_least_squares.cpp + src/pcl_ros/surface/moving_least_squares.cpp ) #rosbuild_add_compile_flags (pcl_ros_surface ${SSE_FLAGS}) target_link_libraries (pcl_ros_surface ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.h b/pcl_ros/include/pcl_ros/features/principal_curvatures.h index 7a0ddfb6..d7dcdf49 100644 --- a/pcl_ros/include/pcl_ros/features/principal_curvatures.h +++ b/pcl_ros/include/pcl_ros/features/principal_curvatures.h @@ -38,6 +38,7 @@ #ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ #define PCL_ROS_PRINCIPAL_CURVATURES_H_ +#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true #include #include "pcl_ros/features/feature.h" diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp index 0e17c5fd..5fa970f6 100644 --- a/pcl_ros/src/pcl_ros/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -55,9 +55,6 @@ pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/features/fpfh.cpp b/pcl_ros/src/pcl_ros/features/fpfh.cpp index 7782fde9..081696b1 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh.cpp @@ -55,9 +55,6 @@ pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp index 51880a9f..09a573f6 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp @@ -55,9 +55,6 @@ pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp index 85bd209a..49838204 100644 --- a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp @@ -54,9 +54,6 @@ pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/features/normal_3d.cpp index a512c41f..31b4dc70 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d.cpp @@ -54,9 +54,6 @@ pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp index e53fe9c7..06c1b31a 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp @@ -54,9 +54,6 @@ pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/features/pfh.cpp b/pcl_ros/src/pcl_ros/features/pfh.cpp index f40e0bab..94b97c88 100644 --- a/pcl_ros/src/pcl_ros/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/features/pfh.cpp @@ -55,9 +55,6 @@ pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp index d6431160..471549ab 100644 --- a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp @@ -55,9 +55,6 @@ pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstP // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/features/vfh.cpp b/pcl_ros/src/pcl_ros/features/vfh.cpp index f13028e3..86ddc137 100644 --- a/pcl_ros/src/pcl_ros/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/features/vfh.cpp @@ -55,9 +55,6 @@ pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, // Set the parameters impl_.setKSearch (k_); impl_.setRadiusSearch (search_radius_); - // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); // Set the inputs impl_.setInputCloud (cloud); diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index 3c067f0a..0bff57bf 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -100,14 +100,12 @@ pcl_ros::EuclideanClusterExtraction::onInit () NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" " - max_queue_size : %d\n" " - use_indices : %s\n" - " - cluster_tolerance : %f\n" - " - spatial_locator : %d", + " - cluster_tolerance : %f\n", getName ().c_str (), max_queue_size_, - (use_indices_) ? "true" : "false", cluster_tolerance, spatial_locator); + (use_indices_) ? "true" : "false", cluster_tolerance); // Set given parameters here - impl_.setSpatialLocator (spatial_locator); impl_.setClusterTolerance (cluster_tolerance); } diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index c79c0195..2cb41cff 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -149,7 +149,6 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr // Reset the indices and surface pointers impl_.setInputCloud (cloud); - impl_.setOutputNormals (normals); IndicesPtr indices_ptr; if (indices) @@ -158,11 +157,9 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr impl_.setIndices (indices_ptr); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, 0); //k_); - impl_.setSearchMethod (tree_); // Do the reconstructon - impl_.reconstruct (output); + //impl_.process (output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied From a289400192a05cdba27ed996effac8d6029eef9d Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Fri, 17 May 2013 17:05:12 -0700 Subject: [PATCH 130/405] 1.0.31 -> 1.0.32 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 4844c7d4..a4ec2954 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.31 + 1.0.32

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index e59b0efd..b21b7116 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.31 + 1.0.32

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 698134ab74a97cf441f8e258efb7a5b0eebf58c2 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 20 May 2013 10:14:17 -0600 Subject: [PATCH 131/405] Fixing catkin python import --- pcl_ros/cfg/Feature.cfg | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/pcl_ros/cfg/Feature.cfg b/pcl_ros/cfg/Feature.cfg index 21fe0b03..aaf9ee9e 100755 --- a/pcl_ros/cfg/Feature.cfg +++ b/pcl_ros/cfg/Feature.cfg @@ -1,12 +1,10 @@ #! /usr/bin/env python -# set up parameters that we care about -PACKAGE = 'pcl_ros' +PACKAGE='pcl_ros' -import roslib; -roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; -gen = ParameterGenerator () +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() #enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator") # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): From 6038a445d829ab91f9fae0a247cefdf95cdd9e9e Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 20 May 2013 10:14:54 -0600 Subject: [PATCH 132/405] Fixing catkin python import --- pcl_ros/cfg/EuclideanClusterExtraction.cfg | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/pcl_ros/cfg/EuclideanClusterExtraction.cfg b/pcl_ros/cfg/EuclideanClusterExtraction.cfg index 50ae3729..e9607f4e 100755 --- a/pcl_ros/cfg/EuclideanClusterExtraction.cfg +++ b/pcl_ros/cfg/EuclideanClusterExtraction.cfg @@ -1,12 +1,10 @@ #! /usr/bin/env python -# set up parameters that we care about -PACKAGE = 'pcl_ros' +PACKAGE='pcl_ros' -import roslib; -roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; -gen = ParameterGenerator () +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() # enabling/disabling the unit limits # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): From 0f6ee511da3741cc2ac2fc24ecfb65e2a455fbd0 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 20 May 2013 10:15:46 -0600 Subject: [PATCH 133/405] Fixing catkin python import --- pcl_ros/cfg/ExtractPolygonalPrismData.cfg | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/pcl_ros/cfg/ExtractPolygonalPrismData.cfg b/pcl_ros/cfg/ExtractPolygonalPrismData.cfg index 109770a9..0705aeaa 100755 --- a/pcl_ros/cfg/ExtractPolygonalPrismData.cfg +++ b/pcl_ros/cfg/ExtractPolygonalPrismData.cfg @@ -1,13 +1,11 @@ #! /usr/bin/env python -# set up parameters that we care about -PACKAGE = 'pcl_ros' +PACKAGE='pcl_ros' -import roslib; roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; -import roslib.packages +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() -gen = ParameterGenerator () # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): gen.add ("height_min", double_t, 0, "The minimum allowed distance to the plane model value a point will be considered from", 0.0, -10.0, 10.0) gen.add ("height_max", double_t, 0, "The maximum allowed distance to the plane model value a point will be considered from", 0.5, -10.0, 10.0) From 3dee819ea941f40aae57d0c3bdfe35b6786b411c Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 20 May 2013 10:16:42 -0600 Subject: [PATCH 134/405] Fixing catkin python import --- pcl_ros/cfg/MLS.cfg | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/pcl_ros/cfg/MLS.cfg b/pcl_ros/cfg/MLS.cfg index db6c2bdf..7f9ff8f6 100755 --- a/pcl_ros/cfg/MLS.cfg +++ b/pcl_ros/cfg/MLS.cfg @@ -1,12 +1,10 @@ #! /usr/bin/env python -# set up parameters that we care about -PACKAGE = 'pcl_ros' +PACKAGE='pcl_ros' -import roslib; -roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; -gen = ParameterGenerator () +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator") # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): From 9244556f9bcba3c7e9d22d943fa6377e76cf6171 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 20 May 2013 10:17:37 -0600 Subject: [PATCH 135/405] Fixing catkin python import --- pcl_ros/cfg/SACSegmentation.cfg | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/pcl_ros/cfg/SACSegmentation.cfg b/pcl_ros/cfg/SACSegmentation.cfg index 8068e5c6..013e8ccf 100755 --- a/pcl_ros/cfg/SACSegmentation.cfg +++ b/pcl_ros/cfg/SACSegmentation.cfg @@ -1,10 +1,9 @@ #! /usr/bin/env python -# set up parameters that we care about -PACKAGE = 'pcl_ros' +PACKAGE='pcl_ros' + +from dynamic_reconfigure.parameter_generator_catkin import * -import roslib; roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; import SACSegmentation_common as common gen = ParameterGenerator () From e6122e2b873205a17881f00dae2e97da94ff8e19 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 20 May 2013 09:18:31 -0700 Subject: [PATCH 136/405] Fixing catkin python import --- pcl_ros/cfg/SACSegmentationFromNormals.cfg | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/pcl_ros/cfg/SACSegmentationFromNormals.cfg b/pcl_ros/cfg/SACSegmentationFromNormals.cfg index d9c3a854..e54cbf0c 100755 --- a/pcl_ros/cfg/SACSegmentationFromNormals.cfg +++ b/pcl_ros/cfg/SACSegmentationFromNormals.cfg @@ -3,12 +3,14 @@ # set up parameters that we care about PACKAGE = 'pcl_ros' -import roslib; roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; +PACKAGE='pcl_ros' + +from dynamic_reconfigure.parameter_generator_catkin import * import roslib.packages import SACSegmentation_common as common -gen = ParameterGenerator () +gen = ParameterGenerator() + # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): gen.add ("normal_distance_weight", double_t, 0, "The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point normals and the plane normal.", 0.1, 0, 1.0) common.add_common_parameters (gen) From 3327eb6a20855e790b8ba323926a647138dd423a Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 20 May 2013 09:22:00 -0700 Subject: [PATCH 137/405] Fixing catkin python imports --- pcl_ros/cfg/SegmentDifferences.cfg | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/pcl_ros/cfg/SegmentDifferences.cfg b/pcl_ros/cfg/SegmentDifferences.cfg index 0cc5172d..1a17a7c8 100755 --- a/pcl_ros/cfg/SegmentDifferences.cfg +++ b/pcl_ros/cfg/SegmentDifferences.cfg @@ -1,12 +1,12 @@ #! /usr/bin/env python # set up parameters that we care about -PACKAGE = 'pcl_ros' +PACKAGE='pcl_ros' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() -import roslib; -roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; -gen = ParameterGenerator () # enabling/disabling the unit limits # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): From 6e4f23f43c92497e904eeda3cbd45b06bbcb085f Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 20 May 2013 09:23:04 -0700 Subject: [PATCH 138/405] 1.0.32 -> 1.0.33 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index a4ec2954..aefca08d 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.32 + 1.0.33

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index b21b7116..e0b8b278 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.32 + 1.0.33

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 6f256968eb280c6fc8d371f0e4ce2c5d476cdc83 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 21 May 2013 14:01:53 -0700 Subject: [PATCH 139/405] fixing catkin python imports --- pcl_ros/cfg/SACSegmentation_common.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/pcl_ros/cfg/SACSegmentation_common.py b/pcl_ros/cfg/SACSegmentation_common.py index 8b2c6668..d0938904 100644 --- a/pcl_ros/cfg/SACSegmentation_common.py +++ b/pcl_ros/cfg/SACSegmentation_common.py @@ -3,8 +3,7 @@ # set up parameters that we care about PACKAGE = 'pcl_ros' -import roslib; roslib.load_manifest (PACKAGE); -from dynamic_reconfigure.parameter_generator import *; +from dynamic_reconfigure.parameter_generator_catkin import *; def add_common_parameters (gen): # add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = "") From fdb4fa9bfe8d7d302e40657054f4a121eed36e45 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 21 May 2013 14:02:31 -0700 Subject: [PATCH 140/405] 1.0.33 -> 1.0.34 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index aefca08d..52e4725c 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.33 + 1.0.34

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index e0b8b278..5aeaac6b 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.33 + 1.0.34

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 8c6b34f6b7a3afd4b4c326be1811ea63d3d9d735 Mon Sep 17 00:00:00 2001 From: Davide Date: Wed, 22 May 2013 01:21:31 +0300 Subject: [PATCH 141/405] removed a whitespace --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 57c8ba61..019ef73c 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -128,7 +128,7 @@ target_link_libraries (pcd_to_pointcloud pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARI add_executable (pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) target_link_libraries (pointcloud_to_pcd pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) -add_executable ( bag_to_pcd tools/bag_to_pcd.cpp) +add_executable (bag_to_pcd tools/bag_to_pcd.cpp) target_link_libraries (bag_to_pcd pcl_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) add_executable (convert_pcd_to_image tools/convert_pcd_to_image.cpp) From d0a5fd4322b77273eeea05f832b28d76c6085b22 Mon Sep 17 00:00:00 2001 From: Davide Date: Wed, 22 May 2013 00:25:12 +0200 Subject: [PATCH 142/405] Changed some #include types to lib --- pcl_ros/tools/bag_to_pcd.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index 0a753d3a..eefb185f 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -48,8 +48,8 @@ Cloud Data) format. #include #include #include -#include "pcl/io/io.h" -#include "pcl/io/pcd_io.h" +#include +#include #include "pcl_ros/transforms.h" #include #include From 57337646218e96995337deeef965d1b842023457 Mon Sep 17 00:00:00 2001 From: Davide Date: Wed, 22 May 2013 02:31:02 +0200 Subject: [PATCH 143/405] Changed #include type to lib --- pcl_ros/include/pcl_ros/publisher.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/include/pcl_ros/publisher.h b/pcl_ros/include/pcl_ros/publisher.h index 7ebc0984..1bba8e2c 100644 --- a/pcl_ros/include/pcl_ros/publisher.h +++ b/pcl_ros/include/pcl_ros/publisher.h @@ -46,7 +46,7 @@ #define PCL_ROS_PUBLISHER_H_ #include -#include "pcl/ros/conversions.h" +#include namespace pcl_ros { From fa2baa52480b2ad7c388c38eeea4a09272e8c4a2 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Thu, 23 May 2013 11:29:48 -0700 Subject: [PATCH 144/405] adding .travis.yml --- .travis.yml | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 00000000..eab705fb --- /dev/null +++ b/.travis.yml @@ -0,0 +1,25 @@ +language: + - cpp + - python +python: + - "2.7" +compiler: + - gcc + +install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update -qq + - sudo apt-get install python-catkin-pkg python-rosdep -qq + - sudo apt-get install ros-groovy-catkin -qq + - sudo rosdep init + - rosdep update + - mkdir -p /tmp/ws/src + - ln -s `pwd` /tmp/ws/src/perception_pcl + - cd /tmp/ws + - rosdep install --from-paths src --ignore-src --rosdistro groovy -y + +script: + - source /opt/ros/groovy/setup.bash + - catkin_make + - catkin_make install From 1a8f70b099d3c8024a3be22e46b378c9942b795a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Wed, 19 Jun 2013 17:38:31 +0200 Subject: [PATCH 145/405] bag_to_pcd: check return code of transformPointCloud() This fixes a bug where bag_to_pcd segfaults because of an ignored tf::ExtrapolationException. --- pcl_ros/tools/bag_to_pcd.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index eefb185f..f66d74b5 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -129,8 +129,13 @@ int ++view_it; continue; } + // Transform it - pcl_ros::transformPointCloud ("/base_link", *cloud, cloud_t, tf_listener); + if (!pcl_ros::transformPointCloud ("/base_link", *cloud, cloud_t, tf_listener)) + { + ++view_it; + continue; + } std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl; From 9d08dd5198c8df31bc779ae3ea91bf93918821d2 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 16:35:01 -0700 Subject: [PATCH 146/405] Depend on pcl_conversions and pcl_msgs --- pcl_ros/CMakeLists.txt | 2 +- pcl_ros/package.xml | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 019ef73c..3415887d 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -3,7 +3,7 @@ project(pcl_ros) # Deal with catkin find_package(Boost COMPONENTS system filesystem thread REQUIRED) -find_package(catkin REQUIRED dynamic_reconfigure genmsg nodelet nodelet_topic_tools roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib pluginlib) +find_package(catkin REQUIRED dynamic_reconfigure genmsg nodelet nodelet_topic_tools pcl_conversions pcl_msgs roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib pluginlib) find_package(Eigen) find_package(PCL) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 52e4725c..c87b9d0e 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -20,6 +20,8 @@ nodelet nodelet_topic_tools pcl + pcl_conversions + pcl_msgs pluginlib rosbag roscpp @@ -32,6 +34,8 @@ nodelet nodelet_topic_tools pcl + pcl_conversions + pcl_msgs pluginlib rosbag roscpp From a931106c5bbc97379ac5b7a4dcdd966ed320eb80 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 16:36:28 -0700 Subject: [PATCH 147/405] Fixes from converting from pcl-1.7, incomplete --- pcl_ros/include/pcl_ros/features/feature.h | 5 ++- pcl_ros/include/pcl_ros/impl/transforms.hpp | 20 ++++++++---- pcl_ros/include/pcl_ros/pcl_nodelet.h | 9 ++++-- pcl_ros/include/pcl_ros/publisher.h | 3 +- pcl_ros/src/pcl_ros/features/feature.cpp | 32 +++++++++---------- pcl_ros/src/transforms.cpp | 1 + pcl_ros/tools/bag_to_pcd.cpp | 1 + pcl_ros/tools/convert_pcd_to_image.cpp | 4 ++- pcl_ros/tools/convert_pointcloud_to_image.cpp | 5 ++- pcl_ros/tools/pcd_to_pointcloud.cpp | 1 + perception_pcl/package.xml | 2 ++ 11 files changed, 54 insertions(+), 29 deletions(-) diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.h index 7ef9d644..3d5f9604 100644 --- a/pcl_ros/include/pcl_ros/features/feature.h +++ b/pcl_ros/include/pcl_ros/features/feature.h @@ -49,6 +49,9 @@ #include #include "pcl_ros/FeatureConfig.h" +// PCL conversions +#include + namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; @@ -147,7 +150,7 @@ namespace pcl_ros input_callback (const PointCloudInConstPtr &input) { PointIndices indices; - indices.header.stamp = input->header.stamp; + indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp; PointCloudIn cloud; cloud.header.stamp = input->header.stamp; nf_pc_.add (cloud.makeShared ()); diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index 26fcbc3b..52b49e25 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -37,8 +37,12 @@ #ifndef pcl_ros_IMPL_TRANSFORMS_H_ #define pcl_ros_IMPL_TRANSFORMS_H_ +#include #include "pcl_ros/transforms.h" +using pcl_conversions::fromPCL; +using pcl_conversions::toPCL; + namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////// @@ -99,7 +103,7 @@ transformPointCloudWithNormals (const std::string &target_frame, tf::StampedTransform transform; try { - tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, cloud_in.header.stamp, transform); + tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform); } catch (tf::LookupException &e) { @@ -133,7 +137,7 @@ transformPointCloud (const std::string &target_frame, tf::StampedTransform transform; try { - tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, cloud_in.header.stamp, transform); + tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform); } catch (tf::LookupException &e) { @@ -162,7 +166,7 @@ transformPointCloud (const std::string &target_frame, tf::StampedTransform transform; try { - tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, cloud_in.header.stamp, fixed_frame, transform); + tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform); } catch (tf::LookupException &e) { @@ -177,7 +181,9 @@ transformPointCloud (const std::string &target_frame, transformPointCloud (cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; - cloud_out.header.stamp = target_time; + std_msgs::Header header; + header.stamp = target_time; + cloud_out.header = toPCL(header); return (true); } @@ -193,7 +199,7 @@ transformPointCloudWithNormals (const std::string &target_frame, tf::StampedTransform transform; try { - tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, cloud_in.header.stamp, fixed_frame, transform); + tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform); } catch (tf::LookupException &e) { @@ -208,7 +214,9 @@ transformPointCloudWithNormals (const std::string &target_frame, transformPointCloudWithNormals (cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; - cloud_out.header.stamp = target_time; + std_msgs::Header header; + header.stamp = target_time; + cloud_out.header = toPCL(header); return (true); } diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.h b/pcl_ros/include/pcl_ros/pcl_nodelet.h index 9fef5ba7..dbb8b460 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.h +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.h @@ -46,9 +46,10 @@ #include // PCL includes -#include +#include #include #include +#include #include "pcl_ros/point_cloud.h" // ROS Nodelet includes #include @@ -60,6 +61,8 @@ // Include TF #include +using pcl_conversions::fromPCL; + namespace pcl_ros { //////////////////////////////////////////////////////////////////////////////////////////// @@ -75,7 +78,7 @@ namespace pcl_ros typedef PointCloud::Ptr PointCloudPtr; typedef PointCloud::ConstPtr PointCloudConstPtr; - typedef pcl::PointIndices PointIndices; + typedef pcl_msgs::PointIndices PointIndices; typedef PointIndices::Ptr PointIndicesPtr; typedef PointIndices::ConstPtr PointIndicesConstPtr; @@ -157,7 +160,7 @@ namespace pcl_ros { 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, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); + 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); } diff --git a/pcl_ros/include/pcl_ros/publisher.h b/pcl_ros/include/pcl_ros/publisher.h index 1bba8e2c..b019ba9b 100644 --- a/pcl_ros/include/pcl_ros/publisher.h +++ b/pcl_ros/include/pcl_ros/publisher.h @@ -46,7 +46,8 @@ #define PCL_ROS_PUBLISHER_H_ #include -#include +#include +#include namespace pcl_ros { diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp index 434707ee..0625279b 100644 --- a/pcl_ros/src/pcl_ros/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -204,25 +204,25 @@ pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cl " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); else NODELET_DEBUG ("[input_surface_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ()); else if (indices) NODELET_DEBUG ("[input_surface_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); else - NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); /// @@ -394,9 +394,9 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); else NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" @@ -404,9 +404,9 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); else if (indices) NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" @@ -414,16 +414,16 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); else NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); /// if ((int)(cloud->width * cloud->height) < k_) diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index d35e2366..75fc6c4e 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include "pcl_ros/transforms.h" #include "pcl_ros/impl/transforms.hpp" diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index f66d74b5..b9114cf9 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -50,6 +50,7 @@ Cloud Data) format. #include #include #include +#include #include "pcl_ros/transforms.h" #include #include diff --git a/pcl_ros/tools/convert_pcd_to_image.cpp b/pcl_ros/tools/convert_pcd_to_image.cpp index 2325f513..ff8e28af 100644 --- a/pcl_ros/tools/convert_pcd_to_image.cpp +++ b/pcl_ros/tools/convert_pcd_to_image.cpp @@ -48,9 +48,11 @@ #include #include +#include #include #include #include +#include /* ---[ */ int @@ -74,7 +76,7 @@ main (int argc, char **argv) { pcl::toROSMsg (cloud, image); //convert the cloud } - catch (std::runtime_error e) + catch (std::runtime_error &e) { ROS_ERROR_STREAM("Error in converting cloud to image message: " << e.what()); diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp index ebadd99d..0fdd5e87 100644 --- a/pcl_ros/tools/convert_pointcloud_to_image.cpp +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -42,8 +42,11 @@ #include //Image message #include +#include //pcl::toROSMsg #include +//conversions from PCL custom types +#include //stl stuff #include @@ -59,7 +62,7 @@ public: { pcl::toROSMsg (*cloud, image_); //convert the cloud } - catch (std::runtime_error e) + catch (std::runtime_error &e) { ROS_ERROR_STREAM("Error in converting cloud to image message: " << e.what()); diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index bbb3e769..655b4341 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include "pcl_ros/publisher.h" diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 5aeaac6b..9ac4e385 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -15,6 +15,8 @@ catkin + pcl_conversions + pcl_msgs pcl_ros From 746a8faf65272bf7437f3a54335158082bad25e6 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 16:36:46 -0700 Subject: [PATCH 148/405] Experimental changes to point_cloud.h --- pcl_ros/include/pcl_ros/point_cloud.h | 62 ++++++++++++++++++++++++++- 1 file changed, 60 insertions(+), 2 deletions(-) diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h index b0dfe433..03c925de 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.h +++ b/pcl_ros/include/pcl_ros/point_cloud.h @@ -5,7 +5,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -109,6 +110,61 @@ namespace ros }; template struct HasHeader > : TrueType {}; + + // This is bad because it will only work for pcl::PointXYZ, but I can't + // find another way around ambiguous partial template specialization... + template<> + struct TimeStamp > + { + // This specialization could be dangerous, but it's the best I can do. + // If this TimeStamp struct is destroyed before they are done with the + // pointer returned by the first functions may go out of scope, but there + // isn't a lot I can do about that. This is a good reason to refuse to + // returning pointers like this... + static ros::Time* pointer(pcl::PointCloud &m) { + header_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_)); + return &(header_->stamp); + } + static ros::Time const* pointer(const pcl::PointCloud& m) { + header_const_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_const_)); + return &(header_const_->stamp); + } + static ros::Time value(const pcl::PointCloud& m) { + return pcl_conversions::fromPCL(m.header).stamp; + } + private: + static boost::shared_ptr header_; + static boost::shared_ptr header_const_; + }; + + template<> + struct TimeStamp > + { + // This specialization could be dangerous, but it's the best I can do. + // If this TimeStamp struct is destroyed before they are done with the + // pointer returned by the first functions may go out of scope, but there + // isn't a lot I can do about that. This is a good reason to refuse to + // returning pointers like this... + static ros::Time* pointer(pcl::PointCloud &m) { + header_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_)); + return &(header_->stamp); + } + static ros::Time const* pointer(const pcl::PointCloud& m) { + header_const_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_const_)); + return &(header_const_->stamp); + } + static ros::Time value(const pcl::PointCloud& m) { + return pcl_conversions::fromPCL(m.header).stamp; + } + private: + static boost::shared_ptr header_; + static boost::shared_ptr header_const_; + }; + } // namespace ros::message_traits namespace serialization @@ -156,7 +212,9 @@ namespace ros template inline static void read(Stream& stream, pcl::PointCloud& m) { - stream.next(m.header); + std_msgs::Header header; + pcl_conversions::fromPCL(m.header, header); + stream.next(header); stream.next(m.height); stream.next(m.width); From 018bb008d4807ccab9ea3ea5871903dbc4645a9c Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 17:38:24 -0700 Subject: [PATCH 149/405] Ignore .pyc files --- .gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..0d20b648 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +*.pyc From 4e64cb25e7bae2e9173c6ce9b15e8116bdcb0c65 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Jul 2013 17:38:48 -0700 Subject: [PATCH 150/405] Use the PointIndices from pcl_msgs --- pcl_ros/include/pcl_ros/features/feature.h | 2 +- .../extract_polygonal_prism_data.h | 2 +- pcl_ros/src/pcl_ros/filters/filter.cpp | 6 ++-- .../extract_polygonal_prism_data.cpp | 29 ++++++++++++------- 4 files changed, 24 insertions(+), 15 deletions(-) diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.h index 3d5f9604..ca0dccc0 100644 --- a/pcl_ros/include/pcl_ros/features/feature.h +++ b/pcl_ros/include/pcl_ros/features/feature.h @@ -40,7 +40,7 @@ // PCL includes #include -#include +#include #include "pcl_ros/pcl_nodelet.h" #include diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h index 33050a41..8c24a889 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h @@ -93,7 +93,7 @@ namespace pcl_ros input_callback (const PointCloudConstPtr &input) { PointIndices cloud; - cloud.header.stamp = input->header.stamp; + cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp; nf_.add (boost::make_shared (cloud)); } diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 6257301a..30104d94 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -133,20 +133,20 @@ pcl_ros::Filter::onInit () if (approximate_sync_) { - sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); + sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); } else { - sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); + sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); } } else // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl::PointIndicesConstPtr ())); + sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ())); NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ()); } diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 23ab6d12..7c54c414 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -40,6 +40,11 @@ #include "pcl_ros/segmentation/extract_polygonal_prism_data.h" #include +#include + +using pcl_conversions::fromPCL; +using pcl_conversions::toPCL; + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ExtractPolygonalPrismData::onInit () @@ -120,14 +125,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( return; // Copy the header (stamp + frame_id) - pcl::PointIndices inliers; - inliers.header = cloud->header; + pcl_msgs::PointIndices inliers; + inliers.header = fromPCL(cloud->header); // If cloud is given, check if it's valid if (!isValid (cloud) || !isValid (hull, "planar_hull")) { NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ()); - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (boost::make_shared (inliers)); return; } // If indices are given, check if they are valid @@ -145,16 +150,16 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); else NODELET_DEBUG ("[%s::input_indices_hull_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ()); /// if (cloud->header.frame_id != hull->header.frame_id) @@ -180,10 +185,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( impl_.setIndices (indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty ()) - impl_.segment (inliers); + if (!cloud->points.empty ()) { + pcl::PointIndices pcl_inliers; + toPCL(inliers, pcl_inliers); + impl_.segment (pcl_inliers); + fromPCL(pcl_inliers, inliers); + } // Enforce that the TF frame and the timestamp are copied - inliers.header = cloud->header; + inliers.header = fromPCL(cloud->header); pub_output_.publish (boost::make_shared (inliers)); NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ()); } From a3840127f8d37a04405292a2db3c83e4c17fb286 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Jul 2013 18:56:23 -0700 Subject: [PATCH 151/405] Refactors to use pcl-1.7 --- .../include/pcl_ros/filters/extract_indices.h | 10 +++- pcl_ros/include/pcl_ros/filters/passthrough.h | 10 +++- .../include/pcl_ros/filters/project_inliers.h | 14 +++-- .../pcl_ros/filters/radius_outlier_removal.h | 10 +++- .../filters/statistical_outlier_removal.h | 10 +++- pcl_ros/include/pcl_ros/filters/voxel_grid.h | 2 +- .../include/pcl_ros/io/concatenate_fields.h | 2 + pcl_ros/include/pcl_ros/pcl_nodelet.h | 4 +- .../pcl_ros/segmentation/sac_segmentation.h | 8 +-- .../pcl_ros/surface/moving_least_squares.h | 2 +- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 9 ++- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 2 + pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 2 + pcl_ros/src/pcl_ros/io/pcd_io.cpp | 9 ++- .../pcl_ros/segmentation/extract_clusters.cpp | 34 +++++++---- .../extract_polygonal_prism_data.cpp | 16 +++--- .../pcl_ros/segmentation/sac_segmentation.cpp | 56 ++++++++++++------- .../segmentation/segment_differences.cpp | 6 +- pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 6 +- .../pcl_ros/surface/moving_least_squares.cpp | 4 +- pcl_ros/tools/pointcloud_to_pcd.cpp | 7 ++- 21 files changed, 147 insertions(+), 76 deletions(-) diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.h b/pcl_ros/include/pcl_ros/filters/extract_indices.h index 01cfde83..cdb60e8f 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.h +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.h @@ -66,9 +66,13 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. @@ -84,7 +88,7 @@ namespace pcl_ros private: /** \brief The PCL filter implementation used. */ - pcl::ExtractIndices impl_; + pcl::ExtractIndices impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.h b/pcl_ros/include/pcl_ros/filters/passthrough.h index c21a7d1b..7d6c9649 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.h +++ b/pcl_ros/include/pcl_ros/filters/passthrough.h @@ -64,9 +64,13 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL (*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. @@ -85,7 +89,7 @@ namespace pcl_ros private: /** \brief The PCL filter implementation used. */ - pcl::PassThrough impl_; + pcl::PassThrough impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.h b/pcl_ros/include/pcl_ros/filters/project_inliers.h index 200ef10e..636291fe 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.h +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.h @@ -68,10 +68,16 @@ namespace pcl_ros filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL (*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.setModelCoefficients (model_); - impl_.filter (output); + pcl::ModelCoefficients::Ptr pcl_model; + pcl_conversions::toPCL(*(model_), *(pcl_model)); + impl_.setModelCoefficients (pcl_model); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } private: @@ -85,7 +91,7 @@ namespace pcl_ros 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_; + pcl::ProjectInliers impl_; /** \brief Nodelet initialization routine. */ virtual void diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h index fc316a83..38babf0c 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h @@ -61,9 +61,13 @@ namespace pcl_ros filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL (*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. @@ -73,7 +77,7 @@ namespace pcl_ros private: /** \brief The PCL filter implementation used. */ - pcl::RadiusOutlierRemoval impl_; + pcl::RadiusOutlierRemoval impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h index e97735d5..22ed8b82 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h @@ -74,9 +74,13 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. @@ -93,7 +97,7 @@ namespace pcl_ros private: /** \brief The PCL filter implementation used. */ - pcl::StatisticalOutlierRemoval impl_; + pcl::StatisticalOutlierRemoval impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.h b/pcl_ros/include/pcl_ros/filters/voxel_grid.h index 95942259..1d887f7e 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.h +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.h @@ -57,7 +57,7 @@ namespace pcl_ros boost::shared_ptr > srv_; /** \brief The PCL filter implementation used. */ - pcl::VoxelGrid impl_; + pcl::VoxelGrid impl_; /** \brief Call the actual filter. * \param input the input point cloud dataset diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.h b/pcl_ros/include/pcl_ros/io/concatenate_fields.h index f6252b68..13a9ced3 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_fields.h +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.h @@ -45,6 +45,8 @@ #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_nodelet.h b/pcl_ros/include/pcl_ros/pcl_nodelet.h index dbb8b460..ba69afdc 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.h +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.h @@ -47,7 +47,7 @@ #include // PCL includes #include -#include +#include #include #include #include "pcl_ros/point_cloud.h" @@ -82,7 +82,7 @@ namespace pcl_ros typedef PointIndices::Ptr PointIndicesPtr; typedef PointIndices::ConstPtr PointIndicesConstPtr; - typedef pcl::ModelCoefficients ModelCoefficients; + typedef pcl_msgs::ModelCoefficients ModelCoefficients; typedef ModelCoefficients::Ptr ModelCoefficientsPtr; typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h index 7ff369b6..fc10d104 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h @@ -112,7 +112,7 @@ namespace pcl_ros /** \brief Null passthrough filter, used for pushing empty elements in the * synchronizer */ - message_filters::PassThrough nf_pi_; + message_filters::PassThrough nf_pi_; /** \brief Nodelet initialization routine. */ virtual void onInit (); @@ -149,7 +149,7 @@ namespace pcl_ros inline void input_callback (const PointCloudConstPtr &input) { - indices_.header = input->header; + indices_.header = fromPCL(input->header); PointIndicesConstPtr indices; indices.reset (new PointIndices (indices_)); nf_pi_.add (indices); @@ -220,7 +220,7 @@ namespace pcl_ros input_callback (const PointCloudConstPtr &cloud) { PointIndices indices; - indices.header.stamp = cloud->header.stamp; + indices.header.stamp = fromPCL(cloud->header).stamp; nf_.add (boost::make_shared (indices)); } @@ -241,7 +241,7 @@ namespace pcl_ros /** \brief Model callback * \param model the sample consensus model found */ - void axis_callback (const pcl::ModelCoefficientsConstPtr &model); + void axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model); /** \brief Dynamic reconfigure callback * \param config the config object diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h index 87aa2662..656a52a9 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h @@ -59,7 +59,7 @@ namespace pcl_ros class MovingLeastSquares : public PCLNodelet { typedef pcl::PointXYZ PointIn; - typedef pcl::Normal NormalOut; + typedef pcl::PointNormal NormalOut; typedef pcl::PointCloud PointCloudIn; typedef PointCloudIn::Ptr PointCloudInPtr; diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 87c6ea43..6405b3f4 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -58,10 +58,13 @@ pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL (*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index e4ccba9c..90000feb 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -40,6 +40,8 @@ #include "pcl_ros/transforms.h" #include "pcl_ros/io/concatenate_data.h" +#include + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index 52b17edd..ea64c1f6 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -41,6 +41,8 @@ #include #include "pcl_ros/io/concatenate_fields.h" +#include + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index 026c0d20..07d6bfae 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -86,13 +86,13 @@ pcl_ros::PCDReader::onInit () { NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ()); file_name = file_name_; - PointCloud2 cloud; + pcl::PCLPointCloud2 cloud; if (impl_.read (file_name_, cloud) < 0) { NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ()); return; } - output_ = boost::make_shared (cloud); + pcl_conversions::moveFromPCL(cloud, *(output_)); output_->header.stamp = ros::Time::now (); output_->header.frame_id = tf_frame_; } @@ -163,7 +163,10 @@ pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud) fname = boost::lexical_cast (cloud->header.stamp.toSec ()) + ".pcd"; else fname = file_name_; - impl_.write (fname, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_); + pcl::PCLPointCloud2 pcl_cloud; + // It is safe to remove the const here because we are the only subscriber callback. + pcl_conversions::moveToPCL(*(const_cast(cloud.get())), pcl_cloud); + impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_); NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ()); } diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index 0bff57bf..686c0006 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -37,8 +37,15 @@ #include #include +#include #include "pcl_ros/segmentation/extract_clusters.h" +#include + +using pcl_conversions::fromPCL; +using pcl_conversions::moveFromPCL; +using pcl_conversions::toPCL; + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::EuclideanClusterExtraction::onInit () @@ -158,15 +165,18 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( } /// DEBUG - if (indices) + if (indices) { + std_msgs::Header cloud_header = fromPCL(cloud->header); + std_msgs::Header indices_header = indices->header; NODELET_DEBUG ("[%s::input_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + } else { + NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + } /// IndicesPtr indices_ptr; @@ -176,7 +186,7 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( impl_.setInputCloud (cloud); impl_.setIndices (indices_ptr); - std::vector clusters; + std::vector clusters; impl_.extract (clusters); if (publish_indices_) @@ -186,8 +196,10 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( if ((int)i >= max_clusters_) break; // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. - clusters[i].header.stamp += ros::Duration (i * 0.001); - pub_output_.publish (boost::make_shared (clusters[i])); + pcl_msgs::PointIndices ros_pi; + moveFromPCL(clusters[i], ros_pi); + ros_pi.header.stamp += ros::Duration (i * 0.001); + pub_output_.publish (ros_pi); } NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ()); @@ -204,11 +216,13 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( //PointCloud output_blob; // Convert from the templated output to the PointCloud blob //pcl::toROSMsg (output, output_blob); // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. - output.header.stamp += ros::Duration (i * 0.001); + std_msgs::Header header = fromPCL(output.header); + header.stamp += ros::Duration (i * 0.001); + toPCL(header, output.header); // Publish a Boost shared ptr const data pub_output_.publish (output.makeShared ()); NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", - i, clusters[i].indices.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); + i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); } } } diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 7c54c414..b75b3ecd 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -42,8 +42,8 @@ #include -using pcl_conversions::fromPCL; -using pcl_conversions::toPCL; +using pcl_conversions::moveFromPCL; +using pcl_conversions::moveToPCL; ////////////////////////////////////////////////////////////////////////////////////////////// void @@ -132,14 +132,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( if (!isValid (cloud) || !isValid (hull, "planar_hull")) { NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ()); - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (inliers); return; } // If indices are given, check if they are valid if (indices && !isValid (indices)) { NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ()); - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (inliers); return; } @@ -169,7 +169,7 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_)) { // Publish empty before return - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (inliers); return; } impl_.setInputPlanarHull (planar_hull.makeShared ()); @@ -187,13 +187,13 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) if (!cloud->points.empty ()) { pcl::PointIndices pcl_inliers; - toPCL(inliers, pcl_inliers); + moveToPCL(inliers, pcl_inliers); impl_.segment (pcl_inliers); - fromPCL(pcl_inliers, inliers); + moveFromPCL(pcl_inliers, inliers); } // Enforce that the TF frame and the timestamp are copied inliers.header = fromPCL(cloud->header); - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (inliers); NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ()); } diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index 8478c71d..fcefc3bc 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -39,6 +39,10 @@ #include "pcl_ros/segmentation/sac_segmentation.h" #include +#include + +using pcl_conversions::fromPCL; + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SACSegmentation::onInit () @@ -249,25 +253,25 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0) return; - PointIndices inliers; - ModelCoefficients model; + pcl_msgs::PointIndices inliers; + pcl_msgs::ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied - inliers.header = model.header = cloud->header; + inliers.header = model.header = fromPCL(cloud->header); // If cloud is given, check if it's valid if (!isValid (cloud)) { NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + pub_indices_.publish (inliers); + pub_model_.publish (model); return; } // If indices are given, check if they are valid if (indices && !isValid (indices)) { NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + pub_indices_.publish (inliers); + pub_model_.publish (model); return; } @@ -277,11 +281,11 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); else NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); /// // Check whether the user has given a different input TF frame @@ -308,8 +312,15 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou impl_.setIndices (indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty ()) - impl_.segment (inliers, model); + if (!cloud->points.empty ()) { + pcl::PointIndices pcl_inliers; + pcl::ModelCoefficients pcl_model; + pcl_conversions::moveToPCL(inliers, pcl_inliers); + pcl_conversions::moveToPCL(model, pcl_model); + impl_.segment (pcl_inliers, pcl_model); + pcl_conversions::moveFromPCL(pcl_inliers, inliers); + pcl_conversions::moveFromPCL(pcl_model, model); + } // Probably need to transform the model of the plane here @@ -453,7 +464,7 @@ pcl_ros::SACSegmentationFromNormals::onInit () ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl::ModelCoefficientsConstPtr &model) +pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model) { boost::mutex::scoped_lock lock (mutex_); @@ -551,7 +562,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( PointIndices inliers; ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied - inliers.header = model.header = cloud->header; + inliers.header = model.header = fromPCL(cloud->header); if (impl_.getModelType () < 0) { @@ -584,16 +595,16 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); else NODELET_DEBUG ("[%s::input_normals_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); /// @@ -618,8 +629,15 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( impl_.setIndices (indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty ()) - impl_.segment (inliers, model); + if (!cloud->points.empty ()) { + pcl::PointIndices pcl_inliers; + pcl::ModelCoefficients pcl_model; + pcl_conversions::moveToPCL(inliers, pcl_inliers); + pcl_conversions::moveToPCL(model, pcl_model); + impl_.segment (pcl_inliers, pcl_model); + pcl_conversions::moveFromPCL(pcl_inliers, inliers); + pcl_conversions::moveFromPCL(pcl_model, model); + } // Check if we have enough inliers, clear inliers + model if not if ((int)inliers.indices.size () <= min_inliers_) diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index f8fbae11..eceb5b17 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -109,8 +109,8 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), cloud_target->header.stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), + cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ()); impl_.setInputCloud (cloud); impl_.setTargetCloud (cloud_target); @@ -120,7 +120,7 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl pub_output_.publish (output.makeShared ()); NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (), - output.points.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); + output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ()); } typedef pcl_ros::SegmentDifferences SegmentDifferences; diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index 94dc5336..97417e58 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -120,10 +120,10 @@ void " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); else - NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); // Reset the indices and surface pointers IndicesPtr indices_ptr; @@ -140,7 +140,7 @@ void if (output.points.size () >= 3) { geometry_msgs::PolygonStamped poly; - poly.header = output.header; + poly.header = fromPCL(output.header); poly.polygon.points.resize (output.points.size ()); // Get three consecutive points (without copying) pcl::Vector4fMap O = output.points[1].getVector4fMap (); diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index 2cb41cff..8f3caf5f 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -141,10 +141,10 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); else - NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); /// // Reset the indices and surface pointers diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index 9b52113c..00e92734 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -37,11 +37,16 @@ // ROS core #include + +#include + // PCL includes #include #include #include +#include + using namespace std; /** @@ -67,7 +72,7 @@ class PointCloudToPCD //////////////////////////////////////////////////////////////////////////////// // Callback void - cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud) + cloud_cb (const pcl::PCLPointCloud2::ConstPtr& cloud) { if ((cloud->width * cloud->height) == 0) return; From edc9fdab857ef220ba744b07f7c1b8be693e3b42 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Jul 2013 22:27:28 -0700 Subject: [PATCH 152/405] Add missing include in one of the installed headers --- pcl_ros/include/pcl_ros/publisher.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pcl_ros/include/pcl_ros/publisher.h b/pcl_ros/include/pcl_ros/publisher.h index b019ba9b..b752418c 100644 --- a/pcl_ros/include/pcl_ros/publisher.h +++ b/pcl_ros/include/pcl_ros/publisher.h @@ -49,6 +49,8 @@ #include #include +#include + namespace pcl_ros { class BasePublisher From eee71fac20dd6b806ddc6a94e8fbcfee12c2771d Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Jul 2013 23:31:43 -0700 Subject: [PATCH 153/405] Adding changelogs --- pcl_ros/CHANGELOG.rst | 135 +++++++++++++++++++++++++++++++++++ perception_pcl/CHANGELOG.rst | 108 ++++++++++++++++++++++++++++ 2 files changed, 243 insertions(+) create mode 100644 pcl_ros/CHANGELOG.rst create mode 100644 perception_pcl/CHANGELOG.rst diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst new file mode 100644 index 00000000..4b97a31c --- /dev/null +++ b/pcl_ros/CHANGELOG.rst @@ -0,0 +1,135 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pcl_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add missing include in one of the installed headers +* Refactors to use pcl-1.7 +* Use the PointIndices from pcl_msgs +* Experimental changes to point_cloud.h +* Fixes from converting from pcl-1.7, incomplete +* Depend on pcl_conversions and pcl_msgs +* bag_to_pcd: check return code of transformPointCloud() + This fixes a bug where bag_to_pcd segfaults because of an ignored + tf::ExtrapolationException. +* Changed #include type to lib +* Changed some #include types to lib +* removed a whitespace + +1.0.34 (2013-05-21) +------------------- +* fixing catkin python imports + +1.0.33 (2013-05-20) +------------------- +* Fixing catkin python imports + +1.0.32 (2013-05-17) +------------------- +* Merge pull request `#11 `_ from k-okada/groovy-devel + revert removed directories +* fix to compileable +* copy features/segmentation/surface from fuerte-devel + +1.0.31 (2013-04-22 11:58) +------------------------- +* No changes + +1.0.30 (2013-04-22 11:47) +------------------------- +* deprecating bin install targets + +1.0.29 (2013-03-04) +------------------- +* Fixes `#7 `_ +* now also works without specifying publishing interval like described in the wiki. + +1.0.28 (2013-02-05 12:29) +------------------------- +* reenabling deprecated install targets - comment added + +1.0.27 (2013-02-05 12:10) +------------------------- +* Update pcl_ros/package.xml +* Fixing target install directory for pcl tools +* update pluginlib macro + +1.0.26 (2013-01-17) +------------------- +* fixing catkin export + +1.0.25 (2013-01-01) +------------------- +* fixes `#1 `_ + +1.0.24 (2012-12-21) +------------------- +* remove obsolete roslib import + +1.0.23 (2012-12-19 16:52) +------------------------- +* clean up shared parameters + +1.0.22 (2012-12-19 15:22) +------------------------- +* fix dyn reconf files + +1.0.21 (2012-12-18 17:42) +------------------------- +* fixing catkin_package debs + +1.0.20 (2012-12-18 14:21) +------------------------- +* adding catkin_project dependencies + +1.0.19 (2012-12-17 21:47) +------------------------- +* adding nodelet_topic_tools dependency + +1.0.18 (2012-12-17 21:17) +------------------------- +* adding pluginlib dependency +* adding nodelet dependencies +* CMake install fixes +* migrating nodelets and tools from fuerte release to pcl_ros +* Updated for new catkin<...> catkin rule + +1.0.17 (2012-10-26 09:28) +------------------------- +* remove useless tags + +1.0.16 (2012-10-26 08:53) +------------------------- +* no need to depend on a meta-package + +1.0.15 (2012-10-24) +------------------- +* do not generrate messages automatically + +1.0.14 (2012-10-23) +------------------- +* bring back the PCL msgs + +1.0.13 (2012-10-11 17:46) +------------------------- +* install library to the right place + +1.0.12 (2012-10-11 17:25) +------------------------- + +1.0.11 (2012-10-10) +------------------- +* fix a few dependencies + +1.0.10 (2012-10-04) +------------------- +* comply to the new catkin API +* fixed pcl_ros manifest +* added pcl exports in manifest.xml +* fixed rosdeb pcl in pcl_ros/manifest.xml +* removing common_rosdeps from manifest.xml +* perception_pcl restructuring in groovy branch +* restructuring perception_pcl in groovy branch +* catkinized version of perception_pcl for groovy +* added PCL 1.6 stack for groovy diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst new file mode 100644 index 00000000..57f09623 --- /dev/null +++ b/perception_pcl/CHANGELOG.rst @@ -0,0 +1,108 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package perception_pcl +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Fixes from converting from pcl-1.7, incomplete + +1.0.34 (2013-05-21) +------------------- +* No changes + +1.0.33 (2013-05-20) +------------------- +* No changes + +1.0.32 (2013-05-17) +------------------- +* No changes + +1.0.31 (2013-04-22 11:58) +------------------------- +* No changes + +1.0.30 (2013-04-22 11:47) +------------------------- +* adding CMakeLists.txt file for metapackage +* adding buildtool_depend to meta package + +1.0.29 (2013-03-04) +------------------- +* No changes + +1.0.28 (2013-02-05 12:29) +------------------------- +* No changes + +1.0.27 (2013-02-05 12:10) +------------------------- +* No changes + +1.0.26 (2013-01-17) +------------------- +* removing build_tool dependency from package.xml + +1.0.25 (2013-01-01) +------------------- +* No changes + +1.0.24 (2012-12-21) +------------------- +* No changes + +1.0.23 (2012-12-19 16:52) +------------------------- +* No changes + +1.0.22 (2012-12-19 15:22) +------------------------- +* No changes + +1.0.21 (2012-12-18 17:42) +------------------------- +* No changes + +1.0.20 (2012-12-18 14:21) +------------------------- +* No changes + +1.0.19 (2012-12-17 21:47) +------------------------- +* No changes + +1.0.18 (2012-12-17 21:17) +------------------------- +* Updated for new catkin<...> catkin rule + +1.0.17 (2012-10-26 09:28) +------------------------- +* remove useless tags + +1.0.16 (2012-10-26 08:53) +------------------------- +* No changes + +1.0.15 (2012-10-24) +------------------- +* No changes + +1.0.14 (2012-10-23) +------------------- +* No changes + +1.0.13 (2012-10-11 17:46) +------------------------- +* No changes + +1.0.12 (2012-10-11 17:25) +------------------------- +* make sure perception_pcl is a meta package + +1.0.11 (2012-10-10) +------------------- +* No changes + +1.0.10 (2012-10-04) +------------------- +* comply to the new catkin API From e0e17d2daac7e6b9e0267dd54aff5f2d8d94e324 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Jul 2013 23:56:10 -0700 Subject: [PATCH 154/405] 1.1.0 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index c87b9d0e..9511fedc 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.0.34 + 1.1.0

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 9ac4e385..44bb900b 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.0.34 + 1.1.0

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 3a688bcf5f1542e63ff1e6e69036b75cb458149f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 10 Jul 2013 16:49:43 -0700 Subject: [PATCH 155/405] Add missing EIGEN define which caused failures on the farm --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/include/pcl_ros/features/boundary.h | 8 +++++--- pcl_ros/src/pcl_ros/features/boundary.cpp | 4 ++-- perception_pcl/CHANGELOG.rst | 4 ++-- 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 4b97a31c..21bf140b 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2013-07-09) +------------------ * Add missing include in one of the installed headers * Refactors to use pcl-1.7 * Use the PointIndices from pcl_msgs diff --git a/pcl_ros/include/pcl_ros/features/boundary.h b/pcl_ros/include/pcl_ros/features/boundary.h index bb91cd61..39a0d10e 100644 --- a/pcl_ros/include/pcl_ros/features/boundary.h +++ b/pcl_ros/include/pcl_ros/features/boundary.h @@ -38,6 +38,8 @@ #ifndef PCL_ROS_BOUNDARY_H_ #define PCL_ROS_BOUNDARY_H_ +#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true + #include #include "pcl_ros/features/feature.h" @@ -55,11 +57,11 @@ namespace pcl_ros { private: pcl::BoundaryEstimation impl_; - + typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ - inline bool + inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher @@ -71,7 +73,7 @@ namespace pcl_ros void emptyPublish (const PointCloudInConstPtr &cloud); /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, + void computePublish (const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &normals, const PointCloudInConstPtr &surface, const IndicesPtr &indices); diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp index 5fa970f6..0b6a8c36 100644 --- a/pcl_ros/src/pcl_ros/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -38,7 +38,7 @@ #include #include "pcl_ros/features/boundary.h" -void +void pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; @@ -46,7 +46,7 @@ pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pub_output_.publish (output.makeShared ()); } -void +void pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &normals, const PointCloudInConstPtr &surface, diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 57f09623..c51638d6 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2013-07-09) +------------------ * Fixes from converting from pcl-1.7, incomplete 1.0.34 (2013-05-21) From 47cc34c7b3770b78d683a15d6d2b45c8de51455f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 10 Jul 2013 16:53:10 -0700 Subject: [PATCH 156/405] Updating change logs --- pcl_ros/CHANGELOG.rst | 4 ++++ perception_pcl/CHANGELOG.rst | 6 +++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 21bf140b..6b568a4a 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2013-07-10) +------------------ +* Add missing EIGEN define which caused failures on the farm + 1.1.0 (2013-07-09) ------------------ * Add missing include in one of the installed headers diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index c51638d6..fa9abca2 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,9 +2,13 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2013-07-10) +------------------ +* No changes + 1.1.0 (2013-07-09) ------------------ -* Fixes from converting from pcl-1.7, incomplete +* No changes 1.0.34 (2013-05-21) ------------------- From 7f197fab15debafb1d70b201b01722293517c637 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 10 Jul 2013 16:54:05 -0700 Subject: [PATCH 157/405] 1.1.1 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 9511fedc..e2ac20fb 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.0 + 1.1.1

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 44bb900b..ec2f472d 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.0 + 1.1.1

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 585ed21709fd1d0aff77017dee3979a3e3637648 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 12 Jul 2013 13:29:43 -0700 Subject: [PATCH 158/405] Make find_package on Eigen and PCL REQUIRED --- pcl_ros/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 3415887d..b1c01f3f 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -4,8 +4,8 @@ project(pcl_ros) # Deal with catkin find_package(Boost COMPONENTS system filesystem thread REQUIRED) find_package(catkin REQUIRED dynamic_reconfigure genmsg nodelet nodelet_topic_tools pcl_conversions pcl_msgs roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib pluginlib) -find_package(Eigen) -find_package(PCL) +find_package(Eigen REQUIRED) +find_package(PCL REQUIRED) # deal with ROS include_directories(SYSTEM ${Boost_INCLUDE_DIRS} From b6c9ebb319ae4eb7669056c137fdf1fcb3bb4664 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 12 Jul 2013 13:45:46 -0700 Subject: [PATCH 159/405] Updating travis for hydro --- .travis.yml | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/.travis.yml b/.travis.yml index eab705fb..a40ed147 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,21 +5,19 @@ python: - "2.7" compiler: - gcc + - clang install: - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - - sudo apt-get update -qq - - sudo apt-get install python-catkin-pkg python-rosdep -qq - - sudo apt-get install ros-groovy-catkin -qq + - sudo apt-get update + - sudo apt-get install python-catkin-pkg python-rosdep - sudo rosdep init - - rosdep update - - mkdir -p /tmp/ws/src - - ln -s `pwd` /tmp/ws/src/perception_pcl - - cd /tmp/ws - - rosdep install --from-paths src --ignore-src --rosdistro groovy -y + - rosdep update + - rosdep install --from-paths src --ignore-src --rosdistro hydro -y script: - - source /opt/ros/groovy/setup.bash - - catkin_make + - source /opt/ros/hydro/setup.bash + - catkin_make tests + - catkin_make run_tests - catkin_make install From d42d6c0be0a8719f4152c4ff3ca658d3950d6f63 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 12 Jul 2013 13:56:37 -0700 Subject: [PATCH 160/405] Update to travis --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index a40ed147..2f89ec1e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,7 +14,7 @@ install: - sudo apt-get install python-catkin-pkg python-rosdep - sudo rosdep init - rosdep update - - rosdep install --from-paths src --ignore-src --rosdistro hydro -y + - rosdep install --from-paths . --ignore-src --rosdistro hydro -y script: - source /opt/ros/hydro/setup.bash From 716b263d4d8885e2ebec33ec28db1c3b6f87798b Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 12 Jul 2013 14:01:43 -0700 Subject: [PATCH 161/405] Use shadow fixed rather than public --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 2f89ec1e..f9b3ae9e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,7 +8,7 @@ compiler: - clang install: - - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' + - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - sudo apt-get install python-catkin-pkg python-rosdep From 45c5de6aff1d161838a542f37485d3426f332bd6 Mon Sep 17 00:00:00 2001 From: rlklaser Date: Fri, 19 Jul 2013 20:37:21 -0300 Subject: [PATCH 162/405] Update common.py filters min max are bounded to only -5,5 when using rosparam load and dynamic reconfigure --- pcl_ros/cfg/common.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/cfg/common.py b/pcl_ros/cfg/common.py index 296951da..de839a8a 100644 --- a/pcl_ros/cfg/common.py +++ b/pcl_ros/cfg/common.py @@ -8,8 +8,8 @@ from dynamic_reconfigure.parameter_generator_catkin import *; 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", 0.0, -5.0, 5.0) - gen.add ("filter_limit_max", double_t, 0, "The maximum allowed field value a point will be considered from", 1.0, -5.0, 5.0) + gen.add ("filter_limit_min", double_t, 0, "The minimum allowed field value a point will be considered from", 0.0, -1000.0, 1000.0) + gen.add ("filter_limit_max", double_t, 0, "The maximum allowed field value a point will be considered from", 1.0, -1000.0, 1000.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].", 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.", 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.", "") From b6315543bb21070c1b52e2b7627c6b6321f37e61 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 19 Jul 2013 17:27:18 -0700 Subject: [PATCH 163/405] Fixed missing package exports also other general CMakeLists.txt cleanup --- pcl_ros/CMakeLists.txt | 265 +++++++++++++++++++++++------------------ 1 file changed, 149 insertions(+), 116 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index b1c01f3f..fd04d869 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -1,157 +1,190 @@ cmake_minimum_required(VERSION 2.8) project(pcl_ros) -# Deal with catkin -find_package(Boost COMPONENTS system filesystem thread REQUIRED) -find_package(catkin REQUIRED dynamic_reconfigure genmsg nodelet nodelet_topic_tools pcl_conversions pcl_msgs roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib pluginlib) +## Find system dependencies +find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen REQUIRED) find_package(PCL REQUIRED) -# deal with ROS -include_directories(SYSTEM ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} +## Find catkin packages +find_package(catkin REQUIRED COMPONENTS + dynamic_reconfigure + genmsg + nodelet + nodelet_topic_tools + pcl_conversions + pcl_msgs + pluginlib + rosbag + rosconsole + roscpp + roslib + sensor_msgs + std_msgs + tf ) -include_directories(include) - -link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) - -# generate the dynamic_reconfigure config file -generate_dynamic_reconfigure_options(cfg/Feature.cfg - cfg/Filter.cfg - cfg/EuclideanClusterExtraction.cfg - cfg/ExtractIndices.cfg - cfg/ExtractPolygonalPrismData.cfg - cfg/MLS.cfg - cfg/SACSegmentation.cfg - cfg/SACSegmentationFromNormals.cfg - cfg/SegmentDifferences.cfg - cfg/StatisticalOutlierRemoval.cfg - cfg/VoxelGrid.cfg +## Add include directories +include_directories(include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) -include_directories(include cfg/cpp) +## Add link directories +link_directories( + ${Boost_LIBRARY_DIRS} + ${Eigen_LIBRARY_DIRS} + ${PCL_LIBRARY_DIRS} +) + +## Generate dynamic_reconfigure +generate_dynamic_reconfigure_options( + cfg/EuclideanClusterExtraction.cfg + cfg/ExtractIndices.cfg + cfg/ExtractPolygonalPrismData.cfg + cfg/Feature.cfg + cfg/Filter.cfg + cfg/MLS.cfg + cfg/SACSegmentation.cfg + cfg/SACSegmentationFromNormals.cfg + cfg/SegmentDifferences.cfg + cfg/StatisticalOutlierRemoval.cfg + cfg/VoxelGrid.cfg +) + +## Declare the catkin package catkin_package( INCLUDE_DIRS include - LIBRARIES pcl_ros_tf pcl_ros_io pcl_ros_filters - CATKIN_DEPENDS roscpp sensor_msgs tf sensor_msgs std_msgs - DEPENDS Eigen PCL + LIBRARIES + pcl_ros_filters + pcl_ros_io + pcl_ros_tf + CATKIN_DEPENDS + dynamic_reconfigure + message_filters + nodelet + pcl_conversions + pcl_ros + rosbag + roscpp + sensor_msgs + std_msgs + tf + DEPENDS + Boost + Eigen + PCL ) -# ---[ Point Cloud Library - Transforms -add_library (pcl_ros_tf SHARED src/transforms.cpp) -target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) -add_dependencies(pcl_ros_tf ros_gencpp pcl_ros_copy) +## Declare the pcl_ros_tf library +add_library(pcl_ros_tf src/transforms.cpp) +target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +add_dependencies(pcl_ros_tf pcl_ros_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) -############ NODELETS - -# ---[ Point Cloud Library - IO -add_library (pcl_ros_io - src/pcl_ros/io/io.cpp - src/pcl_ros/io/concatenate_fields.cpp - src/pcl_ros/io/concatenate_data.cpp - src/pcl_ros/io/pcd_io.cpp - src/pcl_ros/io/bag_io.cpp - ) -#rosbuild_add_compile_flags (pcl_ros_io ${SSE_FLAGS}) -target_link_libraries (pcl_ros_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +## Nodelets +## Declare the pcl_ros_io library +add_library(pcl_ros_io + src/pcl_ros/io/bag_io.cpp + src/pcl_ros/io/concatenate_data.cpp + src/pcl_ros/io/concatenate_fields.cpp + src/pcl_ros/io/io.cpp + src/pcl_ros/io/pcd_io.cpp +) +target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) class_loader_hide_library_symbols(pcl_ros_io) -# ---[ PCL ROS - Features -add_library (pcl_ros_features - src/pcl_ros/features/feature.cpp - # Compilation is much faster if we include all the following CPP files in feature.cpp - src/pcl_ros/features/boundary.cpp - src/pcl_ros/features/fpfh.cpp - src/pcl_ros/features/fpfh_omp.cpp - src/pcl_ros/features/moment_invariants.cpp - src/pcl_ros/features/normal_3d.cpp - src/pcl_ros/features/normal_3d_omp.cpp - src/pcl_ros/features/pfh.cpp - src/pcl_ros/features/principal_curvatures.cpp - src/pcl_ros/features/vfh.cpp - ) -#rosbuild_add_compile_flags (pcl_ros_features ${SSE_FLAGS}) -target_link_libraries (pcl_ros_features ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) - +## Declare the pcl_ros_features library +add_library(pcl_ros_features + src/pcl_ros/features/feature.cpp + # Compilation is much faster if we include all the following CPP files in feature.cpp + src/pcl_ros/features/boundary.cpp + src/pcl_ros/features/fpfh.cpp + src/pcl_ros/features/fpfh_omp.cpp + src/pcl_ros/features/moment_invariants.cpp + src/pcl_ros/features/normal_3d.cpp + src/pcl_ros/features/normal_3d_omp.cpp + src/pcl_ros/features/pfh.cpp + src/pcl_ros/features/principal_curvatures.cpp + src/pcl_ros/features/vfh.cpp +) +target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) class_loader_hide_library_symbols(pcl_ros_features) -# ---[ PCL ROS - Filters -add_library (pcl_ros_filters - src/pcl_ros/filters/filter.cpp - src/pcl_ros/filters/passthrough.cpp - src/pcl_ros/filters/project_inliers.cpp - src/pcl_ros/filters/extract_indices.cpp - src/pcl_ros/filters/radius_outlier_removal.cpp - src/pcl_ros/filters/statistical_outlier_removal.cpp - src/pcl_ros/filters/voxel_grid.cpp - ) -#add_compile_flags (pcl_ros_filters ${SSE_FLAGS}) -target_link_libraries (pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_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 +) +target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) class_loader_hide_library_symbols(pcl_ros_filters) -# ---[ Point Cloud Library - Segmentation +## Declare the pcl_ros_segmentation library add_library (pcl_ros_segmentation - src/pcl_ros/segmentation/segmentation.cpp - src/pcl_ros/segmentation/segment_differences.cpp - src/pcl_ros/segmentation/extract_clusters.cpp - src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp - src/pcl_ros/segmentation/sac_segmentation.cpp - ) -#rosbuild_add_compile_flags (pcl_ros_segmentation ${SSE_FLAGS}) -target_link_libraries (pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) - + src/pcl_ros/segmentation/extract_clusters.cpp + src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp + src/pcl_ros/segmentation/sac_segmentation.cpp + src/pcl_ros/segmentation/segment_differences.cpp + src/pcl_ros/segmentation/segmentation.cpp +) +target_link_libraries(pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) class_loader_hide_library_symbols(pcl_ros_segmentation) -# ---[ Point Cloud Library - Surface +## Declare the pcl_ros_surface library add_library (pcl_ros_surface - src/pcl_ros/surface/surface.cpp - # Compilation is much faster if we include all the following CPP files in surface.cpp - src/pcl_ros/surface/convex_hull.cpp - src/pcl_ros/surface/moving_least_squares.cpp - ) -#rosbuild_add_compile_flags (pcl_ros_surface ${SSE_FLAGS}) -target_link_libraries (pcl_ros_surface ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) - + src/pcl_ros/surface/surface.cpp + # Compilation is much faster if we include all the following CPP files in surface.cpp + src/pcl_ros/surface/convex_hull.cpp + src/pcl_ros/surface/moving_least_squares.cpp +) +target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) class_loader_hide_library_symbols(pcl_ros_surface) -############ TOOLS +## Tools -add_executable (pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) -target_link_libraries (pcd_to_pointcloud pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) +target_link_libraries(pcd_to_pointcloud pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_executable (pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) -target_link_libraries (pointcloud_to_pcd pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) +target_link_libraries(pointcloud_to_pcd pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_executable (bag_to_pcd tools/bag_to_pcd.cpp) -target_link_libraries (bag_to_pcd pcl_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +add_executable(bag_to_pcd tools/bag_to_pcd.cpp) +target_link_libraries(bag_to_pcd pcl_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_executable (convert_pcd_to_image tools/convert_pcd_to_image.cpp) -target_link_libraries (convert_pcd_to_image pcl_io pcl_common ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) +target_link_libraries(convert_pcd_to_image pcl_io pcl_common ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_executable (convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) -target_link_libraries (convert_pointcloud_to_image pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) +target_link_libraries(convert_pointcloud_to_image pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_features pcl_ros_filters pcl_ros_surface pcl_ros_segmentation pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +install( + TARGETS + pcl_ros_tf + pcl_ros_io + pcl_ros_features + pcl_ros_filters + pcl_ros_surface + pcl_ros_segmentation + pcd_to_pointcloud + pointcloud_to_pcd + bag_to_pcd + convert_pcd_to_image + convert_pointcloud_to_image + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -# these target locations are deprecated and will be dropped in Hydro! -#install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image -# DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) - install(FILES pcl_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - - From f758d4ba805bb84b7d0d50680b477a3746b36df6 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 19 Jul 2013 17:28:03 -0700 Subject: [PATCH 164/405] updating changelogs --- pcl_ros/CHANGELOG.rst | 5 +++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 8 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 6b568a4a..a26b00b2 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.2 (2013-07-19) +------------------ +* Fixed missing package exports on pcl_conversions and others +* Make find_package on Eigen and PCL REQUIRED + 1.1.1 (2013-07-10) ------------------ * Add missing EIGEN define which caused failures on the farm diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index fa9abca2..4f007334 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.2 (2013-07-19) +------------------ + 1.1.1 (2013-07-10) ------------------ * No changes From c88a6a1d66d6e9dc35f01841e250ddf14987707e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 19 Jul 2013 17:28:12 -0700 Subject: [PATCH 165/405] 1.1.2 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index e2ac20fb..b84c75d1 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.1 + 1.1.2

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index ec2f472d..5c95f5a7 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.1 + 1.1.2

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From b2b6eba0fccf5455868fbc5b11aa51fcadc259c6 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 19 Jul 2013 21:14:36 -0700 Subject: [PATCH 166/405] fix missing run_depend and wrong CATKIN_DEPENDS --- pcl_ros/CMakeLists.txt | 2 +- pcl_ros/package.xml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index fd04d869..c0659f06 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -66,7 +66,7 @@ catkin_package( message_filters nodelet pcl_conversions - pcl_ros + pcl_msgs rosbag roscpp sensor_msgs diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index b84c75d1..2c535463 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -31,6 +31,7 @@ dynamic_reconfigure eigen + message_filters nodelet nodelet_topic_tools pcl From c9ae3c519fe09ee7dfd7069fdaa63deaf83ce9de Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 19 Jul 2013 21:15:10 -0700 Subject: [PATCH 167/405] 1.1.3 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 2c535463..abe30a54 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.2 + 1.1.3

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 5c95f5a7..4338bcd6 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.2 + 1.1.3

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 99bfd532c0427e96e8e5cd4ba6a832e9e347f8be Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 22 Jul 2013 16:32:44 -0700 Subject: [PATCH 168/405] Fixup .travis.yaml --- .travis.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index f9b3ae9e..ee1414d2 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,6 +18,6 @@ install: script: - source /opt/ros/hydro/setup.bash - - catkin_make tests - - catkin_make run_tests - - catkin_make install + - catkin_make tests --source . + - catkin_make run_tests --source . + - catkin_make install -- source . From 9384112468f4bc01c12b40822e7e9eb3b95c828a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 22 Jul 2013 16:38:22 -0700 Subject: [PATCH 169/405] Fixup .travis.yaml --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index ee1414d2..b3fae60c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,4 +20,4 @@ script: - source /opt/ros/hydro/setup.bash - catkin_make tests --source . - catkin_make run_tests --source . - - catkin_make install -- source . + - catkin_make install --source . From 76f48f8b33f257461628b97a8c266afb42518e97 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 22 Jul 2013 16:50:43 -0700 Subject: [PATCH 170/405] Use -j1 and drop clang in .travis.yaml --- .travis.yml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index b3fae60c..5136721a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,7 +5,6 @@ python: - "2.7" compiler: - gcc - - clang install: - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' @@ -18,6 +17,6 @@ install: script: - source /opt/ros/hydro/setup.bash - - catkin_make tests --source . - - catkin_make run_tests --source . - - catkin_make install --source . + - catkin_make tests --source . -j1 + - catkin_make run_tests --source . -j1 + - catkin_make install --source . -j1 From fd75f3a02c36993585be69a308711aba29ff517f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 23 Jul 2013 10:20:07 -0700 Subject: [PATCH 171/405] Enable testing --- .travis.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 5136721a..3ca1ac58 100644 --- a/.travis.yml +++ b/.travis.yml @@ -17,6 +17,6 @@ install: script: - source /opt/ros/hydro/setup.bash - - catkin_make tests --source . -j1 - - catkin_make run_tests --source . -j1 - - catkin_make install --source . -j1 + - catkin_make tests --source . -j1 -DCATKIN_ENABLE_TESTING=ON + - catkin_make run_tests --source . -j1 -DCATKIN_ENABLE_TESTING=ON + - catkin_make install --source . -j1 -DCATKIN_ENABLE_TESTING=ON From 11d24d0e97169bd2fd2714fba1d4ac2dcdc87628 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 23 Jul 2013 13:32:50 -0700 Subject: [PATCH 172/405] Initialize shared pointers before use Should address runtime errors reported in #29 --- pcl_ros/include/pcl_ros/filters/extract_indices.h | 2 +- pcl_ros/include/pcl_ros/filters/passthrough.h | 2 +- pcl_ros/include/pcl_ros/filters/project_inliers.h | 4 ++-- pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h | 2 +- pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h | 4 ++-- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 2 +- 6 files changed, 8 insertions(+), 8 deletions(-) diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.h b/pcl_ros/include/pcl_ros/filters/extract_indices.h index cdb60e8f..1ae0d1a9 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.h +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.h @@ -66,7 +66,7 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - pcl::PCLPointCloud2::Ptr pcl_input; + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.h b/pcl_ros/include/pcl_ros/filters/passthrough.h index 7d6c9649..4dad5f2d 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.h +++ b/pcl_ros/include/pcl_ros/filters/passthrough.h @@ -64,7 +64,7 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - pcl::PCLPointCloud2::Ptr pcl_input; + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL (*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.h b/pcl_ros/include/pcl_ros/filters/project_inliers.h index 636291fe..8002bcd2 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.h +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.h @@ -68,11 +68,11 @@ namespace pcl_ros filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { - pcl::PCLPointCloud2::Ptr pcl_input; + 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; + pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); pcl_conversions::toPCL(*(model_), *(pcl_model)); impl_.setModelCoefficients (pcl_model); pcl::PCLPointCloud2 pcl_output; diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h index 38babf0c..cb75eaee 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h @@ -61,7 +61,7 @@ namespace pcl_ros filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { - pcl::PCLPointCloud2::Ptr pcl_input; + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL (*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h index 22ed8b82..b3d17e35 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h @@ -58,7 +58,7 @@ namespace pcl_ros * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu */ - class StatisticalOutlierRemoval : public Filter + class StatisticalOutlierRemoval : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ @@ -74,7 +74,7 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - pcl::PCLPointCloud2::Ptr pcl_input; + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 6405b3f4..db718d5b 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -58,7 +58,7 @@ pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - pcl::PCLPointCloud2::Ptr pcl_input; + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL (*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); From 3bc59ab51ba3e397445ab1adbcfcbb8c324fb0a9 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 23 Jul 2013 13:34:33 -0700 Subject: [PATCH 173/405] Fix a serialization error with point_cloud headers --- pcl_ros/include/pcl_ros/point_cloud.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h index 03c925de..167537ee 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.h +++ b/pcl_ros/include/pcl_ros/point_cloud.h @@ -213,8 +213,8 @@ namespace ros inline static void read(Stream& stream, pcl::PointCloud& m) { std_msgs::Header header; - pcl_conversions::fromPCL(m.header, header); stream.next(header); + pcl_conversions::toPCL(header, m.header); stream.next(m.height); stream.next(m.width); From ce64f9450e44d4cf6b44d7ef4c5ae6b575dfb65e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 23 Jul 2013 16:37:12 -0700 Subject: [PATCH 174/405] Updating changelogs --- pcl_ros/CHANGELOG.rst | 7 +++++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 10 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index a26b00b2..e2688683 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.4 (2013-07-23) +------------------ +* Fix a serialization error with point_cloud headers +* Initialize shared pointers before use in part of the pcl_conversions + Should address runtime errors reported in `#29 `_ +* Changed the default bounds on filters to -1000, 1000 from -5, 5 in common.py + 1.1.2 (2013-07-19) ------------------ * Fixed missing package exports on pcl_conversions and others diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 4f007334..879142a9 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.4 (2013-07-23) +------------------ + 1.1.2 (2013-07-19) ------------------ From c13777525fd44697a4c75e4b7069fa6d052109ec Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 23 Jul 2013 16:37:25 -0700 Subject: [PATCH 175/405] 1.1.4 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index abe30a54..909216f8 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.3 + 1.1.4

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 4338bcd6..d6498fbc 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.3 + 1.1.4

PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 3d35f0f74828131640a57108cac9a48235cea180 Mon Sep 17 00:00:00 2001 From: Lukas Bulwahn Date: Sun, 4 Aug 2013 17:50:04 +0200 Subject: [PATCH 176/405] package.xml: tuned whitespaces This commit removes trailing whitespaces and makes the line with the license information in the package.xml bitwise match exactly the common license information line in most ROS packages. The trailing whitespaces were detected when providing a bitbake recipe in the meta-ros project (github.com/bmwcarit/meta-ros). In the recipe, the hash of the license line is declared and is used to check for changes in the license. For this recipe, it was not matching the common one. A related already merged commit is https://github.com/ros/std_msgs/pull/3 and a related pending commit is https://github.com/ros-perception/pcl_msgs/pull/1. --- pcl_ros/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 909216f8..d9912cd5 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -10,7 +10,7 @@ Open Perception Julius Kammerl - BSD + BSD http://ros.org/wiki/perception_pcl catkin From acd12f8132efb99d9be51d18ff5dc0078aed8b99 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 27 Aug 2013 13:43:26 -0700 Subject: [PATCH 177/405] Updated package.xml's to use new libpcl-all rosdep rules --- pcl_ros/package.xml | 17 +++++++++-------- perception_pcl/package.xml | 9 +++++---- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index d9912cd5..c1a1a9d8 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -2,24 +2,25 @@ pcl_ros 1.1.4 -

+ PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry processing in ROS. -

+ Open Perception - Julius Kammerl + William Woodall + Julius Kammerl BSD http://ros.org/wiki/perception_pcl catkin - dynamic_reconfigure + dynamic_reconfigure eigen nodelet nodelet_topic_tools - pcl + libpcl-all-dev pcl_conversions pcl_msgs pluginlib @@ -34,7 +35,7 @@ message_filters nodelet nodelet_topic_tools - pcl + libpcl-all pcl_conversions pcl_msgs pluginlib @@ -43,9 +44,9 @@ sensor_msgs std_msgs tf - + - + diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index d6498fbc..89f9377a 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -2,15 +2,16 @@ perception_pcl 1.1.4 -

+ PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry processing in ROS. -

+ Open Perception - Julius Kammerl - BSD + William Woodall + Julius Kammerl + BSD http://ros.org/wiki/perception_pcl catkin From f51ccbb0ec53e76d1d9c0c5265d8c70caea1f495 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 27 Aug 2013 13:44:09 -0700 Subject: [PATCH 178/405] Updating changelogs --- pcl_ros/CHANGELOG.rst | 8 ++++++++ perception_pcl/CHANGELOG.rst | 4 ++++ 2 files changed, 12 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index e2688683..47b67eae 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.5 (2013-08-27) +------------------ +* Updated package.xml's to use new libpcl-all rosdep rules +* package.xml: tuned whitespaces + This commit removes trailing whitespaces and makes the line with the license information in the package.xml bitwise match exactly the common license information line in most ROS packages. + The trailing whitespaces were detected when providing a bitbake recipe in the meta-ros project (github.com/bmwcarit/meta-ros). In the recipe, the hash of the license line is declared and is used to check for changes in the license. For this recipe, it was not matching the common one. + A related already merged commit is https://github.com/ros/std_msgs/pull/3 and a related pending commit is https://github.com/ros-perception/pcl_msgs/pull/1. + 1.1.4 (2013-07-23) ------------------ * Fix a serialization error with point_cloud headers diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 879142a9..6ccde879 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.5 (2013-08-27) +------------------ +* Updated package.xml's to use new libpcl-all rosdep rules + 1.1.4 (2013-07-23) ------------------ From 2adf6f1d6e39cab0aa77e9dd446ce3b18a5a3516 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 27 Aug 2013 13:45:52 -0700 Subject: [PATCH 179/405] 1.1.5 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index c1a1a9d8..1d4faf8f 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.4 + 1.1.5 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 89f9377a..49347c06 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.4 + 1.1.5 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From a68cc1f7b3282df7d82845661ff00b1a48587d8a Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 18 Sep 2013 14:00:29 -0700 Subject: [PATCH 180/405] add excplicit dependency on gencfg target --- pcl_ros/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index c0659f06..07bcb400 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -125,6 +125,7 @@ add_library(pcl_ros_filters src/pcl_ros/filters/voxel_grid.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) ## Declare the pcl_ros_segmentation library From a397fdc26797376a8b7a7d9bcc1ca56fde428816 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Fri, 20 Sep 2013 10:35:49 -0700 Subject: [PATCH 181/405] adding changelogs --- pcl_ros/CHANGELOG.rst | 4 ++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 7 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 47b67eae..de438bba 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add excplicit dependency on gencfg target + 1.1.5 (2013-08-27) ------------------ * Updated package.xml's to use new libpcl-all rosdep rules diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 6ccde879..c825b8e6 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.5 (2013-08-27) ------------------ * Updated package.xml's to use new libpcl-all rosdep rules From d92092536c3a7b6356263072d6454784af3c90b1 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Fri, 20 Sep 2013 10:36:01 -0700 Subject: [PATCH 182/405] 1.1.6 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index de438bba..abc9c9c3 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.6 (2013-09-20) +------------------ * add excplicit dependency on gencfg target 1.1.5 (2013-08-27) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 1d4faf8f..8b5300c2 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.5 + 1.1.6 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index c825b8e6..ac02dfbe 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.6 (2013-09-20) +------------------ 1.1.5 (2013-08-27) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 49347c06..208abdeb 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.5 + 1.1.6 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 9edc4154b6d7b9b7851b82da921fb0cbcec78e64 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Fri, 20 Sep 2013 11:09:03 -0700 Subject: [PATCH 183/405] adding FeatureConfig dependency too --- pcl_ros/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 07bcb400..bd2b3260 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -111,6 +111,7 @@ add_library(pcl_ros_features src/pcl_ros/features/vfh.cpp ) target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_features) From 5b38aab37e3024f65b5b71b8335838e7d9668f69 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Fri, 20 Sep 2013 11:21:56 -0700 Subject: [PATCH 184/405] adding more uncaught config dependencies --- pcl_ros/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index bd2b3260..48e41e40 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -138,6 +138,7 @@ add_library (pcl_ros_segmentation src/pcl_ros/segmentation/segmentation.cpp ) target_link_libraries(pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_segmentation) ## Declare the pcl_ros_surface library @@ -148,6 +149,7 @@ add_library (pcl_ros_surface src/pcl_ros/surface/moving_least_squares.cpp ) target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_surface) ## Tools From b43d6e18a55bd719fc2295acaa0f593321d01d56 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Fri, 20 Sep 2013 11:37:45 -0700 Subject: [PATCH 185/405] changelog --- pcl_ros/CHANGELOG.rst | 5 +++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 8 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index abc9c9c3..6f94c4d4 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* adding more uncaught config dependencies +* adding FeatureConfig dependency too + 1.1.6 (2013-09-20) ------------------ * add excplicit dependency on gencfg target diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index ac02dfbe..63b78787 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.6 (2013-09-20) ------------------ From 92c4e3771d458188a66bcbd7c74f0856ced9e6b9 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Fri, 20 Sep 2013 11:37:50 -0700 Subject: [PATCH 186/405] 1.1.7 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 6f94c4d4..c0999c32 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.7 (2013-09-20) +------------------ * adding more uncaught config dependencies * adding FeatureConfig dependency too diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 8b5300c2..1e2c3a83 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.6 + 1.1.7 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 63b78787..f73a27be 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.7 (2013-09-20) +------------------ 1.1.6 (2013-09-20) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 208abdeb..1faec4ae 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.6 + 1.1.7 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From cb8c9c4408e45ab528b21c3ef108649b58d68474 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Wed, 26 Feb 2014 17:37:53 -0600 Subject: [PATCH 187/405] Make pcl_ros run_depend on libpcl-all-dev When downstream projects build against pcl_ros, they need the pcl headers provided by libpcl-all-dev. --- pcl_ros/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 1e2c3a83..4f6c3b90 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -35,7 +35,7 @@ message_filters nodelet nodelet_topic_tools - libpcl-all + libpcl-all-dev pcl_conversions pcl_msgs pluginlib From e6430884c6e17bf02edd818300d4530da7d4b881 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Feb 2014 17:37:26 -0800 Subject: [PATCH 188/405] pcl_ros: also run_depend on libpcl-all --- pcl_ros/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 4f6c3b90..682c4b88 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -35,6 +35,7 @@ message_filters nodelet nodelet_topic_tools + libpcl-all libpcl-all-dev pcl_conversions pcl_msgs From 1e6d600c097b15ad3250e96554d18483a4facea0 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Tue, 1 Apr 2014 18:01:31 -0700 Subject: [PATCH 189/405] Use cmake_modules to help find eigen on indigo --- pcl_ros/CMakeLists.txt | 1 + pcl_ros/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 48e41e40..9ee93674 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8) project(pcl_ros) ## Find system dependencies +find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen REQUIRED) find_package(PCL REQUIRED) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 682c4b88..172b16fc 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -16,6 +16,7 @@ catkin + cmake_modules dynamic_reconfigure eigen nodelet From c9d60b1de9c1eb17979929fbdfd8d19fcdd8a338 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 8 Apr 2014 23:34:41 +0000 Subject: [PATCH 190/405] update maintainer info --- pcl_ros/package.xml | 11 ++++++++--- perception_pcl/package.xml | 9 +++++++-- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 172b16fc..ec1c535d 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -8,12 +8,17 @@ processing in ROS. + Open Perception - William Woodall - Julius Kammerl + Julius Kammerl + William Woodall + Paul Bovbel + BSD http://ros.org/wiki/perception_pcl - + https://github.com/ros-perception/perception_pcl + https://github.com/ros-perception/perception_pcl/issues + catkin cmake_modules diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 1faec4ae..7d60776f 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -8,11 +8,16 @@ processing in ROS. + Open Perception - William Woodall - Julius Kammerl + Julius Kammerl + William Woodall + Paul Bovbel + BSD http://ros.org/wiki/perception_pcl + https://github.com/ros-perception/perception_pcl + https://github.com/ros-perception/perception_pcl/issues catkin From 407f8e4bf765a8f138b310186b962d32c9b98755 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Wed, 9 Apr 2014 00:02:26 +0000 Subject: [PATCH 191/405] remove redundant cmake file --- pcl_ros/cmake/FindEigen.cmake | 81 ----------------------------------- 1 file changed, 81 deletions(-) delete mode 100644 pcl_ros/cmake/FindEigen.cmake diff --git a/pcl_ros/cmake/FindEigen.cmake b/pcl_ros/cmake/FindEigen.cmake deleted file mode 100644 index 2666481c..00000000 --- a/pcl_ros/cmake/FindEigen.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN_FOUND - system has eigen lib with correct version -# EIGEN_INCLUDE_DIR - the eigen include directory -# EIGEN_VERSION - eigen version - -# Copyright (c) 2006, 2007 Montel Laurent, -# Copyright (c) 2008, 2009 Gael Guennebaud, -# Copyright (c) 2009 Benoit Jacob -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen_FIND_VERSION) - if(NOT Eigen_FIND_VERSION_MAJOR) - set(Eigen_FIND_VERSION_MAJOR 2) - endif(NOT Eigen_FIND_VERSION_MAJOR) - if(NOT Eigen_FIND_VERSION_MINOR) - set(Eigen_FIND_VERSION_MINOR 91) - endif(NOT Eigen_FIND_VERSION_MINOR) - if(NOT Eigen_FIND_VERSION_PATCH) - set(Eigen_FIND_VERSION_PATCH 0) - endif(NOT Eigen_FIND_VERSION_PATCH) - - set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") -endif(NOT Eigen_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) - if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) - set(EIGEN_VERSION_OK FALSE) - else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) - set(EIGEN_VERSION_OK TRUE) - endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) - - if(NOT EIGEN_VERSION_OK) - - message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " - "but at least version ${Eigen_FIND_VERSION} is required") - endif(NOT EIGEN_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN_INCLUDE_DIRS) - - # in cache already - _eigen3_check_version() - set(EIGEN_FOUND ${EIGEN_VERSION_OK}) - -else () - - find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - - if(EIGEN_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) - - mark_as_advanced(EIGEN_INCLUDE_DIR) - SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") - -endif() From 8a6fbbd99d02306b0e651b33a16fce1e76a6cd21 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Wed, 9 Apr 2014 00:05:32 +0000 Subject: [PATCH 192/405] remove redundant message files --- pcl_ros/msg/ModelCoefficients.msg | 3 --- pcl_ros/msg/PointIndices.msg | 3 --- pcl_ros/msg/PolygonMesh.msg | 6 ------ pcl_ros/msg/Vertices.msg | 2 -- 4 files changed, 14 deletions(-) delete mode 100644 pcl_ros/msg/ModelCoefficients.msg delete mode 100644 pcl_ros/msg/PointIndices.msg delete mode 100644 pcl_ros/msg/PolygonMesh.msg delete mode 100644 pcl_ros/msg/Vertices.msg diff --git a/pcl_ros/msg/ModelCoefficients.msg b/pcl_ros/msg/ModelCoefficients.msg deleted file mode 100644 index 8d3f9b89..00000000 --- a/pcl_ros/msg/ModelCoefficients.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -float32[] values - diff --git a/pcl_ros/msg/PointIndices.msg b/pcl_ros/msg/PointIndices.msg deleted file mode 100644 index 007c2900..00000000 --- a/pcl_ros/msg/PointIndices.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -int32[] indices - diff --git a/pcl_ros/msg/PolygonMesh.msg b/pcl_ros/msg/PolygonMesh.msg deleted file mode 100644 index 8eeb5a4c..00000000 --- a/pcl_ros/msg/PolygonMesh.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Separate header for the polygonal surface -Header header -# Vertices of the mesh as a point cloud -sensor_msgs/PointCloud2 cloud -# List of polygons -Vertices[] polygons diff --git a/pcl_ros/msg/Vertices.msg b/pcl_ros/msg/Vertices.msg deleted file mode 100644 index 6b7c72a0..00000000 --- a/pcl_ros/msg/Vertices.msg +++ /dev/null @@ -1,2 +0,0 @@ -# List of point indices -uint32[] vertices From c178c5a08dcb7b155e47e2553019a169831deca8 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Wed, 9 Apr 2014 15:47:14 -0400 Subject: [PATCH 193/405] Fixes #46 on indigo-devel Tested with velodyne data on indigo on Ubuntu 14.04 --- pcl_ros/tools/bag_to_pcd.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index b9114cf9..d1ec14e7 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -91,6 +91,7 @@ int view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2")); view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage")); + view.addQuery (bag, rosbag::TypeQuery ("tf2_msgs/TFMessage")); view_it = view.begin (); std::string output_dir = std::string (argv[3]); From e89fafe2e7a7894842fc920dbd2fae47a8208e93 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Wed, 9 Apr 2014 16:56:23 -0400 Subject: [PATCH 194/405] Updated changelog, maintainership, bugtracker and repository --- pcl_ros/CHANGELOG.rst | 6 ++++++ pcl_ros/package.xml | 10 +++++----- perception_pcl/CHANGELOG.rst | 4 ++++ perception_pcl/package.xml | 11 +++++------ 4 files changed, 20 insertions(+), 11 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index c0999c32..b9121af6 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.8 (2014-04-09) +------------------ +* Updated maintainership +* Fix TF2 support for bag_to_pcd `#46 `_ +* Use cmake_modules to find eigen on indigo `#45 `_ + 1.1.7 (2013-09-20) ------------------ * adding more uncaught config dependencies diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index ec1c535d..ec02afd1 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.7 + 1.1.8 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred @@ -10,14 +10,14 @@ Open Perception - Julius Kammerl + Julius Kammerl William Woodall Paul Bovbel - + Bill Morris BSD - http://ros.org/wiki/perception_pcl - https://github.com/ros-perception/perception_pcl + http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues + https://github.com/ros-perception/perception_pcl catkin diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index f73a27be..fa84529d 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.8 (2014-04-09) +------------------ +* Updated maintainership and added bugtracker/repository info to package.xml + 1.1.7 (2013-09-20) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 7d60776f..596487f5 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.7 + 1.1.8 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred @@ -8,16 +8,15 @@ processing in ROS. - Open Perception - Julius Kammerl William Woodall + Julius Kammerl Paul Bovbel - + Bill Morris BSD - http://ros.org/wiki/perception_pcl - https://github.com/ros-perception/perception_pcl + http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues + https://github.com/ros-perception/perception_pcl catkin From 84e17581ea3d6a0acbc5fb910f36880bc234e602 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Wed, 9 Apr 2014 17:41:19 -0400 Subject: [PATCH 195/405] changelog --- pcl_ros/CHANGELOG.rst | 2 +- perception_pcl/CHANGELOG.rst | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index b9121af6..96fefa8c 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.1.8 (2014-04-09) +1.2.0 (2014-04-09) ------------------ * Updated maintainership * Fix TF2 support for bag_to_pcd `#46 `_ diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index fa84529d..ea19748c 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.1.8 (2014-04-09) +1.2.0 (2014-04-09) ------------------ * Updated maintainership and added bugtracker/repository info to package.xml From aac231674ad42445b9f11f83f2715e4453ba3c91 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Wed, 9 Apr 2014 17:42:28 -0400 Subject: [PATCH 196/405] 1.2.0 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index ec02afd1..a1e3e4c0 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.1.8 + 1.2.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 596487f5..90fdb712 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.1.8 + 1.2.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 08e83a67bf72f4a409abf2e21712b86095e018f9 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Fri, 1 Aug 2014 07:02:36 -0400 Subject: [PATCH 197/405] merge pull request #60 --- pcl_ros/CMakeLists.txt | 2 + pcl_ros/cfg/CropBox.cfg | 25 +++++ pcl_ros/include/pcl_ros/filters/crop_box.h | 104 +++++++++++++++++++ pcl_ros/pcl_nodelets.xml | 46 +++++++++ pcl_ros/src/pcl_ros/filters/crop_box.cpp | 115 +++++++++++++++++++++ 5 files changed, 292 insertions(+) create mode 100755 pcl_ros/cfg/CropBox.cfg create mode 100644 pcl_ros/include/pcl_ros/filters/crop_box.h create mode 100644 pcl_ros/src/pcl_ros/filters/crop_box.cpp diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 9ee93674..c64b6a20 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -53,6 +53,7 @@ generate_dynamic_reconfigure_options( cfg/SegmentDifferences.cfg cfg/StatisticalOutlierRemoval.cfg cfg/VoxelGrid.cfg + cfg/CropBox.cfg ) ## Declare the catkin package @@ -125,6 +126,7 @@ add_library(pcl_ros_filters 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) diff --git a/pcl_ros/cfg/CropBox.cfg b/pcl_ros/cfg/CropBox.cfg new file mode 100755 index 00000000..8031493c --- /dev/null +++ b/pcl_ros/cfg/CropBox.cfg @@ -0,0 +1,25 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +import os +import sys +sys.path.insert(0, os.path.dirname(__file__)) + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator () +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("min_x", double_t, 0, "X coordinate of the minimum point of the box.", -1, -5, 5) +gen.add ("max_x", double_t, 0, "X coordinate of the maximum point of the box.", 1, -5, 5) +gen.add ("min_y", double_t, 0, "Y coordinate of the minimum point of the box.", -1, -5, 5) +gen.add ("max_y", double_t, 0, "Y coordinate of the maximum point of the box.", 1, -5, 5) +gen.add ("min_z", double_t, 0, "Z coordinate of the minimum point of the box.", -1, -5, 5) +gen.add ("max_z", double_t, 0, "Z coordinate of the maximum point of the box.", 1, -5, 5) +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 ("negative", bool_t, 0, "If True the box will be empty Else the remaining points will be the ones in the box", 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.", "") + +exit (gen.generate (PACKAGE, "pcl_ros", "CropBox")) diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.h b/pcl_ros/include/pcl_ros/filters/crop_box.h new file mode 100644 index 00000000..7fa08c99 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/crop_box.h @@ -0,0 +1,104 @@ +/* + * + * 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: cropbox.cpp + * + */ + +#ifndef PCL_ROS_FILTERS_CROPBOX_H_ +#define PCL_ROS_FILTERS_CROPBOX_H_ + +// PCL includes +#include +#include "pcl_ros/filters/filter.h" + +// Dynamic reconfigure +#include "pcl_ros/CropBoxConfig.h" + +namespace pcl_ros +{ + /** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box. + * + * \author Radu Bogdan Rusu + * \author Justin Rosen + * \author Marti Morta Garriga + */ + class CropBox : public Filter + { + protected: + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; // TODO + + /** \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 + */ + 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); + } + + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + bool + child_init (ros::NodeHandle &nh, bool &has_service); + + /** \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); + + private: + /** \brief The PCL filter implementation used. */ + pcl::CropBox impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_ diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml index e6cd0f90..8fbe4a68 100644 --- a/pcl_ros/pcl_nodelets.xml +++ b/pcl_ros/pcl_nodelets.xml @@ -1,3 +1,42 @@ +<<<<<<< HEAD +======= + + + + + + BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle criterion. The + code makes use of the estimated surface normals at each point in the input data set. + + + + + + FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset + containing points and normals. + + + + + + FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset + containing points and normals, in parallel, using the OpenMP standard. + + + + + + MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. + + + + + + NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and curvatures, + in parallel, using the OpenMP standard. + + +>>>>>>> c3c0d30... Merge pull request #60 from martimorta/hydro-devel @@ -81,6 +120,13 @@ StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. + + + + CropBox is a filter that allows the user to filter all the data inside of a given box. + + + diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp new file mode 100644 index 00000000..fc8acdad --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -0,0 +1,115 @@ +/* + * + * 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: cropbox.cpp + * + */ + +#include +#include "pcl_ros/filters/crop_box.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl_ros::CropBox::child_init (ros::NodeHandle &nh, bool &has_service) +{ + // Enable the dynamic reconfigure service + has_service = true; + srv_ = boost::make_shared > (nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&CropBox::config_callback, this, _1, _2); + srv_->setCallback (f); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock lock (mutex_); + + Eigen::Vector4f min_point,max_point; + min_point = impl_.getMin(); + max_point = impl_.getMax(); + + Eigen::Vector4f new_min_point, new_max_point; + new_min_point << config.min_x, config.min_y, config.min_z, 0.0; + new_max_point << config.max_x, config.max_y, config.max_z, 0.0; + + // Check the current values for minimum point + if (min_point != new_min_point) + { + NODELET_DEBUG ("[%s::config_callback] Setting the minimum point to: %f %f %f.", getName ().c_str (), new_min_point(0),new_min_point(1),new_min_point(2)); + // Set the filter min point if different + impl_.setMin(new_min_point); + } + // Check the current values for the maximum point + if (max_point != new_max_point) + { + NODELET_DEBUG ("[%s::config_callback] Setting the maximum point to: %f %f %f.", getName ().c_str (), new_max_point(0),new_max_point(1),new_max_point(2)); + // Set the filter max point if different + impl_.setMax(new_max_point); + } + + // Check the current value for keep_organized + if (impl_.getKeepOrganized () != config.keep_organized) + { + NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); + // Call the virtual method in the child + impl_.setKeepOrganized (config.keep_organized); + } + + // Check the current value for the negative flag + if (impl_.getNegative() != config.negative) + { + NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.negative ? "true" : "false"); + // Call the virtual method in the child + impl_.setNegative(config.negative); + } + + // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL + if (tf_input_frame_ != config.input_frame) + { + tf_input_frame_ = config.input_frame; + NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); + } + if (tf_output_frame_ != config.output_frame) + { + tf_output_frame_ = config.output_frame; + NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); + } +} + +typedef pcl_ros::CropBox CropBox; +PLUGINLIB_EXPORT_CLASS(CropBox,nodelet::Nodelet); + From 56c091eb38f2d081bd2f2f2a503f72029b673a18 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 13 Sep 2014 16:42:28 -0400 Subject: [PATCH 198/405] clean up merge --- pcl_ros/pcl_nodelets.xml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml index 8fbe4a68..cece6f06 100644 --- a/pcl_ros/pcl_nodelets.xml +++ b/pcl_ros/pcl_nodelets.xml @@ -1,6 +1,3 @@ -<<<<<<< HEAD -======= - @@ -36,8 +33,6 @@ in parallel, using the OpenMP standard. ->>>>>>> c3c0d30... Merge pull request #60 from martimorta/hydro-devel - From 1b4142147119e0c31af97ef850bbaa24dc58506c Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 13 Sep 2014 16:44:12 -0400 Subject: [PATCH 199/405] update changelogs --- pcl_ros/CHANGELOG.rst | 6 ++++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 9 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 96fefa8c..436e9a5c 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* clean up merge +* merge pull request `#60 `_ +* Contributors: Paul Bovbel + 1.2.0 (2014-04-09) ------------------ * Updated maintainership diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index ea19748c..1259b20b 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2014-04-09) ------------------ * Updated maintainership and added bugtracker/repository info to package.xml From c8da9866e48ee197c8f7c4e65dfea1c3597b21b1 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 13 Sep 2014 16:44:18 -0400 Subject: [PATCH 200/405] 1.2.1 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 436e9a5c..2abae57c 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.1 (2014-09-13) +------------------ * clean up merge * merge pull request `#60 `_ * Contributors: Paul Bovbel diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index a1e3e4c0..98c9154a 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.2.0 + 1.2.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 1259b20b..5fb2aa21 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.1 (2014-09-13) +------------------ 1.2.0 (2014-04-09) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 90fdb712..4561b33c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.2.0 + 1.2.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From fa30e01255a80161b953cbd3d7f4fccd2090e169 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 20 Sep 2014 13:17:31 -0400 Subject: [PATCH 201/405] Fix issue #44 --- pcl_ros/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 98c9154a..e216329a 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -35,6 +35,8 @@ sensor_msgs std_msgs tf + python-vtk + libvtk-java dynamic_reconfigure eigen @@ -51,6 +53,8 @@ sensor_msgs std_msgs tf + python-vtk + libvtk-java From 85241bcb537b44339c0d4efc4171344115f43648 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 20 Sep 2014 16:30:19 -0400 Subject: [PATCH 202/405] Initial pointcloud to laserscan commit --- pointcloud_to_laserscan/CMakeLists.txt | 94 +++++++++++++++++++ .../PointCloudToLaserScanBase.h | 88 +++++++++++++++++ pointcloud_to_laserscan/package.xml | 67 +++++++++++++ .../src/PointCloudToLaserScanBase.cpp | 44 +++++++++ .../src/PointCloudToLaserScanNodelet.cpp | 0 .../src/pointcloud_to_laserscan_node.cpp | 16 ++++ 6 files changed, 309 insertions(+) create mode 100644 pointcloud_to_laserscan/CMakeLists.txt create mode 100644 pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h create mode 100644 pointcloud_to_laserscan/package.xml create mode 100644 pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp create mode 100644 pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp create mode 100644 pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt new file mode 100644 index 00000000..6018a529 --- /dev/null +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pointcloud_to_laserscan) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + dynamic_reconfigure + message_filters + nodelet + pcl_ros + roscpp + sensor_msgs +) +find_package(PCL REQUIRED) + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES pointcloud_to_laserscan +# CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs +# DEPENDS system_lib +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(PointCloudToLaserScan src/PointCloudToLaserScanBase.cpp) +#add_dependencies(DepthImageToLaserScanROS ${PROJECT_NAME}_gencfg) +target_link_libraries(PointCloudToLaserScan ${catkin_LIBRARIES}) + +add_executable(pointcloud_to_laserscan src/pointcloud_to_laserscan_node.cpp) +#add_dependencies(pointcloud_to_laserscan pointcloud_to_laserscan_generate_messages_cpp) +target_link_libraries(pointcloud_to_laserscan PointCloudToLaserScan ${catkin_LIBRARIES}) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_pointcloud_to_laserscan.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h new file mode 100644 index 00000000..9bc12ffd --- /dev/null +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h @@ -0,0 +1,88 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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. + */ + +/* + * Author: Chad Rockey + */ + +#ifndef POINTCLOUD_TO_LASERSCAN_ROS +#define POINTCLOUD_TO_LASERSCAN_ROS + +#include +#include + +#include +#include +#include +#include + +#include + +namespace pointcloud_to_laserscan +{ +typedef pcl::PointXYZ Point; +typedef pcl::PointCloud PointCloud; + + +class PointCloudToLaserScanBase +{ +public: + + + PointCloudToLaserScanBase(ros::NodeHandle& n, ros::NodeHandle& pnh, const unsigned int concurrency); + + ~PointCloudToLaserScanBase(); + +private: + + void cloudCb(const PointCloud::ConstPtr& cloud); + + void connectCb(); + + void disconnectCb(); + + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + + ros::Publisher pub_; + ros::Subscriber sub_; + + tf::TransformListener tf_; + + boost::mutex connect_mutex_; + const unsigned int concurrency_; + + std::string target_frame_; + float tolerance_; +}; + + +} // pointcloud_to_laserscan + +#endif diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml new file mode 100644 index 00000000..228a606c --- /dev/null +++ b/pointcloud_to_laserscan/package.xml @@ -0,0 +1,67 @@ + + + pointcloud_to_laserscan + 0.0.0 + The pointcloud_to_laserscan package + + + + + paul + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + dynamic_reconfigure + libpcl-all-dev + message_filters + nodelet + pcl_ros + roscpp + sensor_msgs + dynamic_reconfigure + libpcl-all-dev + message_filters + nodelet + pcl_ros + roscpp + sensor_msgs + + + + + + + + + + + \ No newline at end of file diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp new file mode 100644 index 00000000..3374c9f1 --- /dev/null +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp @@ -0,0 +1,44 @@ +#include +#include + + +namespace pointcloud_to_laserscan +{ + +PointCloudToLaserScanBase::PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh, const unsigned int concurrency) : + nh_(nh), private_nh_(private_nh), concurrency_(concurrency) +{ + boost::mutex::scoped_lock lock(connect_mutex_); + pub_ = nh.advertise("scan", 10, boost::bind(&PointCloudToLaserScanBase::connectCb, this), boost::bind(&PointCloudToLaserScanBase::disconnectCb, this)); + + //todo params + target_frame_ = "base_link"; + +} + +PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){ + sub_.shutdown(); +} + +void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud){ + +} + + +void PointCloudToLaserScanBase::connectCb() { + boost::mutex::scoped_lock lock(connect_mutex_); + if (!sub_ && pub_.getNumSubscribers() > 0) { + ROS_DEBUG("Connecting to depth topic."); + sub_ = nh_.subscribe("cloud_in", concurrency_, &PointCloudToLaserScanBase::cloudCb, this); + } +} + +void PointCloudToLaserScanBase::disconnectCb() { + boost::mutex::scoped_lock lock(connect_mutex_); + if (pub_.getNumSubscribers() == 0) { + ROS_DEBUG("Unsubscribing from depth topic."); + sub_.shutdown(); + } +} + +} // pointcloud_to_laserscan diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp new file mode 100644 index 00000000..e69de29b diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp new file mode 100644 index 00000000..8c56f448 --- /dev/null +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp @@ -0,0 +1,16 @@ +#include +#include + +int main(int argc, char **argv){ + ros::init(argc, argv, "pointcloud_to_laserscan"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + unsigned int concurrency = 4; + + pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, nh_private, concurrency); + + ros::spin(); + + return 0; +} From dc1cf04b07b4064cd14f19728e78129caab4dc79 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 5 Oct 2014 14:55:00 -0400 Subject: [PATCH 203/405] Finalize pointcloud to laserscan --- pointcloud_to_laserscan/CMakeLists.txt | 80 ++------ .../PointCloudToLaserScanBase.h | 72 ++++--- .../launch/sample_node.launch | 24 +++ .../launch/sample_nodelet.launch | 24 +++ pointcloud_to_laserscan/nodelets.xml | 11 + .../src/PointCloudToLaserScanBase.cpp | 193 +++++++++++++++--- .../src/PointCloudToLaserScanNodelet.cpp | 76 +++++++ .../src/pointcloud_to_laserscan_node.cpp | 54 ++++- 8 files changed, 408 insertions(+), 126 deletions(-) create mode 100644 pointcloud_to_laserscan/launch/sample_node.launch create mode 100644 pointcloud_to_laserscan/launch/sample_nodelet.launch create mode 100644 pointcloud_to_laserscan/nodelets.xml diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 6018a529..076adb62 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(pointcloud_to_laserscan) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_filters @@ -14,21 +11,10 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(PCL REQUIRED) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include -# LIBRARIES pointcloud_to_laserscan -# CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs -# DEPENDS system_lib + LIBRARIES PointCloudToLaserScan PointCloudToLaserScanNodelet + CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs ) include_directories( @@ -37,58 +23,22 @@ include_directories( ) add_library(PointCloudToLaserScan src/PointCloudToLaserScanBase.cpp) -#add_dependencies(DepthImageToLaserScanROS ${PROJECT_NAME}_gencfg) target_link_libraries(PointCloudToLaserScan ${catkin_LIBRARIES}) +add_library(PointCloudToLaserScanNodelet src/PointCloudToLaserScanNodelet.cpp) +target_link_libraries(PointCloudToLaserScanNodelet PointCloudToLaserScan ${catkin_LIBRARIES}) + add_executable(pointcloud_to_laserscan src/pointcloud_to_laserscan_node.cpp) -#add_dependencies(pointcloud_to_laserscan pointcloud_to_laserscan_generate_messages_cpp) target_link_libraries(pointcloud_to_laserscan PointCloudToLaserScan ${catkin_LIBRARIES}) +install(TARGETS PointCloudToLaserScan PointCloudToLaserScanNodelet pointcloud_to_laserscan + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_pointcloud_to_laserscan.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h index 9bc12ffd..7e5b9ba5 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h @@ -1,34 +1,41 @@ /* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. + * Software License Agreement (BSD License) * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * Copyright (c) 2010-2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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. * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the 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. */ -/* - * Author: Chad Rockey +/* + * Author: Paul Bovbel */ #ifndef POINTCLOUD_TO_LASERSCAN_ROS @@ -47,15 +54,18 @@ namespace pointcloud_to_laserscan { typedef pcl::PointXYZ Point; -typedef pcl::PointCloud PointCloud; - +typedef pcl::PointCloud PointCloud; +/** +* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot +* pointcloud_to_laserscan implementation. +*/ class PointCloudToLaserScanBase { public: - PointCloudToLaserScanBase(ros::NodeHandle& n, ros::NodeHandle& pnh, const unsigned int concurrency); + PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh); ~PointCloudToLaserScanBase(); @@ -76,13 +86,13 @@ private: tf::TransformListener tf_; boost::mutex connect_mutex_; - const unsigned int concurrency_; + unsigned int input_queue_size_; std::string target_frame_; - float tolerance_; + double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_; + bool use_inf_; }; - } // pointcloud_to_laserscan #endif diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch new file mode 100644 index 00000000..91f7f186 --- /dev/null +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch new file mode 100644 index 00000000..bd4372a7 --- /dev/null +++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/pointcloud_to_laserscan/nodelets.xml b/pointcloud_to_laserscan/nodelets.xml new file mode 100644 index 00000000..c1143973 --- /dev/null +++ b/pointcloud_to_laserscan/nodelets.xml @@ -0,0 +1,11 @@ + + + + + Nodelet to convert sensor_msgs/PointCloud2 to sensor_msgs/LaserScans. + + + + diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp index 3374c9f1..8caaf208 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp @@ -1,44 +1,185 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010-2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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. + * + * + */ + +/* + * Author: Paul Bovbel + */ + #include #include - +#include +#include namespace pointcloud_to_laserscan -{ - -PointCloudToLaserScanBase::PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh, const unsigned int concurrency) : - nh_(nh), private_nh_(private_nh), concurrency_(concurrency) { - boost::mutex::scoped_lock lock(connect_mutex_); - pub_ = nh.advertise("scan", 10, boost::bind(&PointCloudToLaserScanBase::connectCb, this), boost::bind(&PointCloudToLaserScanBase::disconnectCb, this)); - //todo params - target_frame_ = "base_link"; + PointCloudToLaserScanBase::PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh) : + nh_(nh), private_nh_(private_nh) + { -} + private_nh.param("target_frame", target_frame_, "base_link"); + private_nh_.param("min_height", min_height_, 0.0); + private_nh_.param("max_height", max_height_, 1.0); -PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){ - sub_.shutdown(); -} + private_nh_.param("angle_min", angle_min_, -M_PI/2.0); + private_nh_.param("angle_max", angle_max_, M_PI/2.0); + private_nh_.param("angle_increment", angle_increment_, M_PI/360.0); + private_nh_.param("scan_time", scan_time_, 1.0/30.0); + private_nh_.param("range_min", range_min_, 0.45); + private_nh_.param("range_max", range_max_, 4.0); -void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud){ + bool use_concurrency; + private_nh_.param("use_concurrency", use_concurrency, true); + private_nh_.param("use_inf", use_inf_, true); -} + boost::mutex::scoped_lock lock(connect_mutex_); + + //Only allow #threads pointclouds to wait in queue + if(use_concurrency){ + input_queue_size_ = boost::thread::hardware_concurrency(); + }else{ + input_queue_size_ = 1; + } + + pub_ = nh.advertise("scan", 10, + boost::bind(&PointCloudToLaserScanBase::connectCb, this), + boost::bind(&PointCloudToLaserScanBase::disconnectCb, this)); -void PointCloudToLaserScanBase::connectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (!sub_ && pub_.getNumSubscribers() > 0) { - ROS_DEBUG("Connecting to depth topic."); - sub_ = nh_.subscribe("cloud_in", concurrency_, &PointCloudToLaserScanBase::cloudCb, this); } -} -void PointCloudToLaserScanBase::disconnectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) { - ROS_DEBUG("Unsubscribing from depth topic."); + PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){ sub_.shutdown(); } -} + + + void PointCloudToLaserScanBase::connectCb() { + boost::mutex::scoped_lock lock(connect_mutex_); + if (!sub_ && pub_.getNumSubscribers() > 0) { + ROS_DEBUG("Connecting to depth topic."); + sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanBase::cloudCb, this); + } + } + + void PointCloudToLaserScanBase::disconnectCb() { + boost::mutex::scoped_lock lock(connect_mutex_); + if (pub_.getNumSubscribers() == 0) { + ROS_DEBUG("Unsubscribing from depth topic."); + sub_.shutdown(); + } + } + + void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud_msg){ + + //pointer to pointcloud data to transform to laserscan + PointCloud::ConstPtr cloud_scan; + + std_msgs::Header cloud_in_header = pcl_conversions::fromPCL(cloud_scan->header); + + //decide if pointcloud needs to be transformed to a target frame + if(!target_frame_.empty() && cloud_in_header.frame_id != target_frame_){ + bool found_transform = tf_.waitForTransform(cloud_in_header.frame_id, target_frame_, cloud_in_header.stamp, ros::Duration(10.0)); + if(found_transform){ + PointCloud::Ptr cloud_tf(new PointCloud); + pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_); + cloud_scan = cloud_tf; + }else{ + ROS_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_in_header.frame_id << " into lasercan with frame " << target_frame_); + return; + } + }else{ + cloud_scan = cloud_msg; + } + + //build laserscan output + sensor_msgs::LaserScan output; + output.header = cloud_in_header; + output.header.frame_id = target_frame_; + output.angle_min = angle_min_; + output.angle_max = angle_max_; + output.angle_increment = angle_increment_; + output.time_increment = 0.0; + output.scan_time = scan_time_; + output.range_min = range_min_; + output.range_max = range_max_; + + uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); + + //determine if laserscan rays with no obstacle data will evaluate to infinity + if(use_inf_){ + output.ranges.assign(ranges_size, std::numeric_limits::infinity()); + }else{ + output.ranges.assign(ranges_size, output.range_max + 1.0); + } + + for (PointCloud::const_iterator it = cloud_scan->begin(); it != cloud_scan->end(); ++it){ + const float &x = it->x; + const float &y = it->y; + const float &z = it->z; + + if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){ + //NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); + continue; + } + + if (z > max_height_ || z < min_height_){ + //NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_); + continue; + } + + double range = hypot(x,y); + if (range < range_min_){ + //NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z); + continue; + } + + double angle = atan2(y, x); + if (angle < output.angle_min || angle > output.angle_max){ + //NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); + continue; + } + + //overwrite range at laserscan ray if new range is smaller + int index = (angle - output.angle_min) / output.angle_increment; + if (range < output.ranges[index]){ + output.ranges[index] = range; + } + } + pub_.publish(output); + + } } // pointcloud_to_laserscan diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp index e69de29b..b420ed1c 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp @@ -0,0 +1,76 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010-2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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. + * + * + */ + +/* + * Author: Paul Bovbel + */ + +#include +#include + + +namespace pointcloud_to_laserscan +{ + + class PointCloudToLaserScanNodelet : public nodelet::Nodelet + { + public: + PointCloudToLaserScanNodelet() {}; + + ~PointCloudToLaserScanNodelet() {} + + private: + virtual void onInit() + { + bool use_concurrency; + getPrivateNodeHandle().param("use_concurrency", use_concurrency, false); + + if(use_concurrency){ + cloud_to_scan.reset(new PointCloudToLaserScanBase(getMTNodeHandle(), getPrivateNodeHandle())); + }else{ + cloud_to_scan.reset(new PointCloudToLaserScanBase(getNodeHandle(), getPrivateNodeHandle())); + } + + }; + + boost::shared_ptr cloud_to_scan; + }; + +} + +#include +PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet); + diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp index 8c56f448..76661bea 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp @@ -1,16 +1,62 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010-2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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. + * + * + */ + +/* + * Author: Paul Bovbel + */ + #include #include int main(int argc, char **argv){ ros::init(argc, argv, "pointcloud_to_laserscan"); ros::NodeHandle nh; - ros::NodeHandle nh_private("~"); + ros::NodeHandle private_nh("~"); + bool use_concurrency; - unsigned int concurrency = 4; + private_nh.param("use_concurrency", use_concurrency, false); - pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, nh_private, concurrency); + pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, private_nh); - ros::spin(); + if(use_concurrency) { + ros::MultiThreadedSpinner spinner; + spinner.spin(); + }else{ + ros::spin(); + } return 0; } From f44eaf2fa52874219452744cc0feba61d38cb7c8 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Wed, 8 Oct 2014 09:42:50 -0400 Subject: [PATCH 204/405] Fix pointer assertion --- pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp index 8caaf208..a9d6e2a2 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp @@ -107,7 +107,7 @@ namespace pointcloud_to_laserscan //pointer to pointcloud data to transform to laserscan PointCloud::ConstPtr cloud_scan; - std_msgs::Header cloud_in_header = pcl_conversions::fromPCL(cloud_scan->header); + std_msgs::Header cloud_in_header = pcl_conversions::fromPCL(cloud_msg->header); //decide if pointcloud needs to be transformed to a target frame if(!target_frame_.empty() && cloud_in_header.frame_id != target_frame_){ From 11e2ff079798b0f97e5b122d4f9c6b633540e8e9 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Wed, 8 Oct 2014 09:50:06 -0400 Subject: [PATCH 205/405] Fix flow --- .../src/PointCloudToLaserScanBase.cpp | 43 ++++++++++--------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp index a9d6e2a2..46acb4c9 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp @@ -107,12 +107,24 @@ namespace pointcloud_to_laserscan //pointer to pointcloud data to transform to laserscan PointCloud::ConstPtr cloud_scan; - std_msgs::Header cloud_in_header = pcl_conversions::fromPCL(cloud_msg->header); + std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header); + + //build laserscan output + sensor_msgs::LaserScan output; + output.header = cloud_header; + output.angle_min = angle_min_; + output.angle_max = angle_max_; + output.angle_increment = angle_increment_; + output.time_increment = 0.0; + output.scan_time = scan_time_; + output.range_min = range_min_; + output.range_max = range_max_; //decide if pointcloud needs to be transformed to a target frame - if(!target_frame_.empty() && cloud_in_header.frame_id != target_frame_){ - bool found_transform = tf_.waitForTransform(cloud_in_header.frame_id, target_frame_, cloud_in_header.stamp, ros::Duration(10.0)); - if(found_transform){ + if(!target_frame_.empty() && cloud_header.frame_id != target_frame_){ + output.header.frame_id = target_frame_; + + if(tf_.waitForTransform(cloud_header.frame_id, target_frame_, cloud_header.stamp, ros::Duration(10.0))){ PointCloud::Ptr cloud_tf(new PointCloud); pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_); cloud_scan = cloud_tf; @@ -124,21 +136,10 @@ namespace pointcloud_to_laserscan cloud_scan = cloud_msg; } - //build laserscan output - sensor_msgs::LaserScan output; - output.header = cloud_in_header; - output.header.frame_id = target_frame_; - output.angle_min = angle_min_; - output.angle_max = angle_max_; - output.angle_increment = angle_increment_; - output.time_increment = 0.0; - output.scan_time = scan_time_; - output.range_min = range_min_; - output.range_max = range_max_; - + //determine amount of rays to create uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); - //determine if laserscan rays with no obstacle data will evaluate to infinity + //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range if(use_inf_){ output.ranges.assign(ranges_size, std::numeric_limits::infinity()); }else{ @@ -151,24 +152,24 @@ namespace pointcloud_to_laserscan const float &z = it->z; if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){ - //NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); + //ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); continue; } if (z > max_height_ || z < min_height_){ - //NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_); + //ROS_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_); continue; } double range = hypot(x,y); if (range < range_min_){ - //NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z); + //ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z); continue; } double angle = atan2(y, x); if (angle < output.angle_min || angle > output.angle_max){ - //NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); + //ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); continue; } From 0e73b421c92d0309ba380ff26d924b869aab24c6 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Wed, 8 Oct 2014 11:56:42 -0400 Subject: [PATCH 206/405] Fix header reference --- pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp index 46acb4c9..721695fe 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp @@ -129,7 +129,7 @@ namespace pointcloud_to_laserscan pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_); cloud_scan = cloud_tf; }else{ - ROS_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_in_header.frame_id << " into lasercan with frame " << target_frame_); + ROS_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_header.frame_id << " into lasercan with frame " << target_frame_); return; } }else{ From 155a69e484673860313a616da15fede14335c751 Mon Sep 17 00:00:00 2001 From: Matt Derry Date: Mon, 13 Oct 2014 11:39:10 -0500 Subject: [PATCH 207/405] Update pcl_nodelets.xml Included missing closing library tag. This was causing the pcl/Filter nodelets below the missing nodelet tag to not be exported correctly. --- pcl_ros/pcl_nodelets.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml index cece6f06..2be8cace 100644 --- a/pcl_ros/pcl_nodelets.xml +++ b/pcl_ros/pcl_nodelets.xml @@ -33,6 +33,7 @@ in parallel, using the OpenMP standard. + From ac5593dd7c7f1ca3e8290b65070ab1684d9dee03 Mon Sep 17 00:00:00 2001 From: Ruffin Date: Sat, 18 Oct 2014 17:59:47 -0400 Subject: [PATCH 208/405] Adding target_frame [Ability to specify frame in bag_to_pcd #55](https://github.com/ros-perception/perception_pcl/issues/55) --- pcl_ros/tools/bag_to_pcd.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index d1ec14e7..b07f03bb 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -66,8 +66,8 @@ int ros::init (argc, argv, "bag_to_pcd"); if (argc < 4) { - std::cerr << "Syntax is: " << argv[0] << " " << std::endl; - std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds" << std::endl; + std::cerr << "Syntax is: " << argv[0] << " []" << std::endl; + std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl; return (-1); } @@ -132,11 +132,20 @@ int continue; } - // Transform it - if (!pcl_ros::transformPointCloud ("/base_link", *cloud, cloud_t, tf_listener)) + // If a target_frame was specified + if(argc > 4) { - ++view_it; - continue; + // Transform it + if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener)) + { + ++view_it; + continue; + } + } + else + { + // Else, don't transform it + cloud_t = *cloud; } std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl; From 9537f5f1c499d456702d97efd98f3a981cafb74b Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 25 Oct 2014 12:23:33 -0400 Subject: [PATCH 209/405] clean up package.xml --- pointcloud_to_laserscan/package.xml | 83 ++++++++--------------------- 1 file changed, 23 insertions(+), 60 deletions(-) diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 228a606c..8b3f0189 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -1,67 +1,30 @@ - pointcloud_to_laserscan - 0.0.0 - The pointcloud_to_laserscan package + pointcloud_to_laserscan + 1.2.1 + Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM). - - - - paul + Paul Bovbel + Tully Foote + BSD - - - - TODO + catkin + dynamic_reconfigure + libpcl-all-dev + message_filters + nodelet + pcl_ros + roscpp + sensor_msgs + dynamic_reconfigure + libpcl-all-dev + message_filters + nodelet + pcl_ros + roscpp + sensor_msgs - - - - - - - - - - - - - - - - - - - - - - - - - catkin - dynamic_reconfigure - libpcl-all-dev - message_filters - nodelet - pcl_ros - roscpp - sensor_msgs - dynamic_reconfigure - libpcl-all-dev - message_filters - nodelet - pcl_ros - roscpp - sensor_msgs - - - - - - - - - - + + \ No newline at end of file From 841ce1f77cc0a18881c71eef102b2dd16190a1b6 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 25 Oct 2014 12:46:23 -0400 Subject: [PATCH 210/405] update changelogs --- pcl_ros/CHANGELOG.rst | 8 ++++++++ perception_pcl/CHANGELOG.rst | 3 +++ pointcloud_to_laserscan/CHANGELOG.rst | 13 +++++++++++++ 3 files changed, 24 insertions(+) create mode 100644 pointcloud_to_laserscan/CHANGELOG.rst diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 2abae57c..4edbed1b 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adding target_frame + [Ability to specify frame in bag_to_pcd ](https://github.com/ros-perception/perception_pcl/issues/55) +* Update pcl_nodelets.xml + Included missing closing library tag. This was causing the pcl/Filter nodelets below the missing nodelet tag to not be exported correctly. +* Contributors: Matt Derry, Paul Bovbel, Ruffin + 1.2.1 (2014-09-13) ------------------ * clean up merge diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 5fb2aa21..266bc1de 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.1 (2014-09-13) ------------------ diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst new file mode 100644 index 00000000..273f4913 --- /dev/null +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -0,0 +1,13 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pointcloud_to_laserscan +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* clean up package.xml +* Fix header reference +* Fix flow +* Fix pointer assertion +* Finalize pointcloud to laserscan +* Initial pointcloud to laserscan commit +* Contributors: Paul Bovbel \ No newline at end of file From 8010ae7ba86977df2ec3715bc72d899b54701411 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 25 Oct 2014 13:10:07 -0400 Subject: [PATCH 211/405] 1.2.2 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- pointcloud_to_laserscan/CHANGELOG.rst | 4 ++-- pointcloud_to_laserscan/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 4edbed1b..5d0a7cd7 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.2 (2014-10-25) +------------------ * Adding target_frame [Ability to specify frame in bag_to_pcd ](https://github.com/ros-perception/perception_pcl/issues/55) * Update pcl_nodelets.xml diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index e216329a..36cc2860 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.2.1 + 1.2.2 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 266bc1de..2c58b832 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.2 (2014-10-25) +------------------ 1.2.1 (2014-09-13) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 4561b33c..573265f3 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.2.1 + 1.2.2 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index 273f4913..09d7ab9d 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.2 (2014-10-25) +------------------ * clean up package.xml * Fix header reference * Fix flow diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 8b3f0189..bba350b0 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -1,7 +1,7 @@ pointcloud_to_laserscan - 1.2.1 + 1.2.2 Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM). Paul Bovbel From 8d7192c12057629838286ca595fe7b0745393bd1 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 26 Oct 2014 11:36:26 -0400 Subject: [PATCH 212/405] clean up package.xml --- perception_pcl/package.xml | 2 +- pointcloud_to_laserscan/package.xml | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 573265f3..49b7ded2 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -23,7 +23,7 @@ pcl_conversions pcl_msgs pcl_ros - + pointcloud_to_laserscan diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index bba350b0..84af2eae 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -8,6 +8,10 @@ Tully Foote BSD + + http://ros.org/wiki/perception_pcl + https://github.com/ros-perception/perception_pcl/issues + https://github.com/ros-perception/perception_pcl catkin dynamic_reconfigure From e7a67030cb67fbf9ebfe0b91b8fa2101a316de76 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 27 Oct 2014 12:49:03 -0400 Subject: [PATCH 213/405] set default target frame to empty --- pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp index 721695fe..dce3e669 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp @@ -50,7 +50,7 @@ namespace pointcloud_to_laserscan nh_(nh), private_nh_(private_nh) { - private_nh.param("target_frame", target_frame_, "base_link"); + private_nh.param("target_frame", target_frame_, ""); private_nh_.param("min_height", min_height_, 0.0); private_nh_.param("max_height", max_height_, 1.0); From a41d54b3ebaaa9b0f538abc892d13c0e885e7b16 Mon Sep 17 00:00:00 2001 From: Dani Carbonell Date: Thu, 20 Nov 2014 11:51:34 +0100 Subject: [PATCH 214/405] Update common.py MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Extended filter limits up to ±100000.0 in order to support intensity channel filtering. --- pcl_ros/cfg/common.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/cfg/common.py b/pcl_ros/cfg/common.py index de839a8a..e04347ab 100644 --- a/pcl_ros/cfg/common.py +++ b/pcl_ros/cfg/common.py @@ -8,8 +8,8 @@ from dynamic_reconfigure.parameter_generator_catkin import *; 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", 0.0, -1000.0, 1000.0) - gen.add ("filter_limit_max", double_t, 0, "The maximum allowed field value a point will be considered from", 1.0, -1000.0, 1000.0) + 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", 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].", 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.", 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.", "") From a70e770af53acd9a282e62a1137f7104dd54daa1 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 6 Jan 2015 13:19:57 -0500 Subject: [PATCH 215/405] refactor naming and fix nodelet export --- pointcloud_to_laserscan/CMakeLists.txt | 15 ++++++--------- ...rScanBase.h => pointcloud_to_laserscan_base.h} | 0 pointcloud_to_laserscan/launch/sample_node.launch | 2 +- .../launch/sample_nodelet.launch | 2 +- pointcloud_to_laserscan/nodelets.xml | 4 ++-- pointcloud_to_laserscan/package.xml | 1 + ...nBase.cpp => pointcloud_to_laserscan_base.cpp} | 2 +- .../src/pointcloud_to_laserscan_node.cpp | 2 +- ...et.cpp => pointcloud_to_laserscan_nodelet.cpp} | 2 +- 9 files changed, 14 insertions(+), 16 deletions(-) rename pointcloud_to_laserscan/include/pointcloud_to_laserscan/{PointCloudToLaserScanBase.h => pointcloud_to_laserscan_base.h} (100%) rename pointcloud_to_laserscan/src/{PointCloudToLaserScanBase.cpp => pointcloud_to_laserscan_base.cpp} (99%) rename pointcloud_to_laserscan/src/{PointCloudToLaserScanNodelet.cpp => pointcloud_to_laserscan_nodelet.cpp} (97%) diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 076adb62..003aa9a8 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(PCL REQUIRED) catkin_package( INCLUDE_DIRS include - LIBRARIES PointCloudToLaserScan PointCloudToLaserScanNodelet + LIBRARIES pointcloud_to_laserscan CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs ) @@ -22,16 +22,13 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(PointCloudToLaserScan src/PointCloudToLaserScanBase.cpp) -target_link_libraries(PointCloudToLaserScan ${catkin_LIBRARIES}) +add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_base.cpp src/pointcloud_to_laserscan_nodelet.cpp) +target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES}) -add_library(PointCloudToLaserScanNodelet src/PointCloudToLaserScanNodelet.cpp) -target_link_libraries(PointCloudToLaserScanNodelet PointCloudToLaserScan ${catkin_LIBRARIES}) +add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp) +target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES}) -add_executable(pointcloud_to_laserscan src/pointcloud_to_laserscan_node.cpp) -target_link_libraries(pointcloud_to_laserscan PointCloudToLaserScan ${catkin_LIBRARIES}) - -install(TARGETS PointCloudToLaserScan PointCloudToLaserScanNodelet pointcloud_to_laserscan +install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h similarity index 100% rename from pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h rename to pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch index 91f7f186..3161f0a9 100644 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -10,7 +10,7 @@ - + diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch index bd4372a7..d8b00f99 100644 --- a/pointcloud_to_laserscan/launch/sample_nodelet.launch +++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch @@ -10,7 +10,7 @@ - + diff --git a/pointcloud_to_laserscan/nodelets.xml b/pointcloud_to_laserscan/nodelets.xml index c1143973..327a819e 100644 --- a/pointcloud_to_laserscan/nodelets.xml +++ b/pointcloud_to_laserscan/nodelets.xml @@ -1,6 +1,6 @@ - + - diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 84af2eae..f6f0d7ce 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -30,5 +30,6 @@ sensor_msgs + \ No newline at end of file diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp similarity index 99% rename from pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp rename to pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp index dce3e669..40d2d840 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp @@ -38,7 +38,7 @@ * Author: Paul Bovbel */ -#include +#include #include #include #include diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp index 76661bea..b8acb363 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp @@ -39,7 +39,7 @@ */ #include -#include +#include int main(int argc, char **argv){ ros::init(argc, argv, "pointcloud_to_laserscan"); diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp similarity index 97% rename from pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp rename to pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index b420ed1c..2ce17dd6 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -38,7 +38,7 @@ * Author: Paul Bovbel */ -#include +#include #include From 091682c44a0b98b20b176ba58465ca38f09c4f14 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 6 Jan 2015 13:28:20 -0500 Subject: [PATCH 216/405] add launch tests --- pointcloud_to_laserscan/CMakeLists.txt | 11 +++++++++-- pointcloud_to_laserscan/package.xml | 2 ++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 003aa9a8..1bca25ae 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros roscpp sensor_msgs + roslaunch ) find_package(PCL REQUIRED) @@ -28,14 +29,20 @@ target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES}) add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp) target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES}) +roslaunch_add_file_check(launch) + install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) install(FILES nodelets.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index f6f0d7ce..20432b12 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -20,6 +20,7 @@ nodelet pcl_ros roscpp + roslaunch sensor_msgs dynamic_reconfigure libpcl-all-dev @@ -28,6 +29,7 @@ pcl_ros roscpp sensor_msgs + openni2_launch From 0d34c85f135ad4c1caaf1e2ff50f8b5389614f90 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 10 Jan 2015 13:25:18 -0500 Subject: [PATCH 217/405] update changelogs --- pcl_ros/CHANGELOG.rst | 6 ++++++ perception_pcl/CHANGELOG.rst | 5 +++++ pointcloud_to_laserscan/CHANGELOG.rst | 8 ++++++++ 3 files changed, 19 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 5d0a7cd7..4298fac2 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update common.py + Extended filter limits up to ±100000.0 in order to support intensity channel filtering. +* Contributors: Dani Carbonell + 1.2.2 (2014-10-25) ------------------ * Adding target_frame diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 2c58b832..e2987021 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* clean up package.xml +* Contributors: Paul Bovbel + 1.2.2 (2014-10-25) ------------------ diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index 09d7ab9d..81bb3e25 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add launch tests +* refactor naming and fix nodelet export +* set default target frame to empty +* clean up package.xml +* Contributors: Paul Bovbel + 1.2.2 (2014-10-25) ------------------ * clean up package.xml From d604c43332056d6f6547bd5cffde1691a20cfe42 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 10 Jan 2015 13:25:25 -0500 Subject: [PATCH 218/405] 1.2.3 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- pointcloud_to_laserscan/CHANGELOG.rst | 4 ++-- pointcloud_to_laserscan/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 4298fac2..7d834195 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.3 (2015-01-10) +------------------ * Update common.py Extended filter limits up to ±100000.0 in order to support intensity channel filtering. * Contributors: Dani Carbonell diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 36cc2860..7b98d309 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.2.2 + 1.2.3 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index e2987021..820e3dc0 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.3 (2015-01-10) +------------------ * clean up package.xml * Contributors: Paul Bovbel diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 49b7ded2..13d6177f 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.2.2 + 1.2.3 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index 81bb3e25..a035431a 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.3 (2015-01-10) +------------------ * add launch tests * refactor naming and fix nodelet export * set default target frame to empty diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 20432b12..f957d2ca 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -1,7 +1,7 @@ pointcloud_to_laserscan - 1.2.2 + 1.2.3 Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM). Paul Bovbel From eafe961c0c57726a3d210471a05386c2b5831f3b Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 12:03:52 -0500 Subject: [PATCH 219/405] Refactor to allow debug messages from node and nodelet --- pointcloud_to_laserscan/CMakeLists.txt | 2 +- ...se.h => pointcloud_to_laserscan_nodelet.h} | 40 ++-- .../launch/sample_node.launch | 23 ++- .../launch/sample_nodelet.launch | 23 ++- pointcloud_to_laserscan/package.xml | 2 + .../src/pointcloud_to_laserscan_base.cpp | 186 ------------------ .../src/pointcloud_to_laserscan_node.cpp | 33 ++-- .../src/pointcloud_to_laserscan_nodelet.cpp | 153 ++++++++++++-- 8 files changed, 209 insertions(+), 253 deletions(-) rename pointcloud_to_laserscan/include/pointcloud_to_laserscan/{pointcloud_to_laserscan_base.h => pointcloud_to_laserscan_nodelet.h} (80%) delete mode 100644 pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 1bca25ae..47872edb 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -23,7 +23,7 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_base.cpp src/pointcloud_to_laserscan_nodelet.cpp) +add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_nodelet.cpp) target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES}) add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h similarity index 80% rename from pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h rename to pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h index 7e5b9ba5..366cebb8 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h @@ -38,8 +38,8 @@ * Author: Paul Bovbel */ -#ifndef POINTCLOUD_TO_LASERSCAN_ROS -#define POINTCLOUD_TO_LASERSCAN_ROS +#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET +#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET #include #include @@ -50,49 +50,45 @@ #include #include +#include namespace pointcloud_to_laserscan -{ -typedef pcl::PointXYZ Point; -typedef pcl::PointCloud PointCloud; +{ + typedef pcl::PointXYZ Point; + typedef pcl::PointCloud PointCloud; /** * Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot * pointcloud_to_laserscan implementation. */ -class PointCloudToLaserScanBase -{ -public: - + class PointCloudToLaserScanNodelet : public nodelet::Nodelet + { - PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh); - - ~PointCloudToLaserScanBase(); + public: + PointCloudToLaserScanNodelet() { }; -private: + private: + virtual void onInit(); - void cloudCb(const PointCloud::ConstPtr& cloud); + void cloudCb(const PointCloud::ConstPtr &cloud_msg); void connectCb(); void disconnectCb(); - - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; + ros::NodeHandle nh_, private_nh_; ros::Publisher pub_; ros::Subscriber sub_; - tf::TransformListener tf_; - boost::mutex connect_mutex_; + // ROS Parameters unsigned int input_queue_size_; std::string target_frame_; double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_; bool use_inf_; -}; + }; -} // pointcloud_to_laserscan +} // pointcloud_to_laserscan -#endif +#endif // POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch index 3161f0a9..48c0ef47 100644 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -12,12 +12,27 @@ - - - - + + target_frame: base_link + min_height: 0.0 + max_height: 1.0 + + angle_min: -1.5708 # -M_PI/2 + angle_max: 1.5708 # M_PI/2 + angle_increment: 0.087 # M_PI/360.0 + scan_time: 0.3333 + range_min: 0.45 + range_max: 4.0 + use_inf: true + + # Concurrency level, affects number of pointclouds queued for processing and number of threads used + # 0 : Detect number of cores + # 1 : Single threaded + # 2->inf : Parallelism level + concurrency_level: 0 + diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch index d8b00f99..84943f55 100644 --- a/pointcloud_to_laserscan/launch/sample_nodelet.launch +++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch @@ -12,12 +12,27 @@ - - - - + + target_frame: base_link + min_height: 0.0 + max_height: 1.0 + + angle_min: -1.5708 # -M_PI/2 + angle_max: 1.5708 # M_PI/2 + angle_increment: 0.087 # M_PI/360.0 + scan_time: 0.3333 + range_min: 0.45 + range_max: 4.0 + use_inf: true + + # Concurrency level, affects number of pointclouds queued for processing, thread number governed by nodelet manager + # 0 : Detect number of cores + # 1 : Single threaded + # 2->inf : Parallelism level + concurrency_level: 0 + diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index f957d2ca..63420f17 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -14,6 +14,7 @@ https://github.com/ros-perception/perception_pcl catkin + dynamic_reconfigure libpcl-all-dev message_filters @@ -22,6 +23,7 @@ roscpp roslaunch sensor_msgs + dynamic_reconfigure libpcl-all-dev message_filters diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp deleted file mode 100644 index 40d2d840..00000000 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp +++ /dev/null @@ -1,186 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010-2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of 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. - * - * - */ - -/* - * Author: Paul Bovbel - */ - -#include -#include -#include -#include - -namespace pointcloud_to_laserscan -{ - - PointCloudToLaserScanBase::PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh) : - nh_(nh), private_nh_(private_nh) - { - - private_nh.param("target_frame", target_frame_, ""); - private_nh_.param("min_height", min_height_, 0.0); - private_nh_.param("max_height", max_height_, 1.0); - - private_nh_.param("angle_min", angle_min_, -M_PI/2.0); - private_nh_.param("angle_max", angle_max_, M_PI/2.0); - private_nh_.param("angle_increment", angle_increment_, M_PI/360.0); - private_nh_.param("scan_time", scan_time_, 1.0/30.0); - private_nh_.param("range_min", range_min_, 0.45); - private_nh_.param("range_max", range_max_, 4.0); - - bool use_concurrency; - private_nh_.param("use_concurrency", use_concurrency, true); - private_nh_.param("use_inf", use_inf_, true); - - boost::mutex::scoped_lock lock(connect_mutex_); - - //Only allow #threads pointclouds to wait in queue - if(use_concurrency){ - input_queue_size_ = boost::thread::hardware_concurrency(); - }else{ - input_queue_size_ = 1; - } - - pub_ = nh.advertise("scan", 10, - boost::bind(&PointCloudToLaserScanBase::connectCb, this), - boost::bind(&PointCloudToLaserScanBase::disconnectCb, this)); - - - } - - PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){ - sub_.shutdown(); - } - - - void PointCloudToLaserScanBase::connectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (!sub_ && pub_.getNumSubscribers() > 0) { - ROS_DEBUG("Connecting to depth topic."); - sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanBase::cloudCb, this); - } - } - - void PointCloudToLaserScanBase::disconnectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) { - ROS_DEBUG("Unsubscribing from depth topic."); - sub_.shutdown(); - } - } - - void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud_msg){ - - //pointer to pointcloud data to transform to laserscan - PointCloud::ConstPtr cloud_scan; - - std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header); - - //build laserscan output - sensor_msgs::LaserScan output; - output.header = cloud_header; - output.angle_min = angle_min_; - output.angle_max = angle_max_; - output.angle_increment = angle_increment_; - output.time_increment = 0.0; - output.scan_time = scan_time_; - output.range_min = range_min_; - output.range_max = range_max_; - - //decide if pointcloud needs to be transformed to a target frame - if(!target_frame_.empty() && cloud_header.frame_id != target_frame_){ - output.header.frame_id = target_frame_; - - if(tf_.waitForTransform(cloud_header.frame_id, target_frame_, cloud_header.stamp, ros::Duration(10.0))){ - PointCloud::Ptr cloud_tf(new PointCloud); - pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_); - cloud_scan = cloud_tf; - }else{ - ROS_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_header.frame_id << " into lasercan with frame " << target_frame_); - return; - } - }else{ - cloud_scan = cloud_msg; - } - - //determine amount of rays to create - uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); - - //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range - if(use_inf_){ - output.ranges.assign(ranges_size, std::numeric_limits::infinity()); - }else{ - output.ranges.assign(ranges_size, output.range_max + 1.0); - } - - for (PointCloud::const_iterator it = cloud_scan->begin(); it != cloud_scan->end(); ++it){ - const float &x = it->x; - const float &y = it->y; - const float &z = it->z; - - if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){ - //ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); - continue; - } - - if (z > max_height_ || z < min_height_){ - //ROS_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_); - continue; - } - - double range = hypot(x,y); - if (range < range_min_){ - //ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z); - continue; - } - - double angle = atan2(y, x); - if (angle < output.angle_min || angle > output.angle_max){ - //ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); - continue; - } - - //overwrite range at laserscan ray if new range is smaller - int index = (angle - output.angle_min) / output.angle_increment; - if (range < output.ranges[index]){ - output.ranges[index] = range; - } - } - pub_.publish(output); - - } - -} // pointcloud_to_laserscan diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp index b8acb363..7eb7c219 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp @@ -39,24 +39,27 @@ */ #include -#include +#include int main(int argc, char **argv){ - ros::init(argc, argv, "pointcloud_to_laserscan"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - bool use_concurrency; + ros::init(argc, argv, "pointcloud_to_laserscan_node"); + ros::NodeHandle private_nh("~"); + int concurrency_level; + private_nh.param("concurrency_level", concurrency_level, 0); - private_nh.param("use_concurrency", use_concurrency, false); + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + nodelet::V_string nargv; + std::string nodelet_name = ros::this_node::getName(); + nodelet.load(nodelet_name, "pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet", remap, nargv); - pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, private_nh); + boost::shared_ptr spinner; + if(concurrency_level) { + spinner.reset(new ros::MultiThreadedSpinner(concurrency_level)); + }else{ + spinner.reset(new ros::MultiThreadedSpinner()); + } + spinner->spin(); + return 0; - if(use_concurrency) { - ros::MultiThreadedSpinner spinner; - spinner.spin(); - }else{ - ros::spin(); - } - - return 0; } diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 2ce17dd6..de9ec102 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -38,39 +38,150 @@ * Author: Paul Bovbel */ -#include +#include #include - +#include +#include +#include +#include namespace pointcloud_to_laserscan { - class PointCloudToLaserScanNodelet : public nodelet::Nodelet - { - public: - PointCloudToLaserScanNodelet() {}; + void PointCloudToLaserScanNodelet::onInit() + { + nh_ = getMTPrivateNodeHandle(); + private_nh_ = getMTPrivateNodeHandle(); - ~PointCloudToLaserScanNodelet() {} + private_nh_.param("target_frame", target_frame_, ""); + private_nh_.param("min_height", min_height_, 0.0); + private_nh_.param("max_height", max_height_, 1.0); - private: - virtual void onInit() - { - bool use_concurrency; - getPrivateNodeHandle().param("use_concurrency", use_concurrency, false); + private_nh_.param("angle_min", angle_min_, -M_PI/2.0); + private_nh_.param("angle_max", angle_max_, M_PI/2.0); + private_nh_.param("angle_increment", angle_increment_, M_PI/360.0); + private_nh_.param("scan_time", scan_time_, 1.0/30.0); + private_nh_.param("range_min", range_min_, 0.45); + private_nh_.param("range_max", range_max_, 4.0); - if(use_concurrency){ - cloud_to_scan.reset(new PointCloudToLaserScanBase(getMTNodeHandle(), getPrivateNodeHandle())); - }else{ - cloud_to_scan.reset(new PointCloudToLaserScanBase(getNodeHandle(), getPrivateNodeHandle())); - } + int concurrency_level; + private_nh_.param("concurrency_level", concurrency_level, true); + private_nh_.param("use_inf", use_inf_, true); - }; + boost::mutex::scoped_lock lock(connect_mutex_); - boost::shared_ptr cloud_to_scan; - }; + // Only queue one pointcloud per running thread + if(concurrency_level > 0) + { + input_queue_size_ = concurrency_level; + }else{ + input_queue_size_ = boost::thread::hardware_concurrency(); + } + pub_ = nh_.advertise("scan", 10, + boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), + boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); + } + + void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg) + { + //pointer to pointcloud data to transform to laserscan + PointCloud::ConstPtr cloud_scan; + + std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header); + + //build laserscan output + sensor_msgs::LaserScan output; + output.header = cloud_header; + output.angle_min = angle_min_; + output.angle_max = angle_max_; + output.angle_increment = angle_increment_; + output.time_increment = 0.0; + output.scan_time = scan_time_; + output.range_min = range_min_; + output.range_max = range_max_; + + //decide if pointcloud needs to be transformed to a target frame + if(!target_frame_.empty() && cloud_header.frame_id != target_frame_){ + output.header.frame_id = target_frame_; + + if(tf_.waitForTransform(cloud_header.frame_id, target_frame_, cloud_header.stamp, ros::Duration(10.0))){ + PointCloud::Ptr cloud_tf(new PointCloud); + pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_); + cloud_scan = cloud_tf; + }else{ + NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_header.frame_id << " into " + "lasercan with frame " << target_frame_); + return; + } + }else{ + cloud_scan = cloud_msg; + } + + //determine amount of rays to create + uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); + + //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range + if(use_inf_){ + output.ranges.assign(ranges_size, std::numeric_limits::infinity()); + }else{ + output.ranges.assign(ranges_size, output.range_max + 1.0); + } + + for (PointCloud::const_iterator it = cloud_scan->begin(); it != cloud_scan->end(); ++it){ + const float &x = it->x; + const float &y = it->y; + const float &z = it->z; + + if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){ + NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); + continue; + } + + if (z > max_height_ || z < min_height_){ + NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_); + continue; + } + + double range = hypot(x,y); + if (range < range_min_){ + NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z); + continue; + } + + double angle = atan2(y, x); + if (angle < output.angle_min || angle > output.angle_max){ + NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); + continue; + } + + //overwrite range at laserscan ray if new range is smaller + int index = (angle - output.angle_min) / output.angle_increment; + if (range < output.ranges[index]){ + output.ranges[index] = range; + } + } + pub_.publish(output); + } + + void PointCloudToLaserScanNodelet::connectCb() + { + boost::mutex::scoped_lock lock(connect_mutex_); + if (!sub_ && pub_.getNumSubscribers() > 0) { + NODELET_DEBUG("Connecting to depth topic."); + sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this); + } + } + + void PointCloudToLaserScanNodelet::disconnectCb() + { + boost::mutex::scoped_lock lock(connect_mutex_); + if (pub_.getNumSubscribers() == 0) { + NODELET_DEBUG("Unsubscribing from depth topic."); + sub_.shutdown(); + } + } } -#include PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet); From de8a0aca3fbcc175007c29dad938c9c6e936345a Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 13:41:57 -0500 Subject: [PATCH 220/405] Fix regressions --- .../launch/sample_node.launch | 2 +- .../launch/sample_nodelet.launch | 2 +- pointcloud_to_laserscan/package.xml | 3 +- .../src/pointcloud_to_laserscan_nodelet.cpp | 37 ++++++++++--------- 4 files changed, 23 insertions(+), 21 deletions(-) diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch index 48c0ef47..9c077fca 100644 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -15,7 +15,7 @@ - target_frame: base_link + target_frame: base_link # Leave empty for no transform min_height: 0.0 max_height: 1.0 diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch index 84943f55..45027f8a 100644 --- a/pointcloud_to_laserscan/launch/sample_nodelet.launch +++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch @@ -15,7 +15,7 @@ - target_frame: base_link + target_frame: base_link # Leave empty for no transform min_height: 0.0 max_height: 1.0 diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 63420f17..cb6cb0b8 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -23,6 +23,7 @@ roscpp roslaunch sensor_msgs + tf2 dynamic_reconfigure libpcl-all-dev @@ -31,7 +32,7 @@ pcl_ros roscpp sensor_msgs - openni2_launch + tf2 diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index de9ec102..0c63065f 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -50,7 +50,7 @@ namespace pointcloud_to_laserscan void PointCloudToLaserScanNodelet::onInit() { - nh_ = getMTPrivateNodeHandle(); + nh_ = getMTNodeHandle(); private_nh_ = getMTPrivateNodeHandle(); private_nh_.param("target_frame", target_frame_, ""); @@ -83,6 +83,24 @@ namespace pointcloud_to_laserscan boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); } + void PointCloudToLaserScanNodelet::connectCb() + { + boost::mutex::scoped_lock lock(connect_mutex_); + if (!sub_ && pub_.getNumSubscribers() > 0) { + NODELET_DEBUG("Connecting to depth topic."); + sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this); + } + } + + void PointCloudToLaserScanNodelet::disconnectCb() + { + boost::mutex::scoped_lock lock(connect_mutex_); + if (pub_.getNumSubscribers() == 0) { + NODELET_DEBUG("Unsubscribing from depth topic."); + sub_.shutdown(); + } + } + void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg) { //pointer to pointcloud data to transform to laserscan @@ -164,23 +182,6 @@ namespace pointcloud_to_laserscan pub_.publish(output); } - void PointCloudToLaserScanNodelet::connectCb() - { - boost::mutex::scoped_lock lock(connect_mutex_); - if (!sub_ && pub_.getNumSubscribers() > 0) { - NODELET_DEBUG("Connecting to depth topic."); - sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this); - } - } - - void PointCloudToLaserScanNodelet::disconnectCb() - { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) { - NODELET_DEBUG("Unsubscribing from depth topic."); - sub_.shutdown(); - } - } } PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet); From 17db2afd94ab5718ac2c22c438e7a23d73956b7a Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 14:20:27 -0500 Subject: [PATCH 221/405] Remove roslaunch check --- pointcloud_to_laserscan/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 47872edb..37e27e26 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs roslaunch + tf2 ) find_package(PCL REQUIRED) @@ -29,8 +30,6 @@ target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES}) add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp) target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES}) -roslaunch_add_file_check(launch) - install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From 3fb881f8eba20cfd17a41e4e654833a054ca3913 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 15:22:48 -0500 Subject: [PATCH 222/405] Refactor with tf2 and message filters --- pointcloud_to_laserscan/CMakeLists.txt | 8 +- .../pointcloud_to_laserscan_nodelet.h | 32 +-- pointcloud_to_laserscan/package.xml | 2 + .../src/pointcloud_to_laserscan_nodelet.cpp | 218 +++++++++--------- 4 files changed, 135 insertions(+), 125 deletions(-) diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 37e27e26..395b8b4e 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -2,21 +2,19 @@ cmake_minimum_required(VERSION 2.8.3) project(pointcloud_to_laserscan) find_package(catkin REQUIRED COMPONENTS - dynamic_reconfigure message_filters nodelet - pcl_ros roscpp sensor_msgs - roslaunch tf2 + tf2_ros + geometry_msgs ) -find_package(PCL REQUIRED) catkin_package( INCLUDE_DIRS include LIBRARIES pointcloud_to_laserscan - CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs + CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros ) include_directories( diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h index 366cebb8..c15bf8f2 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h @@ -41,22 +41,21 @@ #ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET #define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET -#include -#include +#include "ros/ros.h" +#include "boost/thread/mutex.hpp" -#include -#include -#include -#include +#include "nodelet/nodelet.h" +#include "tf2/buffer_core.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/message_filter.h" +#include "message_filters/subscriber.h" +#include "sensor_msgs/PointCloud2.h" -#include -#include namespace pointcloud_to_laserscan { - typedef pcl::PointXYZ Point; - typedef pcl::PointCloud PointCloud; - + typedef message_filters::Subscriber FilteredSub; + typedef tf2_ros::MessageFilter MessageFilter; /** * Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot * pointcloud_to_laserscan implementation. @@ -65,12 +64,12 @@ namespace pointcloud_to_laserscan { public: - PointCloudToLaserScanNodelet() { }; + PointCloudToLaserScanNodelet(); private: virtual void onInit(); - void cloudCb(const PointCloud::ConstPtr &cloud_msg); + void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg); void connectCb(); @@ -78,10 +77,13 @@ namespace pointcloud_to_laserscan ros::NodeHandle nh_, private_nh_; ros::Publisher pub_; - ros::Subscriber sub_; - tf::TransformListener tf_; boost::mutex connect_mutex_; + tf2_ros::Buffer tf2_; + tf2_ros::TransformListener tf2_listener_; + boost::shared_ptr sub_; + boost::shared_ptr message_filter_; + // ROS Parameters unsigned int input_queue_size_; std::string target_frame_; diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index cb6cb0b8..d601cd6b 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -24,6 +24,7 @@ roslaunch sensor_msgs tf2 + tf2_ros dynamic_reconfigure libpcl-all-dev @@ -33,6 +34,7 @@ roscpp sensor_msgs tf2 + tf2_ros diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 0c63065f..96e8b5c9 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -39,147 +39,155 @@ */ #include -#include #include -#include -#include #include +#include +#include namespace pointcloud_to_laserscan { + PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() + : tf2_(), tf2_listener_(tf2_) + { } + + void PointCloudToLaserScanNodelet::onInit() { - nh_ = getMTNodeHandle(); - private_nh_ = getMTPrivateNodeHandle(); + nh_ = getMTNodeHandle(); + private_nh_ = getMTPrivateNodeHandle(); - private_nh_.param("target_frame", target_frame_, ""); - private_nh_.param("min_height", min_height_, 0.0); - private_nh_.param("max_height", max_height_, 1.0); + private_nh_.param("target_frame", target_frame_, ""); + private_nh_.param("min_height", min_height_, 0.0); + private_nh_.param("max_height", max_height_, 1.0); - private_nh_.param("angle_min", angle_min_, -M_PI/2.0); - private_nh_.param("angle_max", angle_max_, M_PI/2.0); - private_nh_.param("angle_increment", angle_increment_, M_PI/360.0); - private_nh_.param("scan_time", scan_time_, 1.0/30.0); - private_nh_.param("range_min", range_min_, 0.45); - private_nh_.param("range_max", range_max_, 4.0); + private_nh_.param("angle_min", angle_min_, -M_PI/2.0); + private_nh_.param("angle_max", angle_max_, M_PI/2.0); + private_nh_.param("angle_increment", angle_increment_, M_PI/360.0); + private_nh_.param("scan_time", scan_time_, 1.0/30.0); + private_nh_.param("range_min", range_min_, 0.45); + private_nh_.param("range_max", range_max_, 4.0); - int concurrency_level; - private_nh_.param("concurrency_level", concurrency_level, true); - private_nh_.param("use_inf", use_inf_, true); + int concurrency_level; + private_nh_.param("concurrency_level", concurrency_level, true); + private_nh_.param("use_inf", use_inf_, true); - boost::mutex::scoped_lock lock(connect_mutex_); + boost::mutex::scoped_lock lock(connect_mutex_); - // Only queue one pointcloud per running thread - if(concurrency_level > 0) - { - input_queue_size_ = concurrency_level; - }else{ - input_queue_size_ = boost::thread::hardware_concurrency(); - } + // Only queue one pointcloud per running thread + if(concurrency_level > 0) + { + input_queue_size_ = concurrency_level; + }else{ + input_queue_size_ = boost::thread::hardware_concurrency(); + } - pub_ = nh_.advertise("scan", 10, - boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), - boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); + pub_ = nh_.advertise("scan", 10, + boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), + boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); } void PointCloudToLaserScanNodelet::connectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (!sub_ && pub_.getNumSubscribers() > 0) { - NODELET_DEBUG("Connecting to depth topic."); - sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this); + boost::mutex::scoped_lock lock(connect_mutex_); + if (!sub_ && pub_.getNumSubscribers() > 0) { + NODELET_DEBUG("Got a subscriber to laserscan, starting subscriber to point cloud"); + sub_.reset(new FilteredSub(nh_, "cloud_in", input_queue_size_)); + if(!target_frame_.empty()) + { + message_filter_.reset(new MessageFilter(*sub_, tf2_, target_frame_, input_queue_size_, nh_)); + message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); + }else{ + sub_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); } + } } void PointCloudToLaserScanNodelet::disconnectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) { - NODELET_DEBUG("Unsubscribing from depth topic."); - sub_.shutdown(); + boost::mutex::scoped_lock lock(connect_mutex_); + if (pub_.getNumSubscribers() == 0) { + NODELET_DEBUG("No subscibers to laserscan, shutting down subscriber to point cloud"); + if(!target_frame_.empty()) + { + message_filter_.reset(); } + sub_.reset(); + } } - void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg) + void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) { - //pointer to pointcloud data to transform to laserscan - PointCloud::ConstPtr cloud_scan; + if(target_frame_.empty()){ + target_frame_ = cloud_msg->header.frame_id; + } - std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header); + //build laserscan output + sensor_msgs::LaserScan output; + output.header = cloud_msg->header; + output.header.frame_id = target_frame_; + output.angle_min = angle_min_; + output.angle_max = angle_max_; + output.angle_increment = angle_increment_; + output.time_increment = 0.0; + output.scan_time = scan_time_; + output.range_min = range_min_; + output.range_max = range_max_; - //build laserscan output - sensor_msgs::LaserScan output; - output.header = cloud_header; - output.angle_min = angle_min_; - output.angle_max = angle_max_; - output.angle_increment = angle_increment_; - output.time_increment = 0.0; - output.scan_time = scan_time_; - output.range_min = range_min_; - output.range_max = range_max_; + //determine amount of rays to create + uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); - //decide if pointcloud needs to be transformed to a target frame - if(!target_frame_.empty() && cloud_header.frame_id != target_frame_){ - output.header.frame_id = target_frame_; + //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range + if(use_inf_){ + output.ranges.assign(ranges_size, std::numeric_limits::infinity()); + }else{ + output.ranges.assign(ranges_size, output.range_max + 1.0); + } - if(tf_.waitForTransform(cloud_header.frame_id, target_frame_, cloud_header.stamp, ros::Duration(10.0))){ - PointCloud::Ptr cloud_tf(new PointCloud); - pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_); - cloud_scan = cloud_tf; - }else{ - NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_header.frame_id << " into " - "lasercan with frame " << target_frame_); - return; - } - }else{ - cloud_scan = cloud_msg; + sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_msg, "z"); + + geometry_msgs::Point32 point; + for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ + + point.x = *iter_x; + point.y = *iter_y; + point.z = *iter_z; + if(!(output.header.frame_id == cloud_msg->header.frame_id)){ + point = tf2_.transform(point, output.header.frame_id, output.header.stamp, cloud_msg->header.frame_id); } - //determine amount of rays to create - uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); - - //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range - if(use_inf_){ - output.ranges.assign(ranges_size, std::numeric_limits::infinity()); - }else{ - output.ranges.assign(ranges_size, output.range_max + 1.0); + if ( std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z) ){ + NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", point.x, point.y, point.z); + continue; } - for (PointCloud::const_iterator it = cloud_scan->begin(); it != cloud_scan->end(); ++it){ - const float &x = it->x; - const float &y = it->y; - const float &z = it->z; - - if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){ - NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); - continue; - } - - if (z > max_height_ || z < min_height_){ - NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_); - continue; - } - - double range = hypot(x,y); - if (range < range_min_){ - NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z); - continue; - } - - double angle = atan2(y, x); - if (angle < output.angle_min || angle > output.angle_max){ - NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); - continue; - } - - //overwrite range at laserscan ray if new range is smaller - int index = (angle - output.angle_min) / output.angle_increment; - if (range < output.ranges[index]){ - output.ranges[index] = range; - } + if (point.z > max_height_ || point.z < min_height_){ + NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", point.z, min_height_, max_height_); + continue; } - pub_.publish(output); + + double range = hypot(point.x,point.y); + if (range < range_min_){ + NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, point.x, point.y, point.z); + continue; + } + + double angle = atan2(point.y, point.x); + if (angle < output.angle_min || angle > output.angle_max){ + NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); + continue; + } + + //overwrite range at laserscan ray if new range is smaller + int index = (angle - output.angle_min) / output.angle_increment; + if (range < output.ranges[index]){ + output.ranges[index] = range; + } + + } + pub_.publish(output); } } From 55a9ab8f6b0a69613d79ae5b1461d8213e3cdb1d Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 15:37:31 -0500 Subject: [PATCH 223/405] Remove stray dependencies --- pointcloud_to_laserscan/package.xml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index d601cd6b..303b82db 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -16,21 +16,16 @@ catkin dynamic_reconfigure - libpcl-all-dev message_filters nodelet - pcl_ros roscpp - roslaunch sensor_msgs tf2 tf2_ros dynamic_reconfigure - libpcl-all-dev message_filters nodelet - pcl_ros roscpp sensor_msgs tf2 From f822d6ff6ed3d7efea492b8e5a1483c5d4e24188 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 15:37:50 -0500 Subject: [PATCH 224/405] Remove stray dependencies --- pointcloud_to_laserscan/package.xml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 303b82db..35baefbc 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -14,8 +14,7 @@ https://github.com/ros-perception/perception_pcl catkin - - dynamic_reconfigure + message_filters nodelet roscpp @@ -23,7 +22,6 @@ tf2 tf2_ros - dynamic_reconfigure message_filters nodelet roscpp From aa6e728362814e56ef8a2b838e5eb8e255e3ddcd Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 15:47:06 -0500 Subject: [PATCH 225/405] Disable travis until trusty support is available --- .travis.yml | 22 ---------------------- 1 file changed, 22 deletions(-) delete mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 3ca1ac58..00000000 --- a/.travis.yml +++ /dev/null @@ -1,22 +0,0 @@ -language: - - cpp - - python -python: - - "2.7" -compiler: - - gcc - -install: - - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' - - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - - sudo apt-get update - - sudo apt-get install python-catkin-pkg python-rosdep - - sudo rosdep init - - rosdep update - - rosdep install --from-paths . --ignore-src --rosdistro hydro -y - -script: - - source /opt/ros/hydro/setup.bash - - catkin_make tests --source . -j1 -DCATKIN_ENABLE_TESTING=ON - - catkin_make run_tests --source . -j1 -DCATKIN_ENABLE_TESTING=ON - - catkin_make install --source . -j1 -DCATKIN_ENABLE_TESTING=ON From c22d38df090a77f1a911a82f80840e93e0b2b338 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 17:48:29 -0500 Subject: [PATCH 226/405] Update changelogs --- pcl_ros/CHANGELOG.rst | 3 +++ perception_pcl/CHANGELOG.rst | 3 +++ pointcloud_to_laserscan/CHANGELOG.rst | 9 +++++++++ 3 files changed, 15 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 7d834195..14615f2a 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.3 (2015-01-10) ------------------ * Update common.py diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 820e3dc0..233c060b 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.3 (2015-01-10) ------------------ * clean up package.xml diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index a035431a..207be88f 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove stray dependencies +* Refactor with tf2 and message filters +* Remove roslaunch check +* Fix regressions +* Refactor to allow debug messages from node and nodelet +* Contributors: Paul Bovbel + 1.2.3 (2015-01-10) ------------------ * add launch tests From b9e0b5d21358db54aa7b1d48095905b5be653fa0 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 17:48:44 -0500 Subject: [PATCH 227/405] 1.2.4 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- pointcloud_to_laserscan/CHANGELOG.rst | 4 ++-- pointcloud_to_laserscan/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 14615f2a..172e86b1 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2015-01-15) +------------------ 1.2.3 (2015-01-10) ------------------ diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 7b98d309..dff1a021 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.2.3 + 1.2.4 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 233c060b..64975a30 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2015-01-15) +------------------ 1.2.3 (2015-01-10) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 13d6177f..3b021eb3 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.2.3 + 1.2.4 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index 207be88f..47f3e27b 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2015-01-15) +------------------ * Remove stray dependencies * Refactor with tf2 and message filters * Remove roslaunch check diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 35baefbc..6af61cdd 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -1,7 +1,7 @@ pointcloud_to_laserscan - 1.2.3 + 1.2.4 Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM). Paul Bovbel From fb5c47982c527485d37290c8e0e958061be98b66 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 18 Jan 2015 11:43:24 -0500 Subject: [PATCH 228/405] Add tolerance parameter --- .../pointcloud_to_laserscan_nodelet.h | 3 +++ pointcloud_to_laserscan/launch/sample_node.launch | 1 + .../src/pointcloud_to_laserscan_nodelet.cpp | 13 +++++++++++-- 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h index c15bf8f2..bbadc50b 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h @@ -70,6 +70,8 @@ namespace pointcloud_to_laserscan virtual void onInit(); void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg); + void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, + tf2_ros::filter_failure_reasons::FilterFailureReason reason); void connectCb(); @@ -87,6 +89,7 @@ namespace pointcloud_to_laserscan // ROS Parameters unsigned int input_queue_size_; std::string target_frame_; + double tolerance_; double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_; bool use_inf_; }; diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch index 9c077fca..3336a3b9 100644 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -16,6 +16,7 @@ target_frame: base_link # Leave empty for no transform + transform_tolerance: 1.0 min_height: 0.0 max_height: 1.0 diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 96e8b5c9..94d6616a 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -58,6 +58,7 @@ namespace pointcloud_to_laserscan private_nh_ = getMTPrivateNodeHandle(); private_nh_.param("target_frame", target_frame_, ""); + private_nh_.param("tolerance", tolerance_, 0.0); private_nh_.param("min_height", min_height_, 0.0); private_nh_.param("max_height", max_height_, 1.0); @@ -91,23 +92,31 @@ namespace pointcloud_to_laserscan { boost::mutex::scoped_lock lock(connect_mutex_); if (!sub_ && pub_.getNumSubscribers() > 0) { - NODELET_DEBUG("Got a subscriber to laserscan, starting subscriber to point cloud"); + NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud"); sub_.reset(new FilteredSub(nh_, "cloud_in", input_queue_size_)); if(!target_frame_.empty()) { message_filter_.reset(new MessageFilter(*sub_, tf2_, target_frame_, input_queue_size_, nh_)); + message_filter_->setTolerance(ros::Duration(tolerance_)); message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); + message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2)); }else{ sub_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); } } } + void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, + tf2_ros::filter_failure_reasons::FilterFailureReason reason){ + NODELET_WARN_STREAM_THROTTLE(3.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to " + << message_filter_->getTargetFramesString() << " with tolerance " << tolerance_); + } + void PointCloudToLaserScanNodelet::disconnectCb() { boost::mutex::scoped_lock lock(connect_mutex_); if (pub_.getNumSubscribers() == 0) { - NODELET_DEBUG("No subscibers to laserscan, shutting down subscriber to point cloud"); + NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud"); if(!target_frame_.empty()) { message_filter_.reset(); From 8a42c9586b6e9823b2c8e9d253b54844ce015826 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 18 Jan 2015 11:45:12 -0500 Subject: [PATCH 229/405] Set parameters in sample launch files to default --- pointcloud_to_laserscan/launch/sample_node.launch | 4 ++-- pointcloud_to_laserscan/launch/sample_nodelet.launch | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch index 3336a3b9..d5ca6c35 100644 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -15,8 +15,8 @@ - target_frame: base_link # Leave empty for no transform - transform_tolerance: 1.0 + #target_frame: # Leave disabled to output scan in pointcloud frame + transform_tolerance: 0.0 min_height: 0.0 max_height: 1.0 diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch index 45027f8a..79496023 100644 --- a/pointcloud_to_laserscan/launch/sample_nodelet.launch +++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch @@ -15,7 +15,8 @@ - target_frame: base_link # Leave empty for no transform + #target_frame: # Leave disabled to output scan in pointcloud frame + transform_tolerance: 0.0 min_height: 0.0 max_height: 1.0 From 7295940808df22137c3b3ba7adbc519943088665 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 18 Jan 2015 12:24:13 -0500 Subject: [PATCH 230/405] Add proper error checking for tolerance --- .../launch/sample_node.launch | 2 +- .../launch/sample_nodelet.launch | 2 +- .../src/pointcloud_to_laserscan_nodelet.cpp | 60 +++++++++++++------ 3 files changed, 44 insertions(+), 20 deletions(-) diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch index d5ca6c35..1d346365 100644 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -16,7 +16,7 @@ #target_frame: # Leave disabled to output scan in pointcloud frame - transform_tolerance: 0.0 + transform_tolerance: 0.01 min_height: 0.0 max_height: 1.0 diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch index 79496023..ef784310 100644 --- a/pointcloud_to_laserscan/launch/sample_nodelet.launch +++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch @@ -16,7 +16,7 @@ #target_frame: # Leave disabled to output scan in pointcloud frame - transform_tolerance: 0.0 + transform_tolerance: 0.01 min_height: 0.0 max_height: 1.0 diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 94d6616a..3321ef97 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include namespace pointcloud_to_laserscan { @@ -58,7 +58,7 @@ namespace pointcloud_to_laserscan private_nh_ = getMTPrivateNodeHandle(); private_nh_.param("target_frame", target_frame_, ""); - private_nh_.param("tolerance", tolerance_, 0.0); + private_nh_.param("tolerance", tolerance_, 0.01); private_nh_.param("min_height", min_height_, 0.0); private_nh_.param("max_height", max_height_, 1.0); @@ -153,37 +153,61 @@ namespace pointcloud_to_laserscan output.ranges.assign(ranges_size, output.range_max + 1.0); } - sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_msg, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_msg, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_msg, "z"); + // Pre-allocate for transformation + geometry_msgs::TransformStamped transform; + geometry_msgs::PointStamped in, out; + in.header = cloud_msg->header; + + bool need_transform = !(output.header.frame_id == cloud_msg->header.frame_id); + + // Fetch transform if necessary + if(need_transform) + { + try + { + transform = tf2_.lookupTransform(target_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp); + } + catch (tf2::TransformException ex) + { + NODELET_ERROR_STREAM("Transform failure: " << ex.what()); + return; + } + } + + // Iterate through pointcloud + sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_msg, "z"); - geometry_msgs::Point32 point; for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ - point.x = *iter_x; - point.y = *iter_y; - point.z = *iter_z; - if(!(output.header.frame_id == cloud_msg->header.frame_id)){ - point = tf2_.transform(point, output.header.frame_id, output.header.stamp, cloud_msg->header.frame_id); + const double *x = &(*iter_x), *y = &(*iter_y), *z = &(*iter_z); + + // Do transform if necessary + if(need_transform){ + in.point.x = *x; in.point.y = *y; in.point.z = *z; + tf2::doTransform(in, out, transform); + x = &out.point.x; y = &out.point.y; z = &out.point.z; } - if ( std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z) ){ - NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", point.x, point.y, point.z); + if ( std::isnan(*x) || std::isnan(*y) || std::isnan(*z) ){ + NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *x, *y, *z); continue; } - if (point.z > max_height_ || point.z < min_height_){ - NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", point.z, min_height_, max_height_); + if (*z > max_height_ || *z < min_height_){ + NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *z, min_height_, max_height_); continue; } - double range = hypot(point.x,point.y); + double range = hypot(*x,*y); if (range < range_min_){ - NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, point.x, point.y, point.z); + NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *x, *y, + *z); continue; } - double angle = atan2(point.y, point.x); + double angle = atan2(*y, *x); if (angle < output.angle_min || angle > output.angle_max){ NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); continue; From 6aded38494d41c42d8b35b3e146a1a64f7610b20 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 18 Jan 2015 13:04:18 -0500 Subject: [PATCH 231/405] Switch to tf_sensor_msgs for transform --- pointcloud_to_laserscan/CMakeLists.txt | 1 + .../pointcloud_to_laserscan_nodelet.h | 2 +- pointcloud_to_laserscan/package.xml | 2 + .../src/pointcloud_to_laserscan_nodelet.cpp | 50 ++++++++----------- 4 files changed, 25 insertions(+), 30 deletions(-) diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 395b8b4e..7a8d8d2a 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs tf2 tf2_ros + tf2_sensor_msgs geometry_msgs ) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h index bbadc50b..b50f4a72 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h @@ -45,7 +45,7 @@ #include "boost/thread/mutex.hpp" #include "nodelet/nodelet.h" -#include "tf2/buffer_core.h" +#include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/message_filter.h" #include "message_filters/subscriber.h" diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 6af61cdd..0e1f53fc 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -21,6 +21,7 @@ sensor_msgs tf2 tf2_ros + tf2_sensor_msgs message_filters nodelet @@ -28,6 +29,7 @@ sensor_msgs tf2 tf2_ros + tf2_sensor_msgs diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 3321ef97..f00a2a3e 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace pointcloud_to_laserscan { @@ -153,61 +154,52 @@ namespace pointcloud_to_laserscan output.ranges.assign(ranges_size, output.range_max + 1.0); } - // Pre-allocate for transformation - geometry_msgs::TransformStamped transform; - geometry_msgs::PointStamped in, out; - in.header = cloud_msg->header; + sensor_msgs::PointCloud2ConstPtr cloud_out; + sensor_msgs::PointCloud2Ptr cloud; - bool need_transform = !(output.header.frame_id == cloud_msg->header.frame_id); - - // Fetch transform if necessary - if(need_transform) + // Transform cloud if necessary + if(!(output.header.frame_id == cloud_msg->header.frame_id)) { try { - transform = tf2_.lookupTransform(target_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp); + cloud.reset(new sensor_msgs::PointCloud2); + tf2_.transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_)); + cloud_out = cloud; } catch (tf2::TransformException ex) { NODELET_ERROR_STREAM("Transform failure: " << ex.what()); return; } + }else{ + cloud_out = cloud_msg; } // Iterate through pointcloud - sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_msg, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_msg, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_msg, "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_out, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_out, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_out, "z"); for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ - const double *x = &(*iter_x), *y = &(*iter_y), *z = &(*iter_z); - - // Do transform if necessary - if(need_transform){ - in.point.x = *x; in.point.y = *y; in.point.z = *z; - tf2::doTransform(in, out, transform); - x = &out.point.x; y = &out.point.y; z = &out.point.z; - } - - if ( std::isnan(*x) || std::isnan(*y) || std::isnan(*z) ){ - NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *x, *y, *z); + if ( std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z) ){ + NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z); continue; } - if (*z > max_height_ || *z < min_height_){ - NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *z, min_height_, max_height_); + if (*iter_z > max_height_ || *iter_z < min_height_){ + NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_); continue; } - double range = hypot(*x,*y); + double range = hypot(*iter_x,*iter_y); if (range < range_min_){ - NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *x, *y, - *z); + NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y, + *iter_z); continue; } - double angle = atan2(*y, *x); + double angle = atan2(*iter_y, *iter_x); if (angle < output.angle_min || angle > output.angle_max){ NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); continue; From a71bb05a7c544fb91b8ec33b23b50f85774d8751 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 18 Jan 2015 13:05:29 -0500 Subject: [PATCH 232/405] Remove stray depedency --- pointcloud_to_laserscan/CMakeLists.txt | 1 - pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 7a8d8d2a..193e3ac6 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -9,7 +9,6 @@ find_package(catkin REQUIRED COMPONENTS tf2 tf2_ros tf2_sensor_msgs - geometry_msgs ) catkin_package( diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index f00a2a3e..81dd7eb9 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include namespace pointcloud_to_laserscan From 1a8e94ddbed8bc6881d68b9d987d152c569e446a Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 20 Jan 2015 10:14:42 -0500 Subject: [PATCH 233/405] Fix regression with pointcloud2 iter double->float --- .../src/pointcloud_to_laserscan_nodelet.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 81dd7eb9..5bd567c4 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -127,14 +127,14 @@ namespace pointcloud_to_laserscan void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) { - if(target_frame_.empty()){ - target_frame_ = cloud_msg->header.frame_id; - } //build laserscan output sensor_msgs::LaserScan output; output.header = cloud_msg->header; - output.header.frame_id = target_frame_; + if(!target_frame_.empty()){ + output.header.frame_id = target_frame_; + } + output.angle_min = angle_min_; output.angle_max = angle_max_; output.angle_increment = angle_increment_; @@ -175,9 +175,9 @@ namespace pointcloud_to_laserscan } // Iterate through pointcloud - sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_out, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_out, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_out, "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_out, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_out, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_out, "z"); for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ @@ -217,4 +217,3 @@ namespace pointcloud_to_laserscan } PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet); - From 7ef9fed2b80688f189118ecd640d7beef5dcdfa5 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 20 Jan 2015 10:45:55 -0500 Subject: [PATCH 234/405] Update changelog --- pcl_ros/CHANGELOG.rst | 3 +++ perception_pcl/CHANGELOG.rst | 3 +++ pointcloud_to_laserscan/CHANGELOG.rst | 7 +++++++ 3 files changed, 13 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 172e86b1..1ce1eb39 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.4 (2015-01-15) ------------------ diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 64975a30..aa4b40a4 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.4 (2015-01-15) ------------------ diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index 47f3e27b..5248a4df 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Switch to tf_sensor_msgs for transform +* Set parameters in sample launch files to default +* Add tolerance parameter +* Contributors: Paul Bovbel + 1.2.4 (2015-01-15) ------------------ * Remove stray dependencies From e739e0f297d690ad46f74f33bcd7b854fd652b73 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 20 Jan 2015 10:45:59 -0500 Subject: [PATCH 235/405] 1.2.5 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- pointcloud_to_laserscan/CHANGELOG.rst | 4 ++-- pointcloud_to_laserscan/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 1ce1eb39..e2de6ad6 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2015-01-20) +------------------ 1.2.4 (2015-01-15) ------------------ diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index dff1a021..cb8925a6 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.2.4 + 1.2.5 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index aa4b40a4..cfecbe62 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2015-01-20) +------------------ 1.2.4 (2015-01-15) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 3b021eb3..89f1fe3b 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.2.4 + 1.2.5 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index 5248a4df..fc5d9036 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2015-01-20) +------------------ * Switch to tf_sensor_msgs for transform * Set parameters in sample launch files to default * Add tolerance parameter diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 0e1f53fc..324926f2 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -1,7 +1,7 @@ pointcloud_to_laserscan - 1.2.4 + 1.2.5 Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM). Paul Bovbel From 040f68c31faff4088a3769eec717e768093ed50a Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 22 Jan 2015 09:21:45 -0500 Subject: [PATCH 236/405] Fix multithreaded lazy pub sub --- .../pointcloud_to_laserscan_nodelet.h | 5 +- .../src/pointcloud_to_laserscan_nodelet.cpp | 131 ++++++++++-------- 2 files changed, 77 insertions(+), 59 deletions(-) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h index b50f4a72..bed70952 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h @@ -46,7 +46,6 @@ #include "nodelet/nodelet.h" #include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" #include "tf2_ros/message_filter.h" #include "message_filters/subscriber.h" #include "sensor_msgs/PointCloud2.h" @@ -54,7 +53,6 @@ namespace pointcloud_to_laserscan { - typedef message_filters::Subscriber FilteredSub; typedef tf2_ros::MessageFilter MessageFilter; /** * Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot @@ -82,8 +80,7 @@ namespace pointcloud_to_laserscan boost::mutex connect_mutex_; tf2_ros::Buffer tf2_; - tf2_ros::TransformListener tf2_listener_; - boost::shared_ptr sub_; + message_filters::Subscriber sub_; boost::shared_ptr message_filter_; // ROS Parameters diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 5bd567c4..dc783d8e 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -47,25 +47,22 @@ namespace pointcloud_to_laserscan { - PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() - : tf2_(), tf2_listener_(tf2_) - { } - + PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() {} void PointCloudToLaserScanNodelet::onInit() { - nh_ = getMTNodeHandle(); - private_nh_ = getMTPrivateNodeHandle(); + boost::mutex::scoped_lock lock(connect_mutex_); + private_nh_ = getPrivateNodeHandle(); private_nh_.param("target_frame", target_frame_, ""); private_nh_.param("tolerance", tolerance_, 0.01); private_nh_.param("min_height", min_height_, 0.0); private_nh_.param("max_height", max_height_, 1.0); - private_nh_.param("angle_min", angle_min_, -M_PI/2.0); - private_nh_.param("angle_max", angle_max_, M_PI/2.0); - private_nh_.param("angle_increment", angle_increment_, M_PI/360.0); - private_nh_.param("scan_time", scan_time_, 1.0/30.0); + private_nh_.param("angle_min", angle_min_, -M_PI / 2.0); + private_nh_.param("angle_max", angle_max_, M_PI / 2.0); + private_nh_.param("angle_increment", angle_increment_, M_PI / 360.0); + private_nh_.param("scan_time", scan_time_, 1.0 / 30.0); private_nh_.param("range_min", range_min_, 0.45); private_nh_.param("range_max", range_max_, 4.0); @@ -73,65 +70,79 @@ namespace pointcloud_to_laserscan private_nh_.param("concurrency_level", concurrency_level, true); private_nh_.param("use_inf", use_inf_, true); - boost::mutex::scoped_lock lock(connect_mutex_); + //Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size + if (concurrency_level == 1) + { + nh_ = getNodeHandle(); + } + else + { + nh_ = getMTNodeHandle(); + } // Only queue one pointcloud per running thread - if(concurrency_level > 0) + if (concurrency_level > 0) { input_queue_size_ = concurrency_level; - }else{ + } + else + { input_queue_size_ = boost::thread::hardware_concurrency(); } + // if pointcloud target frame specified, we need to filter by transform availability + if (!target_frame_.empty()) + { + message_filter_.reset(new MessageFilter(sub_, tf2_, target_frame_, input_queue_size_, nh_)); + message_filter_->setTolerance(ros::Duration(tolerance_)); + message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); + message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2)); + } + else // otherwise setup direct subscription + { + sub_.registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); + } + pub_ = nh_.advertise("scan", 10, - boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), - boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); + boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), + boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); } void PointCloudToLaserScanNodelet::connectCb() { boost::mutex::scoped_lock lock(connect_mutex_); - if (!sub_ && pub_.getNumSubscribers() > 0) { + if (pub_.getNumSubscribers() > 0 && sub_.getSubscriber().getNumPublishers() == 0) + { NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud"); - sub_.reset(new FilteredSub(nh_, "cloud_in", input_queue_size_)); - if(!target_frame_.empty()) - { - message_filter_.reset(new MessageFilter(*sub_, tf2_, target_frame_, input_queue_size_, nh_)); - message_filter_->setTolerance(ros::Duration(tolerance_)); - message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); - message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2)); - }else{ - sub_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); - } + sub_.subscribe(nh_, "cloud_in", input_queue_size_); } } - void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, - tf2_ros::filter_failure_reasons::FilterFailureReason reason){ - NODELET_WARN_STREAM_THROTTLE(3.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to " - << message_filter_->getTargetFramesString() << " with tolerance " << tolerance_); - } - void PointCloudToLaserScanNodelet::disconnectCb() { boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) { + if (pub_.getNumSubscribers() == 0) + { NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud"); - if(!target_frame_.empty()) - { - message_filter_.reset(); - } - sub_.reset(); + sub_.unsubscribe(); } } + void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, + tf2_ros::filter_failure_reasons::FilterFailureReason reason) + { + NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to " + << message_filter_->getTargetFramesString() << " with tolerance " << tolerance_); + } + void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) { //build laserscan output sensor_msgs::LaserScan output; output.header = cloud_msg->header; - if(!target_frame_.empty()){ + if (!target_frame_.empty()) + { output.header.frame_id = target_frame_; } @@ -147,9 +158,12 @@ namespace pointcloud_to_laserscan uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range - if(use_inf_){ + if (use_inf_) + { output.ranges.assign(ranges_size, std::numeric_limits::infinity()); - }else{ + } + else + { output.ranges.assign(ranges_size, output.range_max + 1.0); } @@ -157,7 +171,7 @@ namespace pointcloud_to_laserscan sensor_msgs::PointCloud2Ptr cloud; // Transform cloud if necessary - if(!(output.header.frame_id == cloud_msg->header.frame_id)) + if (!(output.header.frame_id == cloud_msg->header.frame_id)) { try { @@ -170,43 +184,50 @@ namespace pointcloud_to_laserscan NODELET_ERROR_STREAM("Transform failure: " << ex.what()); return; } - }else{ + } + else + { cloud_out = cloud_msg; } // Iterate through pointcloud - sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_out, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_out, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_out, "z"); + for (sensor_msgs::PointCloud2ConstIterator + iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"), iter_z(*cloud_out, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z) + { - for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ - - if ( std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z) ){ + if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) + { NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z); continue; } - if (*iter_z > max_height_ || *iter_z < min_height_){ + if (*iter_z > max_height_ || *iter_z < min_height_) + { NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_); continue; } - double range = hypot(*iter_x,*iter_y); - if (range < range_min_){ + double range = hypot(*iter_x, *iter_y); + if (range < range_min_) + { NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y, - *iter_z); + *iter_z); continue; } double angle = atan2(*iter_y, *iter_x); - if (angle < output.angle_min || angle > output.angle_max){ + if (angle < output.angle_min || angle > output.angle_max) + { NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); continue; } //overwrite range at laserscan ray if new range is smaller int index = (angle - output.angle_min) / output.angle_increment; - if (range < output.ranges[index]){ + if (range < output.ranges[index]) + { output.ranges[index] = range; } From 6dfaf5c596194ac0e27679cf36ee145a33882bc1 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 1 Feb 2015 16:42:27 -0500 Subject: [PATCH 237/405] Fix default value for concurrency --- pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index dc783d8e..256fddff 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -67,7 +67,7 @@ namespace pointcloud_to_laserscan private_nh_.param("range_max", range_max_, 4.0); int concurrency_level; - private_nh_.param("concurrency_level", concurrency_level, true); + private_nh_.param("concurrency_level", concurrency_level, 1); private_nh_.param("use_inf", use_inf_, true); //Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size From 9b2bd47887899134b7c6e2fc6764caa3f085a583 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Wed, 4 Feb 2015 13:42:41 -0500 Subject: [PATCH 238/405] Update changelogs --- pcl_ros/CHANGELOG.rst | 3 +++ perception_pcl/CHANGELOG.rst | 3 +++ pointcloud_to_laserscan/CHANGELOG.rst | 6 ++++++ 3 files changed, 12 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index e2de6ad6..63279131 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.5 (2015-01-20) ------------------ diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index cfecbe62..17851a87 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.5 (2015-01-20) ------------------ diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index fc5d9036..5f2988d3 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix default value for concurrency +* Fix multithreaded lazy pub sub +* Contributors: Paul Bovbel + 1.2.5 (2015-01-20) ------------------ * Switch to tf_sensor_msgs for transform From d7adb41971ae877b088f4524af30e24033125df3 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Wed, 4 Feb 2015 13:42:46 -0500 Subject: [PATCH 239/405] 1.2.6 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- pointcloud_to_laserscan/CHANGELOG.rst | 4 ++-- pointcloud_to_laserscan/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 63279131..9c0236af 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.6 (2015-02-04) +------------------ 1.2.5 (2015-01-20) ------------------ diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index cb8925a6..165a6190 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.2.5 + 1.2.6 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 17851a87..08682e03 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.6 (2015-02-04) +------------------ 1.2.5 (2015-01-20) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 89f1fe3b..7bb2fc38 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.2.5 + 1.2.6 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst index 5f2988d3..5c2b3e50 100644 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ b/pointcloud_to_laserscan/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pointcloud_to_laserscan ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.6 (2015-02-04) +------------------ * Fix default value for concurrency * Fix multithreaded lazy pub sub * Contributors: Paul Bovbel diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index 324926f2..b3985006 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -1,7 +1,7 @@ pointcloud_to_laserscan - 1.2.5 + 1.2.6 Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM). Paul Bovbel From 10727ec6503bafebcee3e625ac1096b37885eadf Mon Sep 17 00:00:00 2001 From: Mitchell Wills Date: Mon, 16 Feb 2015 17:57:55 -0500 Subject: [PATCH 240/405] Added option to save pointclouds in binary and binary compressed format --- pcl_ros/tools/pointcloud_to_pcd.cpp | 44 +++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index 00e92734..35e9a63d 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -63,6 +63,8 @@ class PointCloudToPCD private: std::string prefix_; + bool binary_; + bool compressed_; public: string cloud_topic_; @@ -86,12 +88,30 @@ class PointCloudToPCD ss << prefix_ << cloud->header.stamp << ".pcd"; ROS_INFO ("Data saved to %s", ss.str ().c_str ()); - pcl::io::savePCDFile (ss.str (), *cloud, Eigen::Vector4f::Zero (), - Eigen::Quaternionf::Identity (), false); + pcl::PCDWriter writer; + if(binary_) + { + if(compressed_) + { + writer.writeBinaryCompressed (ss.str (), *cloud, Eigen::Vector4f::Zero (), + Eigen::Quaternionf::Identity ()); + } + else + { + writer.writeBinary (ss.str (), *cloud, Eigen::Vector4f::Zero (), + Eigen::Quaternionf::Identity ()); + } + } + else + { + writer.writeASCII (ss.str (), *cloud, Eigen::Vector4f::Zero (), + Eigen::Quaternionf::Identity (), 8); + } + } //////////////////////////////////////////////////////////////////////////////// - PointCloudToPCD () + PointCloudToPCD () : binary_(false), compressed_(false) { // Check if a prefix parameter is defined for output file names. ros::NodeHandle priv_nh("~"); @@ -105,6 +125,24 @@ class PointCloudToPCD << prefix_); } + priv_nh.getParam ("binary", binary_); + priv_nh.getParam ("compressed", compressed_); + if(binary_) + { + if(compressed_) + { + ROS_INFO_STREAM ("Saving as binary compressed PCD"); + } + else + { + ROS_INFO_STREAM ("Saving as binary PCD"); + } + } + else + { + ROS_INFO_STREAM ("Saving as binary PCD"); + } + cloud_topic_ = "input"; sub_ = nh_.subscribe (cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this); From 0a958f55ced062f9d93731e2679a6c7a5dda2c61 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Thu, 7 May 2015 15:21:11 -0400 Subject: [PATCH 241/405] Fixes #77 and #80 for indigo --- pcl_ros/CMakeLists.txt | 2 + pcl_ros/include/pcl_ros/features/shot.h | 80 +++++++++++++++++++++ pcl_ros/include/pcl_ros/features/shot_omp.h | 78 ++++++++++++++++++++ pcl_ros/pcl_nodelets.xml | 14 ++++ pcl_ros/src/pcl_ros/features/shot.cpp | 75 +++++++++++++++++++ pcl_ros/src/pcl_ros/features/shot_omp.cpp | 75 +++++++++++++++++++ pcl_ros/src/pcl_ros/filters/filter.cpp | 3 + 7 files changed, 327 insertions(+) create mode 100644 pcl_ros/include/pcl_ros/features/shot.h create mode 100644 pcl_ros/include/pcl_ros/features/shot_omp.h create mode 100644 pcl_ros/src/pcl_ros/features/shot.cpp create mode 100644 pcl_ros/src/pcl_ros/features/shot_omp.cpp diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index c64b6a20..3192e07c 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -105,6 +105,8 @@ add_library(pcl_ros_features src/pcl_ros/features/boundary.cpp src/pcl_ros/features/fpfh.cpp src/pcl_ros/features/fpfh_omp.cpp + src/pcl_ros/features/shot.cpp + src/pcl_ros/features/shot_omp.cpp src/pcl_ros/features/moment_invariants.cpp src/pcl_ros/features/normal_3d.cpp src/pcl_ros/features/normal_3d_omp.cpp diff --git a/pcl_ros/include/pcl_ros/features/shot.h b/pcl_ros/include/pcl_ros/features/shot.h new file mode 100644 index 00000000..73c0d81c --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/shot.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Ryohei Ueda, JSK Lab + * 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 JSK Lab. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_ROS_SHOT_H_ +#define PCL_ROS_SHOT_H_ + +#include +#include "pcl_ros/features/pfh.h" + +namespace pcl_ros +{ + /** \brief @b SHOTEstimation estimates SHOT descriptor. + * + */ + class SHOTEstimation : public FeatureFromNormals + { + private: + pcl::SHOTEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_SHOT_H_ + + diff --git a/pcl_ros/include/pcl_ros/features/shot_omp.h b/pcl_ros/include/pcl_ros/features/shot_omp.h new file mode 100644 index 00000000..bc28939c --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/shot_omp.h @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Ryohei Ueda, JSK Lab + * 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 JSK Lab. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_ROS_SHOT_OMP_H_ +#define PCL_ROS_SHOT_OMP_H_ + +#include +#include "pcl_ros/features/shot.h" + +namespace pcl_ros +{ + /** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP. + */ + class SHOTEstimationOMP : public FeatureFromNormals + { + private: + pcl::SHOTEstimationOMP impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("output", max_queue_size_); + return (true); + } + + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish (const PointCloudInConstPtr &cloud); + + /** \brief Compute the feature and publish it. */ + void computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_SHOT_OMP_H_ + diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml index 2be8cace..64fa27d8 100644 --- a/pcl_ros/pcl_nodelets.xml +++ b/pcl_ros/pcl_nodelets.xml @@ -20,6 +20,20 @@ containing points and normals, in parallel, using the OpenMP standard. + + + + SHOTEstimation estimates SHOT descriptor for a given point cloud dataset + containing points and normals. + + + + + + SHOTEstimationOMP estimates SHOT descriptor for a given point cloud dataset + containing points and normals, in parallel, using the OpenMP standard. + + diff --git a/pcl_ros/src/pcl_ros/features/shot.cpp b/pcl_ros/src/pcl_ros/features/shot.cpp new file mode 100644 index 00000000..82122c8d --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/shot.cpp @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Ryohei Ueda, JSK Lab + * 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 JSK Lab. 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. + * + * + */ + +#include +#include "pcl_ros/features/shot.h" + +void +pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::SHOTEstimation SHOTEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, SHOTEstimation, SHOTEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/shot_omp.cpp b/pcl_ros/src/pcl_ros/features/shot_omp.cpp new file mode 100644 index 00000000..887e4b5d --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/shot_omp.cpp @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Ryohei Ueda, JSK Lab + * 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 JSK Lab. 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. + * + * + */ + +#include +#include "pcl_ros/features/shot_omp.h" + +void +pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + + // Set the inputs + impl_.setInputCloud (cloud); + impl_.setIndices (indices); + impl_.setSearchSurface (surface); + impl_.setInputNormals (normals); + // Estimate the feature + PointCloudOut output; + impl_.compute (output); + + // Publish a Boost shared ptr const data + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP; +PLUGINLIB_DECLARE_CLASS (pcl, SHOTEstimationOMP, SHOTEstimationOMP, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 30104d94..0d39a03e 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -95,6 +95,9 @@ pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const Indic cloud_tf.reset (new PointCloud2 (cloud_transformed)); } + // Copy timestamp to keep it + cloud_tf->header.stamp = input->header.stamp; + // Publish a boost shared ptr pub_output_.publish (cloud_tf); } From 04c29ac9ccddb8d412840fd0e34681a027403eb5 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Mon, 11 May 2015 16:51:52 -0400 Subject: [PATCH 242/405] Fixes #85 for Indigo --- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index 90000feb..f7a09a84 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -215,6 +215,8 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 // Concatenate the results pcl::concatenatePointCloud (*in1_t, *in2_t, out); + // Copy header + out.header.stamp = in1.header.stamp; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -228,21 +230,21 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::input ( PointCloud2::Ptr out1 (new PointCloud2 ()); PointCloud2::Ptr out2 (new PointCloud2 ()); pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1); - if (in3) + if (in3 && in3->width * in3->height > 0) { pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2); out1 = out2; - if (in4) + if (in4 && in4->width * in4->height > 0) { pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1); - if (in5) + if (in5 && in5->width * in5->height > 0) { pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2); out1 = out2; - if (in6) + if (in6 && in6->width * in6->height > 0) { pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1); - if (in7) + if (in7 && in7->width * in7->height > 0) { pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2); out1 = out2; From d245f70f4f0859f849458886249fa25a994def23 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Tue, 26 May 2015 16:01:16 -0400 Subject: [PATCH 243/405] Fixes #87 for Indigo --- pcl_ros/CMakeLists.txt | 1 + pcl_ros/cfg/RadiusOutlierRemoval.cfg | 15 ++++++++ .../pcl_ros/filters/radius_outlier_removal.h | 16 ++++++++- pcl_ros/pcl_nodelets.xml | 5 +++ .../filters/radius_outlier_removal.cpp | 34 +++++++++++++++++++ 5 files changed, 70 insertions(+), 1 deletion(-) create mode 100755 pcl_ros/cfg/RadiusOutlierRemoval.cfg diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 3192e07c..a9275ac8 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -52,6 +52,7 @@ generate_dynamic_reconfigure_options( cfg/SACSegmentationFromNormals.cfg cfg/SegmentDifferences.cfg cfg/StatisticalOutlierRemoval.cfg + cfg/RadiusOutlierRemoval.cfg cfg/VoxelGrid.cfg cfg/CropBox.cfg ) diff --git a/pcl_ros/cfg/RadiusOutlierRemoval.cfg b/pcl_ros/cfg/RadiusOutlierRemoval.cfg new file mode 100755 index 00000000..d54962e6 --- /dev/null +++ b/pcl_ros/cfg/RadiusOutlierRemoval.cfg @@ -0,0 +1,15 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +from dynamic_reconfigure.parameter_generator_catkin import *; + +gen = ParameterGenerator () + +# enabling/disabling the unit limits +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("radius_search", double_t, 0, "Radius of the sphere that will determine which points are neighbors.", 0.1, 0.0, 10.0) +gen.add ("min_neighbors", int_t, 0, "The number of neighbors that need to be present in order to be classified as an inlier.", 5, 0, 1000) + +exit (gen.generate (PACKAGE, "pcl_ros", "RadiusOutlierRemoval")) diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h index cb75eaee..6120f9e5 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h @@ -42,6 +42,9 @@ #include #include "pcl_ros/filters/filter.h" +// Dynamic reconfigure +#include "pcl_ros/RadiusOutlierRemovalConfig.h" + namespace pcl_ros { /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain @@ -52,6 +55,9 @@ 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 @@ -72,9 +78,17 @@ namespace pcl_ros /** \brief Child initialization routine. * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service */ - virtual inline bool child_init (ros::NodeHandle &nh) { return (true); } + virtual inline bool child_init (ros::NodeHandle &nh, bool &has_service); + /** \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); + + private: /** \brief The PCL filter implementation used. */ pcl::RadiusOutlierRemoval impl_; diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml index 64fa27d8..71978735 100644 --- a/pcl_ros/pcl_nodelets.xml +++ b/pcl_ros/pcl_nodelets.xml @@ -130,6 +130,11 @@ StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. + + + RadiusOutlierRemoval uses point neighborhood statistics to filter outlier data. + + diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp index da965c69..46c00a14 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -38,6 +38,40 @@ #include #include "pcl_ros/filters/radius_outlier_removal.h" +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl_ros::RadiusOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) +{ + // Enable the dynamic reconfigure service + has_service = true; + srv_ = boost::make_shared > (nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&RadiusOutlierRemoval::config_callback, this, _1, _2); + srv_->setCallback (f); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::RadiusOutlierRemoval::config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock lock (mutex_); + + if (impl_.getMinNeighborsInRadius () != config.min_neighbors) + { + impl_.setMinNeighborsInRadius (config.min_neighbors); + NODELET_DEBUG ("[%s::config_callback] Setting the number of neighbors in radius: %d.", getName ().c_str (), config.min_neighbors); + } + + if (impl_.getRadiusSearch () != config.radius_search) + { + impl_.setRadiusSearch (config.radius_search); + NODELET_DEBUG ("[%s::config_callback] Setting the radius to search neighbors: %f.", getName ().c_str (), config.radius_search); + } + +} + + typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval,nodelet::Nodelet); From afeb804f30a4373beabe48d5702d043b9c49395a Mon Sep 17 00:00:00 2001 From: Lucid One Date: Tue, 26 May 2015 16:58:11 -0400 Subject: [PATCH 244/405] Sync pcl_nodelets.xml from hydro to indigo Fixes to pass catkin lint -W1 --- pcl_ros/CMakeLists.txt | 29 ++++++-------- pcl_ros/package.xml | 5 ++- pcl_ros/pcl_nodelets.xml | 85 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 100 insertions(+), 19 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index a9275ac8..bdeebd84 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -33,13 +33,6 @@ include_directories(include ${PCL_INCLUDE_DIRS} ) -## Add link directories -link_directories( - ${Boost_LIBRARY_DIRS} - ${Eigen_LIBRARY_DIRS} - ${PCL_LIBRARY_DIRS} -) - ## Generate dynamic_reconfigure generate_dynamic_reconfigure_options( cfg/EuclideanClusterExtraction.cfg @@ -83,7 +76,7 @@ catkin_package( ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) -target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_tf pcl_ros_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) ## Nodelets @@ -96,7 +89,7 @@ add_library(pcl_ros_io src/pcl_ros/io/io.cpp src/pcl_ros/io/pcd_io.cpp ) -target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) class_loader_hide_library_symbols(pcl_ros_io) ## Declare the pcl_ros_features library @@ -115,7 +108,7 @@ add_library(pcl_ros_features src/pcl_ros/features/principal_curvatures.cpp src/pcl_ros/features/vfh.cpp ) -target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_features) @@ -131,7 +124,7 @@ add_library(pcl_ros_filters 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}) +target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_filters) @@ -143,7 +136,7 @@ add_library (pcl_ros_segmentation src/pcl_ros/segmentation/segment_differences.cpp src/pcl_ros/segmentation/segmentation.cpp ) -target_link_libraries(pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_segmentation) @@ -154,26 +147,26 @@ add_library (pcl_ros_surface src/pcl_ros/surface/convex_hull.cpp src/pcl_ros/surface/moving_least_squares.cpp ) -target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_surface) ## Tools add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) -target_link_libraries(pcd_to_pointcloud pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcd_to_pointcloud pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) -target_link_libraries(pointcloud_to_pcd pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pointcloud_to_pcd pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARIES}) add_executable(bag_to_pcd tools/bag_to_pcd.cpp) -target_link_libraries(bag_to_pcd pcl_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(bag_to_pcd pcl_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARIES}) add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) -target_link_libraries(convert_pcd_to_image pcl_io pcl_common ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(convert_pcd_to_image pcl_io pcl_common ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) -target_link_libraries(convert_pointcloud_to_image pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(convert_pointcloud_to_image pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) install(DIRECTORY include/${PROJECT_NAME}/ diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 165a6190..77602acf 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -20,8 +20,11 @@ https://github.com/ros-perception/perception_pcl catkin - cmake_modules + rosconsole + genmsg + roslib + dynamic_reconfigure eigen nodelet diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml index 71978735..215ef9a9 100644 --- a/pcl_ros/pcl_nodelets.xml +++ b/pcl_ros/pcl_nodelets.xml @@ -47,6 +47,41 @@ in parallel, using the OpenMP standard. + + + + NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and curvatures, in + parallel, using Intel's Threading Building Blocks library. + + + + + + NormalEstimation estimates local surface properties at each 3D point, such as surface normals and curvatures. + + + + + + PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset containing + points and normals. + + + + + + PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of principal surface + curvatures for a given point cloud dataset containing points and normals. + + + + + + VFHEstimation estimates the Viewpoint Feature Histogram (VFH) global descriptor for a given point cloud cluster dataset + containing points and normals. + + + @@ -143,5 +178,55 @@ + + + + + ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with a given + height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying inside it. + + + + + EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. + + + + + + SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in the sense that + it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. + + + + + + SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in the sense that + it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. + + + + + + SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the + difference between them for a maximum given distance threshold. + + + + + + + + + + MovingLeastSquares is an implementation of the MLS algorithm for data reconstruction through bivariate polynomial fitting. + + + + + ConvexHull2D represents a 2D ConvexHull implementation. + + + From a32bff82f78e3dd88942b645c5fb48ba94b46511 Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Tue, 10 Feb 2015 15:10:28 -0800 Subject: [PATCH 245/405] specialized HasHeader, TimeStamp, FrameId - HasHeader now returns false - TimeStamp and FrameId specialed for pcl::PointCloud for any point type T These changes allow to use pcl::PointCloud with tf::MessageFilter --- pcl_ros/include/pcl_ros/point_cloud.h | 45 +++++++++------------------ 1 file changed, 14 insertions(+), 31 deletions(-) diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h index 167537ee..711b8307 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.h +++ b/pcl_ros/include/pcl_ros/point_cloud.h @@ -109,29 +109,30 @@ namespace ros static const char* value(const pcl::PointCloud&) { return value(); } }; - template struct HasHeader > : TrueType {}; + // pcl point clouds message don't have a ROS compatible header + // the specialized meta functions below (TimeStamp and FrameId) + // can be used to get the header data. + template struct HasHeader > : FalseType {}; - // This is bad because it will only work for pcl::PointXYZ, but I can't - // find another way around ambiguous partial template specialization... - template<> - struct TimeStamp > + template + struct TimeStamp > { // This specialization could be dangerous, but it's the best I can do. // If this TimeStamp struct is destroyed before they are done with the // pointer returned by the first functions may go out of scope, but there // isn't a lot I can do about that. This is a good reason to refuse to // returning pointers like this... - static ros::Time* pointer(pcl::PointCloud &m) { + static ros::Time* pointer(typename pcl::PointCloud &m) { header_.reset(new std_msgs::Header()); pcl_conversions::fromPCL(m.header, *(header_)); return &(header_->stamp); } - static ros::Time const* pointer(const pcl::PointCloud& m) { + static ros::Time const* pointer(const typename pcl::PointCloud& m) { header_const_.reset(new std_msgs::Header()); pcl_conversions::fromPCL(m.header, *(header_const_)); return &(header_const_->stamp); } - static ros::Time value(const pcl::PointCloud& m) { + static ros::Time value(const typename pcl::PointCloud& m) { return pcl_conversions::fromPCL(m.header).stamp; } private: @@ -139,30 +140,12 @@ namespace ros static boost::shared_ptr header_const_; }; - template<> - struct TimeStamp > + template + struct FrameId > { - // This specialization could be dangerous, but it's the best I can do. - // If this TimeStamp struct is destroyed before they are done with the - // pointer returned by the first functions may go out of scope, but there - // isn't a lot I can do about that. This is a good reason to refuse to - // returning pointers like this... - static ros::Time* pointer(pcl::PointCloud &m) { - header_.reset(new std_msgs::Header()); - pcl_conversions::fromPCL(m.header, *(header_)); - return &(header_->stamp); - } - static ros::Time const* pointer(const pcl::PointCloud& m) { - header_const_.reset(new std_msgs::Header()); - pcl_conversions::fromPCL(m.header, *(header_const_)); - return &(header_const_->stamp); - } - static ros::Time value(const pcl::PointCloud& m) { - return pcl_conversions::fromPCL(m.header).stamp; - } - private: - static boost::shared_ptr header_; - static boost::shared_ptr header_const_; + static std::string* pointer(pcl::PointCloud& m) { return &m.header.frame_id; } + static std::string const* pointer(const pcl::PointCloud& m) { return &m.header.frame_id; } + static std::string value(const pcl::PointCloud& m) { return m.header.frame_id; } }; } // namespace ros::message_traits From 5f276e80b8a3591e8462c8505798afbc28293546 Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Wed, 11 Feb 2015 11:42:25 -0800 Subject: [PATCH 246/405] Adds a test for tf message filters with pcl pointclouds --- pcl_ros/CMakeLists.txt | 14 + pcl_ros/package.xml | 2 + .../src/test/test_tf_message_filter_pcl.cpp | 390 ++++++++++++++++++ pcl_ros/tests/test_tf_message_filter_pcl.xml | 3 + 4 files changed, 409 insertions(+) create mode 100644 pcl_ros/src/test/test_tf_message_filter_pcl.cpp create mode 100644 pcl_ros/tests/test_tf_message_filter_pcl.xml diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index bdeebd84..97e50fa1 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -168,6 +168,20 @@ target_link_libraries(convert_pcd_to_image pcl_io pcl_common ${Boost_LIBRARIES} add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) target_link_libraries(convert_pointcloud_to_image pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING) + # we don't have to find_package GTEST; catkin does it for us + include_directories(${GTEST_INCLUDE_DIRS}) + add_executable(test_tf_message_filter_pcl EXCLUDE_FROM_ALL src/test/test_tf_message_filter_pcl.cpp) + target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) + add_dependencies(tests test_tf_message_filter_pcl) + find_package(rostest) + add_rostest(tests/test_tf_message_filter_pcl.xml) +endif(CATKIN_ENABLE_TESTING) + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 77602acf..e47a1d43 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -58,6 +58,8 @@ tf python-vtk libvtk-java + + rostest diff --git a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp new file mode 100644 index 00000000..d629060a --- /dev/null +++ b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -0,0 +1,390 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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. + */ + +/** \author Josh Faust */ + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +using namespace tf; + +// using a random point type, as we want to make sure that it does work with +// other points than just XYZ +typedef pcl::PointCloud PCDType; + + +/// Sets pcl_stamp from stamp, BUT alters stamp +/// a little to round it to millisecond. This is because converting back +/// and forth from pcd to ros time induces some rounding errors. +void setStamp(ros::Time &stamp, pcl::uint64_t &pcl_stamp) +{ + // round to millisecond + static const uint32_t mult = 1e6; + stamp.nsec /= mult; + stamp.nsec *= mult; + + pcl_conversions::toPCL(stamp, pcl_stamp); + + // verify + { + ros::Time t; + pcl_conversions::fromPCL(pcl_stamp, t); + ROS_ASSERT_MSG(t==stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec); + } +} + +class Notification +{ +public: + Notification(int expected_count) + : count_(0) + , expected_count_(expected_count) + , failure_count_(0) + { + } + + void notify(const PCDType::ConstPtr& message) + { + ++count_; + } + + void failure(const PCDType::ConstPtr& message, FilterFailureReason reason) + { + ++failure_count_; + } + + int count_; + int expected_count_; + int failure_count_; +}; + +TEST(MessageFilter, noTransforms) +{ + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "frame1", 1); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + + PCDType::Ptr msg(new PCDType); + ros::Time stamp = ros::Time::now(); + setStamp(stamp, msg->header.stamp); + msg->header.frame_id = "frame2"; + filter.add(msg); + + EXPECT_EQ(0, n.count_); +} + +TEST(MessageFilter, noTransformsSameFrame) +{ + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "frame1", 1); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + + PCDType::Ptr msg(new PCDType); + ros::Time stamp = ros::Time::now(); + setStamp(stamp, msg->header.stamp); + msg->header.frame_id = "frame1"; + filter.add(msg); + + EXPECT_EQ(1, n.count_); +} + +TEST(MessageFilter, preexistingTransforms) +{ + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "frame1", 1); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + + PCDType::Ptr msg(new PCDType); + + ros::Time stamp = ros::Time::now(); + setStamp(stamp, msg->header.stamp); + + tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf_client.setTransform(transform); + + msg->header.frame_id = "frame2"; + filter.add(msg); + + EXPECT_EQ(1, n.count_); +} + +TEST(MessageFilter, postTransforms) +{ + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "frame1", 1); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + + ros::Time stamp = ros::Time::now(); + PCDType::Ptr msg(new PCDType); + setStamp(stamp, msg->header.stamp); + msg->header.frame_id = "frame2"; + + filter.add(msg); + + EXPECT_EQ(0, n.count_); + + tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf_client.setTransform(transform); + + ros::WallDuration(0.1).sleep(); + ros::spinOnce(); + + EXPECT_EQ(1, n.count_); +} + +TEST(MessageFilter, queueSize) +{ + tf::TransformListener tf_client; + Notification n(10); + MessageFilter filter(tf_client, "frame1", 10); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); + + ros::Time stamp = ros::Time::now(); + pcl::uint64_t pcl_stamp; + setStamp(stamp, pcl_stamp); + + for (int i = 0; i < 20; ++i) + { + PCDType::Ptr msg(new PCDType); + msg->header.stamp = pcl_stamp; + msg->header.frame_id = "frame2"; + + filter.add(msg); + } + + EXPECT_EQ(0, n.count_); + EXPECT_EQ(10, n.failure_count_); + + tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf_client.setTransform(transform); + + ros::WallDuration(0.1).sleep(); + ros::spinOnce(); + + EXPECT_EQ(10, n.count_); +} + +TEST(MessageFilter, setTargetFrame) +{ + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "frame1", 1); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + filter.setTargetFrame("frame1000"); + + ros::Time stamp = ros::Time::now(); + PCDType::Ptr msg(new PCDType); + setStamp(stamp, msg->header.stamp); + msg->header.frame_id = "frame2"; + + tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2"); + tf_client.setTransform(transform); + + filter.add(msg); + + + EXPECT_EQ(1, n.count_); +} + +TEST(MessageFilter, multipleTargetFrames) +{ + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "", 1); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + + std::vector target_frames; + target_frames.push_back("frame1"); + target_frames.push_back("frame2"); + filter.setTargetFrames(target_frames); + + ros::Time stamp = ros::Time::now(); + PCDType::Ptr msg(new PCDType); + setStamp(stamp, msg->header.stamp); + + tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3"); + tf_client.setTransform(transform); + + msg->header.frame_id = "frame3"; + filter.add(msg); + + ros::WallDuration(0.1).sleep(); + ros::spinOnce(); + + EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet) + + //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0)); + + transform.child_frame_id_ = "frame2"; + tf_client.setTransform(transform); + + ros::WallDuration(0.1).sleep(); + ros::spinOnce(); + + EXPECT_EQ(1, n.count_); // frame2->frame3 now exists +} + +TEST(MessageFilter, tolerance) +{ + ros::Duration offset(0.2); + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "frame1", 1); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + filter.setTolerance(offset); + + ros::Time stamp = ros::Time::now(); + pcl::uint64_t pcl_stamp; + setStamp(stamp, pcl_stamp); + tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf_client.setTransform(transform); + + PCDType::Ptr msg(new PCDType); + msg->header.frame_id = "frame2"; + msg->header.stamp = pcl_stamp; + filter.add(msg); + + EXPECT_EQ(0, n.count_); //No return due to lack of space for offset + + //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1)); + + transform.stamp_ += offset*1.1; + tf_client.setTransform(transform); + + ros::WallDuration(0.1).sleep(); + ros::spinOnce(); + + EXPECT_EQ(1, n.count_); // Now have data for the message published earlier + + stamp += offset; + setStamp(stamp, pcl_stamp); + msg->header.stamp = pcl_stamp; + + filter.add(msg); + + EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset +} + +TEST(MessageFilter, outTheBackFailure) +{ + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "frame1", 1); + filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); + + ros::Time stamp = ros::Time::now(); + PCDType::Ptr msg(new PCDType); + setStamp(stamp, msg->header.stamp); + + tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf_client.setTransform(transform); + + transform.stamp_ = stamp + ros::Duration(10000); + tf_client.setTransform(transform); + + msg->header.frame_id = "frame2"; + filter.add(msg); + + EXPECT_EQ(1, n.failure_count_); +} + +TEST(MessageFilter, emptyFrameIDFailure) +{ + tf::TransformListener tf_client; + Notification n(1); + MessageFilter filter(tf_client, "frame1", 1); + filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); + + PCDType::Ptr msg(new PCDType); + msg->header.frame_id = ""; + filter.add(msg); + + EXPECT_EQ(1, n.failure_count_); +} + +TEST(MessageFilter, removeCallback) +{ + // Callback queue in separate thread + ros::CallbackQueue cbqueue; + ros::AsyncSpinner spinner(1, &cbqueue); + ros::NodeHandle threaded_nh; + threaded_nh.setCallbackQueue(&cbqueue); + + // TF filters; no data needs to be published + boost::scoped_ptr tf_listener; + boost::scoped_ptr > tf_filter; + + spinner.start(); + for (int i = 0; i < 3; ++i) { + tf_listener.reset(new tf::TransformListener()); + // Have callback fire at high rate to increase chances of race condition + tf_filter.reset( + new tf::MessageFilter(*tf_listener, + "map", 5, threaded_nh, + ros::Duration(0.000001))); + + // Sleep and reset; sleeping increases chances of race condition + ros::Duration(0.001).sleep(); + tf_filter.reset(); + tf_listener.reset(); + } + spinner.stop(); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::Time::setNow(ros::Time()); + ros::init(argc, argv, "test_message_filter"); + ros::NodeHandle nh; + + int ret = RUN_ALL_TESTS(); + + return ret; +} diff --git a/pcl_ros/tests/test_tf_message_filter_pcl.xml b/pcl_ros/tests/test_tf_message_filter_pcl.xml new file mode 100644 index 00000000..501b0190 --- /dev/null +++ b/pcl_ros/tests/test_tf_message_filter_pcl.xml @@ -0,0 +1,3 @@ + + + From 3a202ced73d8cff4f2e71467dc46c4bfbe53434b Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Wed, 4 Mar 2015 13:59:09 -0800 Subject: [PATCH 247/405] fixed indentation and author --- pcl_ros/src/test/test_tf_message_filter_pcl.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp index d629060a..40861945 100644 --- a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp +++ b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -27,7 +27,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/** \author Josh Faust */ +/** \author Brice Rebsamen + * Copied and adapted from geometry/test_message_filter.cpp + */ #include #include @@ -365,8 +367,8 @@ TEST(MessageFilter, removeCallback) // Have callback fire at high rate to increase chances of race condition tf_filter.reset( new tf::MessageFilter(*tf_listener, - "map", 5, threaded_nh, - ros::Duration(0.000001))); + "map", 5, threaded_nh, + ros::Duration(0.000001))); // Sleep and reset; sleeping increases chances of race condition ros::Duration(0.001).sleep(); From 76f900c0ca9ebe57307609ef5a87d5f41587707e Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Wed, 11 Mar 2015 17:41:33 -0700 Subject: [PATCH 248/405] commented out test_tf_message_filter_pcl Until ros/geometry#80 has been merged the test will fail. --- pcl_ros/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 97e50fa1..a505bdfc 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -175,11 +175,11 @@ target_link_libraries(convert_pointcloud_to_image pcl_io ${Boost_LIBRARIES} ${ca if(CATKIN_ENABLE_TESTING) # we don't have to find_package GTEST; catkin does it for us include_directories(${GTEST_INCLUDE_DIRS}) - add_executable(test_tf_message_filter_pcl EXCLUDE_FROM_ALL src/test/test_tf_message_filter_pcl.cpp) - target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - add_dependencies(tests test_tf_message_filter_pcl) + #add_executable(test_tf_message_filter_pcl EXCLUDE_FROM_ALL src/test/test_tf_message_filter_pcl.cpp) + #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) + #add_dependencies(tests test_tf_message_filter_pcl) find_package(rostest) - add_rostest(tests/test_tf_message_filter_pcl.xml) + #add_rostest(tests/test_tf_message_filter_pcl.xml) endif(CATKIN_ENABLE_TESTING) From 8ffb7a318964393f8523c3a5bf052a4b65927a56 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Wed, 27 May 2015 16:52:23 -0400 Subject: [PATCH 249/405] Fixed test for jade-devel. Progress on #92 --- pcl_ros/CMakeLists.txt | 38 +++++++++---------- ....xml => test_tf_message_filter_pcl.launch} | 0 2 files changed, 17 insertions(+), 21 deletions(-) rename pcl_ros/tests/{test_tf_message_filter_pcl.xml => test_tf_message_filter_pcl.launch} (100%) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index a505bdfc..7ead4297 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -4,7 +4,7 @@ project(pcl_ros) ## Find system dependencies find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) -find_package(Eigen REQUIRED) +find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) ## Find catkin packages @@ -29,7 +29,7 @@ find_package(catkin REQUIRED COMPONENTS include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) @@ -70,13 +70,13 @@ catkin_package( tf DEPENDS Boost - Eigen + Eigen3 PCL ) ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) -target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_tf pcl_ros_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) ## Nodelets @@ -89,7 +89,7 @@ add_library(pcl_ros_io src/pcl_ros/io/io.cpp src/pcl_ros/io/pcd_io.cpp ) -target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) class_loader_hide_library_symbols(pcl_ros_io) ## Declare the pcl_ros_features library @@ -108,7 +108,7 @@ add_library(pcl_ros_features src/pcl_ros/features/principal_curvatures.cpp src/pcl_ros/features/vfh.cpp ) -target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_features) @@ -124,7 +124,7 @@ add_library(pcl_ros_filters 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} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_filters) @@ -136,7 +136,7 @@ add_library (pcl_ros_segmentation src/pcl_ros/segmentation/segment_differences.cpp src/pcl_ros/segmentation/segmentation.cpp ) -target_link_libraries(pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_segmentation) @@ -147,39 +147,35 @@ add_library (pcl_ros_surface src/pcl_ros/surface/convex_hull.cpp src/pcl_ros/surface/moving_least_squares.cpp ) -target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_surface) ## Tools add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) -target_link_libraries(pcd_to_pointcloud pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcd_to_pointcloud pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) -target_link_libraries(pointcloud_to_pcd pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARIES}) +target_link_libraries(pointcloud_to_pcd pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARY_DIRS} ${PCL_LIBRARIES}) add_executable(bag_to_pcd tools/bag_to_pcd.cpp) -target_link_libraries(bag_to_pcd pcl_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARIES}) +target_link_libraries(bag_to_pcd pcl_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARY_DIRS} ${PCL_LIBRARIES}) add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) -target_link_libraries(convert_pcd_to_image pcl_io pcl_common ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(convert_pcd_to_image pcl_io pcl_common ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) -target_link_libraries(convert_pointcloud_to_image pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(convert_pointcloud_to_image pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) ############# ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - # we don't have to find_package GTEST; catkin does it for us - include_directories(${GTEST_INCLUDE_DIRS}) - #add_executable(test_tf_message_filter_pcl EXCLUDE_FROM_ALL src/test/test_tf_message_filter_pcl.cpp) - #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - #add_dependencies(tests test_tf_message_filter_pcl) - find_package(rostest) - #add_rostest(tests/test_tf_message_filter_pcl.xml) + find_package(rostest REQUIRED) + 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}) endif(CATKIN_ENABLE_TESTING) diff --git a/pcl_ros/tests/test_tf_message_filter_pcl.xml b/pcl_ros/tests/test_tf_message_filter_pcl.launch similarity index 100% rename from pcl_ros/tests/test_tf_message_filter_pcl.xml rename to pcl_ros/tests/test_tf_message_filter_pcl.launch From 5472ff8f8aadde033dbbd6b3bf5aa81e52638597 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 8 Jun 2015 08:52:04 -0400 Subject: [PATCH 250/405] Remove pointcloud_to_laserscan package --- perception_pcl/package.xml | 1 - pointcloud_to_laserscan/CHANGELOG.rst | 43 ---- pointcloud_to_laserscan/CMakeLists.txt | 45 ---- .../pointcloud_to_laserscan_nodelet.h | 96 ------- .../launch/sample_node.launch | 40 --- .../launch/sample_nodelet.launch | 40 --- pointcloud_to_laserscan/nodelets.xml | 11 - pointcloud_to_laserscan/package.xml | 37 --- .../src/pointcloud_to_laserscan_node.cpp | 65 ----- .../src/pointcloud_to_laserscan_nodelet.cpp | 240 ------------------ 10 files changed, 618 deletions(-) delete mode 100644 pointcloud_to_laserscan/CHANGELOG.rst delete mode 100644 pointcloud_to_laserscan/CMakeLists.txt delete mode 100644 pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h delete mode 100644 pointcloud_to_laserscan/launch/sample_node.launch delete mode 100644 pointcloud_to_laserscan/launch/sample_nodelet.launch delete mode 100644 pointcloud_to_laserscan/nodelets.xml delete mode 100644 pointcloud_to_laserscan/package.xml delete mode 100644 pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp delete mode 100644 pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 7bb2fc38..f7981fc1 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -23,7 +23,6 @@ pcl_conversions pcl_msgs pcl_ros - pointcloud_to_laserscan diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst deleted file mode 100644 index 5c2b3e50..00000000 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ /dev/null @@ -1,43 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package pointcloud_to_laserscan -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.6 (2015-02-04) ------------------- -* Fix default value for concurrency -* Fix multithreaded lazy pub sub -* Contributors: Paul Bovbel - -1.2.5 (2015-01-20) ------------------- -* Switch to tf_sensor_msgs for transform -* Set parameters in sample launch files to default -* Add tolerance parameter -* Contributors: Paul Bovbel - -1.2.4 (2015-01-15) ------------------- -* Remove stray dependencies -* Refactor with tf2 and message filters -* Remove roslaunch check -* Fix regressions -* Refactor to allow debug messages from node and nodelet -* Contributors: Paul Bovbel - -1.2.3 (2015-01-10) ------------------- -* add launch tests -* refactor naming and fix nodelet export -* set default target frame to empty -* clean up package.xml -* Contributors: Paul Bovbel - -1.2.2 (2014-10-25) ------------------- -* clean up package.xml -* Fix header reference -* Fix flow -* Fix pointer assertion -* Finalize pointcloud to laserscan -* Initial pointcloud to laserscan commit -* Contributors: Paul Bovbel \ No newline at end of file diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt deleted file mode 100644 index 193e3ac6..00000000 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(pointcloud_to_laserscan) - -find_package(catkin REQUIRED COMPONENTS - message_filters - nodelet - roscpp - sensor_msgs - tf2 - tf2_ros - tf2_sensor_msgs -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES pointcloud_to_laserscan - CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_nodelet.cpp) -target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES}) - -add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp) -target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES}) - -install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(FILES nodelets.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h deleted file mode 100644 index bed70952..00000000 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010-2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of 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. - * - * - */ - -/* - * Author: Paul Bovbel - */ - -#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET -#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET - -#include "ros/ros.h" -#include "boost/thread/mutex.hpp" - -#include "nodelet/nodelet.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/message_filter.h" -#include "message_filters/subscriber.h" -#include "sensor_msgs/PointCloud2.h" - - -namespace pointcloud_to_laserscan -{ - typedef tf2_ros::MessageFilter MessageFilter; -/** -* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot -* pointcloud_to_laserscan implementation. -*/ - class PointCloudToLaserScanNodelet : public nodelet::Nodelet - { - - public: - PointCloudToLaserScanNodelet(); - - private: - virtual void onInit(); - - void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg); - void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, - tf2_ros::filter_failure_reasons::FilterFailureReason reason); - - void connectCb(); - - void disconnectCb(); - - ros::NodeHandle nh_, private_nh_; - ros::Publisher pub_; - boost::mutex connect_mutex_; - - tf2_ros::Buffer tf2_; - message_filters::Subscriber sub_; - boost::shared_ptr message_filter_; - - // ROS Parameters - unsigned int input_queue_size_; - std::string target_frame_; - double tolerance_; - double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_; - bool use_inf_; - }; - -} // pointcloud_to_laserscan - -#endif // POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch deleted file mode 100644 index 1d346365..00000000 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - #target_frame: # Leave disabled to output scan in pointcloud frame - transform_tolerance: 0.01 - min_height: 0.0 - max_height: 1.0 - - angle_min: -1.5708 # -M_PI/2 - angle_max: 1.5708 # M_PI/2 - angle_increment: 0.087 # M_PI/360.0 - scan_time: 0.3333 - range_min: 0.45 - range_max: 4.0 - use_inf: true - - # Concurrency level, affects number of pointclouds queued for processing and number of threads used - # 0 : Detect number of cores - # 1 : Single threaded - # 2->inf : Parallelism level - concurrency_level: 0 - - - - - \ No newline at end of file diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch deleted file mode 100644 index ef784310..00000000 --- a/pointcloud_to_laserscan/launch/sample_nodelet.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - #target_frame: # Leave disabled to output scan in pointcloud frame - transform_tolerance: 0.01 - min_height: 0.0 - max_height: 1.0 - - angle_min: -1.5708 # -M_PI/2 - angle_max: 1.5708 # M_PI/2 - angle_increment: 0.087 # M_PI/360.0 - scan_time: 0.3333 - range_min: 0.45 - range_max: 4.0 - use_inf: true - - # Concurrency level, affects number of pointclouds queued for processing, thread number governed by nodelet manager - # 0 : Detect number of cores - # 1 : Single threaded - # 2->inf : Parallelism level - concurrency_level: 0 - - - - - \ No newline at end of file diff --git a/pointcloud_to_laserscan/nodelets.xml b/pointcloud_to_laserscan/nodelets.xml deleted file mode 100644 index 327a819e..00000000 --- a/pointcloud_to_laserscan/nodelets.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - Nodelet to convert sensor_msgs/PointCloud2 to sensor_msgs/LaserScans. - - - - diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml deleted file mode 100644 index b3985006..00000000 --- a/pointcloud_to_laserscan/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - pointcloud_to_laserscan - 1.2.6 - Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM). - - Paul Bovbel - Tully Foote - - BSD - - http://ros.org/wiki/perception_pcl - https://github.com/ros-perception/perception_pcl/issues - https://github.com/ros-perception/perception_pcl - - catkin - - message_filters - nodelet - roscpp - sensor_msgs - tf2 - tf2_ros - tf2_sensor_msgs - - message_filters - nodelet - roscpp - sensor_msgs - tf2 - tf2_ros - tf2_sensor_msgs - - - - - \ No newline at end of file diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp deleted file mode 100644 index 7eb7c219..00000000 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010-2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of 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. - * - * - */ - -/* - * Author: Paul Bovbel - */ - -#include -#include - -int main(int argc, char **argv){ - ros::init(argc, argv, "pointcloud_to_laserscan_node"); - ros::NodeHandle private_nh("~"); - int concurrency_level; - private_nh.param("concurrency_level", concurrency_level, 0); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - std::string nodelet_name = ros::this_node::getName(); - nodelet.load(nodelet_name, "pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet", remap, nargv); - - boost::shared_ptr spinner; - if(concurrency_level) { - spinner.reset(new ros::MultiThreadedSpinner(concurrency_level)); - }else{ - spinner.reset(new ros::MultiThreadedSpinner()); - } - spinner->spin(); - return 0; - -} diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp deleted file mode 100644 index 256fddff..00000000 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010-2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of 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. - * - * - */ - -/* - * Author: Paul Bovbel - */ - -#include -#include -#include -#include -#include - -namespace pointcloud_to_laserscan -{ - - PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() {} - - void PointCloudToLaserScanNodelet::onInit() - { - boost::mutex::scoped_lock lock(connect_mutex_); - private_nh_ = getPrivateNodeHandle(); - - private_nh_.param("target_frame", target_frame_, ""); - private_nh_.param("tolerance", tolerance_, 0.01); - private_nh_.param("min_height", min_height_, 0.0); - private_nh_.param("max_height", max_height_, 1.0); - - private_nh_.param("angle_min", angle_min_, -M_PI / 2.0); - private_nh_.param("angle_max", angle_max_, M_PI / 2.0); - private_nh_.param("angle_increment", angle_increment_, M_PI / 360.0); - private_nh_.param("scan_time", scan_time_, 1.0 / 30.0); - private_nh_.param("range_min", range_min_, 0.45); - private_nh_.param("range_max", range_max_, 4.0); - - int concurrency_level; - private_nh_.param("concurrency_level", concurrency_level, 1); - private_nh_.param("use_inf", use_inf_, true); - - //Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size - if (concurrency_level == 1) - { - nh_ = getNodeHandle(); - } - else - { - nh_ = getMTNodeHandle(); - } - - // Only queue one pointcloud per running thread - if (concurrency_level > 0) - { - input_queue_size_ = concurrency_level; - } - else - { - input_queue_size_ = boost::thread::hardware_concurrency(); - } - - // if pointcloud target frame specified, we need to filter by transform availability - if (!target_frame_.empty()) - { - message_filter_.reset(new MessageFilter(sub_, tf2_, target_frame_, input_queue_size_, nh_)); - message_filter_->setTolerance(ros::Duration(tolerance_)); - message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); - message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2)); - } - else // otherwise setup direct subscription - { - sub_.registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); - } - - pub_ = nh_.advertise("scan", 10, - boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), - boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); - } - - void PointCloudToLaserScanNodelet::connectCb() - { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() > 0 && sub_.getSubscriber().getNumPublishers() == 0) - { - NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud"); - sub_.subscribe(nh_, "cloud_in", input_queue_size_); - } - } - - void PointCloudToLaserScanNodelet::disconnectCb() - { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) - { - NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud"); - sub_.unsubscribe(); - } - } - - void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, - tf2_ros::filter_failure_reasons::FilterFailureReason reason) - { - NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to " - << message_filter_->getTargetFramesString() << " with tolerance " << tolerance_); - } - - void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) - { - - //build laserscan output - sensor_msgs::LaserScan output; - output.header = cloud_msg->header; - if (!target_frame_.empty()) - { - output.header.frame_id = target_frame_; - } - - output.angle_min = angle_min_; - output.angle_max = angle_max_; - output.angle_increment = angle_increment_; - output.time_increment = 0.0; - output.scan_time = scan_time_; - output.range_min = range_min_; - output.range_max = range_max_; - - //determine amount of rays to create - uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); - - //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range - if (use_inf_) - { - output.ranges.assign(ranges_size, std::numeric_limits::infinity()); - } - else - { - output.ranges.assign(ranges_size, output.range_max + 1.0); - } - - sensor_msgs::PointCloud2ConstPtr cloud_out; - sensor_msgs::PointCloud2Ptr cloud; - - // Transform cloud if necessary - if (!(output.header.frame_id == cloud_msg->header.frame_id)) - { - try - { - cloud.reset(new sensor_msgs::PointCloud2); - tf2_.transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_)); - cloud_out = cloud; - } - catch (tf2::TransformException ex) - { - NODELET_ERROR_STREAM("Transform failure: " << ex.what()); - return; - } - } - else - { - cloud_out = cloud_msg; - } - - // Iterate through pointcloud - for (sensor_msgs::PointCloud2ConstIterator - iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"), iter_z(*cloud_out, "z"); - iter_x != iter_x.end(); - ++iter_x, ++iter_y, ++iter_z) - { - - if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) - { - NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z); - continue; - } - - if (*iter_z > max_height_ || *iter_z < min_height_) - { - NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_); - continue; - } - - double range = hypot(*iter_x, *iter_y); - if (range < range_min_) - { - NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y, - *iter_z); - continue; - } - - double angle = atan2(*iter_y, *iter_x); - if (angle < output.angle_min || angle > output.angle_max) - { - NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); - continue; - } - - //overwrite range at laserscan ray if new range is smaller - int index = (angle - output.angle_min) / output.angle_increment; - if (range < output.ranges[index]) - { - output.ranges[index] = range; - } - - } - pub_.publish(output); - } - -} - -PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet); From d050db471edd04e283b9786513114f76355b5b93 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 8 Jun 2015 10:28:55 -0400 Subject: [PATCH 251/405] Add deprecation notice --- pointcloud_to_laserscan/README.md | 1 + 1 file changed, 1 insertion(+) create mode 100644 pointcloud_to_laserscan/README.md diff --git a/pointcloud_to_laserscan/README.md b/pointcloud_to_laserscan/README.md new file mode 100644 index 00000000..2ed7b5be --- /dev/null +++ b/pointcloud_to_laserscan/README.md @@ -0,0 +1 @@ +pointcloud_to_laserscan has moved to http://github.com/ros-perception/pointcloud_to_laserscan.git From c40e5afdbd22531cdef36475946b57ca6e8a884b Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 22 Jun 2015 17:31:48 +0200 Subject: [PATCH 252/405] cleanup broken library links All removed library names are included in ${PCL_LIBRARIES}. However, the plain library names broke catkin's overlay mechanism: Where ${PCL_LIBRARIES} could point to a local installation of the PCL, e.g. pcd_ros_segmentation might still link to the system-wide installed version of pcl_segmentation. --- pcl_ros/CMakeLists.txt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 7ead4297..f11bd183 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -136,7 +136,7 @@ add_library (pcl_ros_segmentation src/pcl_ros/segmentation/segment_differences.cpp src/pcl_ros/segmentation/segmentation.cpp ) -target_link_libraries(pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_segmentation) @@ -154,19 +154,19 @@ class_loader_hide_library_symbols(pcl_ros_surface) ## Tools add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) -target_link_libraries(pcd_to_pointcloud pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) -target_link_libraries(pointcloud_to_pcd pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARY_DIRS} ${PCL_LIBRARIES}) +target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARY_DIRS} ${PCL_LIBRARIES}) add_executable(bag_to_pcd tools/bag_to_pcd.cpp) -target_link_libraries(bag_to_pcd pcl_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARY_DIRS} ${PCL_LIBRARIES}) +target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARY_DIRS} ${PCL_LIBRARIES}) add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) -target_link_libraries(convert_pcd_to_image pcl_io pcl_common ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) -target_link_libraries(convert_pointcloud_to_image pcl_io ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) ############# ## Testing ## From 8d457bbd610fbe4bbf0d3925cbe4f526ded34f30 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 22 Jun 2015 17:14:08 -0400 Subject: [PATCH 253/405] Update changelogs --- pcl_ros/CHANGELOG.rst | 25 +++++++++++++++++++++++++ perception_pcl/CHANGELOG.rst | 5 +++++ 2 files changed, 30 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 9c0236af..1763c9bd 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* cleanup broken library links + All removed library names are included in ${PCL_LIBRARIES}. + However, the plain library names broke catkin's overlay mechanism: + Where ${PCL_LIBRARIES} could point to a local installation of the PCL, + e.g. pcd_ros_segmentation might still link to the system-wide installed version + of pcl_segmentation. +* Fixed test for jade-devel. Progress on `#92 `_ +* commented out test_tf_message_filter_pcl + Until `ros/geometry#80 `_ has been merged the test will fail. +* fixed indentation and author +* Adds a test for tf message filters with pcl pointclouds +* specialized HasHeader, TimeStamp, FrameId + - HasHeader now returns false + - TimeStamp and FrameId specialed for pcl::PointCloud for any point type T + These changes allow to use pcl::PointCloud with tf::MessageFilter +* Sync pcl_nodelets.xml from hydro to indigo + Fixes to pass catkin lint -W1 +* Fixes `#87 `_ for Indigo +* Fixes `#85 `_ for Indigo +* Fixes `#77 `_ and `#80 `_ for indigo +* Added option to save pointclouds in binary and binary compressed format +* Contributors: Brice Rebsamen, Lucid One, Mitchell Wills, v4hn + 1.2.6 (2015-02-04) ------------------ diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 08682e03..3d278376 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove pointcloud_to_laserscan package +* Contributors: Paul Bovbel + 1.2.6 (2015-02-04) ------------------ From 8bb614fc3a14e26d182fc2c99c4a0dcf1908ff65 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 22 Jun 2015 17:14:42 -0400 Subject: [PATCH 254/405] 1.3.0 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 1763c9bd..411d9f6c 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2015-06-22) +------------------ * cleanup broken library links All removed library names are included in ${PCL_LIBRARIES}. However, the plain library names broke catkin's overlay mechanism: diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index e47a1d43..f8e0d7d1 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.2.6 + 1.3.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 3d278376..ebb57820 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2015-06-22) +------------------ * Remove pointcloud_to_laserscan package * Contributors: Paul Bovbel diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index f7981fc1..baa31b9d 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.2.6 + 1.3.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 96f6657b23efca4bb0a92a4d80ff09516d22666b Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Fri, 22 Apr 2016 10:54:56 -0400 Subject: [PATCH 255/405] Remove python-vtk for kinetic-devel, see issue #44 --- pcl_ros/package.xml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index f8e0d7d1..da53ce27 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -18,7 +18,7 @@ http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl - + catkin cmake_modules rosconsole @@ -38,7 +38,6 @@ sensor_msgs std_msgs tf - python-vtk libvtk-java dynamic_reconfigure @@ -56,9 +55,8 @@ sensor_msgs std_msgs tf - python-vtk libvtk-java - + rostest From 7fd1bac6769c6679053cb4bb7b5c82f208cbc236 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 21 Apr 2016 15:18:33 -0700 Subject: [PATCH 256/405] manually remove dependency on vtkproj from PCL_LIBRARIES --- pcl_ros/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index f11bd183..73a41404 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -5,7 +5,11 @@ project(pcl_ros) find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED) +find_package(PCL 1.7 REQUIRED) + +if(NOT "${PCL_LIBRARIES}" STREQUAL "") + list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") +endif() ## Find catkin packages find_package(catkin REQUIRED COMPONENTS From eba277ffc0360f2b6bd3d616c9da34b3ae02c4a5 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 21 Apr 2016 16:01:44 -0700 Subject: [PATCH 257/405] Add build depend on libproj, since it's not provided by vtk right now --- pcl_ros/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index da53ce27..d08967c1 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -39,6 +39,8 @@ std_msgs tf libvtk-java + + libproj-dev dynamic_reconfigure eigen From 20ca7cb16af0911c488562b812137566334be18f Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Fri, 22 Apr 2016 10:58:46 -0400 Subject: [PATCH 258/405] Fixup libproj-dev rosdep --- pcl_ros/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index d08967c1..fc242e7e 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -40,7 +40,7 @@ tf libvtk-java - libproj-dev + proj dynamic_reconfigure eigen From 6e20601cf1fdb1a0ae8f11d784f22d1e069b87d7 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Fri, 22 Apr 2016 11:00:26 -0400 Subject: [PATCH 259/405] Update changelogs --- pcl_ros/CHANGELOG.rst | 8 ++++++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 11 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 411d9f6c..56b0f2a5 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixup libproj-dev rosdep +* Add build depend on libproj, since it's not provided by vtk right now +* manually remove dependency on vtkproj from PCL_LIBRARIES +* Remove python-vtk for kinetic-devel, see issue `#44 `_ +* Contributors: Jackie Kay, Paul Bovbel + 1.3.0 (2015-06-22) ------------------ * cleanup broken library links diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index ebb57820..9f90a3ac 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2015-06-22) ------------------ * Remove pointcloud_to_laserscan package From 2aa0495516396cd1cf5c66993eb63aa4f8c6eb4f Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Fri, 22 Apr 2016 11:00:45 -0400 Subject: [PATCH 260/405] 1.4.0 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 56b0f2a5..d874a4c8 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.4.0 (2016-04-22) +------------------ * Fixup libproj-dev rosdep * Add build depend on libproj, since it's not provided by vtk right now * manually remove dependency on vtkproj from PCL_LIBRARIES diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index fc242e7e..ecc1811f 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,6 +1,6 @@ pcl_ros - 1.3.0 + 1.4.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 9f90a3ac..a712baae 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.4.0 (2016-04-22) +------------------ 1.3.0 (2015-06-22) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index baa31b9d..9598331c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.3.0 + 1.4.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From aa370cefc883842d470daa8fb96e1c5b36d683eb Mon Sep 17 00:00:00 2001 From: Jochen Sprickerhof Date: Tue, 18 Aug 2015 17:01:24 +0200 Subject: [PATCH 261/405] Add fixed_frame to pointcloud_to_pcd --- pcl_ros/tools/pointcloud_to_pcd.cpp | 36 +++++++++++++++++++++++------ 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index 35e9a63d..5c503eb7 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -40,6 +40,10 @@ #include +#include +#include +#include + // PCL includes #include #include @@ -47,6 +51,8 @@ #include +#include + using namespace std; /** @@ -65,6 +71,9 @@ class PointCloudToPCD std::string prefix_; bool binary_; bool compressed_; + std::string fixed_frame_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; public: string cloud_topic_; @@ -84,6 +93,21 @@ class PointCloudToPCD cloud->header.frame_id.c_str (), pcl::getFieldsList (*cloud).c_str ()); + Eigen::Vector4f v = Eigen::Vector4f::Zero (); + Eigen::Quaternionf q = Eigen::Quaternionf::Identity (); + if (!fixed_frame_.empty ()) { + if (!tf_buffer_.canTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp), ros::Duration (3.0))) { + ROS_WARN("Could not get transform!"); + return; + } + + Eigen::Affine3d transform; + transform = tf2::transformToEigen (tf_buffer_.lookupTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp))); + v = Eigen::Vector4f::Zero (); + v.head<3> () = transform.translation ().cast (); + q = transform.rotation ().cast (); + } + std::stringstream ss; ss << prefix_ << cloud->header.stamp << ".pcd"; ROS_INFO ("Data saved to %s", ss.str ().c_str ()); @@ -93,25 +117,22 @@ class PointCloudToPCD { if(compressed_) { - writer.writeBinaryCompressed (ss.str (), *cloud, Eigen::Vector4f::Zero (), - Eigen::Quaternionf::Identity ()); + writer.writeBinaryCompressed (ss.str (), *cloud, v, q); } else { - writer.writeBinary (ss.str (), *cloud, Eigen::Vector4f::Zero (), - Eigen::Quaternionf::Identity ()); + writer.writeBinary (ss.str (), *cloud, v, q); } } else { - writer.writeASCII (ss.str (), *cloud, Eigen::Vector4f::Zero (), - Eigen::Quaternionf::Identity (), 8); + writer.writeASCII (ss.str (), *cloud, v, q, 8); } } //////////////////////////////////////////////////////////////////////////////// - PointCloudToPCD () : binary_(false), compressed_(false) + PointCloudToPCD () : binary_(false), compressed_(false), tf_listener_(tf_buffer_) { // Check if a prefix parameter is defined for output file names. ros::NodeHandle priv_nh("~"); @@ -125,6 +146,7 @@ class PointCloudToPCD << prefix_); } + priv_nh.getParam ("fixed_frame", fixed_frame_); priv_nh.getParam ("binary", binary_); priv_nh.getParam ("compressed", compressed_); if(binary_) From 360b87131bb9767beb3b8e82f7897234c695a23a Mon Sep 17 00:00:00 2001 From: Martin Guenther Date: Fri, 22 Apr 2016 14:46:11 +0200 Subject: [PATCH 262/405] Fix handling of unorganized input in convert_pointcloud_to_image Without this patch, convert_pointcloud_to_image doesn't correctly recognize unorganized point clouds and outputs garbage that crashes image_view. --- pcl_ros/tools/convert_pointcloud_to_image.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp index 0fdd5e87..678bf0e4 100644 --- a/pcl_ros/tools/convert_pointcloud_to_image.cpp +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -56,18 +56,21 @@ public: void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud) { - if ((cloud->width * cloud->height) == 0) - return; //return if the cloud is not dense! + if (cloud->height <= 1) + { + ROS_ERROR("Input point cloud is not organized, ignoring!"); + return; + } try { pcl::toROSMsg (*cloud, image_); //convert the cloud + image_pub_.publish (image_); //publish our cloud image } catch (std::runtime_error &e) { ROS_ERROR_STREAM("Error in converting cloud to image message: " << e.what()); } - image_pub_.publish (image_); //publish our cloud image } PointCloudToImage () : cloud_topic_("input"),image_topic_("output") { From b1c6428c83ac4273023b9b6762a9bf41ff9e5c64 Mon Sep 17 00:00:00 2001 From: Martin Guenther Date: Fri, 22 Apr 2016 14:51:07 +0200 Subject: [PATCH 263/405] Copy header in convert_pointcloud_to_image --- pcl_ros/tools/convert_pointcloud_to_image.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp index 678bf0e4..a7a4e1c1 100644 --- a/pcl_ros/tools/convert_pointcloud_to_image.cpp +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -64,6 +64,7 @@ public: try { pcl::toROSMsg (*cloud, image_); //convert the cloud + image_.header = cloud->header; image_pub_.publish (image_); //publish our cloud image } catch (std::runtime_error &e) From 9f629d35c974896e2f31f31c95db93cbb577da59 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Wed, 4 May 2016 17:02:14 -0400 Subject: [PATCH 264/405] Switch to package format 2 --- pcl_ros/package.xml | 51 ++++++++++++++++----------------------------- 1 file changed, 18 insertions(+), 33 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index ecc1811f..fe646241 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,4 +1,5 @@ - + + pcl_ros 1.4.0 @@ -25,39 +26,23 @@ genmsg roslib - dynamic_reconfigure - eigen - nodelet - nodelet_topic_tools - libpcl-all-dev - pcl_conversions - pcl_msgs - pluginlib - rosbag - roscpp - sensor_msgs - std_msgs - tf - libvtk-java + dynamic_reconfigure + eigen + message_filters + nodelet + nodelet_topic_tools + libpcl-all-dev + pcl_conversions + pcl_msgs + pluginlib + rosbag + roscpp + sensor_msgs + std_msgs + tf + libvtk-java - proj - - dynamic_reconfigure - eigen - message_filters - nodelet - nodelet_topic_tools - libpcl-all - libpcl-all-dev - pcl_conversions - pcl_msgs - pluginlib - rosbag - roscpp - sensor_msgs - std_msgs - tf - libvtk-java + proj rostest From d273c545a7be7afdcc07feb864c52c8ce9ac2bc1 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 8 May 2016 09:41:31 -0400 Subject: [PATCH 265/405] Add tf2_eigen dependency --- pcl_ros/CMakeLists.txt | 1 + pcl_ros/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 73a41404..9665fd34 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs tf + tf2_eigen ) ## Add include directories diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index fe646241..63ca83e8 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -40,6 +40,7 @@ sensor_msgs std_msgs tf + tf2_eigen libvtk-java proj From 909b76d77308f390f6168ea4c356a9e5d9fb4de6 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 8 May 2016 09:41:44 -0400 Subject: [PATCH 266/405] 1.4.1 --- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 63ca83e8..d0fcded4 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.4.0 + 1.4.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 9598331c..ee361f08 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.4.0 + 1.4.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From d1368c0f650d291696a232a9520aeaa25623ba31 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 11 Apr 2017 00:10:10 +0900 Subject: [PATCH 267/405] Fix syntax of nodelet manifest file By splitting files for each library. Close #131 --- pcl_ros/package.xml | 6 +- pcl_ros/pcl_nodelets.xml | 232 ------------------ .../plugins/nodelet/libpcl_ros_features.xml | 85 +++++++ .../plugins/nodelet/libpcl_ros_filters.xml | 43 ++++ pcl_ros/plugins/nodelet/libpcl_ros_io.xml | 49 ++++ .../nodelet/libpcl_ros_segmentation.xml | 37 +++ .../plugins/nodelet/libpcl_ros_surface.xml | 13 + 7 files changed, 232 insertions(+), 233 deletions(-) delete mode 100644 pcl_ros/pcl_nodelets.xml create mode 100644 pcl_ros/plugins/nodelet/libpcl_ros_features.xml create mode 100644 pcl_ros/plugins/nodelet/libpcl_ros_filters.xml create mode 100644 pcl_ros/plugins/nodelet/libpcl_ros_io.xml create mode 100644 pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml create mode 100644 pcl_ros/plugins/nodelet/libpcl_ros_surface.xml diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index d0fcded4..08bd6874 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -48,7 +48,11 @@ rostest - + + + + + diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml deleted file mode 100644 index 215ef9a9..00000000 --- a/pcl_ros/pcl_nodelets.xml +++ /dev/null @@ -1,232 +0,0 @@ - - - - - BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle criterion. The - code makes use of the estimated surface normals at each point in the input data set. - - - - - - FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset - containing points and normals. - - - - - - FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset - containing points and normals, in parallel, using the OpenMP standard. - - - - - - SHOTEstimation estimates SHOT descriptor for a given point cloud dataset - containing points and normals. - - - - - - SHOTEstimationOMP estimates SHOT descriptor for a given point cloud dataset - containing points and normals, in parallel, using the OpenMP standard. - - - - - - MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. - - - - - - NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and curvatures, - in parallel, using the OpenMP standard. - - - - - - NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and curvatures, in - parallel, using Intel's Threading Building Blocks library. - - - - - - NormalEstimation estimates local surface properties at each 3D point, such as surface normals and curvatures. - - - - - - PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset containing - points and normals. - - - - - - PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of principal surface - curvatures for a given point cloud dataset containing points and normals. - - - - - - VFHEstimation estimates the Viewpoint Feature Histogram (VFH) global descriptor for a given point cloud cluster dataset - containing points and normals. - - - - - - - - - - NodeletMUX represent a mux nodelet for PointCloud topics: it takes N (up - to 8) input topics, and publishes all of them on one output topic. - - - - - - NodeletDEMUX represent a demux nodelet for PointCloud topics: it - publishes 1 input topic to N output topics. - - - - - - PCDReader reads in a PCD (Point Cloud Data) v.5 file from disk and converts it to a PointCloud message. - - - - - - BAGReader reads in sensor_msgs/PointCloud2 messages from BAG files. - - - - - - PCDWriter writes a PointCloud message to disk in a PCD (Point Cloud Data) v.5 file format. - - - - - - PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of input PointCloud messages on the - same topic, checks their timestamps, and concatenates their fields together into a single PointCloud output message. - - - - - PointCloudConcatenateDataSynchronizer is a special form of data - synchronizer: it listens for a set of input PointCloud messages on - different topics, and concatenates them together into a single PointCloud - output message. - - - - - - - - - PassThrough is a filter that uses the basic Filter class mechanisms for passing data around. - - - - - - VoxelGrid assembles a local 3D grid over a given PointCloud, and uses that to downsample the data. - - - - - - ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a separate PointCloud. - - - - - - ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. - - - - - - StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. - - - - - RadiusOutlierRemoval uses point neighborhood statistics to filter outlier data. - - - - - - CropBox is a filter that allows the user to filter all the data inside of a given box. - - - - - - - - - ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with a given - height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying inside it. - - - - - - EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. - - - - - - SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in the sense that - it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. - - - - - - SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in the sense that - it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. - - - - - - SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the - difference between them for a maximum given distance threshold. - - - - - - - - - - MovingLeastSquares is an implementation of the MLS algorithm for data reconstruction through bivariate polynomial fitting. - - - - - ConvexHull2D represents a 2D ConvexHull implementation. - - - - diff --git a/pcl_ros/plugins/nodelet/libpcl_ros_features.xml b/pcl_ros/plugins/nodelet/libpcl_ros_features.xml new file mode 100644 index 00000000..2593eb83 --- /dev/null +++ b/pcl_ros/plugins/nodelet/libpcl_ros_features.xml @@ -0,0 +1,85 @@ + + + + + BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle criterion. The + code makes use of the estimated surface normals at each point in the input data set. + + + + + + FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset + containing points and normals. + + + + + + FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset + containing points and normals, in parallel, using the OpenMP standard. + + + + + + SHOTEstimation estimates SHOT descriptor for a given point cloud dataset + containing points and normals. + + + + + + SHOTEstimationOMP estimates SHOT descriptor for a given point cloud dataset + containing points and normals, in parallel, using the OpenMP standard. + + + + + + MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. + + + + + + NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and curvatures, + in parallel, using the OpenMP standard. + + + + + + NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and curvatures, in + parallel, using Intel's Threading Building Blocks library. + + + + + + NormalEstimation estimates local surface properties at each 3D point, such as surface normals and curvatures. + + + + + + PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset containing + points and normals. + + + + + + PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of principal surface + curvatures for a given point cloud dataset containing points and normals. + + + + + + VFHEstimation estimates the Viewpoint Feature Histogram (VFH) global descriptor for a given point cloud cluster dataset + containing points and normals. + + + + diff --git a/pcl_ros/plugins/nodelet/libpcl_ros_filters.xml b/pcl_ros/plugins/nodelet/libpcl_ros_filters.xml new file mode 100644 index 00000000..15476eea --- /dev/null +++ b/pcl_ros/plugins/nodelet/libpcl_ros_filters.xml @@ -0,0 +1,43 @@ + + + + + PassThrough is a filter that uses the basic Filter class mechanisms for passing data around. + + + + + + VoxelGrid assembles a local 3D grid over a given PointCloud, and uses that to downsample the data. + + + + + + ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a separate PointCloud. + + + + + + ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. + + + + + + StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. + + + + + RadiusOutlierRemoval uses point neighborhood statistics to filter outlier data. + + + + + + CropBox is a filter that allows the user to filter all the data inside of a given box. + + + diff --git a/pcl_ros/plugins/nodelet/libpcl_ros_io.xml b/pcl_ros/plugins/nodelet/libpcl_ros_io.xml new file mode 100644 index 00000000..498d5d8b --- /dev/null +++ b/pcl_ros/plugins/nodelet/libpcl_ros_io.xml @@ -0,0 +1,49 @@ + + + + + NodeletMUX represent a mux nodelet for PointCloud topics: it takes N (up + to 8) input topics, and publishes all of them on one output topic. + + + + + + NodeletDEMUX represent a demux nodelet for PointCloud topics: it + publishes 1 input topic to N output topics. + + + + + + PCDReader reads in a PCD (Point Cloud Data) v.5 file from disk and converts it to a PointCloud message. + + + + + + BAGReader reads in sensor_msgs/PointCloud2 messages from BAG files. + + + + + + PCDWriter writes a PointCloud message to disk in a PCD (Point Cloud Data) v.5 file format. + + + + + + PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of input PointCloud messages on the + same topic, checks their timestamps, and concatenates their fields together into a single PointCloud output message. + + + + + PointCloudConcatenateDataSynchronizer is a special form of data + synchronizer: it listens for a set of input PointCloud messages on + different topics, and concatenates them together into a single PointCloud + output message. + + + diff --git a/pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml b/pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml new file mode 100644 index 00000000..845b74a7 --- /dev/null +++ b/pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml @@ -0,0 +1,37 @@ + + + + + ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with a given + height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying inside it. + + + + + + EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. + + + + + + SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in the sense that + it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. + + + + + + SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in the sense that + it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. + + + + + + SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the + difference between them for a maximum given distance threshold. + + + + diff --git a/pcl_ros/plugins/nodelet/libpcl_ros_surface.xml b/pcl_ros/plugins/nodelet/libpcl_ros_surface.xml new file mode 100644 index 00000000..d7c6398b --- /dev/null +++ b/pcl_ros/plugins/nodelet/libpcl_ros_surface.xml @@ -0,0 +1,13 @@ + + + + + MovingLeastSquares is an implementation of the MLS algorithm for data reconstruction through bivariate polynomial fitting. + + + + + ConvexHull2D represents a 2D ConvexHull implementation. + + + From 64996ea8d8b7dd0876139c113b834fb3219b3ffd Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Mon, 24 Apr 2017 01:40:00 +0900 Subject: [PATCH 268/405] Install xml files declaring nodelets https://github.com/ros-perception/perception_pcl/commit/f81cded18b4f6d398b460a36c953fe6620a02bd6#commitcomment-21871201 @scottnothing Thanks! --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 9665fd34..4176da60 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -205,4 +205,4 @@ install( LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -install(FILES pcl_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY plugins DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) From a00763f038d3b2be311562a2769bd5d1f105c534 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 13 Apr 2017 23:26:41 +0000 Subject: [PATCH 269/405] Detect automatically the version of PCL --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 4176da60..e1517ff4 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -5,7 +5,7 @@ project(pcl_ros) find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) +find_package(PCL REQUIRED) if(NOT "${PCL_LIBRARIES}" STREQUAL "") list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") From 28711dd491fb98efbd407bb283a895c8d8582f10 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 13 Apr 2017 23:54:13 +0000 Subject: [PATCH 270/405] Fix lib name duplication error --- pcl_ros/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index e1517ff4..633059ae 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -9,6 +9,8 @@ find_package(PCL REQUIRED) if(NOT "${PCL_LIBRARIES}" STREQUAL "") list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") + # FIXME: this causes duplicates and not found error in ubuntu:zesty + list(REMOVE_ITEM PCL_LIBRARIES "/usr/lib/libmpi.so") endif() ## Find catkin packages From 0a75478fe679b63830f5928d494d034bf2a25342 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 25 Apr 2017 23:21:59 +0900 Subject: [PATCH 271/405] Update CHANGELOG.rst --- pcl_ros/CHANGELOG.rst | 8 ++++++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 11 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index d874a4c8..69a8a1e8 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix lib name duplication error in ubunt:zesty +* Detect automatically the version of PCL in cmake +* Install xml files declaring nodelets +* Fix syntax of nodelet manifest file by splitting files for each library. +* Contributors: Kentaro Wada + 1.4.0 (2016-04-22) ------------------ * Fixup libproj-dev rosdep diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index a712baae..7b7a4c62 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.4.0 (2016-04-22) ------------------ From dbe985e9d4b48795918f6a9f853be60bcb41caf6 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 25 Apr 2017 23:22:30 +0900 Subject: [PATCH 272/405] 1.5.0 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 69a8a1e8..2923250a 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.0 (2017-04-25) +------------------ * Fix lib name duplication error in ubunt:zesty * Detect automatically the version of PCL in cmake * Install xml files declaring nodelets diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 08bd6874..1ca712f2 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.4.1 + 1.5.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 7b7a4c62..850e4d66 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.0 (2017-04-25) +------------------ 1.4.0 (2016-04-22) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index ee361f08..b05be1fc 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.4.1 + 1.5.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 8ebf5d2e0b027bdb83592f80f31f1c271b2db539 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 25 Apr 2017 23:27:42 +0900 Subject: [PATCH 273/405] Remove pointcloud_to_laserscan --- pointcloud_to_laserscan/README.md | 1 - 1 file changed, 1 deletion(-) delete mode 100644 pointcloud_to_laserscan/README.md diff --git a/pointcloud_to_laserscan/README.md b/pointcloud_to_laserscan/README.md deleted file mode 100644 index 2ed7b5be..00000000 --- a/pointcloud_to_laserscan/README.md +++ /dev/null @@ -1 +0,0 @@ -pointcloud_to_laserscan has moved to http://github.com/ros-perception/pointcloud_to_laserscan.git From ad76ef2efec37c418ada27db9269dfbdb438d503 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 25 Apr 2017 23:33:41 +0900 Subject: [PATCH 274/405] Add myname as a maintainer --- pcl_ros/package.xml | 1 + perception_pcl/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 1ca712f2..6d4d30ac 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -15,6 +15,7 @@ William Woodall Paul Bovbel Bill Morris + Kentaro Wada BSD http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index b05be1fc..53b56ca3 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -13,6 +13,7 @@ Julius Kammerl Paul Bovbel Bill Morris + Kentaro Wada BSD http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues From 9c93a021428685722ee0dd5147181abe963e89ec Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 26 Apr 2017 01:24:05 +0900 Subject: [PATCH 275/405] Update CHANGELOG --- pcl_ros/CHANGELOG.rst | 5 +++++ perception_pcl/CHANGELOG.rst | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 2923250a..b78a16c5 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add my name as a maintainer +* Contributors: Kentaro Wada + 1.5.0 (2017-04-25) ------------------ * Fix lib name duplication error in ubunt:zesty diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 850e4d66..ddeeea6b 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add my name as a maintainer +* Contributors: Kentaro Wada + 1.5.0 (2017-04-25) ------------------ From fb2d0c69558102ce5abfa49f098fb7ad099b5a46 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 26 Apr 2017 01:25:02 +0900 Subject: [PATCH 276/405] 1.5.1 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index b78a16c5..77c8321c 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.1 (2017-04-26) +------------------ * Add my name as a maintainer * Contributors: Kentaro Wada diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 6d4d30ac..07d0c8ef 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.5.0 + 1.5.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index ddeeea6b..8cb3a0c9 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.1 (2017-04-26) +------------------ * Add my name as a maintainer * Contributors: Kentaro Wada diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 53b56ca3..9b1c5e7c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.5.0 + 1.5.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From cddbb655bc5c0825562dfa743799e5c6d28dabe3 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Fri, 28 Apr 2017 16:21:42 +0000 Subject: [PATCH 277/405] Find Qt5Widgets to fix -lQt5::Widgets error --- pcl_ros/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 633059ae..dd97155d 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -6,6 +6,8 @@ find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) +# For debian: https://github.com/ros-perception/perception_pcl/issues/139 +find_package(Qt5Widgets QUIET) if(NOT "${PCL_LIBRARIES}" STREQUAL "") list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") From c1d493f355e4cc58b0cd7faaa942dcc67a23b43c Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 29 Apr 2017 17:41:31 +0900 Subject: [PATCH 278/405] Update CHANGELOG.rst --- pcl_ros/CHANGELOG.rst | 5 +++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 8 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 77c8321c..96989318 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Find Qt5Widgets to fix -lQt5::Widgets error +* Contributors: Kentaro Wada + 1.5.1 (2017-04-26) ------------------ * Add my name as a maintainer diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 8cb3a0c9..9c219572 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.5.1 (2017-04-26) ------------------ * Add my name as a maintainer From bd227b5c035786591ea165e961787f38aa9211af Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 29 Apr 2017 17:41:49 +0900 Subject: [PATCH 279/405] 1.5.2 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 96989318..20d1743b 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.2 (2017-04-29) +------------------ * Find Qt5Widgets to fix -lQt5::Widgets error * Contributors: Kentaro Wada diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 07d0c8ef..e8691b38 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.5.1 + 1.5.2 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 9c219572..6732c6bf 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.2 (2017-04-29) +------------------ 1.5.1 (2017-04-26) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 9b1c5e7c..12634468 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.5.1 + 1.5.2 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 075139bff817c6914dd9c43f0c4a854cd3da4353 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 29 Apr 2017 09:35:07 +0000 Subject: [PATCH 280/405] Add dependency on qtbase5-dev for find_package(Qt5Widgets) See https://github.com/ros-perception/perception_pcl/pull/117#issuecomment-298158272 for detail. --- pcl_ros/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index e8691b38..2650e1d6 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -45,6 +45,8 @@ libvtk-java proj + + qtbase5-dev rostest From 3d8e48a27ce3efb5aa73da3c2c4c9bb4d593b58c Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 3 May 2017 02:19:36 +0900 Subject: [PATCH 281/405] Update CHANGELOG.rst --- pcl_ros/CHANGELOG.rst | 6 ++++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 9 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 20d1743b..7cd4e3d9 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add dependency on qtbase5-dev for find_package(Qt5Widgets) + See https://github.com/ros-perception/perception_pcl/pull/117#issuecomment-298158272 for detail. +* Contributors: Kentaro Wada + 1.5.2 (2017-04-29) ------------------ * Find Qt5Widgets to fix -lQt5::Widgets error diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 6732c6bf..811231f8 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.5.2 (2017-04-29) ------------------ From 4b37e42c6e1cebd82a73d7c2c9b2a8831aa661f8 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 3 May 2017 02:19:47 +0900 Subject: [PATCH 282/405] 1.5.3 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 7cd4e3d9..982958c0 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.3 (2017-05-03) +------------------ * Add dependency on qtbase5-dev for find_package(Qt5Widgets) See https://github.com/ros-perception/perception_pcl/pull/117#issuecomment-298158272 for detail. * Contributors: Kentaro Wada diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 2650e1d6..2af733df 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.5.2 + 1.5.3 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 811231f8..d0d896c9 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.3 (2017-05-03) +------------------ 1.5.2 (2017-04-29) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 12634468..0f84ec0c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.5.2 + 1.5.3 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From fc55edf7ad8ff79f4a8052787803875b7d068a99 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sun, 3 Apr 2016 02:28:34 +0900 Subject: [PATCH 283/405] Add travis config Refering to https://github.com/ros-perception/vision_opencv --- .travis.sh | 77 +++++++++++++++++++++++++++++++++++++++++++++++++++++ .travis.yml | 18 +++++++++++++ 2 files changed, 95 insertions(+) create mode 100755 .travis.sh create mode 100644 .travis.yml diff --git a/.travis.sh b/.travis.sh new file mode 100755 index 00000000..d886213a --- /dev/null +++ b/.travis.sh @@ -0,0 +1,77 @@ +#!/bin/bash + +set -e + +function travis_time_start { + set +x + TRAVIS_START_TIME=$(date +%s%N) + TRAVIS_TIME_ID=$(cat /dev/urandom | tr -dc 'a-z0-9' | fold -w 8 | head -n 1) + TRAVIS_FOLD_NAME=$1 + echo -e "\e[0Ktraivs_fold:start:$TRAVIS_FOLD_NAME" + echo -e "\e[0Ktraivs_time:start:$TRAVIS_TIME_ID" + set -x +} +function travis_time_end { + set +x + _COLOR=${1:-32} + TRAVIS_END_TIME=$(date +%s%N) + TIME_ELAPSED_SECONDS=$(( ($TRAVIS_END_TIME - $TRAVIS_START_TIME)/1000000000 )) + echo -e "traivs_time:end:$TRAVIS_TIME_ID:start=$TRAVIS_START_TIME,finish=$TRAVIS_END_TIME,duration=$(($TRAVIS_END_TIME - $TRAVIS_START_TIME))\n\e[0K" + echo -e "traivs_fold:end:$TRAVIS_FOLD_NAME" + echo -e "\e[0K\e[${_COLOR}mFunction $TRAVIS_FOLD_NAME takes $(( $TIME_ELAPSED_SECONDS / 60 )) min $(( $TIME_ELAPSED_SECONDS % 60 )) sec\e[0m" + set -x +} + +apt-get update -qq && apt-get install -qq -y -q wget sudo lsb-release # for docker + +travis_time_start setup.before_install +#before_install: +# Define some config vars. +# Install ROS +sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main\" > /etc/apt/sources.list.d/ros-latest.list" +wget http://packages.ros.org/ros.key -O - | sudo apt-key add - +sudo apt-get update -qq +# Install ROS +sudo apt-get install -qq -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin +source /opt/ros/$ROS_DISTRO/setup.bash +# Setup for rosdep +sudo rosdep init +rosdep update +travis_time_end + +travis_time_start setup.install +# Create a catkin workspace with the package under test. +#install: +mkdir -p ~/catkin_ws/src + +# Add the package under test to the workspace. +cd ~/catkin_ws/src +ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace +travis_time_end + +travis_time_start setup.before_script +# Install all dependencies, using wstool and rosdep. +# wstool looks for a ROSINSTALL_FILE defined in before_install. +#before_script: +# source dependencies: install using wstool. +cd ~/catkin_ws/src +wstool init +wstool up + +# package depdencies: install using rosdep. +cd ~/catkin_ws +rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO +travis_time_end + +travis_time_start setup.script +# Compile and test. +#script: +source /opt/ros/$ROS_DISTRO/setup.bash +cd ~/catkin_ws +catkin build -p1 -j1 +catkin run_tests -p1 -j1 +catkin_test_results --all build +catkin clean -b --yes +catkin config --install +catkin build -p1 -j1 +travis_time_end diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 00000000..25a39c65 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,18 @@ +sudo: required +dist: trusty +language: generic +env: + matrix: + - ROS_DISTRO=lunar DOCKER_IMAGE=ubuntu:zesty +# Install system dependencies, namely ROS. +before_install: + # Define some config vars. + - export CI_SOURCE_PATH=$(pwd) + - export REPOSITORY_NAME=${PWD##*/} + - export ROS_PARALLEL_JOBS='-j8 -l6' +script: + - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" + - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" +after_failure: + - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; + - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done From feb5ca3c54865abd3bde003928d2239934d44d91 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 29 Apr 2017 02:38:47 +0900 Subject: [PATCH 284/405] Add various docker images --- .travis.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 25a39c65..7a391780 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,8 +2,13 @@ sudo: required dist: trusty language: generic env: + global: + - ROS_DISTRO=lunar matrix: - - ROS_DISTRO=lunar DOCKER_IMAGE=ubuntu:zesty + - DOCKER_IMAGE=debian:stretch + - DOCKER_IMAGE=ubuntu:xenial + - DOCKER_IMAGE=ubuntu:yakkety + - DOCKER_IMAGE=ubuntu:zesty # Install system dependencies, namely ROS. before_install: # Define some config vars. From 410e6ba650e0f55d55b5b23936003489b9a6bdb5 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 29 Apr 2017 17:48:29 +0900 Subject: [PATCH 285/405] Install gnupg for debian --- .travis.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.sh b/.travis.sh index d886213a..12f0286c 100755 --- a/.travis.sh +++ b/.travis.sh @@ -22,7 +22,8 @@ function travis_time_end { set -x } -apt-get update -qq && apt-get install -qq -y -q wget sudo lsb-release # for docker +apt-get update -qq && apt-get install -qq -y -q wget sudo lsb-release gnupg # for docker + travis_time_start setup.before_install #before_install: From debdde93f71b297dd28c0bf15edd41cbc114a19f Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 29 Apr 2017 18:02:42 +0900 Subject: [PATCH 286/405] Add --os option to rosdep for debian --- .travis.sh | 2 +- .travis.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.sh b/.travis.sh index 12f0286c..506b430b 100755 --- a/.travis.sh +++ b/.travis.sh @@ -61,7 +61,7 @@ wstool up # package depdencies: install using rosdep. cd ~/catkin_ws -rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO +rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO --os $DOCKER_IMAGE travis_time_end travis_time_start setup.script diff --git a/.travis.yml b/.travis.yml index 7a391780..f7b83248 100644 --- a/.travis.yml +++ b/.travis.yml @@ -17,7 +17,7 @@ before_install: - export ROS_PARALLEL_JOBS='-j8 -l6' script: - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" + - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -e "DOCKER_IMAGE=$DOCKER_IMAGE" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" after_failure: - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done From bc99f755bf0bf9e70a097290c96aa7ffd908b386 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sun, 14 May 2017 12:57:54 +0900 Subject: [PATCH 287/405] Remove dependency on vtk/libproj-dev (#145) * Remove dependency on vtk/libproj-dev These dependencies were introduced in #124 to temporarily fix missing / wrong dependencies in upstream vtk. This hack is no longer necessary, since fixed vtk packages have been uploaded to packages.ros.org (see #124 and ros-infrastructure/reprepro-updater#32). * Remove vtk hack from CMakeLists.txt --- pcl_ros/CMakeLists.txt | 1 - pcl_ros/package.xml | 3 --- 2 files changed, 4 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index dd97155d..c5605e1f 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(PCL REQUIRED) find_package(Qt5Widgets QUIET) if(NOT "${PCL_LIBRARIES}" STREQUAL "") - list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") # FIXME: this causes duplicates and not found error in ubuntu:zesty list(REMOVE_ITEM PCL_LIBRARIES "/usr/lib/libmpi.so") endif() diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 2af733df..67416087 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -42,9 +42,6 @@ std_msgs tf tf2_eigen - libvtk-java - - proj qtbase5-dev From 3d23cce80746fce23a88f33286a444d59899c2f3 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 11 May 2017 01:19:03 +0900 Subject: [PATCH 288/405] Add README.md --- README.md | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 00000000..000649aa --- /dev/null +++ b/README.md @@ -0,0 +1,7 @@ +# perception_pcl + +[![Build Status](https://travis-ci.org/ros-perception/perception_pcl.svg)](https://travis-ci.org/ros-perception/perception_pcl) + +PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred +bridge for 3D applications involving n-D Point Clouds and 3D geometry +processing in ROS. From 5a8d278fb98101154559735fa809e549956ac006 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Fri, 14 Jul 2017 23:57:34 +0000 Subject: [PATCH 289/405] find only PCL components used instead of all PCL --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index c5605e1f..a9bf0b80 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -5,7 +5,7 @@ project(pcl_ros) find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED) +find_package(PCL REQUIRED COMPONENTS core io surface) # For debian: https://github.com/ros-perception/perception_pcl/issues/139 find_package(Qt5Widgets QUIET) From 157874336ca1ec7fca29a30f990f3875c6970674 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Fri, 14 Jul 2017 23:59:58 +0000 Subject: [PATCH 290/405] exclude PCL IO libraries exporting Qt flag --- pcl_ros/CMakeLists.txt | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index a9bf0b80..799bb679 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -6,14 +6,23 @@ find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS core io surface) -# For debian: https://github.com/ros-perception/perception_pcl/issues/139 -find_package(Qt5Widgets QUIET) if(NOT "${PCL_LIBRARIES}" STREQUAL "") # FIXME: this causes duplicates and not found error in ubuntu:zesty list(REMOVE_ITEM PCL_LIBRARIES "/usr/lib/libmpi.so") + + # For debian: https://github.com/ros-perception/perception_pcl/issues/139 + list(REMOVE_DUPLICATES PCL_LIBRARIES) + list(REMOVE_ITEM PCL_LIBRARIES + "vtkGUISupportQt" + "vtkGUISupportQtOpenGL" + "vtkGUISupportQtSQL" + "vtkGUISupportQtWebkit" + "vtkViewsQt" + "vtkRenderingQt") endif() + ## Find catkin packages find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure From a5aacd7a7b2b28bf0e752e1a0ad09d1a1edae56d Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Sat, 15 Jul 2017 00:01:56 +0000 Subject: [PATCH 291/405] remove now unnecessary build_depend on qtbase5 --- pcl_ros/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 67416087..42f9b9a7 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -42,8 +42,6 @@ std_msgs tf tf2_eigen - - qtbase5-dev rostest From 20db374c2682f1297ecc7fcdc5fed094362e0a0c Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Sat, 15 Jul 2017 00:03:56 +0000 Subject: [PATCH 292/405] spourious line change --- pcl_ros/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 799bb679..779d0905 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -22,7 +22,6 @@ if(NOT "${PCL_LIBRARIES}" STREQUAL "") "vtkRenderingQt") endif() - ## Find catkin packages find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure From 259cde88333d3d21978e28cd418c6d3fea07b84b Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Sat, 15 Jul 2017 00:07:19 +0000 Subject: [PATCH 293/405] no need to remove duplicates --- pcl_ros/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 779d0905..306773ee 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -12,7 +12,6 @@ if(NOT "${PCL_LIBRARIES}" STREQUAL "") list(REMOVE_ITEM PCL_LIBRARIES "/usr/lib/libmpi.so") # For debian: https://github.com/ros-perception/perception_pcl/issues/139 - list(REMOVE_DUPLICATES PCL_LIBRARIES) list(REMOVE_ITEM PCL_LIBRARIES "vtkGUISupportQt" "vtkGUISupportQtOpenGL" From b187fcb854f52e96c0a2fb7fa4107893685c310d Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 31 May 2017 02:29:09 +0900 Subject: [PATCH 294/405] Add sample and test for pcl/VoxelGrid Conflicts: pcl_ros/CMakeLists.txt --- pcl_ros/CMakeLists.txt | 9 ++ pcl_ros/samples/config/voxel_grid.rviz | 169 +++++++++++++++++++++++ pcl_ros/samples/data/.gitignore | 2 + pcl_ros/samples/sample_voxel_grid.launch | 41 ++++++ 4 files changed, 221 insertions(+) create mode 100644 pcl_ros/samples/config/voxel_grid.rviz create mode 100644 pcl_ros/samples/data/.gitignore create mode 100644 pcl_ros/samples/sample_voxel_grid.launch diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 306773ee..2979a600 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -183,6 +183,14 @@ target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +## Downloads + +catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd + DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data + MD5 546b5b4822fb1de21b0cf83d41ad6683 +) +add_custom_target(download ALL DEPENDS download_extra_data) + ############# ## Testing ## ############# @@ -191,6 +199,7 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) 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/sample_voxel_grid.launch ARGS gui:=false) endif(CATKIN_ENABLE_TESTING) diff --git a/pcl_ros/samples/config/voxel_grid.rviz b/pcl_ros/samples/config/voxel_grid.rviz new file mode 100644 index 00000000..e395065f --- /dev/null +++ b/pcl_ros/samples/config/voxel_grid.rviz @@ -0,0 +1,169 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Filtered1 + Splitter Ratio: 0.5 + Tree Height: 565 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Raw +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 246 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Raw + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 246 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Filtered + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.01 + Style: Points + Topic: /voxel_grid/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.08107 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.047887 + Y: -0.164255 + Z: -0.385388 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.4548 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.73358 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: -10 + Y: 14 diff --git a/pcl_ros/samples/data/.gitignore b/pcl_ros/samples/data/.gitignore new file mode 100644 index 00000000..d6b7ef32 --- /dev/null +++ b/pcl_ros/samples/data/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/pcl_ros/samples/sample_voxel_grid.launch b/pcl_ros/samples/sample_voxel_grid.launch new file mode 100644 index 00000000..3e9892b6 --- /dev/null +++ b/pcl_ros/samples/sample_voxel_grid.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + filter_field_name: '' + leaf_size: $(arg leaf_size) + + + + + + topic: /voxel_grid/output + hz: 20 + hzerror: 15 + test_duration: 5.0 + + + + + + + + + From 443981ed88761aec8d8043272506afbd91a9c661 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 31 May 2017 02:45:14 +0900 Subject: [PATCH 295/405] Add sample and test for pcl/StatisticalOutlierRemoval Conflicts: pcl_ros/CMakeLists.txt --- pcl_ros/CMakeLists.txt | 1 + .../config/statistical_outlier_removal.rviz | 169 ++++++++++++++++++ .../sample_statistical_outlier_removal.launch | 39 ++++ 3 files changed, 209 insertions(+) create mode 100644 pcl_ros/samples/config/statistical_outlier_removal.rviz create mode 100644 pcl_ros/samples/sample_statistical_outlier_removal.launch diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 2979a600..bfceb35b 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -199,6 +199,7 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) 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/sample_statistical_outlier_removal.launch ARGS gui:=false) add_rostest(samples/sample_voxel_grid.launch ARGS gui:=false) endif(CATKIN_ENABLE_TESTING) diff --git a/pcl_ros/samples/config/statistical_outlier_removal.rviz b/pcl_ros/samples/config/statistical_outlier_removal.rviz new file mode 100644 index 00000000..2c581d6a --- /dev/null +++ b/pcl_ros/samples/config/statistical_outlier_removal.rviz @@ -0,0 +1,169 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Filtered1 + Splitter Ratio: 0.5 + Tree Height: 565 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Raw +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 246 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Raw + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /voxel_grid/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 246 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Filtered + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.01 + Style: Points + Topic: /statistical_outlier_removal/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.08107 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.047887 + Y: -0.164255 + Z: -0.385388 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.4548 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.73358 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: -10 + Y: 14 diff --git a/pcl_ros/samples/sample_statistical_outlier_removal.launch b/pcl_ros/samples/sample_statistical_outlier_removal.launch new file mode 100644 index 00000000..032f74b1 --- /dev/null +++ b/pcl_ros/samples/sample_statistical_outlier_removal.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + mean_k: 10 + stddev: 1.0 + + + + + + topic: /statistical_outlier_removal/output + hz: 20 + hzerror: 15 + test_duration: 5.0 + + + + + + + + + From a1f56af638a910632d4d039dca1f291512ddb484 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 31 May 2017 02:54:50 +0900 Subject: [PATCH 296/405] Install samples --- pcl_ros/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index bfceb35b..b051ac11 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -225,4 +225,5 @@ install( LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -install(DIRECTORY plugins DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY plugins samples + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) From fa0813e124767a1e61a99ff4edab6776d8753d70 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 31 May 2017 05:13:10 +0900 Subject: [PATCH 297/405] Set leaf_size 0.02 --- pcl_ros/samples/sample_statistical_outlier_removal.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/samples/sample_statistical_outlier_removal.launch b/pcl_ros/samples/sample_statistical_outlier_removal.launch index 032f74b1..7e4b7e8a 100644 --- a/pcl_ros/samples/sample_statistical_outlier_removal.launch +++ b/pcl_ros/samples/sample_statistical_outlier_removal.launch @@ -5,7 +5,7 @@ - + Date: Wed, 9 Aug 2017 04:46:35 +0900 Subject: [PATCH 298/405] Inherit NodeletLazy for pipeline with less cpu load --- pcl_ros/CMakeLists.txt | 1 + pcl_ros/include/pcl_ros/filters/filter.h | 8 +++ pcl_ros/include/pcl_ros/pcl_nodelet.h | 15 +++--- pcl_ros/src/pcl_ros/filters/filter.cpp | 66 ++++++++++++++---------- 4 files changed, 57 insertions(+), 33 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index b051ac11..00e61d87 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -76,6 +76,7 @@ catkin_package( dynamic_reconfigure message_filters nodelet + nodelet_topic_tools pcl_conversions pcl_msgs rosbag diff --git a/pcl_ros/include/pcl_ros/filters/filter.h b/pcl_ros/include/pcl_ros/filters/filter.h index 24fca0b0..ccc861f9 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.h +++ b/pcl_ros/include/pcl_ros/filters/filter.h @@ -114,6 +114,14 @@ namespace pcl_ros filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) = 0; + /** \brief Lazy transport subscribe routine. */ + virtual void + subscribe(); + + /** \brief Lazy transport unsubscribe routine. */ + virtual void + unsubscribe(); + /** \brief Nodelet initialization routine. */ virtual void onInit (); diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.h b/pcl_ros/include/pcl_ros/pcl_nodelet.h index ba69afdc..0e22dc7c 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.h +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.h @@ -52,7 +52,7 @@ #include #include "pcl_ros/point_cloud.h" // ROS Nodelet includes -#include +#include #include #include #include @@ -69,7 +69,7 @@ namespace pcl_ros //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */ - class PCLNodelet : public nodelet::Nodelet + class PCLNodelet : public nodelet_topic_tools::NodeletLazy { public: typedef sensor_msgs::PointCloud2 PointCloud2; @@ -94,9 +94,6 @@ namespace pcl_ros max_queue_size_ (3), approximate_sync_ (false) {}; protected: - /** \brief The ROS NodeHandle used for parameters, publish/subscribe, etc. */ - boost::shared_ptr pnh_; - /** \brief Set to true if point indices are used. * * When receiving a point cloud, if use_indices_ is false, the entire @@ -197,12 +194,16 @@ namespace pcl_ros 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 () { - pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); - + nodelet_topic_tools::NodeletLazy::onInit(); + // Parameters that we care about only at startup pnh_->getParam ("max_queue_size", max_queue_size_); diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 0d39a03e..4fa5731a 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -104,29 +104,8 @@ pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const Indic ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::onInit () +pcl_ros::Filter::subscribe() { - // Call the super onInit () - PCLNodelet::onInit (); - - // Call the child's local init - bool has_service = false; - if (!child_init (*pnh_, has_service)) - { - NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ()); - return; - } - - pub_output_ = pnh_->advertise ("output", max_queue_size_); - - // Enable the dynamic reconfigure service - if (!has_service) - { - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2); - srv_->setCallback (f); - } - // If we're supposed to look for PointIndices (indices) if (use_indices_) { @@ -150,6 +129,45 @@ pcl_ros::Filter::onInit () else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::unsubscribe() +{ + if (use_indices_) + { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } + else + sub_input_.shutdown(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + // Call the child's local init + bool has_service = false; + if (!child_init (*pnh_, has_service)) + { + NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ()); + return; + } + + pub_output_ = advertise (*pnh_, "output", max_queue_size_); + + // Enable the dynamic reconfigure service + if (!has_service) + { + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2); + srv_->setCallback (f); + } NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ()); } @@ -175,10 +193,6 @@ pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level) void pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices) { - // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0) - return; - // If cloud is given, check if it's valid if (!isValid (cloud)) { From 34c4131455085fb849e7c79bc8c3d22aebe6db54 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 10 Aug 2017 18:03:00 +0900 Subject: [PATCH 299/405] Add missing COMPONENTS of PCL --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 00e61d87..31453796 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -5,7 +5,7 @@ project(pcl_ros) find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS core io surface) +find_package(PCL REQUIRED COMPONENTS core features filters io surface) if(NOT "${PCL_LIBRARIES}" STREQUAL "") # FIXME: this causes duplicates and not found error in ubuntu:zesty From 1efe3679bb4b927a9b316798827116646fbb4c71 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 23:41:16 +0900 Subject: [PATCH 300/405] Set branch to show correct badge status --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 000649aa..10ec332b 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # perception_pcl -[![Build Status](https://travis-ci.org/ros-perception/perception_pcl.svg)](https://travis-ci.org/ros-perception/perception_pcl) +[![Build Status](https://travis-ci.org/ros-perception/perception_pcl.svg?branch=lunar-devel)](https://travis-ci.org/ros-perception/perception_pcl) PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From f0a28c9338987fbad11a964ec081acacb1c8cea1 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 23:42:58 +0900 Subject: [PATCH 301/405] Limit branch of push build on Travis --- .travis.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.travis.yml b/.travis.yml index f7b83248..80ee3d64 100644 --- a/.travis.yml +++ b/.travis.yml @@ -21,3 +21,6 @@ script: after_failure: - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done +branches: + only: + - /.*-devel/ From 7c70b159da250dbe8271cdc6e852477d99090cdd Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 16:08:45 +0000 Subject: [PATCH 302/405] LazyNodelet for surface/ConvexHull2D --- pcl_ros/include/pcl_ros/surface/convex_hull.h | 4 ++ pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 43 ++++++++++++++----- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.h b/pcl_ros/include/pcl_ros/surface/convex_hull.h index 59fa4f6f..fa8d11d7 100644 --- a/pcl_ros/include/pcl_ros/surface/convex_hull.h +++ b/pcl_ros/include/pcl_ros/surface/convex_hull.h @@ -63,6 +63,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief LazyNodelet connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Input point cloud callback. * \param cloud the pointer to the input point cloud * \param indices the pointer to the input point cloud indices diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index 97417e58..fffaf568 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -44,20 +44,33 @@ void pcl_ros::ConvexHull2D::onInit () { - ros::NodeHandle pnh = getMTPrivateNodeHandle (); - pub_output_ = pnh.advertise ("output", max_queue_size_); - pub_plane_ = pnh.advertise("output_polygon", max_queue_size_); + PCLNodelet::onInit (); + + pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_plane_ = advertise (*pnh_, "output_polygon", max_queue_size_); // ---[ Optional parameters - pnh.getParam ("use_indices", use_indices_); + pnh_->getParam ("use_indices", use_indices_); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_indices : %s", + getName ().c_str (), + (use_indices_) ? "true" : "false"); + + onInitPostProcess(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ConvexHull2D::subscribe() +{ // If we're supposed to look for PointIndices (indices) if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe (pnh, "input", 1); + sub_input_filter_.subscribe (*pnh_, "input", 1); // If indices are enabled, subscribe to the indices - sub_indices_filter_.subscribe (pnh, "indices", 1); + sub_indices_filter_.subscribe (*pnh_, "indices", 1); if (approximate_sync_) { @@ -76,12 +89,20 @@ pcl_ros::ConvexHull2D::onInit () } else // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh.subscribe ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ())); + sub_input_ = pnh_->subscribe ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ())); +} - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_indices : %s", - getName ().c_str (), - (use_indices_) ? "true" : "false"); +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ConvexHull2D::unsubscribe() +{ + if (use_indices_) + { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } + else + sub_input_.shutdown(); } ////////////////////////////////////////////////////////////////////////////////////////////// From 6f2d30996afa23e1500489a54da41804520b6147 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 16:16:17 +0000 Subject: [PATCH 303/405] LazyNodelet for surface/MovingLeastSquares --- .../pcl_ros/surface/moving_least_squares.h | 8 +++-- .../pcl_ros/surface/moving_least_squares.cpp | 33 +++++++++++++++---- 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h index 656a52a9..b909edf8 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h @@ -110,11 +110,15 @@ namespace pcl_ros * \param level the dynamic reconfigure level */ void config_callback (MLSConfig &config, uint32_t level); - - private: + /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief LazyNodelet connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + + private: /** \brief Input point cloud callback. * \param cloud the pointer to the input point cloud * \param indices the pointer to the input point cloud indices diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index 8f3caf5f..514c2f48 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -45,8 +45,8 @@ pcl_ros::MovingLeastSquares::onInit () PCLNodelet::onInit (); //ros::NodeHandle private_nh = getMTPrivateNodeHandle (); - pub_output_ = pnh_->advertise ("output", max_queue_size_); - pub_normals_ = pnh_->advertise ("normals", max_queue_size_); + pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_normals_ = advertise (*pnh_, "normals", max_queue_size_); //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) if (!pnh_->getParam ("search_radius", search_radius_)) @@ -69,6 +69,18 @@ pcl_ros::MovingLeastSquares::onInit () // ---[ Optional parameters pnh_->getParam ("use_indices", use_indices_); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_indices : %s", + getName ().c_str (), + (use_indices_) ? "true" : "false"); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::MovingLeastSquares::subscribe () +{ // If we're supposed to look for PointIndices (indices) if (use_indices_) { @@ -95,12 +107,19 @@ pcl_ros::MovingLeastSquares::onInit () else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe ("input", 1, bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ())); +} - - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_indices : %s", - getName ().c_str (), - (use_indices_) ? "true" : "false"); +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::MovingLeastSquares::unsubscribe () +{ + if (use_indices_) + { + sub_input_filter_.unsubscribe (); + sub_indices_filter_.unsubscribe (); + } + else + sub_input_.shutdown (); } ////////////////////////////////////////////////////////////////////////////////////////////// From 76ea38194ec9e710f0c9353a2f054cc8f38d1d62 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 16:22:30 +0000 Subject: [PATCH 304/405] LazyNodelet for segmentation/EuclideanClusterExtraction --- .../pcl_ros/segmentation/extract_clusters.h | 4 ++ .../pcl_ros/segmentation/extract_clusters.cpp | 44 ++++++++++++++----- 2 files changed, 36 insertions(+), 12 deletions(-) diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h index 38a6d379..dc79f796 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h +++ b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h @@ -74,6 +74,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ void onInit (); + /** \brief LazyNodelet connection routine. */ + void subscribe (); + void unsubscribe (); + /** \brief Dynamic reconfigure callback * \param config the config object * \param level the dynamic reconfigure level diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index 686c0006..ce23f4b5 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -71,15 +71,33 @@ pcl_ros::EuclideanClusterExtraction::onInit () pnh_->getParam ("publish_indices", publish_indices_); if (publish_indices_) - pub_output_ = pnh_->advertise ("output", max_queue_size_); + pub_output_ = advertise (*pnh_, "output", max_queue_size_); else - pub_output_ = pnh_->advertise ("output", max_queue_size_); + pub_output_ = advertise (*pnh_, "output", max_queue_size_); // Enable the dynamic reconfigure service srv_ = boost::make_shared > (*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2); srv_->setCallback (f); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - max_queue_size : %d\n" + " - use_indices : %s\n" + " - cluster_tolerance : %f\n", + getName ().c_str (), + max_queue_size_, + (use_indices_) ? "true" : "false", cluster_tolerance); + + // Set given parameters here + impl_.setClusterTolerance (cluster_tolerance); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::EuclideanClusterExtraction::subscribe () +{ // If we're supposed to look for PointIndices (indices) if (use_indices_) { @@ -103,17 +121,19 @@ pcl_ros::EuclideanClusterExtraction::onInit () else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ())); +} - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - max_queue_size : %d\n" - " - use_indices : %s\n" - " - cluster_tolerance : %f\n", - getName ().c_str (), - max_queue_size_, - (use_indices_) ? "true" : "false", cluster_tolerance); - - // Set given parameters here - impl_.setClusterTolerance (cluster_tolerance); +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::EuclideanClusterExtraction::unsubscribe () +{ + if (use_indices_) + { + sub_input_filter_.unsubscribe (); + sub_indices_filter_.unsubscribe (); + } + else + sub_input_.shutdown (); } ////////////////////////////////////////////////////////////////////////////////////////////// From ebe25b4b891d4eedbdc6afeee7023336bbe0b1eb Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 16:28:36 +0000 Subject: [PATCH 305/405] LazyNodelet for segmentation/ExtractPolygonalPrismData --- .../extract_polygonal_prism_data.h | 6 +++- .../extract_polygonal_prism_data.cpp | 28 +++++++++++++++---- 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h index 8c24a889..7134f905 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h @@ -97,9 +97,13 @@ namespace pcl_ros nf_.add (boost::make_shared (cloud)); } - /** \brief Nodelet initialization routine. */ + /** \brief Nodelet initialization routine. */ void onInit (); + /** \brief LazyNodelet connection routine. */ + void subscribe (); + void unsubscribe (); + /** \brief Dynamic reconfigure callback * \param config the config object * \param level the dynamic reconfigure level diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index b75b3ecd..683a9ec0 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -52,14 +52,24 @@ pcl_ros::ExtractPolygonalPrismData::onInit () // Call the super onInit () PCLNodelet::onInit (); - sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_); - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - // Enable the dynamic reconfigure service srv_ = boost::make_shared > (*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2); srv_->setCallback (f); + // Advertise the output topics + pub_output_ = advertise (*pnh_, "output", max_queue_size_); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ExtractPolygonalPrismData::subscribe () +{ + sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_); + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + // Create the objects here if (approximate_sync_) sync_input_hull_indices_a_ = boost::make_shared > > (max_queue_size_); @@ -88,9 +98,17 @@ pcl_ros::ExtractPolygonalPrismData::onInit () sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3)); else sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3)); +} - // Advertise the output topics - pub_output_ = pnh_->advertise ("output", max_queue_size_); +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ExtractPolygonalPrismData::unsubscribe () +{ + sub_hull_filter_.unsubscribe (); + sub_input_filter_.unsubscribe (); + + if (use_indices_) + sub_indices_filter_.unsubscribe (); } ////////////////////////////////////////////////////////////////////////////////////////////// From 3ef2b1bf8aee66b1a7dd75650a74f40646883e9c Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 16:42:33 +0000 Subject: [PATCH 306/405] LazyNodelet for segmentation/SACSegmentation --- .../pcl_ros/segmentation/sac_segmentation.h | 4 + .../pcl_ros/segmentation/sac_segmentation.cpp | 117 ++++++++++-------- 2 files changed, 71 insertions(+), 50 deletions(-) diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h index fc10d104..2d96a95b 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h @@ -117,6 +117,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief LazyNodelet connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Dynamic reconfigure callback * \param config the config object * \param level the dynamic reconfigure level diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index fcefc3bc..22a976ab 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -50,54 +50,10 @@ pcl_ros::SACSegmentation::onInit () // Call the super onInit () PCLNodelet::onInit (); - // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { - // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); - - // when "use_indices" is set to true, and "latched_indices" is set to true, - // we'll subscribe and get a separate callback for PointIndices that will - // save the indices internally, and a PointCloud + PointIndices callback - // will take care of meshing the new PointClouds with the old saved indices. - if (latched_indices_) - { - // Subscribe to a callback that saves the indices - sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1)); - // Subscribe to a callback that sets the header of the saved indices to the cloud header - sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1)); - - // Synchronize the two topics. No need for an approximate synchronizer here, as we'll - // match the timestamps exactly - sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_); - sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); - } - // "latched_indices" not set, proceed with regular pairs - else - { - if (approximate_sync_) - { - sync_input_indices_a_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); - } - else - { - sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); - } - } - } - else - // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ())); // Advertise the output topics - pub_indices_ = pnh_->advertise ("inliers", max_queue_size_); - pub_model_ = pnh_->advertise ("model", max_queue_size_); + pub_indices_ = advertise (*pnh_, "inliers", max_queue_size_); + pub_model_ = advertise (*pnh_, "model", max_queue_size_); // ---[ Mandatory parameters int model_type; @@ -167,6 +123,71 @@ pcl_ros::SACSegmentation::onInit () impl_.setModelType (model_type); impl_.setMethodType (method_type); impl_.setAxis (axis); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::subscribe () +{ + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + + // when "use_indices" is set to true, and "latched_indices" is set to true, + // we'll subscribe and get a separate callback for PointIndices that will + // save the indices internally, and a PointCloud + PointIndices callback + // will take care of meshing the new PointClouds with the old saved indices. + if (latched_indices_) + { + // Subscribe to a callback that saves the indices + sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1)); + // Subscribe to a callback that sets the header of the saved indices to the cloud header + sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1)); + + // Synchronize the two topics. No need for an approximate synchronizer here, as we'll + // match the timestamps exactly + sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_); + sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + } + // "latched_indices" not set, proceed with regular pairs + else + { + if (approximate_sync_) + { + sync_input_indices_a_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + } + else + { + sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); + sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + } + } + } + else + // Subscribe in an old fashion to input only (no filters) + sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::unsubscribe () +{ + if (use_indices_) + { + sub_input_filter_.unsubscribe (); + sub_indices_filter_.unsubscribe (); + } + else + sub_input_.shutdown (); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -249,10 +270,6 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou { boost::mutex::scoped_lock lock (mutex_); - // No subscribers, no work - if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0) - return; - pcl_msgs::PointIndices inliers; pcl_msgs::ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied From 3cab2abb155359398cd33005fac45293cbca36a2 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 16:42:50 +0000 Subject: [PATCH 307/405] LazyNodelet for segmentation/SACSegmentationFromNormals --- .../pcl_ros/segmentation/sac_segmentation.h | 4 + .../pcl_ros/segmentation/sac_segmentation.cpp | 106 ++++++++++-------- 2 files changed, 65 insertions(+), 45 deletions(-) diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h index 2d96a95b..af2c9126 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h @@ -242,6 +242,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief LazyNodelet connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Model callback * \param model the sample consensus model found */ diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index 22a976ab..f623d7c4 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -371,48 +371,9 @@ pcl_ros::SACSegmentationFromNormals::onInit () dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2); srv_->setCallback (f); - // Subscribe to the input and normals using filters - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); - - // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) - sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this); - - if (approximate_sync_) - sync_input_normals_indices_a_ = boost::make_shared > > (max_queue_size_); - else - sync_input_normals_indices_e_ = boost::make_shared > > (max_queue_size_); - - // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { - // Subscribe to the input using a filter - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); - - if (approximate_sync_) - sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); - else - sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); - } - else - { - // Create a different callback for copying over the timestamp to fake indices - sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1)); - - if (approximate_sync_) - sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); - else - sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); - } - - if (approximate_sync_) - sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); - else - sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); - // Advertise the output topics - pub_indices_ = pnh_->advertise ("inliers", max_queue_size_); - pub_model_ = pnh_->advertise ("model", max_queue_size_); + pub_indices_ = advertise (*pnh_, "inliers", max_queue_size_); + pub_model_ = advertise (*pnh_, "model", max_queue_size_); // ---[ Mandatory parameters int model_type; @@ -477,6 +438,65 @@ pcl_ros::SACSegmentationFromNormals::onInit () impl_.setModelType (model_type); impl_.setMethodType (method_type); impl_.setAxis (axis); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::subscribe () +{ + // Subscribe to the input and normals using filters + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + + // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) + sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this); + + if (approximate_sync_) + sync_input_normals_indices_a_ = boost::make_shared > > (max_queue_size_); + else + sync_input_normals_indices_e_ = boost::make_shared > > (max_queue_size_); + + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + + if (approximate_sync_) + sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); + else + sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); + } + else + { + // Create a different callback for copying over the timestamp to fake indices + sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1)); + + if (approximate_sync_) + sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); + else + sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); + } + + if (approximate_sync_) + sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); + else + sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::unsubscribe () +{ + sub_input_filter_.unsubscribe (); + sub_normals_filter_.unsubscribe (); + + sub_axis_.shutdown (); + + if (use_indices_) + sub_indices_filter_.unsubscribe (); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -572,10 +592,6 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( { boost::mutex::scoped_lock lock (mutex_); - // No subscribers, no work - if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0) - return; - PointIndices inliers; ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied From 071de1e3b44e7cd6614c6eae627b83aa3ea35486 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 16:48:25 +0000 Subject: [PATCH 308/405] LazyNodelet for segmentation/SegmentDifferences --- .../segmentation/segment_differences.h | 4 +++ .../segmentation/segment_differences.cpp | 25 +++++++++++++++---- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.h b/pcl_ros/include/pcl_ros/segmentation/segment_differences.h index 287d8fb2..4914bc86 100644 --- a/pcl_ros/include/pcl_ros/segmentation/segment_differences.h +++ b/pcl_ros/include/pcl_ros/segmentation/segment_differences.h @@ -81,6 +81,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ void onInit (); + /** \brief LazyNodelet connection routine. */ + void subscribe (); + void unsubscribe (); + /** \brief Dynamic reconfigure callback * \param config the config object * \param level the dynamic reconfigure level diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index eceb5b17..22a7482b 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -46,8 +46,20 @@ pcl_ros::SegmentDifferences::onInit () // Call the super onInit () PCLNodelet::onInit (); - pub_output_ = pnh_->advertise ("output", max_queue_size_); + pub_output_ = advertise (*pnh_, "output", max_queue_size_); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - max_queue_size : %d", + getName ().c_str (), + max_queue_size_); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SegmentDifferences::subscribe () +{ // Subscribe to the input using a filter sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_); @@ -69,11 +81,14 @@ pcl_ros::SegmentDifferences::onInit () sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_); sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2)); } +} - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - max_queue_size : %d", - getName ().c_str (), - max_queue_size_); +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SegmentDifferences::unsubscribe () +{ + sub_input_filter_.unsubscribe (); + sub_target_filter_.unsubscribe (); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// From 7124f54462f55ecdddb070933e11166e18816f82 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 17:07:46 +0000 Subject: [PATCH 309/405] LazyNodelet for io/PointCloudConcatenateDataSynchronizer --- pcl_ros/include/pcl_ros/io/concatenate_data.h | 12 +- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 246 +++++++++--------- 2 files changed, 134 insertions(+), 124 deletions(-) diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.h b/pcl_ros/include/pcl_ros/io/concatenate_data.h index 7f00c6d5..89603d2a 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_data.h +++ b/pcl_ros/include/pcl_ros/io/concatenate_data.h @@ -40,7 +40,7 @@ // ROS includes #include -#include +#include #include #include #include @@ -57,7 +57,7 @@ namespace pcl_ros * PointCloud output message. * \author Radu Bogdan Rusu */ - class PointCloudConcatenateDataSynchronizer: public nodelet::Nodelet + class PointCloudConcatenateDataSynchronizer: public nodelet_topic_tools::NodeletLazy { public: typedef sensor_msgs::PointCloud2 PointCloud2; @@ -76,11 +76,10 @@ namespace pcl_ros virtual ~PointCloudConcatenateDataSynchronizer () {}; void onInit (); + void subscribe (); + void unsubscribe (); private: - /** \brief ROS local node handle. */ - ros::NodeHandle private_nh_; - /** \brief The output PointCloud publisher. */ ros::Publisher pub_output_; @@ -96,6 +95,9 @@ namespace pcl_ros /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; + /** \brief Input point cloud topics. */ + XmlRpc::XmlRpcValue input_topics_; + /** \brief TF listener object. */ tf::TransformListener tf_; diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index f7a09a84..a7a00754 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -46,10 +46,11 @@ void pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () { - private_nh_ = getMTPrivateNodeHandle (); + nodelet_topic_tools::NodeletLazy::onInit (); + // ---[ Mandatory parameters - private_nh_.getParam ("output_frame", output_frame_); - private_nh_.getParam ("approximate_sync", approximate_sync_); + pnh_->getParam ("output_frame", output_frame_); + pnh_->getParam ("approximate_sync", approximate_sync_); if (output_frame_.empty ()) { @@ -57,133 +58,130 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () return; } - // ---[ Optional parameters - private_nh_.getParam ("max_queue_size", maximum_queue_size_); - - // Output - pub_output_ = private_nh_.advertise ("output", maximum_queue_size_); - - XmlRpc::XmlRpcValue input_topics; - if (!private_nh_.getParam ("input_topics", input_topics)) + if (!pnh_->getParam ("input_topics", input_topics_)) { NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!"); - return; + return; } - // Check the type - switch (input_topics.getType ()) + if (input_topics_.getType () != XmlRpc::XmlRpcValue::TypeArray) { - case XmlRpc::XmlRpcValue::TypeArray: + NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!"); + return; + } + if (input_topics_.size () == 1) + { + NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue."); + return; + } + if (input_topics_.size () > 8) + { + NODELET_ERROR ("[onInit] More than 8 topics passed!"); + return; + } + + // ---[ Optional parameters + pnh_->getParam ("max_queue_size", maximum_queue_size_); + + // Output + pub_output_ = advertise (*pnh_, "output", maximum_queue_size_); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe () +{ + ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:"); + for (int d = 0; d < input_topics_.size (); ++d) + ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d])); + + // Subscribe to the filters + filters_.resize (input_topics_.size ()); + + // 8 topics + if (approximate_sync_) + ts_a_.reset (new message_filters::Synchronizer + > (maximum_queue_size_)); + else + ts_e_.reset (new message_filters::Synchronizer + > (maximum_queue_size_)); + + // First input_topics_.size () filters are valid + for (int d = 0; d < input_topics_.size (); ++d) + { + filters_[d].reset (new message_filters::Subscriber ()); + filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_); + } + + // Bogus null filter + filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1)); + + switch (input_topics_.size ()) + { + case 2: { - if (input_topics.size () == 1) - { - NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue."); - return; - } - - if (input_topics.size () > 8) - { - NODELET_ERROR ("[onInit] More than 8 topics passed!"); - return; - } - - ROS_INFO_STREAM ("[onInit] Subscribing to " << input_topics.size () << " user given topics as inputs:"); - for (int d = 0; d < input_topics.size (); ++d) - ROS_INFO_STREAM (" - " << (std::string)(input_topics[d])); - - // Subscribe to the filters - filters_.resize (input_topics.size ()); - - // 8 topics if (approximate_sync_) - ts_a_.reset (new message_filters::Synchronizer - > (maximum_queue_size_)); + ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); else - ts_e_.reset (new message_filters::Synchronizer - > (maximum_queue_size_)); - - // First input_topics.size () filters are valid - for (int d = 0; d < input_topics.size (); ++d) - { - filters_[d].reset (new message_filters::Subscriber ()); - filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), maximum_queue_size_); - } - - // Bogus null filter - filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1)); - - switch (input_topics.size ()) - { - case 2: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); - break; - } - case 3: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); - break; - } - case 4: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); - break; - } - case 5: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); - break; - } - case 6: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); - break; - } - case 7: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); - break; - } - case 8: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); - break; - } - default: - { - NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!"); - return; - } - } + ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); + break; + } + case 3: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); + break; + } + case 4: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); + break; + } + case 5: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); + break; + } + case 6: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); + break; + } + case 7: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); + break; + } + case 8: + { + if (approximate_sync_) + ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); + else + ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); break; } default: { - NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!"); + NODELET_FATAL ("Invalid 'input_topics' parameter given!"); return; } } @@ -194,6 +192,16 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8)); } +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe () +{ + for (int d = 0; d < filters_.size (); ++d) + { + filters_[d]->unsubscribe (); + } +} + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out) From 2d13abfc8ea6ca52d03f7e6071c410e94635ea51 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 17:15:43 +0000 Subject: [PATCH 310/405] LazyNodelet for io/PointCloudConcatenateFieldsSynchronizer --- .../include/pcl_ros/io/concatenate_fields.h | 9 +++--- pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 30 ++++++++++++++----- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.h b/pcl_ros/include/pcl_ros/io/concatenate_fields.h index 13a9ced3..efa1c458 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_fields.h +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.h @@ -39,7 +39,7 @@ #define PCL_IO_CONCATENATE_FIELDS_H_ // ROS includes -#include +#include #include #include #include @@ -54,7 +54,7 @@ namespace pcl_ros * a single PointCloud output message. * \author Radu Bogdan Rusu */ - class PointCloudConcatenateFieldsSynchronizer: public nodelet::Nodelet + class PointCloudConcatenateFieldsSynchronizer: public nodelet_topic_tools::NodeletLazy { public: typedef sensor_msgs::PointCloud2 PointCloud; @@ -73,12 +73,11 @@ namespace pcl_ros virtual ~PointCloudConcatenateFieldsSynchronizer () {}; void onInit (); + void subscribe (); + void unsubscribe (); void input_callback (const PointCloudConstPtr &cloud); private: - /** \brief ROS local node handle. */ - ros::NodeHandle private_nh_; - /** \brief The input PointCloud subscriber. */ ros::Subscriber sub_input_; diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index ea64c1f6..159f531f 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -47,9 +47,10 @@ void pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () { - private_nh_ = getMTPrivateNodeHandle (); + nodelet_topic_tools::NodeletLazy::onInit (); + // ---[ Mandatory parameters - if (!private_nh_.getParam ("input_messages", input_messages_)) + if (!pnh_->getParam ("input_messages", input_messages_)) { NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!"); return; @@ -60,10 +61,25 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () return; } // ---[ Optional parameters - private_nh_.getParam ("max_queue_size", maximum_queue_size_); - private_nh_.getParam ("maximum_seconds", maximum_seconds_); - sub_input_ = private_nh_.subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this); - pub_output_ = private_nh_.advertise ("output", maximum_queue_size_); + pnh_->getParam ("max_queue_size", maximum_queue_size_); + pnh_->getParam ("maximum_seconds", maximum_seconds_); + pub_output_ = advertise (*pnh_, "output", maximum_queue_size_); + + onInitPostProcess (); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe () +{ + sub_input_ = pnh_->subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe () +{ + sub_input_.shutdown (); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -71,7 +87,7 @@ void pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud) { NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), private_nh_.resolveName ("input").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); // Erase old data in the queue if (maximum_seconds_ > 0 && queue_.size () > 0) From 560fd2cd63d3850e7740c8807eff28f3a0a5a686 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 17:16:01 +0000 Subject: [PATCH 311/405] Refactor io/PCDReader and io/PCDWriter as child of PCLNodelet --- pcl_ros/src/pcl_ros/io/pcd_io.cpp | 32 +++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index 07d6bfae..fe396b5d 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -42,12 +42,12 @@ void pcl_ros::PCDReader::onInit () { - ros::NodeHandle private_nh = getMTPrivateNodeHandle (); + PCLNodelet::onInit (); // Provide a latched topic - ros::Publisher pub_output = private_nh.advertise ("output", max_queue_size_, true); + ros::Publisher pub_output = pnh_->advertise ("output", max_queue_size_, true); - private_nh.getParam ("publish_rate", publish_rate_); - private_nh.getParam ("tf_frame", tf_frame_); + pnh_->getParam ("publish_rate", publish_rate_); + pnh_->getParam ("tf_frame", tf_frame_); NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" " - publish_rate : %f\n" @@ -66,14 +66,14 @@ pcl_ros::PCDReader::onInit () ros::spinOnce (); ros::Duration (0.01).sleep (); } - while (private_nh.ok () && pub_output.getNumSubscribers () == 0); + while (pnh_->ok () && pub_output.getNumSubscribers () == 0); std::string file_name; - while (private_nh.ok ()) + while (pnh_->ok ()) { // Get the current filename parameter. If no filename set, loop - if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ()) + if (!pnh_->getParam ("filename", file_name_) && file_name_.empty ()) { ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ()); ros::Duration (0.01).sleep (); @@ -122,9 +122,11 @@ pcl_ros::PCDReader::onInit () ros::spinOnce (); // Update parameters from server - private_nh.getParam ("publish_rate", publish_rate_); - private_nh.getParam ("tf_frame", tf_frame_); + pnh_->getParam ("publish_rate", publish_rate_); + pnh_->getParam ("tf_frame", tf_frame_); } + + onInitPostProcess (); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -133,11 +135,12 @@ pcl_ros::PCDReader::onInit () void pcl_ros::PCDWriter::onInit () { - ros::NodeHandle pnh = getMTPrivateNodeHandle (); - sub_input_ = pnh.subscribe ("input", 1, &PCDWriter::input_callback, this); + PCLNodelet::onInit (); + + sub_input_ = pnh_->subscribe ("input", 1, &PCDWriter::input_callback, this); // ---[ Optional parameters - pnh.getParam ("filename", file_name_); - pnh.getParam ("binary_mode", binary_mode_); + pnh_->getParam ("filename", file_name_); + pnh_->getParam ("binary_mode", binary_mode_); NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" " - filename : %s\n" @@ -145,6 +148,7 @@ pcl_ros::PCDWriter::onInit () getName ().c_str (), file_name_.c_str (), (binary_mode_) ? "true" : "false"); + onInitPostProcess (); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -154,7 +158,7 @@ pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud) if (!isValid (cloud)) return; - getMTPrivateNodeHandle ().getParam ("filename", file_name_); + pnh_->getParam ("filename", file_name_); NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); From c4c7f309779f4f87b8ce2888f68054f625256228 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 17:21:39 +0000 Subject: [PATCH 312/405] LazyNodelet for filters/ProjectInliers --- .../include/pcl_ros/filters/project_inliers.h | 4 ++ .../src/pcl_ros/filters/project_inliers.cpp | 46 +++++++++++++------ 2 files changed, 36 insertions(+), 14 deletions(-) diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.h b/pcl_ros/include/pcl_ros/filters/project_inliers.h index 8002bcd2..1245e1c3 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.h +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.h @@ -97,6 +97,10 @@ namespace pcl_ros virtual void onInit (); + /** \brief NodeletLazy connection routine. */ + void subscribe (); + void unsubscribe (); + /** \brief PointCloud2 + Indices + Model data callback. */ void input_indices_model_callback (const PointCloud2::ConstPtr &cloud, diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index 77349c46..6b7239c3 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -43,8 +43,6 @@ void pcl_ros::ProjectInliers::onInit () { - // No need to call the super onInit as we are overwriting everything - //Filter::onInit (); PCLNodelet::onInit (); // ---[ Mandatory parameters @@ -65,11 +63,30 @@ pcl_ros::ProjectInliers::onInit () pnh_->getParam ("copy_all_data", copy_all_data); pnh_->getParam ("copy_all_fields", copy_all_fields); - pub_output_ = pnh_->advertise ("output", max_queue_size_); + pub_output_ = advertise (*pnh_, "output", max_queue_size_); // Subscribe to the input using a filter sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - copy_all_data : %s\n" + " - copy_all_fields : %s", + getName ().c_str (), + model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); + + // Set given parameters here + impl_.setModelType (model_type); + impl_.setCopyAllFields (copy_all_fields); + impl_.setCopyAllData (copy_all_data); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ProjectInliers::subscribe () +{ /* TODO : implement use_indices_ if (use_indices_) @@ -91,18 +108,19 @@ pcl_ros::ProjectInliers::onInit () sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); } - - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - copy_all_data : %s\n" - " - copy_all_fields : %s", - getName ().c_str (), - model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); +} - // Set given parameters here - impl_.setModelType (model_type); - impl_.setCopyAllFields (copy_all_fields); - impl_.setCopyAllData (copy_all_data); +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ProjectInliers::unsubscribe () +{ +/* + TODO : implement use_indices_ + if (use_indices_) + {*/ + + sub_input_filter_.unsubscribe (); + sub_model_.unsubscribe (); } ////////////////////////////////////////////////////////////////////////////////////////////// From 9ac65d9d208f1ab9f75973ef7b5cde8bda22a423 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 17:40:39 +0000 Subject: [PATCH 313/405] LazyNodelet for features/* --- pcl_ros/include/pcl_ros/features/boundary.h | 2 +- pcl_ros/include/pcl_ros/features/feature.h | 8 ++ pcl_ros/include/pcl_ros/features/fpfh.h | 2 +- pcl_ros/include/pcl_ros/features/fpfh_omp.h | 2 +- .../pcl_ros/features/moment_invariants.h | 2 +- pcl_ros/include/pcl_ros/features/normal_3d.h | 2 +- .../include/pcl_ros/features/normal_3d_omp.h | 2 +- .../include/pcl_ros/features/normal_3d_tbb.h | 2 +- pcl_ros/include/pcl_ros/features/pfh.h | 2 +- .../pcl_ros/features/principal_curvatures.h | 2 +- pcl_ros/include/pcl_ros/features/shot.h | 2 +- pcl_ros/include/pcl_ros/features/shot_omp.h | 2 +- pcl_ros/include/pcl_ros/features/vfh.h | 2 +- pcl_ros/src/pcl_ros/features/feature.cpp | 87 +++++++++++++++---- 14 files changed, 90 insertions(+), 29 deletions(-) diff --git a/pcl_ros/include/pcl_ros/features/boundary.h b/pcl_ros/include/pcl_ros/features/boundary.h index 39a0d10e..84a92d80 100644 --- a/pcl_ros/include/pcl_ros/features/boundary.h +++ b/pcl_ros/include/pcl_ros/features/boundary.h @@ -65,7 +65,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.h index ca0dccc0..af128950 100644 --- a/pcl_ros/include/pcl_ros/features/feature.h +++ b/pcl_ros/include/pcl_ros/features/feature.h @@ -165,6 +165,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief NodeletLazy connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. * \param cloud the pointer to the input point cloud * \param cloud_surface the pointer to the surface point cloud @@ -224,6 +228,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief NodeletLazy connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. * \param cloud the pointer to the input point cloud * \param cloud_normals the pointer to the input point cloud normals diff --git a/pcl_ros/include/pcl_ros/features/fpfh.h b/pcl_ros/include/pcl_ros/features/fpfh.h index aca59cd3..9f7800b5 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh.h +++ b/pcl_ros/include/pcl_ros/features/fpfh.h @@ -77,7 +77,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/fpfh_omp.h b/pcl_ros/include/pcl_ros/features/fpfh_omp.h index 2db8eb18..c024bbef 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh_omp.h +++ b/pcl_ros/include/pcl_ros/features/fpfh_omp.h @@ -74,7 +74,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/moment_invariants.h b/pcl_ros/include/pcl_ros/features/moment_invariants.h index ef6df20a..3915bc92 100644 --- a/pcl_ros/include/pcl_ros/features/moment_invariants.h +++ b/pcl_ros/include/pcl_ros/features/moment_invariants.h @@ -61,7 +61,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/normal_3d.h b/pcl_ros/include/pcl_ros/features/normal_3d.h index 7a1506e4..468a069c 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d.h @@ -63,7 +63,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_omp.h b/pcl_ros/include/pcl_ros/features/normal_3d_omp.h index ab265162..0cfe81fb 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_omp.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d_omp.h @@ -59,7 +59,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h index ec570f75..7c2b8050 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h @@ -62,7 +62,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/pfh.h b/pcl_ros/include/pcl_ros/features/pfh.h index 262ce37c..faef1bc9 100644 --- a/pcl_ros/include/pcl_ros/features/pfh.h +++ b/pcl_ros/include/pcl_ros/features/pfh.h @@ -77,7 +77,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.h b/pcl_ros/include/pcl_ros/features/principal_curvatures.h index d7dcdf49..0ffc0722 100644 --- a/pcl_ros/include/pcl_ros/features/principal_curvatures.h +++ b/pcl_ros/include/pcl_ros/features/principal_curvatures.h @@ -63,7 +63,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/shot.h b/pcl_ros/include/pcl_ros/features/shot.h index 73c0d81c..a3a3eea9 100644 --- a/pcl_ros/include/pcl_ros/features/shot.h +++ b/pcl_ros/include/pcl_ros/features/shot.h @@ -57,7 +57,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/shot_omp.h b/pcl_ros/include/pcl_ros/features/shot_omp.h index bc28939c..409f6b79 100644 --- a/pcl_ros/include/pcl_ros/features/shot_omp.h +++ b/pcl_ros/include/pcl_ros/features/shot_omp.h @@ -56,7 +56,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/vfh.h b/pcl_ros/include/pcl_ros/features/vfh.h index 90e588f1..0bc6fae6 100644 --- a/pcl_ros/include/pcl_ros/features/vfh.h +++ b/pcl_ros/include/pcl_ros/features/vfh.h @@ -62,7 +62,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp index 0625279b..b7027c70 100644 --- a/pcl_ros/src/pcl_ros/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -85,6 +85,21 @@ pcl_ros::Feature::onInit () dynamic_reconfigure::Server::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); srv_->setCallback (f); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName ().c_str (), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + + onInitPostProcess (); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::subscribe () +{ // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages if (use_indices_ || use_surface_) { @@ -138,14 +153,26 @@ pcl_ros::Feature::onInit () else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); +} - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::unsubscribe () +{ + if (use_indices_ || use_surface_) + { + sub_input_filter_.unsubscribe (); + if (use_indices_) + { + sub_indices_filter_.unsubscribe (); + if (use_surface_) + sub_surface_filter_.unsubscribe (); + } + else + sub_surface_filter_.unsubscribe (); + } + else + sub_input_.shutdown (); } //////////////////////////////////////////////////////////////////////////////////////////// @@ -273,14 +300,29 @@ pcl_ros::FeatureFromNormals::onInit () // ---[ Optional parameters pnh_->getParam ("use_surface", use_surface_); - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); - // Enable the dynamic reconfigure service srv_ = boost::make_shared > (*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); srv_->setCallback (f); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName ().c_str (), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::subscribe () +{ + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + // Create the objects here if (approximate_sync_) sync_input_normals_surface_indices_a_ = boost::make_shared > > (max_queue_size_); @@ -341,14 +383,25 @@ pcl_ros::FeatureFromNormals::onInit () sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); else sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); +} - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::unsubscribe () +{ + sub_input_filter_.unsubscribe (); + sub_normals_filter_.unsubscribe (); + if (use_indices_ || use_surface_) + { + if (use_indices_) + { + sub_indices_filter_.unsubscribe (); + if (use_surface_) + sub_surface_filter_.unsubscribe (); + } + else + sub_surface_filter_.unsubscribe (); + } } ////////////////////////////////////////////////////////////////////////////////////////////// From 9a99734362941efd925456d2ccdca112ff916387 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 18:43:45 +0000 Subject: [PATCH 314/405] Add test arg to avoid duplicated testing --- .../sample_statistical_outlier_removal.launch | 5 ++-- pcl_ros/samples/sample_voxel_grid.launch | 23 +++++++++++-------- 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/pcl_ros/samples/sample_statistical_outlier_removal.launch b/pcl_ros/samples/sample_statistical_outlier_removal.launch index 7e4b7e8a..1aeeed36 100644 --- a/pcl_ros/samples/sample_statistical_outlier_removal.launch +++ b/pcl_ros/samples/sample_statistical_outlier_removal.launch @@ -3,8 +3,9 @@ - + + @@ -32,7 +33,7 @@ + args="-d $(find pcl_ros)/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz"> diff --git a/pcl_ros/samples/sample_voxel_grid.launch b/pcl_ros/samples/sample_voxel_grid.launch index 3e9892b6..86596bdf 100644 --- a/pcl_ros/samples/sample_voxel_grid.launch +++ b/pcl_ros/samples/sample_voxel_grid.launch @@ -1,6 +1,7 @@ + @@ -20,16 +21,18 @@ - - - topic: /voxel_grid/output - hz: 20 - hzerror: 15 - test_duration: 5.0 - - + + + + topic: /voxel_grid/output + hz: 20 + hzerror: 15 + test_duration: 5.0 + + + Date: Tue, 22 Aug 2017 18:44:52 +0000 Subject: [PATCH 315/405] Organize samples of pcl_ros/features --- .../{ => pcl_ros/filters}/config/statistical_outlier_removal.rviz | 0 pcl_ros/samples/{ => pcl_ros/filters}/config/voxel_grid.rviz | 0 .../filters}/sample_statistical_outlier_removal.launch | 0 pcl_ros/samples/{ => pcl_ros/filters}/sample_voxel_grid.launch | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename pcl_ros/samples/{ => pcl_ros/filters}/config/statistical_outlier_removal.rviz (100%) rename pcl_ros/samples/{ => pcl_ros/filters}/config/voxel_grid.rviz (100%) rename pcl_ros/samples/{ => pcl_ros/filters}/sample_statistical_outlier_removal.launch (100%) rename pcl_ros/samples/{ => pcl_ros/filters}/sample_voxel_grid.launch (100%) diff --git a/pcl_ros/samples/config/statistical_outlier_removal.rviz b/pcl_ros/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz similarity index 100% rename from pcl_ros/samples/config/statistical_outlier_removal.rviz rename to pcl_ros/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz diff --git a/pcl_ros/samples/config/voxel_grid.rviz b/pcl_ros/samples/pcl_ros/filters/config/voxel_grid.rviz similarity index 100% rename from pcl_ros/samples/config/voxel_grid.rviz rename to pcl_ros/samples/pcl_ros/filters/config/voxel_grid.rviz diff --git a/pcl_ros/samples/sample_statistical_outlier_removal.launch b/pcl_ros/samples/pcl_ros/filters/sample_statistical_outlier_removal.launch similarity index 100% rename from pcl_ros/samples/sample_statistical_outlier_removal.launch rename to pcl_ros/samples/pcl_ros/filters/sample_statistical_outlier_removal.launch diff --git a/pcl_ros/samples/sample_voxel_grid.launch b/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch similarity index 100% rename from pcl_ros/samples/sample_voxel_grid.launch rename to pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch From 3e5c704046d61fab0bd1776f7fd21b593cbcf9de Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 18:48:07 +0000 Subject: [PATCH 316/405] Add sample & test for features/normal_3d.cpp --- pcl_ros/CMakeLists.txt | 5 ++- .../pcl_ros/features/sample_normal_3d.launch | 44 +++++++++++++++++++ 2 files changed, 47 insertions(+), 2 deletions(-) create mode 100644 pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 31453796..03f7a3d0 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -200,8 +200,9 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) 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/sample_statistical_outlier_removal.launch ARGS gui:=false) - add_rostest(samples/sample_voxel_grid.launch ARGS gui:=false) + add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false) + add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false) + add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) endif(CATKIN_ENABLE_TESTING) diff --git a/pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch b/pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch new file mode 100644 index 00000000..01ad46de --- /dev/null +++ b/pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + radius_search: 0 + k_search: 10 + # 0, => ANN, 1 => FLANN, 2 => Organized + spatial_locator: 1 + + + + + + topic: /normal_estimation/output + hz: 10 + hzerror: 8 + test_duration: 5.0 + + + + + + From 419366b5f5d5861125e695edd2d5103bf1063294 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 18:55:46 +0000 Subject: [PATCH 317/405] Add sample & test for io/concatenate_data.cpp --- .../pcl_ros/io/sample_concatenate_data.launch | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch diff --git a/pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch b/pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch new file mode 100644 index 00000000..ac5943af --- /dev/null +++ b/pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + output_frame: /base_link + input_topics: + - /voxel_grid/output + - /voxel_grid/output + + + + + + topic: /concatenate_data/output + hz: 10 + hzerror: 8 + test_duration: 5.0 + + + + + + From f16207321bf8c42c3fc449243195748ef90e6b37 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 19:10:38 +0000 Subject: [PATCH 318/405] Add sample & test for segmentation/extract_clusters.cpp --- pcl_ros/CMakeLists.txt | 3 +- .../sample_extract_clusters.launch | 42 +++++++++++++++++++ 2 files changed, 44 insertions(+), 1 deletion(-) create mode 100644 pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 03f7a3d0..9c40a697 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -5,7 +5,7 @@ project(pcl_ros) find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS core features filters io surface) +find_package(PCL REQUIRED COMPONENTS core features filters io segmentation surface) if(NOT "${PCL_LIBRARIES}" STREQUAL "") # FIXME: this causes duplicates and not found error in ubuntu:zesty @@ -203,6 +203,7 @@ if(CATKIN_ENABLE_TESTING) add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false) add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false) add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) + add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false) endif(CATKIN_ENABLE_TESTING) diff --git a/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch b/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch new file mode 100644 index 00000000..4f2d79a8 --- /dev/null +++ b/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + cluster_tolerance: 0.03 + spatial_locator: 1 # FLANN + + + + + + topic: /extract_clusters/output + hz: 3000 + hzerror: 1000 + test_duration: 5.0 + + + + + + From bb2fa3f9361e833d1e84c4b89c63a8b797f30ec4 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 19:13:41 +0000 Subject: [PATCH 319/405] Add sample & test for surface/convex_hull --- pcl_ros/CMakeLists.txt | 1 + .../pcl_ros/surface/sample_convex_hull.launch | 40 +++++++++++++++++++ 2 files changed, 41 insertions(+) create mode 100644 pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 9c40a697..10325874 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -204,6 +204,7 @@ if(CATKIN_ENABLE_TESTING) add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false) add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false) + add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false) endif(CATKIN_ENABLE_TESTING) diff --git a/pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch b/pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch new file mode 100644 index 00000000..50e54f80 --- /dev/null +++ b/pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + topic: /convex_hull/output + hz: 10 + hzerror: 8 + test_duration: 5.0 + + + + + + From cef8df686cd6cdea928cb55141f33a9134fb5992 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 22:58:58 +0000 Subject: [PATCH 320/405] Looser hzerror in test for extract_clusters to make it pass on Travis --- .../samples/pcl_ros/segmentation/sample_extract_clusters.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch b/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch index 4f2d79a8..35a474af 100644 --- a/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch +++ b/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch @@ -25,7 +25,7 @@ topic: /extract_clusters/output hz: 3000 - hzerror: 1000 + hzerror: 2400 test_duration: 5.0 From 1d1f9c2b77b9ff59013ecd8f5ba15b276cc1a719 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Mon, 28 Aug 2017 09:53:24 -0700 Subject: [PATCH 321/405] remove hack now that upstream pcl has been rebuilt --- pcl_ros/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 10325874..72a54a27 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -8,9 +8,6 @@ find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS core features filters io segmentation surface) if(NOT "${PCL_LIBRARIES}" STREQUAL "") - # FIXME: this causes duplicates and not found error in ubuntu:zesty - list(REMOVE_ITEM PCL_LIBRARIES "/usr/lib/libmpi.so") - # For debian: https://github.com/ros-perception/perception_pcl/issues/139 list(REMOVE_ITEM PCL_LIBRARIES "vtkGUISupportQt" From a243765cc04aeb4da319479b40c1921fa5db8760 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 23 Aug 2017 08:03:21 +0900 Subject: [PATCH 322/405] sudo: false in .travis.yml --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 80ee3d64..91e4e7b6 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,4 +1,4 @@ -sudo: required +sudo: false dist: trusty language: generic env: From 6e9618797356119d0d5a4f5fd343bbfe0be14cdb Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 23 Aug 2017 08:05:26 +0900 Subject: [PATCH 323/405] Use ccache for faster testing --- .travis.sh | 3 +++ .travis.yml | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/.travis.sh b/.travis.sh index 506b430b..1d0dd0d7 100755 --- a/.travis.sh +++ b/.travis.sh @@ -24,6 +24,9 @@ function travis_time_end { apt-get update -qq && apt-get install -qq -y -q wget sudo lsb-release gnupg # for docker +# Setup ccache +apt-get install -qq -y -q ccache +export PATH=/usr/lib/ccache:$PATH travis_time_start setup.before_install #before_install: diff --git a/.travis.yml b/.travis.yml index 91e4e7b6..4c046f0e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,9 @@ sudo: false dist: trusty language: generic +cache: + directories: + - $HOME/.ccache env: global: - ROS_DISTRO=lunar @@ -17,7 +20,7 @@ before_install: - export ROS_PARALLEL_JOBS='-j8 -l6' script: - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -e "DOCKER_IMAGE=$DOCKER_IMAGE" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" + - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -v $HOME:$HOME -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -e "DOCKER_IMAGE=$DOCKER_IMAGE" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" after_failure: - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done From 271c5e153826ae0cd45e7817cc8d5de28ad55fdd Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 23 Aug 2017 08:08:10 +0900 Subject: [PATCH 324/405] Add services: - docker to .travis.yml --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index 4c046f0e..25d84413 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,8 @@ sudo: false dist: trusty language: generic +services: + - docker cache: directories: - $HOME/.ccache From 58c7c8648fa5a81fbab6bd39ea1ebb38b0316663 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 26 Aug 2017 05:04:14 +0900 Subject: [PATCH 325/405] Update .travis.yml --- .travis.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 25d84413..7e99d175 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,9 +3,7 @@ dist: trusty language: generic services: - docker -cache: - directories: - - $HOME/.ccache +cache: ccache env: global: - ROS_DISTRO=lunar From d34cd5eda86e7dc011edff038146c1eb4b67f766 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Fri, 25 Aug 2017 15:01:13 +0900 Subject: [PATCH 326/405] Fix config path of sample_voxel_grid.launch --- pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch b/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch index 86596bdf..45b61294 100644 --- a/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch +++ b/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch @@ -37,7 +37,7 @@ + args="-d $(find pcl_ros)/samples/pcl_ros/filters/config/voxel_grid.rviz"> From 08c7c12cff5badf90f3a33537b952380f176c85b Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Thu, 26 Oct 2017 18:50:38 -0700 Subject: [PATCH 327/405] update to use non deprecated pluginlib macro --- pcl_ros/src/pcl_ros/features/boundary.cpp | 2 +- pcl_ros/src/pcl_ros/features/fpfh.cpp | 2 +- pcl_ros/src/pcl_ros/features/fpfh_omp.cpp | 2 +- pcl_ros/src/pcl_ros/features/moment_invariants.cpp | 2 +- pcl_ros/src/pcl_ros/features/normal_3d.cpp | 2 +- pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp | 2 +- pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp | 2 +- pcl_ros/src/pcl_ros/features/pfh.cpp | 2 +- pcl_ros/src/pcl_ros/features/principal_curvatures.cpp | 2 +- pcl_ros/src/pcl_ros/features/shot.cpp | 2 +- pcl_ros/src/pcl_ros/features/shot_omp.cpp | 2 +- pcl_ros/src/pcl_ros/features/vfh.cpp | 2 +- pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp | 2 +- .../src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp | 2 +- pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp | 4 ++-- pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp | 2 +- pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 2 +- pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp | 2 +- 18 files changed, 19 insertions(+), 19 deletions(-) diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp index 0b6a8c36..9334641a 100644 --- a/pcl_ros/src/pcl_ros/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -71,4 +71,4 @@ pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::BoundaryEstimation BoundaryEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, BoundaryEstimation, BoundaryEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/fpfh.cpp b/pcl_ros/src/pcl_ros/features/fpfh.cpp index 081696b1..3f698aad 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh.cpp @@ -72,5 +72,5 @@ pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::FPFHEstimation FPFHEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimation, FPFHEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp index 09a573f6..58dd911f 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp @@ -72,5 +72,5 @@ pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; -PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimationOMP, FPFHEstimationOMP, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp index 49838204..d0ec3441 100644 --- a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp @@ -70,5 +70,5 @@ pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr } typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, MomentInvariantsEstimation, MomentInvariantsEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/features/normal_3d.cpp index 31b4dc70..9e700f78 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d.cpp @@ -70,5 +70,5 @@ pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::NormalEstimation NormalEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimation, NormalEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp index 06c1b31a..a741c052 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp @@ -70,5 +70,5 @@ pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; -PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationOMP, NormalEstimationOMP, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp index d6636f73..a4a8581e 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp @@ -75,7 +75,7 @@ pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; -PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationTBB, NormalEstimationTBB, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet) #endif // HAVE_TBB diff --git a/pcl_ros/src/pcl_ros/features/pfh.cpp b/pcl_ros/src/pcl_ros/features/pfh.cpp index 94b97c88..38b4d19c 100644 --- a/pcl_ros/src/pcl_ros/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/features/pfh.cpp @@ -72,5 +72,5 @@ pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::PFHEstimation PFHEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, PFHEstimation, PFHEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp index 471549ab..113124dc 100644 --- a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp @@ -72,5 +72,5 @@ pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstP } typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, PrincipalCurvaturesEstimation, PrincipalCurvaturesEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/shot.cpp b/pcl_ros/src/pcl_ros/features/shot.cpp index 82122c8d..d051ab0f 100644 --- a/pcl_ros/src/pcl_ros/features/shot.cpp +++ b/pcl_ros/src/pcl_ros/features/shot.cpp @@ -71,5 +71,5 @@ pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::SHOTEstimation SHOTEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, SHOTEstimation, SHOTEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/shot_omp.cpp b/pcl_ros/src/pcl_ros/features/shot_omp.cpp index 887e4b5d..1ac1b065 100644 --- a/pcl_ros/src/pcl_ros/features/shot_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/shot_omp.cpp @@ -71,5 +71,5 @@ pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP; -PLUGINLIB_DECLARE_CLASS (pcl, SHOTEstimationOMP, SHOTEstimationOMP, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/features/vfh.cpp b/pcl_ros/src/pcl_ros/features/vfh.cpp index 86ddc137..9d0fe361 100644 --- a/pcl_ros/src/pcl_ros/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/features/vfh.cpp @@ -72,5 +72,5 @@ pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, } typedef pcl_ros::VFHEstimation VFHEstimation; -PLUGINLIB_DECLARE_CLASS (pcl, VFHEstimation, VFHEstimation, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index ce23f4b5..fdf06b2d 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -248,5 +248,5 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( } typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction; -PLUGINLIB_DECLARE_CLASS (pcl, EuclideanClusterExtraction, EuclideanClusterExtraction, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(EuclideanClusterExtraction, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 683a9ec0..1963b6e8 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -216,5 +216,5 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( } typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData; -PLUGINLIB_DECLARE_CLASS (pcl, ExtractPolygonalPrismData, ExtractPolygonalPrismData, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index f623d7c4..7dc0c60a 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -691,6 +691,6 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( typedef pcl_ros::SACSegmentation SACSegmentation; typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals; -PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentation, SACSegmentation, nodelet::Nodelet); -PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentationFromNormals, SACSegmentationFromNormals, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index 22a7482b..402b7790 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -139,5 +139,5 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl } typedef pcl_ros::SegmentDifferences SegmentDifferences; -PLUGINLIB_DECLARE_CLASS (pcl, SegmentDifferences, SegmentDifferences, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(SegmentDifferences, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index fffaf568..a577881c 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -199,5 +199,5 @@ void } typedef pcl_ros::ConvexHull2D ConvexHull2D; -PLUGINLIB_DECLARE_CLASS (pcl, ConvexHull2D, ConvexHull2D, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet) diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index 514c2f48..321f3e6a 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -230,4 +230,4 @@ pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level) } typedef pcl_ros::MovingLeastSquares MovingLeastSquares; -PLUGINLIB_DECLARE_CLASS (pcl, MovingLeastSquares, MovingLeastSquares, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(MovingLeastSquares, nodelet::Nodelet) From d2792accbfce66b357dddc1461c4b1ee78671c53 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 16 Nov 2017 02:17:32 +0900 Subject: [PATCH 328/405] Drop yakkety from CI: Yakkety went EOL in July Close https://github.com/ros-perception/perception_pcl/issues/166 https://discourse.ros.org/t/suspension-of-debian-packaging-for-eol-ubuntu-yakkety/2444 --- .travis.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 7e99d175..c925f020 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,7 +10,6 @@ env: matrix: - DOCKER_IMAGE=debian:stretch - DOCKER_IMAGE=ubuntu:xenial - - DOCKER_IMAGE=ubuntu:yakkety - DOCKER_IMAGE=ubuntu:zesty # Install system dependencies, namely ROS. before_install: From 72e5cbf64f44062c31c2bfbf1b6e0d6c4c835cd9 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 31 Mar 2018 15:56:20 +0900 Subject: [PATCH 329/405] Drop zesty from CI: zesty went EOL https://github.com/ros/rosdistro/blob/03f2bb4fd0367e1e706cde0745fd52ba52afea74/lunar/distribution.yaml#L11 --- .travis.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index c925f020..1211050c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,7 +10,6 @@ env: matrix: - DOCKER_IMAGE=debian:stretch - DOCKER_IMAGE=ubuntu:xenial - - DOCKER_IMAGE=ubuntu:zesty # Install system dependencies, namely ROS. before_install: # Define some config vars. From c8167b0e18645cf124547dcc4e32977193826b29 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 31 Mar 2018 20:21:19 +0900 Subject: [PATCH 330/405] Restrict push build only /.*-devel$/ branches --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 1211050c..477b0b99 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,4 +24,4 @@ after_failure: - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done branches: only: - - /.*-devel/ + - /.*-devel$/ From 273b011e4e38c59f04c5a8a3574185e68a1daec2 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 31 Mar 2018 18:13:34 +0900 Subject: [PATCH 331/405] Update CHANGELOG.rst --- pcl_ros/CHANGELOG.rst | 48 ++++++++++++++++++++++++++++++++++++ perception_pcl/CHANGELOG.rst | 3 +++ 2 files changed, 51 insertions(+) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 982958c0..a20411ca 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,54 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* update to use non deprecated pluginlib macro +* Fix config path of sample_voxel_grid.launch +* remove hack now that upstream pcl has been rebuilt +* Looser hzerror in test for extract_clusters to make it pass on Travis +* Add sample & test for surface/convex_hull +* Add sample & test for segmentation/extract_clusters.cpp +* Add sample & test for io/concatenate_data.cpp +* Add sample & test for features/normal_3d.cpp +* Organize samples of pcl_ros/features +* Add test arg to avoid duplicated testing +* LazyNodelet for features/* +* LazyNodelet for filters/ProjectInliers +* Refactor io/PCDReader and io/PCDWriter as child of PCLNodelet +* LazyNodelet for io/PointCloudConcatenateFieldsSynchronizer +* LazyNodelet for io/PointCloudConcatenateDataSynchronizer +* LazyNodelet for segmentation/SegmentDifferences +* LazyNodelet for segmentation/SACSegmentationFromNormals +* LazyNodelet for segmentation/SACSegmentation +* LazyNodelet for segmentation/ExtractPolygonalPrismData +* LazyNodelet for segmentation/EuclideanClusterExtraction +* LazyNodelet for surface/MovingLeastSquares +* LazyNodelet for surface/ConvexHull2D +* Add missing COMPONENTS of PCL +* Inherit NodeletLazy for pipeline with less cpu load +* Set leaf_size 0.02 +* Install samples +* Add sample and test for pcl/StatisticalOutlierRemoval + Conflicts: + pcl_ros/CMakeLists.txt +* Add sample and test for pcl/VoxelGrid + Conflicts: + pcl_ros/CMakeLists.txt +* no need to remove duplicates +* spourious line change +* remove now unnecessary build_depend on qtbase5 +* exclude PCL IO libraries exporting Qt flag +* find only PCL components used instead of all PCL +* Remove dependency on vtk/libproj-dev (`#145 `_) + * Remove dependency on vtk/libproj-dev + These dependencies were introduced in `#124 `_ to temporarily fix + missing / wrong dependencies in upstream vtk. This hack is no longer + necessary, since fixed vtk packages have been uploaded to + packages.ros.org (see `#124 `_ and `ros-infrastructure/reprepro-updater#32 `_). + * Remove vtk hack from CMakeLists.txt +* Contributors: Kentaro Wada, Mikael Arguedas + 1.5.3 (2017-05-03) ------------------ * Add dependency on qtbase5-dev for find_package(Qt5Widgets) diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index d0d896c9..85bf7c39 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.5.3 (2017-05-03) ------------------ From 492e4f917e5c35e7ff22ba1a3e1f8618f2092b2b Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 31 Mar 2018 18:17:39 +0900 Subject: [PATCH 332/405] 1.5.4 --- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index a20411ca..775afe5d 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.4 (2018-03-31) +------------------ * update to use non deprecated pluginlib macro * Fix config path of sample_voxel_grid.launch * remove hack now that upstream pcl has been rebuilt diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 42f9b9a7..a4b93dbe 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.5.3 + 1.5.4 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 85bf7c39..1a8d592a 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.5.4 (2018-03-31) +------------------ 1.5.3 (2017-05-03) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 0f84ec0c..59713e50 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.5.3 + 1.5.4 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred From 853a8f91645f8bc6b89ffeb52495eacf0f0ccd7a Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 24 Apr 2018 19:40:46 +0000 Subject: [PATCH 333/405] Remove unnecessary dependency on genmsg --- pcl_ros/CMakeLists.txt | 3 +-- pcl_ros/package.xml | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 72a54a27..eb3a6e60 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -21,7 +21,6 @@ endif() ## Find catkin packages find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure - genmsg nodelet nodelet_topic_tools pcl_conversions @@ -90,7 +89,7 @@ catkin_package( ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_tf pcl_ros_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) +add_dependencies(pcl_ros_tf ${catkin_EXPORTED_TARGETS}) ## Nodelets diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index a4b93dbe..4e0cf8e0 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -24,7 +24,6 @@ catkin cmake_modules rosconsole - genmsg roslib dynamic_reconfigure From 118784a4dd06437847182fe5e77829003d103def Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 24 Apr 2018 19:42:24 +0000 Subject: [PATCH 334/405] Add message_filters to find_package ``` % catkin lint . pcl_ros: error: nodelet plugin file 'plugins/nodelet/libpcl_ros_features.xml' is not installed to ${CATKIN_PACKAGE_SHARE_DESTINATION} pcl_ros: error: nodelet plugin file 'plugins/nodelet/libpcl_ros_filters.xml' is not installed to ${CATKIN_PACKAGE_SHARE_DESTINATION} pcl_ros: error: nodelet plugin file 'plugins/nodelet/libpcl_ros_io.xml' is not installed to ${CATKIN_PACKAGE_SHARE_DESTINATION} pcl_ros: error: nodelet plugin file 'plugins/nodelet/libpcl_ros_segmentation.xml' is not installed to ${CATKIN_PACKAGE_SHARE_DESTINATION} pcl_ros: error: nodelet plugin file 'plugins/nodelet/libpcl_ros_surface.xml' is not installed to ${CATKIN_PACKAGE_SHARE_DESTINATION} pcl_ros: error: unconfigured build_depend on 'message_filters' catkin_lint: checked 1 packages and found 6 problems catkin_lint: 40 notices have been ignored. Use -W2 to see them ``` --- pcl_ros/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index eb3a6e60..1eecf8e2 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -21,6 +21,7 @@ endif() ## Find catkin packages find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure + message_filters nodelet nodelet_topic_tools pcl_conversions From 371bf7d78939681dbc0a61e617f302bfe5f4a472 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 30 Apr 2018 12:03:07 -0400 Subject: [PATCH 335/405] Fix build and update maintainers --- pcl_conversions/CMakeLists.txt | 10 +++++++--- pcl_conversions/package.xml | 5 +++-- pcl_ros/package.xml | 4 +++- perception_pcl/package.xml | 16 +++++++++------- 4 files changed, 22 insertions(+), 13 deletions(-) diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index 61e72446..7781f3b9 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(pcl_conversions) -find_package(catkin REQUIRED COMPONENTS pcl_msgs roscpp sensor_msgs std_msgs) +find_package(catkin REQUIRED COMPONENTS) find_package(PCL REQUIRED COMPONENTS common io) find_package(Eigen3 REQUIRED) @@ -9,7 +9,7 @@ find_package(Eigen3 REQUIRED) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS pcl_msgs roscpp sensor_msgs std_msgs - DEPENDS Eigen3 PCL + DEPENDS EIGEN3 PCL ) install(DIRECTORY include/${PROJECT_NAME}/ @@ -17,11 +17,15 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) if(CATKIN_ENABLE_TESTING) + find_package(pcl_msgs) + find_package(roscpp) + find_package(sensor_msgs ) + find_package(std_msgs) include_directories( include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS}) + ${EIGEN3_INCLUDE_DIRS}) catkin_add_gtest(test_pcl_conversions test/test_pcl_conversions.cpp) target_link_libraries(test_pcl_conversions ${catkin_LIBRARIES}) diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index d33d5175..37b93dca 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,12 +1,13 @@ pcl_conversions - 0.2.1 + 1.5.4 Provides conversions from PCL data types and ROS message types William Woodall + Paul Bovbel - Bill Morris + Kentaro Wada BSD diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 4e0cf8e0..2e28d5e0 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -13,10 +13,12 @@ Open Perception Julius Kammerl William Woodall + Paul Bovbel - Bill Morris Kentaro Wada + BSD + http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 59713e50..31bb53fd 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,29 +1,31 @@ - + perception_pcl 1.5.4 - PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry processing in ROS. - + Open Perception William Woodall Julius Kammerl + Paul Bovbel - Bill Morris Kentaro Wada + BSD + http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl catkin - pcl_conversions - pcl_msgs - pcl_ros + pcl_conversions + pcl_msgs + pcl_ros + From e0a6c0644887db3c27ecca8bb60b26d08daebcdc Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 30 Apr 2018 12:10:14 -0400 Subject: [PATCH 336/405] 1.6.0 --- pcl_conversions/package.xml | 2 +- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index 37b93dca..fcfec9bb 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 1.5.4 + 1.6.0 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 2e28d5e0..126bf376 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.5.4 + 1.6.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 31bb53fd..383f411d 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.5.4 + 1.6.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From 2be69aee71c3c3683df0e767990a7c1ab841ee4b Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Mon, 30 Apr 2018 12:37:44 -0400 Subject: [PATCH 337/405] Fixup pcl_conversions test --- pcl_conversions/CMakeLists.txt | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index 7781f3b9..28e23bef 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -17,10 +17,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) if(CATKIN_ENABLE_TESTING) - find_package(pcl_msgs) - find_package(roscpp) - find_package(sensor_msgs ) - find_package(std_msgs) + find_package(catkin REQUIRED COMPONENTS roscpp pcl_msgs sensor_msgs std_msgs) include_directories( include ${catkin_INCLUDE_DIRS} From 3e97d7ce9c5ddd71f6a72e30e0b97799b641f36e Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 25 Apr 2018 08:39:51 +0000 Subject: [PATCH 338/405] Fix the use of Eigen3 in cmake (cherry picked from commit 27c02d1f49468f450c4f91992ddaf5f3e8698b56) --- pcl_ros/CMakeLists.txt | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 1eecf8e2..af79302c 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -41,7 +41,7 @@ find_package(catkin REQUIRED COMPONENTS include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) @@ -83,13 +83,13 @@ catkin_package( tf DEPENDS Boost - Eigen3 + EIGEN3 PCL ) ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) -target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_tf ${catkin_EXPORTED_TARGETS}) ## Nodelets @@ -102,7 +102,7 @@ add_library(pcl_ros_io src/pcl_ros/io/io.cpp src/pcl_ros/io/pcd_io.cpp ) -target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) class_loader_hide_library_symbols(pcl_ros_io) ## Declare the pcl_ros_features library @@ -121,7 +121,7 @@ add_library(pcl_ros_features src/pcl_ros/features/principal_curvatures.cpp src/pcl_ros/features/vfh.cpp ) -target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_features) @@ -137,7 +137,7 @@ add_library(pcl_ros_filters 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} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +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) @@ -149,7 +149,7 @@ add_library (pcl_ros_segmentation src/pcl_ros/segmentation/segment_differences.cpp src/pcl_ros/segmentation/segmentation.cpp ) -target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_segmentation) @@ -160,26 +160,26 @@ add_library (pcl_ros_surface src/pcl_ros/surface/convex_hull.cpp src/pcl_ros/surface/moving_least_squares.cpp ) -target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_surface) ## Tools add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) -target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) -target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARY_DIRS} ${PCL_LIBRARIES}) +target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(bag_to_pcd tools/bag_to_pcd.cpp) -target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARY_DIRS} ${PCL_LIBRARIES}) +target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) -target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) -target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) ## Downloads From 242ce31b32da388b4163175c9e25dd20790cd9d7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 2 May 2018 14:50:33 +0000 Subject: [PATCH 339/405] Fix a bug building on artful. The comment explains it in more detail. While we are at it, fix the link in the README.rst to point to the right repository. Signed-off-by: Chris Lalancette --- pcl_conversions/CMakeLists.txt | 12 +++++++++++- pcl_conversions/README.rst | 2 +- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index 28e23bef..d7e59c64 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.7) project(pcl_conversions) find_package(catkin REQUIRED COMPONENTS) @@ -6,6 +6,16 @@ find_package(catkin REQUIRED COMPONENTS) find_package(PCL REQUIRED COMPONENTS common io) find_package(Eigen3 REQUIRED) +# There is a bug in the Ubuntu Artful (17.10) version of the VTK package, +# where it includes /usr/include/*-linux-gnu/freetype2 in the include +# directories (which doesn't exist). This filters down to the PCL_INCLUDE_DIRS, +# and causes downstream projects trying to use these libraries to fail to +# configure properly. Here we remove those bogus entries so that downstream +# consumers of this package succeed. +if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") + list(FILTER PCL_INCLUDE_DIRS EXCLUDE REGEX "/usr/include/.*-linux-gnu/freetype2") +endif() + catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS pcl_msgs roscpp sensor_msgs std_msgs diff --git a/pcl_conversions/README.rst b/pcl_conversions/README.rst index 4ec7b6a5..2478c439 100644 --- a/pcl_conversions/README.rst +++ b/pcl_conversions/README.rst @@ -15,7 +15,7 @@ Code & tickets +-----------------+------------------------------------------------------------+ | pcl_conversions | http://ros.org/wiki/pcl_conversions | +-----------------+------------------------------------------------------------+ -| Issues | http://github.com/ros-perception/pcl_conversions/issues | +| Issues | http://github.com/ros-perception/perception_pcl/issues | +-----------------+------------------------------------------------------------+ .. | Documentation | http://ros-perception.github.com/pcl_conversions/doc | .. +-----------------+------------------------------------------------------------+ From 235612cca915a9d110ff968dd2a8a6289d5b8b0a Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Mon, 7 May 2018 08:13:20 -0700 Subject: [PATCH 340/405] CMake 3.6.3 is sufficient --- pcl_conversions/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index d7e59c64..92c9f8c9 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.7) +cmake_minimum_required(VERSION 3.6.3) project(pcl_conversions) find_package(catkin REQUIRED COMPONENTS) From 0b4ad1f1d6b824a428eac193f6178e3280160ba1 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Mon, 7 May 2018 08:13:36 -0700 Subject: [PATCH 341/405] update package.xml links to point to new repository --- pcl_conversions/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index fcfec9bb..e183d49c 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -12,8 +12,8 @@ BSD http://wiki.ros.org/pcl_conversions - https://github.com/ros-perception/pcl_conversions - https://github.com/ros-perception/pcl_conversions/issues + https://github.com/ros-perception/perception_pcl + https://github.com/ros-perception/perception_pcl/issues catkin From 595067b122b32a650ff0577954bf0e2b1232219b Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 01:45:14 +0900 Subject: [PATCH 342/405] Just update .travis.yml for melodic To divide the build failure problem to following: 1. current industrial_ci config is bad? 2. perception_pcl cannot be built on stretch, artful, and bionic? --- .travis.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 477b0b99..236571d0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -6,10 +6,11 @@ services: cache: ccache env: global: - - ROS_DISTRO=lunar + - ROS_DISTRO=melodic matrix: - DOCKER_IMAGE=debian:stretch - - DOCKER_IMAGE=ubuntu:xenial + - DOCKER_IMAGE=ubuntu:artful + - DOCKER_IMAGE=ubuntu:bionic # Install system dependencies, namely ROS. before_install: # Define some config vars. From 19cd3cfd75fb5eac27c486af54b905cfb11936af Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 02:00:12 +0900 Subject: [PATCH 343/405] Test backward compatibility for users who build from source --- .travis.yml | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 236571d0..b6a4aa4c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,12 +5,17 @@ services: - docker cache: ccache env: - global: - - ROS_DISTRO=melodic matrix: - - DOCKER_IMAGE=debian:stretch - - DOCKER_IMAGE=ubuntu:artful - - DOCKER_IMAGE=ubuntu:bionic + # Test the target distro. + - ROS_DISTRO=melodic DOCKER_IMAGE=debian:stretch + - ROS_DISTRO=melodic DOCKER_IMAGE=ubuntu:artful + - ROS_DISTRO=melodic DOCKER_IMAGE=ubuntu:bionic + # To test backward compatibility for users who build from source. + - ROS_DISTRO=lunar DOCKER_IMAGE=debian:stretch + - ROS_DISTRO=lunar DOCKER_IMAGE=ubuntu:xenial + - ROS_DISTRO=kinetic DOCKER_IMAGE=debian:jessie + - ROS_DISTRO=kinetic DOCKER_IMAGE=ubuntu:xenial + - ROS_DISTRO=indigo DOCKER_IMAGE=ubuntu:trusty # Install system dependencies, namely ROS. before_install: # Define some config vars. From d61aa376ec749d291c01f855ad121f20a0255b27 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 02:14:01 +0900 Subject: [PATCH 344/405] Downgrade the required cmake version for backward compatibility I think 3.7 is not required currently and requiring 2.8 is better. --- pcl_conversions/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index 92c9f8c9..6d22af97 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.6.3) +cmake_minimum_required(VERSION 2.8) project(pcl_conversions) find_package(catkin REQUIRED COMPONENTS) From cd2a12af43b7db93a1ab26f2dee3fcb84aabd6db Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 02:26:00 +0900 Subject: [PATCH 345/405] Use foreach + string regex to implement list(filter on old cmake --- pcl_conversions/CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index 6d22af97..c2fd331a 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -13,7 +13,12 @@ find_package(Eigen3 REQUIRED) # configure properly. Here we remove those bogus entries so that downstream # consumers of this package succeed. if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") - list(FILTER PCL_INCLUDE_DIRS EXCLUDE REGEX "/usr/include/.*-linux-gnu/freetype2") + foreach(item ${PCL_INCLUDE_DIRS}) + string(REGEX MATCH "/usr/include/.*-linux-gnu/freetype2" item ${item}) + if(item) + list(REMOVE_ITEM PCL_INCLUDE_DIRS ${item}) + endif() + endforeach() endif() catkin_package( From 090e105e7733d2bc34ff525e4ec6f877f47d7cd4 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 02:28:34 +0900 Subject: [PATCH 346/405] Add DEBIAN_FRONTEND=noninteractive --- .travis.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.sh b/.travis.sh index 1d0dd0d7..701b5640 100755 --- a/.travis.sh +++ b/.travis.sh @@ -22,6 +22,8 @@ function travis_time_end { set -x } +export DEBIAN_FRONTEND=noninteractive + apt-get update -qq && apt-get install -qq -y -q wget sudo lsb-release gnupg # for docker # Setup ccache From 3be6003f1b011ad7b5692bfef212632004a43980 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 03:10:22 +0900 Subject: [PATCH 347/405] sudo -E to enable DEBIAN_FRONTEND=noninteractive --- .travis.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.sh b/.travis.sh index 701b5640..022dc8f2 100755 --- a/.travis.sh +++ b/.travis.sh @@ -22,6 +22,7 @@ function travis_time_end { set -x } +# Mainly for https://github.com/ros-perception/perception_pcl/pull/197#issuecomment-386056906 export DEBIAN_FRONTEND=noninteractive apt-get update -qq && apt-get install -qq -y -q wget sudo lsb-release gnupg # for docker @@ -38,7 +39,7 @@ sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_rele wget http://packages.ros.org/ros.key -O - | sudo apt-key add - sudo apt-get update -qq # Install ROS -sudo apt-get install -qq -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin +sudo -E apt-get install -qq -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin source /opt/ros/$ROS_DISTRO/setup.bash # Setup for rosdep sudo rosdep init From 25c86b21153be0981c6daae1643b0e7237f24174 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 03:29:41 +0900 Subject: [PATCH 348/405] Update travis badge in README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 10ec332b..8db6bf6d 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # perception_pcl -[![Build Status](https://travis-ci.org/ros-perception/perception_pcl.svg?branch=lunar-devel)](https://travis-ci.org/ros-perception/perception_pcl) +[![Build Status](https://travis-ci.org/ros-perception/perception_pcl.svg?branch=melodic-devel)](https://travis-ci.org/ros-perception/perception_pcl) PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From 9b4d31c9125c6830f80693d11431e57c4bdbe208 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 04:54:48 +0900 Subject: [PATCH 349/405] Add NOT_TEST_INSTALL option to .travis.sh --- .travis.sh | 11 ++++++++--- .travis.yml | 10 +++++----- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/.travis.sh b/.travis.sh index 022dc8f2..b8ec99b8 100755 --- a/.travis.sh +++ b/.travis.sh @@ -22,6 +22,9 @@ function travis_time_end { set -x } +# Default configuration +test "$NOT_TEST_INSTALL" = "" && export NOT_TEST_INSTALL=false + # Mainly for https://github.com/ros-perception/perception_pcl/pull/197#issuecomment-386056906 export DEBIAN_FRONTEND=noninteractive @@ -78,7 +81,9 @@ cd ~/catkin_ws catkin build -p1 -j1 catkin run_tests -p1 -j1 catkin_test_results --all build -catkin clean -b --yes -catkin config --install -catkin build -p1 -j1 +if [ "$NOT_TEST_INSTALL" != "true" ]; then + catkin clean -b --yes + catkin config --install + catkin build -p1 -j1 +fi travis_time_end diff --git a/.travis.yml b/.travis.yml index b6a4aa4c..68f5b38d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,11 +11,11 @@ env: - ROS_DISTRO=melodic DOCKER_IMAGE=ubuntu:artful - ROS_DISTRO=melodic DOCKER_IMAGE=ubuntu:bionic # To test backward compatibility for users who build from source. - - ROS_DISTRO=lunar DOCKER_IMAGE=debian:stretch - - ROS_DISTRO=lunar DOCKER_IMAGE=ubuntu:xenial - - ROS_DISTRO=kinetic DOCKER_IMAGE=debian:jessie - - ROS_DISTRO=kinetic DOCKER_IMAGE=ubuntu:xenial - - ROS_DISTRO=indigo DOCKER_IMAGE=ubuntu:trusty + - ROS_DISTRO=lunar DOCKER_IMAGE=debian:stretch NOT_TEST_INSTALL=true + - ROS_DISTRO=lunar DOCKER_IMAGE=ubuntu:xenial NOT_TEST_INSTALL=true + - ROS_DISTRO=kinetic DOCKER_IMAGE=debian:jessie NOT_TEST_INSTALL=true + - ROS_DISTRO=kinetic DOCKER_IMAGE=ubuntu:xenial NOT_TEST_INSTALL=true + - ROS_DISTRO=indigo DOCKER_IMAGE=ubuntu:trusty NOT_TEST_INSTALL=true # Install system dependencies, namely ROS. before_install: # Define some config vars. From 80131757e8fa2cacf712ea8ed798f15d489a19e7 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Thu, 3 May 2018 04:55:00 +0900 Subject: [PATCH 350/405] Remove no need ROS_PARALLEL_JOBS env in .travis.yml --- .travis.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 68f5b38d..7d2bb754 100644 --- a/.travis.yml +++ b/.travis.yml @@ -21,7 +21,6 @@ before_install: # Define some config vars. - export CI_SOURCE_PATH=$(pwd) - export REPOSITORY_NAME=${PWD##*/} - - export ROS_PARALLEL_JOBS='-j8 -l6' script: - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -v $HOME:$HOME -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -e "DOCKER_IMAGE=$DOCKER_IMAGE" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" From 9dee4fb36dd96102ca42b6c908f532003bfcfaa0 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 8 May 2018 18:54:33 +0900 Subject: [PATCH 351/405] Add 1.6.0 section to CHANGELOG.rst --- pcl_conversions/CHANGELOG.rst | 6 ++++++ pcl_ros/CHANGELOG.rst | 8 ++++++++ perception_pcl/CHANGELOG.rst | 6 ++++++ 3 files changed, 20 insertions(+) diff --git a/pcl_conversions/CHANGELOG.rst b/pcl_conversions/CHANGELOG.rst index 1bce5a49..a890a169 100644 --- a/pcl_conversions/CHANGELOG.rst +++ b/pcl_conversions/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.6.0 (2018-04-30) +------------------ + +* Fix build and update maintainers +* Contributors: Paul Bovbel, Kentaro Wada + 0.2.1 (2015-06-08) ------------------ * Added a test for rounding errors in stamp conversion diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 775afe5d..4c60e7fc 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.6.0 (2018-04-30) +------------------ + +* Fix build and update maintainers +* Add message_filters to find_package +* Remove unnecessary dependency on genmsg +* Contributors: Paul Bovbel, Kentaro Wada + 1.5.4 (2018-03-31) ------------------ * update to use non deprecated pluginlib macro diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 1a8d592a..bc92f664 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.6.0 (2018-04-30) +------------------ + +* Fix build and update maintainers +* Contributors: Paul Bovbel, Kentaro Wada + 1.5.4 (2018-03-31) ------------------ From 283782b4200148aad1727e9620b23d2a87a39d11 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 8 May 2018 18:56:01 +0900 Subject: [PATCH 352/405] Update CHANGELOG.rst --- pcl_conversions/CHANGELOG.rst | 11 +++++++++++ pcl_ros/CHANGELOG.rst | 6 ++++++ perception_pcl/CHANGELOG.rst | 5 +++++ 3 files changed, 22 insertions(+) diff --git a/pcl_conversions/CHANGELOG.rst b/pcl_conversions/CHANGELOG.rst index a890a169..18ac76bf 100644 --- a/pcl_conversions/CHANGELOG.rst +++ b/pcl_conversions/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add 1.6.0 section to CHANGELOG.rst +* Use foreach + string regex to implement list(filter on old cmake +* Downgrade the required cmake version for backward compatibility +* update package.xml links to point to new repository +* CMake 3.6.3 is sufficient +* Fix a bug building on artful. +* Fixup pcl_conversions test +* Contributors: Chris Lalancette, Kentaro Wada, Mikael Arguedas, Paul Bovbel + 1.6.0 (2018-04-30) ------------------ diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 4c60e7fc..3541e215 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add 1.6.0 section to CHANGELOG.rst +* Fix the use of Eigen3 in cmake +* Contributors: Kentaro Wada + 1.6.0 (2018-04-30) ------------------ diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index bc92f664..7f0fe94b 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add 1.6.0 section to CHANGELOG.rst +* Contributors: Kentaro Wada + 1.6.0 (2018-04-30) ------------------ From 32d3e6a459eaa5e84a8aea3a8094322c4f98f9f3 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 8 May 2018 18:56:16 +0900 Subject: [PATCH 353/405] 1.6.1 --- pcl_conversions/CHANGELOG.rst | 4 ++-- pcl_conversions/package.xml | 2 +- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pcl_conversions/CHANGELOG.rst b/pcl_conversions/CHANGELOG.rst index 18ac76bf..f2a4c6e7 100644 --- a/pcl_conversions/CHANGELOG.rst +++ b/pcl_conversions/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.6.1 (2018-05-08) +------------------ * Add 1.6.0 section to CHANGELOG.rst * Use foreach + string regex to implement list(filter on old cmake * Downgrade the required cmake version for backward compatibility diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index e183d49c..aeeb6ffc 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 1.6.0 + 1.6.1 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 3541e215..f8b79cdf 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.6.1 (2018-05-08) +------------------ * Add 1.6.0 section to CHANGELOG.rst * Fix the use of Eigen3 in cmake * Contributors: Kentaro Wada diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 126bf376..347da5ab 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.6.0 + 1.6.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 7f0fe94b..9378367c 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.6.1 (2018-05-08) +------------------ * Add 1.6.0 section to CHANGELOG.rst * Contributors: Kentaro Wada diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 383f411d..7469570e 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.6.0 + 1.6.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From cd3720923a1a0bbf75b5967c120974f76d2b7e56 Mon Sep 17 00:00:00 2001 From: James Ward Date: Tue, 8 May 2018 09:53:32 +1000 Subject: [PATCH 354/405] Increase limits on CropBox filter parameters Min and max of CropBox filter was +/- 5m. For a pointcloud from a Velodyne, for example, this is not enough. Increased to +/- 1000m. --- pcl_ros/cfg/CropBox.cfg | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pcl_ros/cfg/CropBox.cfg b/pcl_ros/cfg/CropBox.cfg index 8031493c..5f72d3fb 100755 --- a/pcl_ros/cfg/CropBox.cfg +++ b/pcl_ros/cfg/CropBox.cfg @@ -11,12 +11,12 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator () # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): -gen.add ("min_x", double_t, 0, "X coordinate of the minimum point of the box.", -1, -5, 5) -gen.add ("max_x", double_t, 0, "X coordinate of the maximum point of the box.", 1, -5, 5) -gen.add ("min_y", double_t, 0, "Y coordinate of the minimum point of the box.", -1, -5, 5) -gen.add ("max_y", double_t, 0, "Y coordinate of the maximum point of the box.", 1, -5, 5) -gen.add ("min_z", double_t, 0, "Z coordinate of the minimum point of the box.", -1, -5, 5) -gen.add ("max_z", double_t, 0, "Z coordinate of the maximum point of the box.", 1, -5, 5) +gen.add ("min_x", double_t, 0, "X coordinate of the minimum point of the box.", -1, -1000, 1000) +gen.add ("max_x", double_t, 0, "X coordinate of the maximum point of the box.", 1, -1000, 1000) +gen.add ("min_y", double_t, 0, "Y coordinate of the minimum point of the box.", -1, -1000, 1000) +gen.add ("max_y", double_t, 0, "Y coordinate of the maximum point of the box.", 1, -1000, 1000) +gen.add ("min_z", double_t, 0, "Z coordinate of the minimum point of the box.", -1, -1000, 1000) +gen.add ("max_z", double_t, 0, "Z coordinate of the maximum point of the box.", 1, -1000, 1000) 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 ("negative", bool_t, 0, "If True the box will be empty Else the remaining points will be the ones in the box", 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.", "") From f8fee6128aff90e5a211506929287870a26978cf Mon Sep 17 00:00:00 2001 From: Jiri Horner Date: Thu, 17 May 2018 23:14:13 +0200 Subject: [PATCH 355/405] pcl_ros: fix exported includes in Ubuntu Artful pcl_ros needs the same fix as pcl_conversions --- pcl_ros/CMakeLists.txt | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index af79302c..7d845687 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -18,6 +18,21 @@ if(NOT "${PCL_LIBRARIES}" STREQUAL "") "vtkRenderingQt") endif() +# There is a bug in the Ubuntu Artful (17.10) version of the VTK package, +# where it includes /usr/include/*-linux-gnu/freetype2 in the include +# directories (which doesn't exist). This filters down to the PCL_INCLUDE_DIRS, +# and causes downstream projects trying to use these libraries to fail to +# configure properly. Here we remove those bogus entries so that downstream +# consumers of this package succeed. +if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") + foreach(item ${PCL_INCLUDE_DIRS}) + string(REGEX MATCH "/usr/include/.*-linux-gnu/freetype2" item ${item}) + if(item) + list(REMOVE_ITEM PCL_INCLUDE_DIRS ${item}) + endif() + endforeach() +endif() + ## Find catkin packages find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure From 99f7caacb96bae752af9e5b845331aceb512efa5 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sun, 20 May 2018 04:14:16 +0900 Subject: [PATCH 356/405] Update CHANGELOG.rst --- pcl_conversions/CHANGELOG.rst | 3 +++ pcl_ros/CHANGELOG.rst | 6 ++++++ perception_pcl/CHANGELOG.rst | 3 +++ 3 files changed, 12 insertions(+) diff --git a/pcl_conversions/CHANGELOG.rst b/pcl_conversions/CHANGELOG.rst index f2a4c6e7..80b97930 100644 --- a/pcl_conversions/CHANGELOG.rst +++ b/pcl_conversions/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.6.1 (2018-05-08) ------------------ * Add 1.6.0 section to CHANGELOG.rst diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index f8b79cdf..5a7ba88c 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix exported includes in Ubuntu Artful +* Increase limits on CropBox filter parameters +* Contributors: James Ward, Jiri Horner + 1.6.1 (2018-05-08) ------------------ * Add 1.6.0 section to CHANGELOG.rst diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 9378367c..0194011d 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.6.1 (2018-05-08) ------------------ * Add 1.6.0 section to CHANGELOG.rst From 12482e555dfc295c47dba1e7c14560870c6a6d0e Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sun, 20 May 2018 04:14:40 +0900 Subject: [PATCH 357/405] 1.6.2 --- pcl_conversions/CHANGELOG.rst | 4 ++-- pcl_conversions/package.xml | 2 +- pcl_ros/CHANGELOG.rst | 4 ++-- pcl_ros/package.xml | 2 +- perception_pcl/CHANGELOG.rst | 4 ++-- perception_pcl/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pcl_conversions/CHANGELOG.rst b/pcl_conversions/CHANGELOG.rst index 80b97930..945c9860 100644 --- a/pcl_conversions/CHANGELOG.rst +++ b/pcl_conversions/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.6.2 (2018-05-20) +------------------ 1.6.1 (2018-05-08) ------------------ diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index aeeb6ffc..df7f8439 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 1.6.1 + 1.6.2 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/CHANGELOG.rst b/pcl_ros/CHANGELOG.rst index 5a7ba88c..af7b1788 100644 --- a/pcl_ros/CHANGELOG.rst +++ b/pcl_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.6.2 (2018-05-20) +------------------ * Fix exported includes in Ubuntu Artful * Increase limits on CropBox filter parameters * Contributors: James Ward, Jiri Horner diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 347da5ab..2d6341f7 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.6.1 + 1.6.2 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/CHANGELOG.rst b/perception_pcl/CHANGELOG.rst index 0194011d..f5f3523c 100644 --- a/perception_pcl/CHANGELOG.rst +++ b/perception_pcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.6.2 (2018-05-20) +------------------ 1.6.1 (2018-05-08) ------------------ diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 7469570e..ef13be34 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,6 +1,6 @@ perception_pcl - 1.6.1 + 1.6.2 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From a8ba2c790db5db181725606649e1fc7c776c1ffd Mon Sep 17 00:00:00 2001 From: James Xu Date: Fri, 5 Apr 2019 07:50:11 -0700 Subject: [PATCH 358/405] use and on Windows (#221) * Changing from usleep to c++14 style sleep_for for Windows support --- pcl_ros/tools/pcd_to_pointcloud.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index 655b4341..03638183 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -43,6 +43,10 @@ **/ +// STL +#include +#include + // ROS core #include #include @@ -116,7 +120,7 @@ class PCDGenerator continue; } - usleep (interval); + std::this_thread::sleep_for(std::chrono::microseconds(static_cast(interval))); if (interval == 0) // We only publish once if a 0 seconds interval is given { From a1fd4d2a09533adeca171b0e9285cba49d292785 Mon Sep 17 00:00:00 2001 From: Andreas Klintberg Date: Tue, 29 Oct 2019 19:10:32 -0400 Subject: [PATCH 359/405] Updated ROS2 PR: pcl_conversion only to ROS2 (dashing) (#244) * pcl_conversion to ros2 * travis for ros2 and fixed cmakelists * exporting dependencies * updated travis * fixed smaller things * added colcon ignore to meta package * merge pcl_conversions.h with version released in Dashing * merge test script, CMakeListst, package.xml * append authors, clean up * PR feedback * fixed PR comments * fixed PR comments, cmakelists message_filters, reverted type masq, added maintainer * removed dashing and made ros_distro matrix env --- .travis.sh | 89 ------- .travis.yml | 43 ++-- pcl_conversions/CMakeLists.txt | 75 +++--- .../include/pcl_conversions/pcl_conversions.h | 225 +++++++++--------- pcl_conversions/package.xml | 29 ++- pcl_conversions/test/test_pcl_conversions.cpp | 46 ++-- pcl_ros/COLCON_IGNORE | 0 perception_pcl/CMakeLists.txt | 6 +- perception_pcl/COLCON_IGNORE | 0 perception_pcl/package.xml | 10 +- 10 files changed, 227 insertions(+), 296 deletions(-) delete mode 100755 .travis.sh create mode 100644 pcl_ros/COLCON_IGNORE create mode 100644 perception_pcl/COLCON_IGNORE diff --git a/.travis.sh b/.travis.sh deleted file mode 100755 index b8ec99b8..00000000 --- a/.travis.sh +++ /dev/null @@ -1,89 +0,0 @@ -#!/bin/bash - -set -e - -function travis_time_start { - set +x - TRAVIS_START_TIME=$(date +%s%N) - TRAVIS_TIME_ID=$(cat /dev/urandom | tr -dc 'a-z0-9' | fold -w 8 | head -n 1) - TRAVIS_FOLD_NAME=$1 - echo -e "\e[0Ktraivs_fold:start:$TRAVIS_FOLD_NAME" - echo -e "\e[0Ktraivs_time:start:$TRAVIS_TIME_ID" - set -x -} -function travis_time_end { - set +x - _COLOR=${1:-32} - TRAVIS_END_TIME=$(date +%s%N) - TIME_ELAPSED_SECONDS=$(( ($TRAVIS_END_TIME - $TRAVIS_START_TIME)/1000000000 )) - echo -e "traivs_time:end:$TRAVIS_TIME_ID:start=$TRAVIS_START_TIME,finish=$TRAVIS_END_TIME,duration=$(($TRAVIS_END_TIME - $TRAVIS_START_TIME))\n\e[0K" - echo -e "traivs_fold:end:$TRAVIS_FOLD_NAME" - echo -e "\e[0K\e[${_COLOR}mFunction $TRAVIS_FOLD_NAME takes $(( $TIME_ELAPSED_SECONDS / 60 )) min $(( $TIME_ELAPSED_SECONDS % 60 )) sec\e[0m" - set -x -} - -# Default configuration -test "$NOT_TEST_INSTALL" = "" && export NOT_TEST_INSTALL=false - -# Mainly for https://github.com/ros-perception/perception_pcl/pull/197#issuecomment-386056906 -export DEBIAN_FRONTEND=noninteractive - -apt-get update -qq && apt-get install -qq -y -q wget sudo lsb-release gnupg # for docker - -# Setup ccache -apt-get install -qq -y -q ccache -export PATH=/usr/lib/ccache:$PATH - -travis_time_start setup.before_install -#before_install: -# Define some config vars. -# Install ROS -sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main\" > /etc/apt/sources.list.d/ros-latest.list" -wget http://packages.ros.org/ros.key -O - | sudo apt-key add - -sudo apt-get update -qq -# Install ROS -sudo -E apt-get install -qq -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin -source /opt/ros/$ROS_DISTRO/setup.bash -# Setup for rosdep -sudo rosdep init -rosdep update -travis_time_end - -travis_time_start setup.install -# Create a catkin workspace with the package under test. -#install: -mkdir -p ~/catkin_ws/src - -# Add the package under test to the workspace. -cd ~/catkin_ws/src -ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace -travis_time_end - -travis_time_start setup.before_script -# Install all dependencies, using wstool and rosdep. -# wstool looks for a ROSINSTALL_FILE defined in before_install. -#before_script: -# source dependencies: install using wstool. -cd ~/catkin_ws/src -wstool init -wstool up - -# package depdencies: install using rosdep. -cd ~/catkin_ws -rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO --os $DOCKER_IMAGE -travis_time_end - -travis_time_start setup.script -# Compile and test. -#script: -source /opt/ros/$ROS_DISTRO/setup.bash -cd ~/catkin_ws -catkin build -p1 -j1 -catkin run_tests -p1 -j1 -catkin_test_results --all build -if [ "$NOT_TEST_INSTALL" != "true" ]; then - catkin clean -b --yes - catkin config --install - catkin build -p1 -j1 -fi -travis_time_end diff --git a/.travis.yml b/.travis.yml index 7d2bb754..aa6f57f0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,32 +1,27 @@ -sudo: false -dist: trusty language: generic + services: - docker -cache: ccache + +cache: + directories: + - $HOME/.ccache + env: + global: + - ROS_REPO=ros2 + - CCACHE_DIR=$HOME/.ccache + # travis build will time out with no output unless we use verbose output + - VERBOSE_OUTPUT=true + - VERBOSE_TESTS=true matrix: - # Test the target distro. - - ROS_DISTRO=melodic DOCKER_IMAGE=debian:stretch - - ROS_DISTRO=melodic DOCKER_IMAGE=ubuntu:artful - - ROS_DISTRO=melodic DOCKER_IMAGE=ubuntu:bionic - # To test backward compatibility for users who build from source. - - ROS_DISTRO=lunar DOCKER_IMAGE=debian:stretch NOT_TEST_INSTALL=true - - ROS_DISTRO=lunar DOCKER_IMAGE=ubuntu:xenial NOT_TEST_INSTALL=true - - ROS_DISTRO=kinetic DOCKER_IMAGE=debian:jessie NOT_TEST_INSTALL=true - - ROS_DISTRO=kinetic DOCKER_IMAGE=ubuntu:xenial NOT_TEST_INSTALL=true - - ROS_DISTRO=indigo DOCKER_IMAGE=ubuntu:trusty NOT_TEST_INSTALL=true -# Install system dependencies, namely ROS. -before_install: - # Define some config vars. - - export CI_SOURCE_PATH=$(pwd) - - export REPOSITORY_NAME=${PWD##*/} + - ROS_DISTRO=dashing OS_NAME=ubuntu OS_CODE_NAME=bionic + +install: + - git clone --branch master --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci script: - - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -v $HOME:$HOME -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -e "DOCKER_IMAGE=$DOCKER_IMAGE" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" -after_failure: - - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; - - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done + - .industrial_ci/travis.sh + branches: only: - - /.*-devel$/ + - /.*-devel$/ \ No newline at end of file diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index c2fd331a..e5677dda 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -1,44 +1,53 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) project(pcl_conversions) -find_package(catkin REQUIRED COMPONENTS) - -find_package(PCL REQUIRED COMPONENTS common io) -find_package(Eigen3 REQUIRED) - -# There is a bug in the Ubuntu Artful (17.10) version of the VTK package, -# where it includes /usr/include/*-linux-gnu/freetype2 in the include -# directories (which doesn't exist). This filters down to the PCL_INCLUDE_DIRS, -# and causes downstream projects trying to use these libraries to fail to -# configure properly. Here we remove those bogus entries so that downstream -# consumers of this package succeed. -if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") - foreach(item ${PCL_INCLUDE_DIRS}) - string(REGEX MATCH "/usr/include/.*-linux-gnu/freetype2" item ${item}) - if(item) - list(REMOVE_ITEM PCL_INCLUDE_DIRS ${item}) - endif() - endforeach() +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) endif() -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS pcl_msgs roscpp sensor_msgs std_msgs - DEPENDS EIGEN3 PCL +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(message_filters REQUIRED) +find_package(PCL REQUIRED QUIET COMPONENTS common io) +find_package(pcl_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +set(dependencies + message_filters + pcl_msgs + rclcpp + sensor_msgs + std_msgs +) + +include_directories( + include + ${PCL_COMMON_INCLUDE_DIRS} ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME}/ ) -if(CATKIN_ENABLE_TESTING) - find_package(catkin REQUIRED COMPONENTS roscpp pcl_msgs sensor_msgs std_msgs) - include_directories( - include - ${catkin_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS}) +# Add gtest based cpp test target +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) - catkin_add_gtest(test_pcl_conversions test/test_pcl_conversions.cpp) - target_link_libraries(test_pcl_conversions ${catkin_LIBRARIES}) + 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}) endif() + +ament_export_include_directories(include) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/pcl_conversions/include/pcl_conversions/pcl_conversions.h b/pcl_conversions/include/pcl_conversions/pcl_conversions.h index 9fdf988a..a8de8dde 100644 --- a/pcl_conversions/include/pcl_conversions/pcl_conversions.h +++ b/pcl_conversions/include/pcl_conversions/pcl_conversions.h @@ -39,33 +39,35 @@ #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 -#include +#include #include -#include +#include #include @@ -77,27 +79,27 @@ namespace pcl_conversions { /** PCLHeader <=> Header **/ inline - void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp) + void fromPCL(const pcl::uint64_t &pcl_stamp, rclcpp::Time &stamp) { - stamp.fromNSec(pcl_stamp * 1000ull); // Convert from us to ns + stamp = rclcpp::Time(pcl_stamp * 1000ull); // Convert from us to ns } inline - void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp) + void toPCL(const rclcpp::Time &stamp, pcl::uint64_t &pcl_stamp) { - pcl_stamp = stamp.toNSec() / 1000ull; // Convert from ns to us + pcl_stamp = stamp.nanoseconds() / 1000ull; // Convert from ns to us } inline - ros::Time fromPCL(const pcl::uint64_t &pcl_stamp) + rclcpp::Time fromPCL(const pcl::uint64_t &pcl_stamp) { - ros::Time stamp; + rclcpp::Time stamp; fromPCL(pcl_stamp, stamp); return stamp; } inline - pcl::uint64_t toPCL(const ros::Time &stamp) + pcl::uint64_t toPCL(const rclcpp::Time &stamp) { pcl::uint64_t pcl_stamp; toPCL(stamp, pcl_stamp); @@ -107,31 +109,35 @@ namespace pcl_conversions { /** PCLHeader <=> Header **/ inline - void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header) + void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::msg::Header &header) { - fromPCL(pcl_header.stamp, header.stamp); - header.seq = pcl_header.seq; + auto time_stamp = rclcpp::Time(header.stamp); + fromPCL(pcl_header.stamp, time_stamp); header.frame_id = pcl_header.frame_id; } inline - void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) + void toPCL(const std_msgs::msg::Header &header, pcl::PCLHeader &pcl_header) { toPCL(header.stamp, pcl_header.stamp); - pcl_header.seq = header.seq; + // TODO(clalancette): Seq doesn't exist in the ROS2 header + // anymore. wjwwood suggests that we might be able to get this + // information from the middleware in the future, but for now we + // just set it to 0. + pcl_header.seq = 0; pcl_header.frame_id = header.frame_id; } inline - std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) + std_msgs::msg::Header fromPCL(const pcl::PCLHeader &pcl_header) { - std_msgs::Header header; + std_msgs::msg::Header header; fromPCL(pcl_header, header); return header; } inline - pcl::PCLHeader toPCL(const std_msgs::Header &header) + pcl::PCLHeader toPCL(const std_msgs::msg::Header &header) { pcl::PCLHeader pcl_header; toPCL(header, pcl_header); @@ -141,7 +147,7 @@ namespace pcl_conversions { /** PCLImage <=> Image **/ inline - void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::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; @@ -152,21 +158,21 @@ namespace pcl_conversions { } inline - void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::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::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::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; @@ -177,14 +183,14 @@ namespace pcl_conversions { } inline - void toPCL(const sensor_msgs::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::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); @@ -193,7 +199,7 @@ namespace pcl_conversions { /** PCLPointField <=> PointField **/ inline - void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) + void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::msg::PointField &pf) { pf.name = pcl_pf.name; pf.offset = pcl_pf.offset; @@ -202,7 +208,7 @@ namespace pcl_conversions { } inline - void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) + void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) { pfs.resize(pcl_pfs.size()); std::vector::const_iterator it = pcl_pfs.begin(); @@ -213,7 +219,7 @@ namespace pcl_conversions { } inline - void toPCL(const sensor_msgs::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; @@ -222,10 +228,10 @@ namespace pcl_conversions { } inline - void toPCL(const std::vector &pfs, std::vector &pcl_pfs) + void toPCL(const std::vector &pfs, std::vector &pcl_pfs) { pcl_pfs.resize(pfs.size()); - std::vector::const_iterator it = pfs.begin(); + std::vector::const_iterator it = pfs.begin(); int i = 0; for(; it != pfs.end(); ++it, ++i) { toPCL(*(it), pcl_pfs[i]); @@ -235,7 +241,7 @@ namespace pcl_conversions { /** PCLPointCloud2 <=> PointCloud2 **/ inline - void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::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; @@ -248,21 +254,21 @@ namespace pcl_conversions { } inline - void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::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::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::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; @@ -275,14 +281,14 @@ namespace pcl_conversions { } inline - void toPCL(const sensor_msgs::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::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); @@ -291,28 +297,28 @@ namespace pcl_conversions { /** pcl::PointIndices <=> pcl_msgs::PointIndices **/ inline - void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::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::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::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::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); @@ -321,28 +327,28 @@ namespace pcl_conversions { /** pcl::ModelCoefficients <=> pcl_msgs::ModelCoefficients **/ inline - void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::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::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::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::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); @@ -351,50 +357,50 @@ namespace pcl_conversions { /** pcl::Vertices <=> pcl_msgs::Vertices **/ inline - void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert) + void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert) { vert.vertices = pcl_vert.vertices; } inline - void fromPCL(const std::vector &pcl_verts, std::vector &verts) + void fromPCL(const std::vector &pcl_verts, std::vector &verts) { verts.resize(pcl_verts.size()); std::vector::const_iterator it = pcl_verts.begin(); - std::vector::iterator jt = verts.begin(); + std::vector::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::Vertices &vert) + void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert) { vert.vertices.swap(pcl_vert.vertices); } inline - void fromPCL(std::vector &pcl_verts, std::vector &verts) + void fromPCL(std::vector &pcl_verts, std::vector &verts) { verts.resize(pcl_verts.size()); std::vector::iterator it = pcl_verts.begin(); - std::vector::iterator jt = verts.begin(); + std::vector::iterator jt = verts.begin(); for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) { moveFromPCL(*(it), *(jt)); } } inline - void toPCL(const pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert) + void toPCL(const pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert) { pcl_vert.vertices = vert.vertices; } inline - void toPCL(const std::vector &verts, std::vector &pcl_verts) + void toPCL(const std::vector &verts, std::vector &pcl_verts) { pcl_verts.resize(verts.size()); - std::vector::const_iterator it = verts.begin(); + std::vector::const_iterator it = verts.begin(); std::vector::iterator jt = pcl_verts.begin(); for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) { toPCL(*(it), *(jt)); @@ -402,16 +408,16 @@ namespace pcl_conversions { } inline - void moveToPCL(pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert) + void moveToPCL(pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert) { pcl_vert.vertices.swap(vert.vertices); } inline - void moveToPCL(std::vector &verts, std::vector &pcl_verts) + void moveToPCL(std::vector &verts, std::vector &pcl_verts) { pcl_verts.resize(verts.size()); - std::vector::iterator it = verts.begin(); + std::vector::iterator it = verts.begin(); std::vector::iterator jt = pcl_verts.begin(); for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) { moveToPCL(*(it), *(jt)); @@ -421,7 +427,7 @@ namespace pcl_conversions { /** pcl::PolygonMesh <=> pcl_msgs::PolygonMesh **/ inline - void fromPCL(const pcl::PolygonMesh &pcl_mesh, pcl_msgs::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); @@ -429,14 +435,14 @@ namespace pcl_conversions { } inline - void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::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::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); @@ -444,7 +450,7 @@ namespace pcl_conversions { } inline - void moveToPCL(pcl_msgs::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); @@ -457,7 +463,7 @@ namespace pcl { /** Overload pcl::getFieldIndex **/ - inline int getFieldIndex(const sensor_msgs::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) { @@ -470,7 +476,7 @@ namespace pcl { /** Overload pcl::getFieldsList **/ - inline std::string getFieldsList(const sensor_msgs::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) { @@ -483,7 +489,7 @@ namespace pcl { /** Provide pcl::toROSMsg **/ inline - void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::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); @@ -493,7 +499,7 @@ namespace pcl { } inline - void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) + void moveToROSMsg(sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::Image &image) { pcl::PCLPointCloud2 pcl_cloud; pcl_conversions::moveToPCL(cloud, pcl_cloud); @@ -503,7 +509,7 @@ namespace pcl { } template void - toROSMsg (const pcl::PointCloud &cloud, sensor_msgs::Image& msg) + toROSMsg (const pcl::PointCloud &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) @@ -532,10 +538,10 @@ namespace pcl { } } - /** Provide to/fromROSMsg for sensor_msgs::PointCloud2 <=> pcl::PointCloud **/ + /** Provide to/fromROSMsg for sensor_msgs::msg::PointCloud2 <=> pcl::PointCloud **/ template - void toROSMsg(const pcl::PointCloud &pcl_cloud, sensor_msgs::PointCloud2 &cloud) + void toROSMsg(const pcl::PointCloud &pcl_cloud, sensor_msgs::msg::PointCloud2 &cloud) { pcl::PCLPointCloud2 pcl_pc2; pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2); @@ -543,7 +549,7 @@ namespace pcl { } template - void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + void fromROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) { pcl::PCLPointCloud2 pcl_pc2; pcl_conversions::toPCL(cloud, pcl_pc2); @@ -551,7 +557,7 @@ namespace pcl { } template - void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + void moveFromROSMsg(sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) { pcl::PCLPointCloud2 pcl_pc2; pcl_conversions::moveToPCL(cloud, pcl_pc2); @@ -561,7 +567,7 @@ namespace pcl { /** Overload pcl::createMapping **/ template - void createMapping(const std::vector& msg_fields, MsgFieldMap& field_map) + void createMapping(const std::vector& msg_fields, MsgFieldMap& field_map) { std::vector pcl_msg_fields; pcl_conversions::toPCL(msg_fields, pcl_msg_fields); @@ -573,7 +579,7 @@ namespace pcl { /** Overload pcl::io::savePCDFile **/ inline int - savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, + 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) @@ -584,7 +590,7 @@ namespace pcl { } inline int - destructiveSavePCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud, + 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) @@ -596,7 +602,7 @@ namespace pcl { /** Overload pcl::io::loadPCDFile **/ - inline int loadPCDFile(const std::string &file_name, sensor_msgs::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); @@ -609,9 +615,9 @@ namespace pcl { /** Overload asdf **/ inline - bool concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1, - const sensor_msgs::PointCloud2 &cloud2, - sensor_msgs::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) @@ -655,7 +661,7 @@ namespace pcl { if (strip) { // Get the field sizes for the second cloud - std::vector fields2; + std::vector fields2; std::vector fields2_sizes; for (size_t j = 0; j < cloud2.fields.size (); ++j) { @@ -717,68 +723,72 @@ namespace pcl { } // namespace pcl +/* TODO when ROS2 type masquareding is implemented */ +/** namespace ros { template<> struct DefaultMessageCreator { - boost::shared_ptr operator() () + std::shared_ptr operator() () { - boost::shared_ptr msg(new pcl::PCLPointCloud2()); + std::shared_ptr msg(new pcl::PCLPointCloud2()); return msg; } }; - + namespace message_traits { template<> struct MD5Sum { - static const char* value() { return MD5Sum::value(); } + static const char* value() { return MD5Sum::value(); } static const char* value(const pcl::PCLPointCloud2&) { return value(); } - static const uint64_t static_value1 = MD5Sum::static_value1; - static const uint64_t static_value2 = MD5Sum::static_value2; + static const uint64_t static_value1 = MD5Sum::static_value1; + static const uint64_t static_value2 = MD5Sum::static_value2; // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here. - ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL); - ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL); + static_assert(static_value1 == 0x1158d486dd51d683ULL); + static_assert(static_value2 == 0xce2f1be655c3c181ULL); }; template<> struct DataType { - static const char* value() { return DataType::value(); } + static const char* value() { return DataType::value(); } static const char* value(const pcl::PCLPointCloud2&) { return value(); } }; template<> struct Definition { - static const char* value() { return Definition::value(); } + static const char* value() { return Definition::value(); } static const char* value(const pcl::PCLPointCloud2&) { return value(); } }; - template<> struct HasHeader : TrueType {}; - } // namespace ros::message_traits + template<> struct HasHeader : std::true_type {}; + } // namespace message_filters::message_traits namespace serialization { + **/ /* * Provide a custom serialization for pcl::PCLPointCloud2 */ + /** template<> struct Serializer { template inline static void write(Stream& stream, const pcl::PCLPointCloud2& m) { - std_msgs::Header header; + std_msgs::msg::Header header; pcl_conversions::fromPCL(m.header, header); stream.next(header); stream.next(m.height); stream.next(m.width); - std::vector pfs; + std::vector pfs; pcl_conversions::fromPCL(m.fields, pfs); stream.next(pfs); stream.next(m.is_bigendian); @@ -791,12 +801,12 @@ namespace ros template inline static void read(Stream& stream, pcl::PCLPointCloud2& m) { - std_msgs::Header header; + std_msgs::msg::Header header; stream.next(header); pcl_conversions::toPCL(header, m.header); stream.next(m.height); stream.next(m.width); - std::vector pfs; + std::vector pfs; stream.next(pfs); pcl_conversions::toPCL(pfs, m.fields); stream.next(m.is_bigendian); @@ -810,12 +820,12 @@ namespace ros { uint32_t length = 0; - std_msgs::Header header; + std_msgs::msg::Header header; pcl_conversions::fromPCL(m.header, header); length += serializationLength(header); length += 4; // height length += 4; // width - std::vector pfs; + std::vector pfs; pcl_conversions::fromPCL(m.fields, pfs); length += serializationLength(pfs); // fields length += 1; // is_bigendian @@ -828,10 +838,11 @@ namespace ros return length; } }; - + **/ /* * Provide a custom serialization for pcl::PCLPointField */ + /** template<> struct Serializer { @@ -865,17 +876,18 @@ namespace ros return length; } }; - + **/ /* * Provide a custom serialization for pcl::PCLHeader */ + /** template<> struct Serializer { template inline static void write(Stream& stream, const pcl::PCLHeader& m) { - std_msgs::Header header; + std_msgs::msg::Header header; pcl_conversions::fromPCL(m, header); stream.next(header); } @@ -883,7 +895,7 @@ namespace ros template inline static void read(Stream& stream, pcl::PCLHeader& m) { - std_msgs::Header header; + std_msgs::msg::Header header; stream.next(header); pcl_conversions::toPCL(header, m); } @@ -892,7 +904,7 @@ namespace ros { uint32_t length = 0; - std_msgs::Header header; + std_msgs::msg::Header header; pcl_conversions::fromPCL(m, header); length += serializationLength(header); @@ -900,8 +912,7 @@ namespace ros } }; } // namespace ros::serialization - } // namespace ros - +**/ #endif /* PCL_CONVERSIONS_H__ */ diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index df7f8439..90008f8b 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -5,9 +5,13 @@ Provides conversions from PCL data types and ROS message types William Woodall + Paul Bovbel + Bill Morris + Andreas Klintberg Paul Bovbel Kentaro Wada + Steve Macenski BSD @@ -15,20 +19,19 @@ https://github.com/ros-perception/perception_pcl https://github.com/ros-perception/perception_pcl/issues - catkin + ament_cmake - eigen - libpcl-all-dev - pcl_msgs - roscpp - sensor_msgs - std_msgs + eigen + libpcl-all-dev + message_filters + pcl_msgs + rclcpp + sensor_msgs + std_msgs - eigen - libpcl-all-dev - pcl_msgs - roscpp - sensor_msgs - std_msgs + ament_cmake_gtest + + ament_cmake + diff --git a/pcl_conversions/test/test_pcl_conversions.cpp b/pcl_conversions/test/test_pcl_conversions.cpp index c07720a7..bdecb504 100644 --- a/pcl_conversions/test/test_pcl_conversions.cpp +++ b/pcl_conversions/test/test_pcl_conversions.cpp @@ -41,21 +41,21 @@ protected: } pcl::PCLImage pcl_image; - sensor_msgs::Image image; + sensor_msgs::msg::Image image; pcl::PCLPointCloud2 pcl_pc2; - sensor_msgs::PointCloud2 pc2; + sensor_msgs::msg::PointCloud2 pc2; }; template void test_image(T &image) { EXPECT_EQ(std::string("pcl"), image.header.frame_id); - EXPECT_EQ(1, image.height); - EXPECT_EQ(2, image.width); - EXPECT_EQ(1, image.step); + EXPECT_EQ(1U, image.height); + EXPECT_EQ(2U, image.width); + EXPECT_EQ(1U, image.step); EXPECT_TRUE(image.is_bigendian); EXPECT_EQ(std::string("bgr8"), image.encoding); - EXPECT_EQ(2, image.data.size()); + EXPECT_EQ(2U, image.data.size()); EXPECT_EQ(0x42, image.data[0]); EXPECT_EQ(0x43, image.data[1]); } @@ -71,21 +71,21 @@ TEST_F(PCLConversionTests, imageConversion) { template void test_pc(T &pc) { EXPECT_EQ(std::string("pcl"), pc.header.frame_id); - EXPECT_EQ(1, pc.height); - EXPECT_EQ(2, pc.width); - EXPECT_EQ(1, pc.point_step); - EXPECT_EQ(1, pc.row_step); + EXPECT_EQ(1U, pc.height); + EXPECT_EQ(2U, pc.width); + EXPECT_EQ(1U, pc.point_step); + EXPECT_EQ(1U, pc.row_step); EXPECT_TRUE(pc.is_bigendian); EXPECT_TRUE(pc.is_dense); EXPECT_EQ("XYZ", pc.fields[0].name); EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[0].datatype); - EXPECT_EQ(3, pc.fields[0].count); - EXPECT_EQ(0, pc.fields[0].offset); + EXPECT_EQ(3U, pc.fields[0].count); + EXPECT_EQ(0U, pc.fields[0].offset); EXPECT_EQ("RGB", pc.fields[1].name); EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[1].datatype); - EXPECT_EQ(3, pc.fields[1].count); - EXPECT_EQ(8 * 3, pc.fields[1].offset); - EXPECT_EQ(2, pc.data.size()); + EXPECT_EQ(3U, pc.fields[1].count); + EXPECT_EQ(8U * 3U, pc.fields[1].offset); + EXPECT_EQ(2U, pc.data.size()); EXPECT_EQ(0x42, pc.data[0]); EXPECT_EQ(0x43, pc.data[1]); } @@ -103,10 +103,10 @@ TEST_F(PCLConversionTests, pointcloud2Conversion) { struct StampTestData { - const ros::Time stamp_; - ros::Time stamp2_; + const rclcpp::Time stamp_; + rclcpp::Time stamp2_; - explicit StampTestData(const ros::Time &stamp) + explicit StampTestData(const rclcpp::Time &stamp) : stamp_(stamp) { pcl::uint64_t pcl_stamp; @@ -118,27 +118,27 @@ struct StampTestData TEST(PCLConversionStamp, Stamps) { { - const StampTestData d(ros::Time(1.000001)); + const StampTestData d(rclcpp::Time(1, 1000)); EXPECT_TRUE(d.stamp_==d.stamp2_); } { - const StampTestData d(ros::Time(1.999999)); + const StampTestData d(rclcpp::Time(1, 999999000)); EXPECT_TRUE(d.stamp_==d.stamp2_); } { - const StampTestData d(ros::Time(1.999)); + const StampTestData d(rclcpp::Time(1, 999000000)); EXPECT_TRUE(d.stamp_==d.stamp2_); } { - const StampTestData d(ros::Time(1423680574, 746000000)); + const StampTestData d(rclcpp::Time(1423680574, 746000000)); EXPECT_TRUE(d.stamp_==d.stamp2_); } { - const StampTestData d(ros::Time(1423680629, 901000000)); + const StampTestData d(rclcpp::Time(1423680629, 901000000)); EXPECT_TRUE(d.stamp_==d.stamp2_); } } diff --git a/pcl_ros/COLCON_IGNORE b/pcl_ros/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/perception_pcl/CMakeLists.txt b/perception_pcl/CMakeLists.txt index b74e3f0b..c97773ad 100644 --- a/perception_pcl/CMakeLists.txt +++ b/perception_pcl/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(perception_pcl) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/perception_pcl/COLCON_IGNORE b/perception_pcl/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index ef13be34..a03e40a0 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -1,4 +1,6 @@ - + + + perception_pcl 1.6.2 @@ -20,13 +22,13 @@ https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl - catkin + ament_cmake pcl_conversions pcl_msgs - pcl_ros + - + ament_cmake From 1273c7581d172f0a3c99e5f267e8c7c61692c38d Mon Sep 17 00:00:00 2001 From: Steven Macenski Date: Mon, 2 Dec 2019 09:39:11 -0800 Subject: [PATCH 360/405] changing base version to 2.1.0 for first ros2 sync [eloquent] (#253) * changing base version to 2.0.0 for first ros2 sync * changing industrial CI to use eloquent * increment 2.0.0 to 2.1.0 for eloquent release --- .travis.yml | 2 +- pcl_conversions/package.xml | 2 +- pcl_ros/package.xml | 3 ++- perception_pcl/COLCON_IGNORE | 0 perception_pcl/package.xml | 3 ++- 5 files changed, 6 insertions(+), 4 deletions(-) delete mode 100644 perception_pcl/COLCON_IGNORE diff --git a/.travis.yml b/.travis.yml index aa6f57f0..1df8be18 100644 --- a/.travis.yml +++ b/.travis.yml @@ -15,7 +15,7 @@ env: - VERBOSE_OUTPUT=true - VERBOSE_TESTS=true matrix: - - ROS_DISTRO=dashing OS_NAME=ubuntu OS_CODE_NAME=bionic + - ROS_DISTRO=eloquent OS_NAME=ubuntu OS_CODE_NAME=bionic install: - git clone --branch master --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index 90008f8b..9ad11855 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 1.6.2 + 2.1.0 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 2d6341f7..1642ed3d 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 1.6.2 + 2.1.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred @@ -15,6 +15,7 @@ William Woodall Paul Bovbel + Steve Macenski Kentaro Wada BSD diff --git a/perception_pcl/COLCON_IGNORE b/perception_pcl/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index a03e40a0..f8c1fcb3 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -2,7 +2,7 @@ perception_pcl - 1.6.2 + 2.1.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry @@ -14,6 +14,7 @@ Julius Kammerl Paul Bovbel + Steve Macenski Kentaro Wada BSD From 36eb607be7d2f946174acee4662577ed05d0164e Mon Sep 17 00:00:00 2001 From: Ruffin Date: Mon, 16 Mar 2020 13:08:48 -0700 Subject: [PATCH 361/405] Use std::uint* types. (#265) (#266) Co-authored-by: Mike Purvis --- .../include/pcl_conversions/pcl_conversions.h | 18 +++++++++--------- pcl_conversions/test/test_pcl_conversions.cpp | 2 +- pcl_ros/include/pcl_ros/point_cloud.h | 12 ++++++------ .../src/test/test_tf_message_filter_pcl.cpp | 6 +++--- 4 files changed, 19 insertions(+), 19 deletions(-) diff --git a/pcl_conversions/include/pcl_conversions/pcl_conversions.h b/pcl_conversions/include/pcl_conversions/pcl_conversions.h index a8de8dde..4f652bd2 100644 --- a/pcl_conversions/include/pcl_conversions/pcl_conversions.h +++ b/pcl_conversions/include/pcl_conversions/pcl_conversions.h @@ -79,19 +79,19 @@ namespace pcl_conversions { /** PCLHeader <=> Header **/ inline - void fromPCL(const pcl::uint64_t &pcl_stamp, rclcpp::Time &stamp) + void fromPCL(const std::uint64_t &pcl_stamp, rclcpp::Time &stamp) { stamp = rclcpp::Time(pcl_stamp * 1000ull); // Convert from us to ns } inline - void toPCL(const rclcpp::Time &stamp, pcl::uint64_t &pcl_stamp) + void toPCL(const rclcpp::Time &stamp, std::uint64_t &pcl_stamp) { pcl_stamp = stamp.nanoseconds() / 1000ull; // Convert from ns to us } inline - rclcpp::Time fromPCL(const pcl::uint64_t &pcl_stamp) + rclcpp::Time fromPCL(const std::uint64_t &pcl_stamp) { rclcpp::Time stamp; fromPCL(pcl_stamp, stamp); @@ -99,9 +99,9 @@ namespace pcl_conversions { } inline - pcl::uint64_t toPCL(const rclcpp::Time &stamp) + std::uint64_t toPCL(const rclcpp::Time &stamp) { - pcl::uint64_t pcl_stamp; + std::uint64_t pcl_stamp; toPCL(stamp, pcl_stamp); return pcl_stamp; } @@ -526,14 +526,14 @@ namespace pcl { // sensor_msgs::image_encodings::BGR8; msg.encoding = "bgr8"; - msg.step = msg.width * sizeof (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++) { - uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); - memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t)); + std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); + memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(std::uint8_t)); } } } @@ -832,7 +832,7 @@ namespace ros length += 4; // point_step length += 4; // row_step length += 4; // data's size - length += m.data.size() * sizeof(pcl::uint8_t); + length += m.data.size() * sizeof(std::uint8_t); length += 1; // is_dense return length; diff --git a/pcl_conversions/test/test_pcl_conversions.cpp b/pcl_conversions/test/test_pcl_conversions.cpp index bdecb504..f827ab01 100644 --- a/pcl_conversions/test/test_pcl_conversions.cpp +++ b/pcl_conversions/test/test_pcl_conversions.cpp @@ -109,7 +109,7 @@ struct StampTestData explicit StampTestData(const rclcpp::Time &stamp) : stamp_(stamp) { - pcl::uint64_t pcl_stamp; + std::uint64_t pcl_stamp; pcl_conversions::toPCL(stamp_, pcl_stamp); pcl_conversions::fromPCL(pcl_stamp, stamp2_); } diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h index 711b8307..ee7c381d 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.h +++ b/pcl_ros/include/pcl_ros/point_cloud.h @@ -23,18 +23,18 @@ namespace pcl template void operator() () { const char* name = traits::name::value; - uint32_t name_length = strlen(name); + std::uint32_t name_length = strlen(name); stream_.next(name_length); if (name_length > 0) memcpy(stream_.advance(name_length), name, name_length); - uint32_t offset = traits::offset::value; + std::uint32_t offset = traits::offset::value; stream_.next(offset); - uint8_t datatype = traits::datatype::value; + std::uint8_t datatype = traits::datatype::value; stream_.next(datatype); - uint32_t count = traits::datatype::size; + std::uint32_t count = traits::datatype::size; stream_.next(count); } @@ -48,11 +48,11 @@ namespace pcl template void operator() () { - uint32_t name_length = strlen(traits::name::value); + std::uint32_t name_length = strlen(traits::name::value); length += name_length + 13; } - uint32_t length; + std::uint32_t length; }; } // namespace pcl::detail } // namespace pcl diff --git a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp index 40861945..28cfbb63 100644 --- a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp +++ b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -58,7 +58,7 @@ typedef pcl::PointCloud PCDType; /// Sets pcl_stamp from stamp, BUT alters stamp /// a little to round it to millisecond. This is because converting back /// and forth from pcd to ros time induces some rounding errors. -void setStamp(ros::Time &stamp, pcl::uint64_t &pcl_stamp) +void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp) { // round to millisecond static const uint32_t mult = 1e6; @@ -187,7 +187,7 @@ TEST(MessageFilter, queueSize) filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp = ros::Time::now(); - pcl::uint64_t pcl_stamp; + std::uint64_t pcl_stamp; setStamp(stamp, pcl_stamp); for (int i = 0; i < 20; ++i) @@ -281,7 +281,7 @@ TEST(MessageFilter, tolerance) filter.setTolerance(offset); ros::Time stamp = ros::Time::now(); - pcl::uint64_t pcl_stamp; + std::uint64_t pcl_stamp; setStamp(stamp, pcl_stamp); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); From 2b770b15ed76a5dfe626f8c553607015b77fc729 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Tue, 12 May 2020 10:50:15 +0900 Subject: [PATCH 362/405] Changes in preparation for PCL 1.11 (#273) (#279) * Deriving typedef from pcl type * Explicit boost shared_ptr for function parameters * Use boost::shared_ptr instead of PCL::Ptr * Implementing boost-std compatibility * Using the compatibility layer --- pcl_ros/include/pcl_ros/features/feature.h | 14 +- pcl_ros/include/pcl_ros/filters/filter.h | 4 +- pcl_ros/include/pcl_ros/pcl_nodelet.h | 9 +- pcl_ros/include/pcl_ros/point_cloud.h | 121 ++++++++++++++++++ .../extract_polygonal_prism_data.h | 4 +- .../pcl_ros/segmentation/sac_segmentation.h | 12 +- .../segmentation/segment_differences.h | 4 +- pcl_ros/include/pcl_ros/surface/convex_hull.h | 4 +- .../pcl_ros/surface/moving_least_squares.h | 4 +- pcl_ros/src/pcl_ros/features/boundary.cpp | 10 +- pcl_ros/src/pcl_ros/features/fpfh.cpp | 10 +- pcl_ros/src/pcl_ros/features/fpfh_omp.cpp | 10 +- .../pcl_ros/features/moment_invariants.cpp | 8 +- pcl_ros/src/pcl_ros/features/normal_3d.cpp | 8 +- .../src/pcl_ros/features/normal_3d_omp.cpp | 8 +- .../src/pcl_ros/features/normal_3d_tbb.cpp | 4 +- pcl_ros/src/pcl_ros/features/pfh.cpp | 10 +- .../pcl_ros/features/principal_curvatures.cpp | 10 +- pcl_ros/src/pcl_ros/features/shot.cpp | 10 +- pcl_ros/src/pcl_ros/features/shot_omp.cpp | 10 +- pcl_ros/src/pcl_ros/features/vfh.cpp | 10 +- .../pcl_ros/segmentation/extract_clusters.cpp | 4 +- .../extract_polygonal_prism_data.cpp | 6 +- .../pcl_ros/segmentation/sac_segmentation.cpp | 6 +- .../segmentation/segment_differences.cpp | 8 +- pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 8 +- .../pcl_ros/surface/moving_least_squares.cpp | 10 +- pcl_ros/tools/pointcloud_to_pcd.cpp | 2 +- 28 files changed, 225 insertions(+), 103 deletions(-) diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.h index af128950..85118b20 100644 --- a/pcl_ros/include/pcl_ros/features/feature.h +++ b/pcl_ros/include/pcl_ros/features/feature.h @@ -70,11 +70,11 @@ namespace pcl_ros typedef pcl::KdTree::Ptr KdTreePtr; typedef pcl::PointCloud PointCloudIn; - typedef PointCloudIn::Ptr PointCloudInPtr; - typedef PointCloudIn::ConstPtr PointCloudInConstPtr; + typedef boost::shared_ptr PointCloudInPtr; + typedef boost::shared_ptr PointCloudInConstPtr; - typedef boost::shared_ptr > IndicesPtr; - typedef boost::shared_ptr > IndicesConstPtr; + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; /** \brief Empty constructor. */ Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0), @@ -153,7 +153,7 @@ namespace pcl_ros indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp; PointCloudIn cloud; cloud.header.stamp = input->header.stamp; - nf_pc_.add (cloud.makeShared ()); + nf_pc_.add (ros_ptr(cloud.makeShared ())); nf_pi_.add (boost::make_shared (indices)); } @@ -191,8 +191,8 @@ namespace pcl_ros typedef sensor_msgs::PointCloud2 PointCloud2; typedef pcl::PointCloud PointCloudN; - typedef PointCloudN::Ptr PointCloudNPtr; - typedef PointCloudN::ConstPtr PointCloudNConstPtr; + typedef boost::shared_ptr PointCloudNPtr; + typedef boost::shared_ptr PointCloudNConstPtr; FeatureFromNormals () : normals_() {}; diff --git a/pcl_ros/include/pcl_ros/filters/filter.h b/pcl_ros/include/pcl_ros/filters/filter.h index ccc861f9..f43edca0 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.h +++ b/pcl_ros/include/pcl_ros/filters/filter.h @@ -59,8 +59,8 @@ namespace pcl_ros public: typedef sensor_msgs::PointCloud2 PointCloud2; - typedef boost::shared_ptr > IndicesPtr; - typedef boost::shared_ptr > IndicesConstPtr; + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; Filter () {} diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.h b/pcl_ros/include/pcl_ros/pcl_nodelet.h index 0e22dc7c..4b9839d5 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.h +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.h @@ -48,6 +48,7 @@ // PCL includes #include #include +#include #include #include #include "pcl_ros/point_cloud.h" @@ -75,8 +76,8 @@ namespace pcl_ros typedef sensor_msgs::PointCloud2 PointCloud2; typedef pcl::PointCloud PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; typedef pcl_msgs::PointIndices PointIndices; typedef PointIndices::Ptr PointIndicesPtr; @@ -86,8 +87,8 @@ namespace pcl_ros typedef ModelCoefficients::Ptr ModelCoefficientsPtr; typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; - typedef boost::shared_ptr > IndicesPtr; - typedef boost::shared_ptr > IndicesConstPtr; + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; /** \brief Empty constructor. */ PCLNodelet () : use_indices_ (false), latched_indices_ (false), diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.h index ee7c381d..32837d5c 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.h +++ b/pcl_ros/include/pcl_ros/point_cloud.h @@ -294,4 +294,125 @@ namespace ros } // namespace ros +// test if testing machinery can be implemented +#if defined(__cpp_rvalue_references) && defined(__cpp_constexpr) +#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1 +#else +#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0 +#endif + +#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED +#include // for std::is_same +#include // for std::shared_ptr + +#include +#if PCL_VERSION_COMPARE(>=, 1, 11, 0) +#include +#elif PCL_VERSION_COMPARE(>=, 1, 10, 0) +#include +#endif +#endif + +namespace pcl +{ + namespace detail + { +#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED +#if PCL_VERSION_COMPARE(>=, 1, 10, 0) + template + constexpr static bool pcl_uses_boost = std::is_same, + pcl::shared_ptr>::value; +#else + template + constexpr static bool pcl_uses_boost = true; +#endif + + template struct Holder + { + SharedPointer p; + + Holder(const SharedPointer &p) : p(p) {} + Holder(const Holder &other) : p(other.p) {} + Holder(Holder &&other) : p(std::move(other.p)) {} + + void operator () (...) { p.reset(); } + }; + + template + inline std::shared_ptr to_std_ptr(const boost::shared_ptr &p) + { + typedef Holder> H; + if(H *h = boost::get_deleter(p)) + { + return h->p; + } + else + { + return std::shared_ptr(p.get(), Holder>(p)); + } + } + + template + inline boost::shared_ptr to_boost_ptr(const std::shared_ptr &p) + { + typedef Holder> H; + if(H * h = std::get_deleter(p)) + { + return h->p; + } + else + { + return boost::shared_ptr(p.get(), Holder>(p)); + } + } +#endif + } // namespace pcl::detail + +// add functions to convert to smart pointer used by ROS + template + inline boost::shared_ptr ros_ptr(const boost::shared_ptr &p) + { + return p; + } + +#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED + template + inline boost::shared_ptr ros_ptr(const std::shared_ptr &p) + { + return detail::to_boost_ptr(p); + } + +// add functions to convert to smart pointer used by PCL, based on PCL's own pointer + template >::type> + inline std::shared_ptr pcl_ptr(const std::shared_ptr &p) + { + return p; + } + + template >::type> + inline std::shared_ptr pcl_ptr(const boost::shared_ptr &p) + { + return detail::to_std_ptr(p); + } + + template >::type> + inline boost::shared_ptr pcl_ptr(const std::shared_ptr &p) + { + return detail::to_boost_ptr(p); + } + + template >::type> + inline boost::shared_ptr pcl_ptr(const boost::shared_ptr &p) + { + return p; + } +#else + template + inline boost::shared_ptr pcl_ptr(const boost::shared_ptr &p) + { + return p; + } +#endif +} // namespace pcl + #endif diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h index 7134f905..13b85316 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h @@ -64,8 +64,8 @@ namespace pcl_ros class ExtractPolygonalPrismData : public PCLNodelet { typedef pcl::PointCloud PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; protected: /** \brief The output PointIndices publisher. */ diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h index af2c9126..9243e363 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h @@ -61,8 +61,8 @@ namespace pcl_ros class SACSegmentation : public PCLNodelet { typedef pcl::PointCloud PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; public: /** \brief Constructor. */ @@ -181,12 +181,12 @@ namespace pcl_ros class SACSegmentationFromNormals: public SACSegmentation { typedef pcl::PointCloud PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; typedef pcl::PointCloud PointCloudN; - typedef PointCloudN::Ptr PointCloudNPtr; - typedef PointCloudN::ConstPtr PointCloudNConstPtr; + typedef boost::shared_ptr PointCloudNPtr; + typedef boost::shared_ptr PointCloudNConstPtr; public: /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.h b/pcl_ros/include/pcl_ros/segmentation/segment_differences.h index 4914bc86..da767ab3 100644 --- a/pcl_ros/include/pcl_ros/segmentation/segment_differences.h +++ b/pcl_ros/include/pcl_ros/segmentation/segment_differences.h @@ -60,8 +60,8 @@ namespace pcl_ros class SegmentDifferences : public PCLNodelet { typedef pcl::PointCloud PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; public: /** \brief Empty constructor. */ diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.h b/pcl_ros/include/pcl_ros/surface/convex_hull.h index fa8d11d7..50741e10 100644 --- a/pcl_ros/include/pcl_ros/surface/convex_hull.h +++ b/pcl_ros/include/pcl_ros/surface/convex_hull.h @@ -56,8 +56,8 @@ namespace pcl_ros class ConvexHull2D : public PCLNodelet { typedef pcl::PointCloud PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; private: /** \brief Nodelet initialization routine. */ diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h index b909edf8..e90f562a 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h @@ -62,8 +62,8 @@ namespace pcl_ros typedef pcl::PointNormal NormalOut; typedef pcl::PointCloud PointCloudIn; - typedef PointCloudIn::Ptr PointCloudInPtr; - typedef PointCloudIn::ConstPtr PointCloudInConstPtr; + typedef boost::shared_ptr PointCloudInPtr; + typedef boost::shared_ptr PointCloudInConstPtr; typedef pcl::PointCloud NormalCloudOut; typedef pcl::KdTree KdTree; diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp index 9334641a..26ee07c1 100644 --- a/pcl_ros/src/pcl_ros/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -43,7 +43,7 @@ pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -57,17 +57,17 @@ pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputNormals (pcl_ptr(normals)); // Estimate the feature PointCloudOut output; impl_.compute (output); // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::BoundaryEstimation BoundaryEstimation; diff --git a/pcl_ros/src/pcl_ros/features/fpfh.cpp b/pcl_ros/src/pcl_ros/features/fpfh.cpp index 3f698aad..53be549c 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh.cpp @@ -43,7 +43,7 @@ pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -57,10 +57,10 @@ pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputNormals (pcl_ptr(normals)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -68,7 +68,7 @@ pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::FPFHEstimation FPFHEstimation; diff --git a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp index 58dd911f..e4adcabb 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp @@ -43,7 +43,7 @@ pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -57,10 +57,10 @@ pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputNormals (pcl_ptr(normals)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -68,7 +68,7 @@ pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; diff --git a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp index d0ec3441..a6e2249a 100644 --- a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp @@ -43,7 +43,7 @@ pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &c { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -56,9 +56,9 @@ pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); + impl_.setSearchSurface (pcl_ptr(surface)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -66,7 +66,7 @@ pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; diff --git a/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/features/normal_3d.cpp index 9e700f78..042186a9 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d.cpp @@ -43,7 +43,7 @@ pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -56,9 +56,9 @@ pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); + impl_.setSearchSurface (pcl_ptr(surface)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -66,7 +66,7 @@ pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::NormalEstimation NormalEstimation; diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp index a741c052..3e92d2f2 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp @@ -43,7 +43,7 @@ pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -56,9 +56,9 @@ pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); + impl_.setSearchSurface (pcl_ptr(surface)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -66,7 +66,7 @@ pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp index a4a8581e..680a4a02 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp @@ -45,7 +45,7 @@ pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloud output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -71,7 +71,7 @@ pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; diff --git a/pcl_ros/src/pcl_ros/features/pfh.cpp b/pcl_ros/src/pcl_ros/features/pfh.cpp index 38b4d19c..dd8409e2 100644 --- a/pcl_ros/src/pcl_ros/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/features/pfh.cpp @@ -43,7 +43,7 @@ pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -57,10 +57,10 @@ pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputNormals (pcl_ptr(normals)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -68,7 +68,7 @@ pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::PFHEstimation PFHEstimation; diff --git a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp index 113124dc..501d686e 100644 --- a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp @@ -43,7 +43,7 @@ pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -57,10 +57,10 @@ pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstP impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputNormals (pcl_ptr(normals)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -68,7 +68,7 @@ pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstP // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; diff --git a/pcl_ros/src/pcl_ros/features/shot.cpp b/pcl_ros/src/pcl_ros/features/shot.cpp index d051ab0f..ed6ba44b 100644 --- a/pcl_ros/src/pcl_ros/features/shot.cpp +++ b/pcl_ros/src/pcl_ros/features/shot.cpp @@ -42,7 +42,7 @@ pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -56,10 +56,10 @@ pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputNormals (pcl_ptr(normals)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -67,7 +67,7 @@ pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::SHOTEstimation SHOTEstimation; diff --git a/pcl_ros/src/pcl_ros/features/shot_omp.cpp b/pcl_ros/src/pcl_ros/features/shot_omp.cpp index 1ac1b065..4563f123 100644 --- a/pcl_ros/src/pcl_ros/features/shot_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/shot_omp.cpp @@ -42,7 +42,7 @@ pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -56,10 +56,10 @@ pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputNormals (pcl_ptr(normals)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -67,7 +67,7 @@ pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP; diff --git a/pcl_ros/src/pcl_ros/features/vfh.cpp b/pcl_ros/src/pcl_ros/features/vfh.cpp index 9d0fe361..ece448fd 100644 --- a/pcl_ros/src/pcl_ros/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/features/vfh.cpp @@ -43,7 +43,7 @@ pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } void @@ -57,10 +57,10 @@ pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, impl_.setRadiusSearch (search_radius_); // Set the inputs - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputNormals (pcl_ptr(normals)); // Estimate the feature PointCloudOut output; impl_.compute (output); @@ -68,7 +68,7 @@ pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::VFHEstimation VFHEstimation; diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index fdf06b2d..266bb4aa 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -203,7 +203,7 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( if (indices) indices_ptr.reset (new std::vector (indices->indices)); - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices_ptr); std::vector clusters; @@ -240,7 +240,7 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( header.stamp += ros::Duration (i * 0.001); toPCL(header, output.header); // Publish a Boost shared ptr const data - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); } diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 1963b6e8..67255788 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -190,16 +190,16 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( pub_output_.publish (inliers); return; } - impl_.setInputPlanarHull (planar_hull.makeShared ()); + impl_.setInputPlanarHull (pcl_ptr(planar_hull.makeShared ())); } else - impl_.setInputPlanarHull (hull); + impl_.setInputPlanarHull (pcl_ptr(hull)); IndicesPtr indices_ptr; if (indices && !indices->header.frame_id.empty ()) indices_ptr.reset (new std::vector (indices->indices)); - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index 7dc0c60a..9ba87987 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -325,7 +325,7 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou if (indices && !indices->header.frame_id.empty ()) indices_ptr.reset (new std::vector (indices->indices)); - impl_.setInputCloud (cloud_tf); + impl_.setInputCloud (pcl_ptr(cloud_tf)); impl_.setIndices (indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) @@ -652,8 +652,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( return; } - impl_.setInputCloud (cloud); - impl_.setInputNormals (cloud_normals); + impl_.setInputCloud (pcl_ptr(cloud)); + impl_.setInputNormals (pcl_ptr(cloud_normals)); IndicesPtr indices_ptr; if (indices && !indices->header.frame_id.empty ()) diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index 402b7790..d283a059 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -116,7 +116,7 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); PointCloud output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); return; } @@ -127,13 +127,13 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ()); - impl_.setInputCloud (cloud); - impl_.setTargetCloud (cloud_target); + impl_.setInputCloud (pcl_ptr(cloud)); + impl_.setTargetCloud (pcl_ptr(cloud_target)); PointCloud output; impl_.segment (output); - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (), output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ()); } diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index a577881c..23780325 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -122,7 +122,7 @@ void NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); // Publish an empty message output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); return; } // If indices are given, check if they are valid @@ -131,7 +131,7 @@ void NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); // Publish an empty message output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); return; } @@ -151,7 +151,7 @@ void if (indices) indices_ptr.reset (new std::vector (indices->indices)); - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); impl_.setIndices (indices_ptr); // Estimate the feature @@ -195,7 +195,7 @@ void } // Publish a Boost shared ptr const data output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); } typedef pcl_ros::ConvexHull2D ConvexHull2D; diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index 321f3e6a..6360cc02 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -142,7 +142,7 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr { NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); return; } // If indices are given, check if they are valid @@ -150,7 +150,7 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr { NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); return; } @@ -167,7 +167,7 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr /// // Reset the indices and surface pointers - impl_.setInputCloud (cloud); + impl_.setInputCloud (pcl_ptr(cloud)); IndicesPtr indices_ptr; if (indices) @@ -183,9 +183,9 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish (ros_ptr(output.makeShared ())); normals->header = cloud->header; - pub_normals_.publish (normals); + pub_normals_.publish (ros_ptr(normals)); } ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index 5c503eb7..2044b880 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -83,7 +83,7 @@ class PointCloudToPCD //////////////////////////////////////////////////////////////////////////////// // Callback void - cloud_cb (const pcl::PCLPointCloud2::ConstPtr& cloud) + cloud_cb (const boost::shared_ptr& cloud) { if ((cloud->width * cloud->height) == 0) return; From 5e27157ec2e366775a14f24dd02e8647c8b01513 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 13 May 2020 11:39:16 -0700 Subject: [PATCH 363/405] Updating CI and versions for foxy release (#280) * changing CI to foxy from eloquent * increment up version --- .travis.yml | 4 ++-- pcl_conversions/package.xml | 2 +- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 1df8be18..f2b2ff82 100644 --- a/.travis.yml +++ b/.travis.yml @@ -15,7 +15,7 @@ env: - VERBOSE_OUTPUT=true - VERBOSE_TESTS=true matrix: - - ROS_DISTRO=eloquent OS_NAME=ubuntu OS_CODE_NAME=bionic + - ROS_DISTRO=foxy OS_NAME=ubuntu OS_CODE_NAME=bionic install: - git clone --branch master --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci @@ -24,4 +24,4 @@ script: branches: only: - - /.*-devel$/ \ No newline at end of file + - /.*-devel$/ diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index 9ad11855..367e1939 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 2.1.0 + 2.2.0 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 1642ed3d..88d40c40 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,7 +1,7 @@ pcl_ros - 2.1.0 + 2.2.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index f8c1fcb3..2a68f39c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -2,7 +2,7 @@ perception_pcl - 2.1.0 + 2.2.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From 4ed0d3ecefa74b264a0384baf723b55f7288b4d0 Mon Sep 17 00:00:00 2001 From: Shivam Pandey <46261813+ShivamPR21@users.noreply.github.com> Date: Thu, 2 Jul 2020 00:43:29 +0530 Subject: [PATCH 364/405] link pcl library to targets (#287) * link pcl library to targets * changes OS_CODE_NAME to focal in travis config file --- .travis.yml | 2 +- pcl_conversions/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index f2b2ff82..6558554a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -15,7 +15,7 @@ env: - VERBOSE_OUTPUT=true - VERBOSE_TESTS=true matrix: - - ROS_DISTRO=foxy OS_NAME=ubuntu OS_CODE_NAME=bionic + - ROS_DISTRO=foxy OS_NAME=ubuntu OS_CODE_NAME=focal install: - git clone --branch master --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index e5677dda..499ac38e 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -45,7 +45,7 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}-test ${dependencies} ) - target_link_libraries(${PROJECT_NAME}-test ${Boost_LIBRARIES}) + target_link_libraries(${PROJECT_NAME}-test ${Boost_LIBRARIES} ${PCL_LIBRARIES}) endif() ament_export_include_directories(include) From 21cf907c46b1303f21164082a3bb5f82c2d64271 Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Thu, 30 Jul 2020 20:52:57 +0200 Subject: [PATCH 365/405] Add missing include for new PCL versions (#293) After this change https://github.com/PointCloudLibrary/pcl/commit/6df3e602a72ea16657f901c9a6911d95b263ba08#diff-24324a084e511502d7f34054ec31b353L50, an explicit include for KdTree is needed. It was formerly a transitive include via pcl/surface/mls.h - pcl/search/pcl_search.h - pcl/search/kdtree.h - pcl/kdtree/kdtree_flann.h. --- pcl_ros/include/pcl_ros/surface/moving_least_squares.h | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h index e90f562a..9486dfc1 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h @@ -42,6 +42,7 @@ // PCL includes #include +#include // for KdTree // Dynamic reconfigure #include From 0ac68106885f170ab5ba30d7ba96e6ba5a048253 Mon Sep 17 00:00:00 2001 From: Sean Kelly Date: Thu, 6 Aug 2020 14:13:01 -0400 Subject: [PATCH 366/405] Rename headers from .h to .hpp (#296) * Rename headers from .h to .hpp per ROS2 guidelines This change is the result of the following command run from pcl_ros dir: $ find -name *.h | xargs -I {} mv {} {}pp Signed-off-by: Sean Kelly * Update internal includes for the renamed headers This change was the result of the following bash script: $ find -name *.h -o -name *.cpp -o -name *.hpp | xargs -I {} sed -i 's/\(pcl_ros\/.*\)\(h\)\([">]\)$/\1\2pp\3/g' {} Signed-off-by: Sean Kelly --- .../include/pcl_ros/features/{boundary.h => boundary.hpp} | 2 +- pcl_ros/include/pcl_ros/features/{feature.h => feature.hpp} | 4 ++-- pcl_ros/include/pcl_ros/features/{fpfh.h => fpfh.hpp} | 2 +- .../include/pcl_ros/features/{fpfh_omp.h => fpfh_omp.hpp} | 2 +- .../features/{moment_invariants.h => moment_invariants.hpp} | 2 +- .../include/pcl_ros/features/{normal_3d.h => normal_3d.hpp} | 2 +- .../pcl_ros/features/{normal_3d_omp.h => normal_3d_omp.hpp} | 2 +- .../pcl_ros/features/{normal_3d_tbb.h => normal_3d_tbb.hpp} | 4 ++-- pcl_ros/include/pcl_ros/features/{pfh.h => pfh.hpp} | 2 +- .../{principal_curvatures.h => principal_curvatures.hpp} | 2 +- pcl_ros/include/pcl_ros/features/{shot.h => shot.hpp} | 2 +- .../include/pcl_ros/features/{shot_omp.h => shot_omp.hpp} | 2 +- pcl_ros/include/pcl_ros/features/{vfh.h => vfh.hpp} | 2 +- .../include/pcl_ros/filters/{crop_box.h => crop_box.hpp} | 4 ++-- .../filters/{extract_indices.h => extract_indices.hpp} | 4 ++-- pcl_ros/include/pcl_ros/filters/{filter.h => filter.hpp} | 4 ++-- .../pcl_ros/filters/{passthrough.h => passthrough.hpp} | 2 +- .../filters/{project_inliers.h => project_inliers.hpp} | 2 +- ...{radius_outlier_removal.h => radius_outlier_removal.hpp} | 4 ++-- ...al_outlier_removal.h => statistical_outlier_removal.hpp} | 4 ++-- .../pcl_ros/filters/{voxel_grid.h => voxel_grid.hpp} | 4 ++-- pcl_ros/include/pcl_ros/impl/transforms.hpp | 2 +- pcl_ros/include/pcl_ros/io/{bag_io.h => bag_io.hpp} | 2 +- .../pcl_ros/io/{concatenate_data.h => concatenate_data.hpp} | 0 .../io/{concatenate_fields.h => concatenate_fields.hpp} | 0 pcl_ros/include/pcl_ros/io/{pcd_io.h => pcd_io.hpp} | 2 +- pcl_ros/include/pcl_ros/{pcl_nodelet.h => pcl_nodelet.hpp} | 2 +- pcl_ros/include/pcl_ros/{point_cloud.h => point_cloud.hpp} | 0 pcl_ros/include/pcl_ros/{publisher.h => publisher.hpp} | 0 .../{extract_clusters.h => extract_clusters.hpp} | 4 ++-- ...ygonal_prism_data.h => extract_polygonal_prism_data.hpp} | 4 ++-- .../{sac_segmentation.h => sac_segmentation.hpp} | 6 +++--- .../{segment_differences.h => segment_differences.hpp} | 4 ++-- .../pcl_ros/surface/{convex_hull.h => convex_hull.hpp} | 2 +- .../{moving_least_squares.h => moving_least_squares.hpp} | 2 +- pcl_ros/include/pcl_ros/{transforms.h => transforms.hpp} | 0 pcl_ros/src/pcl_ros/features/boundary.cpp | 2 +- pcl_ros/src/pcl_ros/features/feature.cpp | 2 +- pcl_ros/src/pcl_ros/features/fpfh.cpp | 2 +- pcl_ros/src/pcl_ros/features/fpfh_omp.cpp | 2 +- pcl_ros/src/pcl_ros/features/moment_invariants.cpp | 2 +- pcl_ros/src/pcl_ros/features/normal_3d.cpp | 2 +- pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp | 2 +- pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp | 2 +- pcl_ros/src/pcl_ros/features/pfh.cpp | 2 +- pcl_ros/src/pcl_ros/features/principal_curvatures.cpp | 2 +- pcl_ros/src/pcl_ros/features/shot.cpp | 2 +- pcl_ros/src/pcl_ros/features/shot_omp.cpp | 2 +- pcl_ros/src/pcl_ros/features/vfh.cpp | 2 +- pcl_ros/src/pcl_ros/filters/crop_box.cpp | 2 +- pcl_ros/src/pcl_ros/filters/extract_indices.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/boundary.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/feature.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/fpfh.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/pfh.cpp | 2 +- .../src/pcl_ros/filters/features/principal_curvatures.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/vfh.cpp | 2 +- pcl_ros/src/pcl_ros/filters/filter.cpp | 4 ++-- pcl_ros/src/pcl_ros/filters/passthrough.cpp | 2 +- pcl_ros/src/pcl_ros/filters/project_inliers.cpp | 2 +- pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp | 2 +- pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp | 2 +- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 2 +- pcl_ros/src/pcl_ros/io/bag_io.cpp | 2 +- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 4 ++-- pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 2 +- pcl_ros/src/pcl_ros/io/io.cpp | 2 +- pcl_ros/src/pcl_ros/io/pcd_io.cpp | 2 +- pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp | 2 +- .../pcl_ros/segmentation/extract_polygonal_prism_data.cpp | 4 ++-- pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp | 2 +- pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp | 2 +- pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 2 +- pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp | 2 +- pcl_ros/src/test/test_tf_message_filter_pcl.cpp | 2 +- pcl_ros/src/transforms.cpp | 2 +- pcl_ros/tools/bag_to_pcd.cpp | 2 +- pcl_ros/tools/pcd_to_pointcloud.cpp | 2 +- 83 files changed, 94 insertions(+), 94 deletions(-) rename pcl_ros/include/pcl_ros/features/{boundary.h => boundary.hpp} (98%) rename pcl_ros/include/pcl_ros/features/{feature.h => feature.hpp} (99%) rename pcl_ros/include/pcl_ros/features/{fpfh.h => fpfh.hpp} (99%) rename pcl_ros/include/pcl_ros/features/{fpfh_omp.h => fpfh_omp.hpp} (99%) rename pcl_ros/include/pcl_ros/features/{moment_invariants.h => moment_invariants.hpp} (98%) rename pcl_ros/include/pcl_ros/features/{normal_3d.h => normal_3d.hpp} (98%) rename pcl_ros/include/pcl_ros/features/{normal_3d_omp.h => normal_3d_omp.hpp} (98%) rename pcl_ros/include/pcl_ros/features/{normal_3d_tbb.h => normal_3d_tbb.hpp} (97%) rename pcl_ros/include/pcl_ros/features/{pfh.h => pfh.hpp} (99%) rename pcl_ros/include/pcl_ros/features/{principal_curvatures.h => principal_curvatures.hpp} (98%) rename pcl_ros/include/pcl_ros/features/{shot.h => shot.hpp} (98%) rename pcl_ros/include/pcl_ros/features/{shot_omp.h => shot_omp.hpp} (98%) rename pcl_ros/include/pcl_ros/features/{vfh.h => vfh.hpp} (98%) rename pcl_ros/include/pcl_ros/filters/{crop_box.h => crop_box.hpp} (98%) rename pcl_ros/include/pcl_ros/filters/{extract_indices.h => extract_indices.hpp} (97%) rename pcl_ros/include/pcl_ros/filters/{filter.h => filter.hpp} (98%) rename pcl_ros/include/pcl_ros/filters/{passthrough.h => passthrough.hpp} (99%) rename pcl_ros/include/pcl_ros/filters/{project_inliers.h => project_inliers.hpp} (99%) rename pcl_ros/include/pcl_ros/filters/{radius_outlier_removal.h => radius_outlier_removal.hpp} (97%) rename pcl_ros/include/pcl_ros/filters/{statistical_outlier_removal.h => statistical_outlier_removal.hpp} (97%) rename pcl_ros/include/pcl_ros/filters/{voxel_grid.h => voxel_grid.hpp} (97%) rename pcl_ros/include/pcl_ros/io/{bag_io.h => bag_io.hpp} (99%) rename pcl_ros/include/pcl_ros/io/{concatenate_data.h => concatenate_data.hpp} (100%) rename pcl_ros/include/pcl_ros/io/{concatenate_fields.h => concatenate_fields.hpp} (100%) rename pcl_ros/include/pcl_ros/io/{pcd_io.h => pcd_io.hpp} (99%) rename pcl_ros/include/pcl_ros/{pcl_nodelet.h => pcl_nodelet.hpp} (99%) rename pcl_ros/include/pcl_ros/{point_cloud.h => point_cloud.hpp} (100%) rename pcl_ros/include/pcl_ros/{publisher.h => publisher.hpp} (100%) rename pcl_ros/include/pcl_ros/segmentation/{extract_clusters.h => extract_clusters.hpp} (97%) rename pcl_ros/include/pcl_ros/segmentation/{extract_polygonal_prism_data.h => extract_polygonal_prism_data.hpp} (98%) rename pcl_ros/include/pcl_ros/segmentation/{sac_segmentation.h => sac_segmentation.hpp} (98%) rename pcl_ros/include/pcl_ros/segmentation/{segment_differences.h => segment_differences.hpp} (98%) rename pcl_ros/include/pcl_ros/surface/{convex_hull.h => convex_hull.hpp} (99%) rename pcl_ros/include/pcl_ros/surface/{moving_least_squares.h => moving_least_squares.hpp} (99%) rename pcl_ros/include/pcl_ros/{transforms.h => transforms.hpp} (100%) diff --git a/pcl_ros/include/pcl_ros/features/boundary.h b/pcl_ros/include/pcl_ros/features/boundary.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/features/boundary.h rename to pcl_ros/include/pcl_ros/features/boundary.hpp index 84a92d80..350426e8 100644 --- a/pcl_ros/include/pcl_ros/features/boundary.h +++ b/pcl_ros/include/pcl_ros/features/boundary.hpp @@ -41,7 +41,7 @@ #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true #include -#include "pcl_ros/features/feature.h" +#include "pcl_ros/features/feature.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/features/feature.h rename to pcl_ros/include/pcl_ros/features/feature.hpp index 85118b20..155e6b61 100644 --- a/pcl_ros/include/pcl_ros/features/feature.h +++ b/pcl_ros/include/pcl_ros/features/feature.hpp @@ -42,12 +42,12 @@ #include #include -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" #include // Dynamic reconfigure #include -#include "pcl_ros/FeatureConfig.h" +#include "pcl_ros/FeatureConfig.hpp" // PCL conversions #include diff --git a/pcl_ros/include/pcl_ros/features/fpfh.h b/pcl_ros/include/pcl_ros/features/fpfh.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/features/fpfh.h rename to pcl_ros/include/pcl_ros/features/fpfh.hpp index 9f7800b5..8e3647a0 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh.h +++ b/pcl_ros/include/pcl_ros/features/fpfh.hpp @@ -39,7 +39,7 @@ #define PCL_ROS_FPFH_H_ #include -#include "pcl_ros/features/pfh.h" +#include "pcl_ros/features/pfh.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/fpfh_omp.h b/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/features/fpfh_omp.h rename to pcl_ros/include/pcl_ros/features/fpfh_omp.hpp index c024bbef..5710120a 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh_omp.h +++ b/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp @@ -39,7 +39,7 @@ #define PCL_ROS_FPFH_OMP_H_ #include -#include "pcl_ros/features/fpfh.h" +#include "pcl_ros/features/fpfh.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/moment_invariants.h b/pcl_ros/include/pcl_ros/features/moment_invariants.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/features/moment_invariants.h rename to pcl_ros/include/pcl_ros/features/moment_invariants.hpp index 3915bc92..c5b255c6 100644 --- a/pcl_ros/include/pcl_ros/features/moment_invariants.h +++ b/pcl_ros/include/pcl_ros/features/moment_invariants.hpp @@ -39,7 +39,7 @@ #define PCL_ROS_MOMENT_INVARIANTS_H_ #include -#include "pcl_ros/features/feature.h" +#include "pcl_ros/features/feature.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/normal_3d.h b/pcl_ros/include/pcl_ros/features/normal_3d.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/features/normal_3d.h rename to pcl_ros/include/pcl_ros/features/normal_3d.hpp index 468a069c..82ff188a 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d.hpp @@ -39,7 +39,7 @@ #define PCL_ROS_NORMAL_3D_H_ #include -#include "pcl_ros/features/feature.h" +#include "pcl_ros/features/feature.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_omp.h b/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/features/normal_3d_omp.h rename to pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp index 0cfe81fb..624d780f 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_omp.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp @@ -39,7 +39,7 @@ #define PCL_ROS_NORMAL_3D_OMP_H_ #include -#include "pcl_ros/features/normal_3d.h" +#include "pcl_ros/features/normal_3d.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp similarity index 97% rename from pcl_ros/include/pcl_ros/features/normal_3d_tbb.h rename to pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp index 7c2b8050..15282141 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp @@ -38,11 +38,11 @@ #ifndef PCL_ROS_NORMAL_3D_TBB_H_ #define PCL_ROS_NORMAL_3D_TBB_H_ -//#include "pcl_ros/pcl_ros_config.h" +//#include "pcl_ros/pcl_ros_config.hpp" //#if defined(HAVE_TBB) #include -#include "pcl_ros/features/normal_3d.h" +#include "pcl_ros/features/normal_3d.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/pfh.h b/pcl_ros/include/pcl_ros/features/pfh.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/features/pfh.h rename to pcl_ros/include/pcl_ros/features/pfh.hpp index faef1bc9..9ce4d101 100644 --- a/pcl_ros/include/pcl_ros/features/pfh.h +++ b/pcl_ros/include/pcl_ros/features/pfh.hpp @@ -39,7 +39,7 @@ #define PCL_ROS_PFH_H_ #include -#include "pcl_ros/features/feature.h" +#include "pcl_ros/features/feature.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.h b/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/features/principal_curvatures.h rename to pcl_ros/include/pcl_ros/features/principal_curvatures.hpp index 0ffc0722..98cd4134 100644 --- a/pcl_ros/include/pcl_ros/features/principal_curvatures.h +++ b/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp @@ -40,7 +40,7 @@ #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true #include -#include "pcl_ros/features/feature.h" +#include "pcl_ros/features/feature.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/shot.h b/pcl_ros/include/pcl_ros/features/shot.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/features/shot.h rename to pcl_ros/include/pcl_ros/features/shot.hpp index a3a3eea9..214736be 100644 --- a/pcl_ros/include/pcl_ros/features/shot.h +++ b/pcl_ros/include/pcl_ros/features/shot.hpp @@ -38,7 +38,7 @@ #define PCL_ROS_SHOT_H_ #include -#include "pcl_ros/features/pfh.h" +#include "pcl_ros/features/pfh.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/shot_omp.h b/pcl_ros/include/pcl_ros/features/shot_omp.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/features/shot_omp.h rename to pcl_ros/include/pcl_ros/features/shot_omp.hpp index 409f6b79..c02ca498 100644 --- a/pcl_ros/include/pcl_ros/features/shot_omp.h +++ b/pcl_ros/include/pcl_ros/features/shot_omp.hpp @@ -38,7 +38,7 @@ #define PCL_ROS_SHOT_OMP_H_ #include -#include "pcl_ros/features/shot.h" +#include "pcl_ros/features/shot.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/features/vfh.h b/pcl_ros/include/pcl_ros/features/vfh.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/features/vfh.h rename to pcl_ros/include/pcl_ros/features/vfh.hpp index 0bc6fae6..526cdcdf 100644 --- a/pcl_ros/include/pcl_ros/features/vfh.h +++ b/pcl_ros/include/pcl_ros/features/vfh.hpp @@ -39,7 +39,7 @@ #define PCL_ROS_FEATURES_VFH_H_ #include -#include "pcl_ros/features/fpfh.h" +#include "pcl_ros/features/fpfh.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.h b/pcl_ros/include/pcl_ros/filters/crop_box.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/filters/crop_box.h rename to pcl_ros/include/pcl_ros/filters/crop_box.hpp index 7fa08c99..cb376caf 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.h +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -41,10 +41,10 @@ // PCL includes #include -#include "pcl_ros/filters/filter.h" +#include "pcl_ros/filters/filter.hpp" // Dynamic reconfigure -#include "pcl_ros/CropBoxConfig.h" +#include "pcl_ros/CropBoxConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.h b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp similarity index 97% rename from pcl_ros/include/pcl_ros/filters/extract_indices.h rename to pcl_ros/include/pcl_ros/filters/extract_indices.hpp index 1ae0d1a9..a3b12678 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.h +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -41,8 +41,8 @@ // PCL includes #include -#include "pcl_ros/filters/filter.h" -#include "pcl_ros/ExtractIndicesConfig.h" +#include "pcl_ros/filters/filter.hpp" +#include "pcl_ros/ExtractIndicesConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/filters/filter.h b/pcl_ros/include/pcl_ros/filters/filter.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/filters/filter.h rename to pcl_ros/include/pcl_ros/filters/filter.hpp index f43edca0..cd622991 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.h +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -40,11 +40,11 @@ // PCL includes #include -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" // Dynamic reconfigure #include -#include "pcl_ros/FilterConfig.h" +#include "pcl_ros/FilterConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.h b/pcl_ros/include/pcl_ros/filters/passthrough.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/filters/passthrough.h rename to pcl_ros/include/pcl_ros/filters/passthrough.hpp index 4dad5f2d..1be1ef97 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.h +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -40,7 +40,7 @@ // PCL includes #include -#include "pcl_ros/filters/filter.h" +#include "pcl_ros/filters/filter.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.h b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/filters/project_inliers.h rename to pcl_ros/include/pcl_ros/filters/project_inliers.hpp index 1245e1c3..f72a8c2d 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.h +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -40,7 +40,7 @@ // PCL includes #include -#include "pcl_ros/filters/filter.h" +#include "pcl_ros/filters/filter.hpp" #include diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp similarity index 97% rename from pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h rename to pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp index 6120f9e5..3b1120ad 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -40,10 +40,10 @@ // PCL includes #include -#include "pcl_ros/filters/filter.h" +#include "pcl_ros/filters/filter.hpp" // Dynamic reconfigure -#include "pcl_ros/RadiusOutlierRemovalConfig.h" +#include "pcl_ros/RadiusOutlierRemovalConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp similarity index 97% rename from pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h rename to pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp index b3d17e35..6776cc19 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -40,10 +40,10 @@ // PCL includes #include -#include "pcl_ros/filters/filter.h" +#include "pcl_ros/filters/filter.hpp" // Dynamic reconfigure -#include "pcl_ros/StatisticalOutlierRemovalConfig.h" +#include "pcl_ros/StatisticalOutlierRemovalConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.h b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp similarity index 97% rename from pcl_ros/include/pcl_ros/filters/voxel_grid.h rename to pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index 1d887f7e..6f900f23 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.h +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -40,10 +40,10 @@ // PCL includes #include -#include "pcl_ros/filters/filter.h" +#include "pcl_ros/filters/filter.hpp" // Dynamic reconfigure -#include "pcl_ros/VoxelGridConfig.h" +#include "pcl_ros/VoxelGridConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index 52b49e25..a4ab2121 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -38,7 +38,7 @@ #define pcl_ros_IMPL_TRANSFORMS_H_ #include -#include "pcl_ros/transforms.h" +#include "pcl_ros/transforms.hpp" using pcl_conversions::fromPCL; using pcl_conversions::toPCL; diff --git a/pcl_ros/include/pcl_ros/io/bag_io.h b/pcl_ros/include/pcl_ros/io/bag_io.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/io/bag_io.h rename to pcl_ros/include/pcl_ros/io/bag_io.hpp index 18f53de2..f526f2ea 100644 --- a/pcl_ros/include/pcl_ros/io/bag_io.h +++ b/pcl_ros/include/pcl_ros/io/bag_io.hpp @@ -38,7 +38,7 @@ #ifndef PCL_ROS_IO_BAG_IO_H_ #define PCL_ROS_IO_BAG_IO_H_ -#include +#include #include #include #include diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.h b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp similarity index 100% rename from pcl_ros/include/pcl_ros/io/concatenate_data.h rename to pcl_ros/include/pcl_ros/io/concatenate_data.hpp diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.h b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp similarity index 100% rename from pcl_ros/include/pcl_ros/io/concatenate_fields.h rename to pcl_ros/include/pcl_ros/io/concatenate_fields.hpp diff --git a/pcl_ros/include/pcl_ros/io/pcd_io.h b/pcl_ros/include/pcl_ros/io/pcd_io.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/io/pcd_io.h rename to pcl_ros/include/pcl_ros/io/pcd_io.hpp index 89663451..f551817b 100644 --- a/pcl_ros/include/pcl_ros/io/pcd_io.h +++ b/pcl_ros/include/pcl_ros/io/pcd_io.hpp @@ -39,7 +39,7 @@ #define PCL_ROS_IO_PCD_IO_H_ #include -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.h b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/pcl_nodelet.h rename to pcl_ros/include/pcl_ros/pcl_nodelet.hpp index 4b9839d5..fc4b64c8 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.h +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp @@ -51,7 +51,7 @@ #include #include #include -#include "pcl_ros/point_cloud.h" +#include "pcl_ros/point_cloud.hpp" // ROS Nodelet includes #include #include diff --git a/pcl_ros/include/pcl_ros/point_cloud.h b/pcl_ros/include/pcl_ros/point_cloud.hpp similarity index 100% rename from pcl_ros/include/pcl_ros/point_cloud.h rename to pcl_ros/include/pcl_ros/point_cloud.hpp diff --git a/pcl_ros/include/pcl_ros/publisher.h b/pcl_ros/include/pcl_ros/publisher.hpp similarity index 100% rename from pcl_ros/include/pcl_ros/publisher.h rename to pcl_ros/include/pcl_ros/publisher.hpp diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp similarity index 97% rename from pcl_ros/include/pcl_ros/segmentation/extract_clusters.h rename to pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp index dc79f796..b9c48425 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h +++ b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp @@ -39,11 +39,11 @@ #define PCL_ROS_EXTRACT_CLUSTERS_H_ #include -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" // Dynamic reconfigure #include -#include "pcl_ros/EuclideanClusterExtractionConfig.h" +#include "pcl_ros/EuclideanClusterExtractionConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h rename to pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp index 13b85316..458e565d 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp @@ -38,7 +38,7 @@ #ifndef PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ #define PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" #include #include #include @@ -48,7 +48,7 @@ // Dynamic reconfigure #include -#include "pcl_ros/ExtractPolygonalPrismDataConfig.h" +#include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h rename to pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp index 9243e363..e0f68690 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -38,7 +38,7 @@ #ifndef PCL_ROS_SAC_SEGMENTATION_H_ #define PCL_ROS_SAC_SEGMENTATION_H_ -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" #include // PCL includes @@ -46,8 +46,8 @@ // Dynamic reconfigure #include -#include "pcl_ros/SACSegmentationConfig.h" -#include "pcl_ros/SACSegmentationFromNormalsConfig.h" +#include "pcl_ros/SACSegmentationConfig.hpp" +#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp" namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.h b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp similarity index 98% rename from pcl_ros/include/pcl_ros/segmentation/segment_differences.h rename to pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp index da767ab3..ee7fb28c 100644 --- a/pcl_ros/include/pcl_ros/segmentation/segment_differences.h +++ b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp @@ -39,11 +39,11 @@ #define PCL_ROS_SEGMENT_DIFFERENCES_H_ #include -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" // Dynamic reconfigure #include -#include "pcl_ros/SegmentDifferencesConfig.h" +#include "pcl_ros/SegmentDifferencesConfig.hpp" namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.h b/pcl_ros/include/pcl_ros/surface/convex_hull.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/surface/convex_hull.h rename to pcl_ros/include/pcl_ros/surface/convex_hull.hpp index 50741e10..066d7a16 100644 --- a/pcl_ros/include/pcl_ros/surface/convex_hull.h +++ b/pcl_ros/include/pcl_ros/surface/convex_hull.hpp @@ -38,7 +38,7 @@ #ifndef PCL_ROS_CONVEX_HULL_2D_H_ #define PCL_ROS_CONVEX_HULL_2D_H_ -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" // PCL includes #include diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h b/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp similarity index 99% rename from pcl_ros/include/pcl_ros/surface/moving_least_squares.h rename to pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp index 9486dfc1..36542087 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp @@ -38,7 +38,7 @@ #ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ #define PCL_ROS_MOVING_LEAST_SQUARES_H_ -#include "pcl_ros/pcl_nodelet.h" +#include "pcl_ros/pcl_nodelet.hpp" // PCL includes #include diff --git a/pcl_ros/include/pcl_ros/transforms.h b/pcl_ros/include/pcl_ros/transforms.hpp similarity index 100% rename from pcl_ros/include/pcl_ros/transforms.h rename to pcl_ros/include/pcl_ros/transforms.hpp diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp index 26ee07c1..6571ed30 100644 --- a/pcl_ros/src/pcl_ros/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/boundary.h" +#include "pcl_ros/features/boundary.hpp" void pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp index b7027c70..fcb9516d 100644 --- a/pcl_ros/src/pcl_ros/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -48,7 +48,7 @@ //#include "principal_curvatures.cpp" //#include "vfh.cpp" #include -#include "pcl_ros/features/feature.h" +#include "pcl_ros/features/feature.hpp" #include //////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/features/fpfh.cpp b/pcl_ros/src/pcl_ros/features/fpfh.cpp index 53be549c..c1ac221a 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/fpfh.h" +#include "pcl_ros/features/fpfh.hpp" void pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp index e4adcabb..8dfaf044 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/fpfh_omp.h" +#include "pcl_ros/features/fpfh_omp.hpp" void pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp index a6e2249a..a6652dec 100644 --- a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/moment_invariants.h" +#include "pcl_ros/features/moment_invariants.hpp" void pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/features/normal_3d.cpp index 042186a9..489fa473 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/normal_3d.h" +#include "pcl_ros/features/normal_3d.hpp" void pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp index 3e92d2f2..b5aadc94 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/normal_3d_omp.h" +#include "pcl_ros/features/normal_3d_omp.hpp" void pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp index 680a4a02..b76bf1aa 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/normal_3d_tbb.h" +#include "pcl_ros/features/normal_3d_tbb.hpp" #if defined HAVE_TBB diff --git a/pcl_ros/src/pcl_ros/features/pfh.cpp b/pcl_ros/src/pcl_ros/features/pfh.cpp index dd8409e2..b7210316 100644 --- a/pcl_ros/src/pcl_ros/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/features/pfh.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/pfh.h" +#include "pcl_ros/features/pfh.hpp" void pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp index 501d686e..a5332a62 100644 --- a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/principal_curvatures.h" +#include "pcl_ros/features/principal_curvatures.hpp" void pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/shot.cpp b/pcl_ros/src/pcl_ros/features/shot.cpp index ed6ba44b..eca60020 100644 --- a/pcl_ros/src/pcl_ros/features/shot.cpp +++ b/pcl_ros/src/pcl_ros/features/shot.cpp @@ -35,7 +35,7 @@ */ #include -#include "pcl_ros/features/shot.h" +#include "pcl_ros/features/shot.hpp" void pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/shot_omp.cpp b/pcl_ros/src/pcl_ros/features/shot_omp.cpp index 4563f123..70ed3974 100644 --- a/pcl_ros/src/pcl_ros/features/shot_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/shot_omp.cpp @@ -35,7 +35,7 @@ */ #include -#include "pcl_ros/features/shot_omp.h" +#include "pcl_ros/features/shot_omp.hpp" void pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/features/vfh.cpp b/pcl_ros/src/pcl_ros/features/vfh.cpp index ece448fd..c7cb6cc3 100644 --- a/pcl_ros/src/pcl_ros/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/features/vfh.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/vfh.h" +#include "pcl_ros/features/vfh.hpp" void pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp index fc8acdad..c7f1d517 100644 --- a/pcl_ros/src/pcl_ros/filters/crop_box.cpp +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -37,7 +37,7 @@ */ #include -#include "pcl_ros/filters/crop_box.h" +#include "pcl_ros/filters/crop_box.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp index 98419b09..59554f0e 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/filters/extract_indices.h" +#include "pcl_ros/filters/extract_indices.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool diff --git a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp index d90ac374..13a66c84 100644 --- a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/boundary.h" +#include "pcl_ros/features/boundary.hpp" void pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/features/feature.cpp b/pcl_ros/src/pcl_ros/filters/features/feature.cpp index 434707ee..1e36d828 100644 --- a/pcl_ros/src/pcl_ros/filters/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/feature.cpp @@ -48,7 +48,7 @@ //#include "principal_curvatures.cpp" //#include "vfh.cpp" #include -#include "pcl_ros/features/feature.h" +#include "pcl_ros/features/feature.hpp" #include //////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp index c7f98f1d..728f0d50 100644 --- a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/fpfh.h" +#include "pcl_ros/features/fpfh.hpp" void pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp index 57c508c0..822817a2 100644 --- a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/fpfh_omp.h" +#include "pcl_ros/features/fpfh_omp.hpp" void pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp index 0d1c039a..d8be14ac 100644 --- a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/moment_invariants.h" +#include "pcl_ros/features/moment_invariants.hpp" void pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp index fbc81abc..4d62f381 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/normal_3d.h" +#include "pcl_ros/features/normal_3d.hpp" void pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp index 0a257afa..c64e85bd 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/normal_3d_omp.h" +#include "pcl_ros/features/normal_3d_omp.hpp" void pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp index 330e2de7..5874eea8 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/normal_3d_tbb.h" +#include "pcl_ros/features/normal_3d_tbb.hpp" #if defined HAVE_TBB diff --git a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp index b8e3d8d2..01b1c8f2 100644 --- a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/pfh.h" +#include "pcl_ros/features/pfh.hpp" void pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp index 5e8c10f7..cb7ce548 100644 --- a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/principal_curvatures.h" +#include "pcl_ros/features/principal_curvatures.hpp" void pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp index 351a8af7..5f77d67a 100644 --- a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/features/vfh.h" +#include "pcl_ros/features/vfh.hpp" void pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 4fa5731a..2970504a 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -36,8 +36,8 @@ */ #include -#include "pcl_ros/transforms.h" -#include "pcl_ros/filters/filter.h" +#include "pcl_ros/transforms.hpp" +#include "pcl_ros/filters/filter.hpp" /*//#include //#include diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index e0de5af3..693442e8 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/filters/passthrough.h" +#include "pcl_ros/filters/passthrough.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index 6b7239c3..2065f415 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/filters/project_inliers.h" +#include "pcl_ros/filters/project_inliers.hpp" #include ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp index 46c00a14..34e0240a 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/filters/radius_outlier_removal.h" +#include "pcl_ros/filters/radius_outlier_removal.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp index 16535b7f..282419c6 100644 --- a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/filters/statistical_outlier_removal.h" +#include "pcl_ros/filters/statistical_outlier_removal.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index db718d5b..be00eaa5 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/filters/voxel_grid.h" +#include "pcl_ros/filters/voxel_grid.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool diff --git a/pcl_ros/src/pcl_ros/io/bag_io.cpp b/pcl_ros/src/pcl_ros/io/bag_io.cpp index a17edd14..12930fc2 100644 --- a/pcl_ros/src/pcl_ros/io/bag_io.cpp +++ b/pcl_ros/src/pcl_ros/io/bag_io.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/io/bag_io.h" +#include "pcl_ros/io/bag_io.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index a7a00754..13027d51 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -37,8 +37,8 @@ #include #include -#include "pcl_ros/transforms.h" -#include "pcl_ros/io/concatenate_data.h" +#include "pcl_ros/transforms.hpp" +#include "pcl_ros/io/concatenate_data.hpp" #include diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index 159f531f..35618ea1 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -39,7 +39,7 @@ #include #include -#include "pcl_ros/io/concatenate_fields.h" +#include "pcl_ros/io/concatenate_fields.hpp" #include diff --git a/pcl_ros/src/pcl_ros/io/io.cpp b/pcl_ros/src/pcl_ros/io/io.cpp index c9887581..48fa54d6 100644 --- a/pcl_ros/src/pcl_ros/io/io.cpp +++ b/pcl_ros/src/pcl_ros/io/io.cpp @@ -38,7 +38,7 @@ #include #include #include -//#include +//#include #include #include diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index fe396b5d..df1fc24a 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -36,7 +36,7 @@ */ #include -#include +#include //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index 266bb4aa..b953497b 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -38,7 +38,7 @@ #include #include #include -#include "pcl_ros/segmentation/extract_clusters.h" +#include "pcl_ros/segmentation/extract_clusters.hpp" #include diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 67255788..b05770b7 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -36,8 +36,8 @@ */ #include -#include "pcl_ros/transforms.h" -#include "pcl_ros/segmentation/extract_polygonal_prism_data.h" +#include "pcl_ros/transforms.hpp" +#include "pcl_ros/segmentation/extract_polygonal_prism_data.hpp" #include #include diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index 9ba87987..cfb46d9b 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/segmentation/sac_segmentation.h" +#include "pcl_ros/segmentation/sac_segmentation.hpp" #include #include diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index d283a059..79d01d12 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/segmentation/segment_differences.h" +#include "pcl_ros/segmentation/segment_differences.hpp" #include ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index 23780325..22cc2b98 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -37,7 +37,7 @@ #include #include -#include "pcl_ros/surface/convex_hull.h" +#include "pcl_ros/surface/convex_hull.hpp" #include ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index 6360cc02..26b6f3f6 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -36,7 +36,7 @@ */ #include -#include "pcl_ros/surface/moving_least_squares.h" +#include "pcl_ros/surface/moving_least_squares.hpp" #include ////////////////////////////////////////////////////////////////////////////////////////////// void diff --git a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp index 28cfbb63..e3f4930e 100644 --- a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp +++ b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index 75fc6c4e..76a93c4e 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -38,7 +38,7 @@ #include #include #include -#include "pcl_ros/transforms.h" +#include "pcl_ros/transforms.hpp" #include "pcl_ros/impl/transforms.hpp" namespace pcl_ros diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index b07f03bb..d48f2c58 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -51,7 +51,7 @@ Cloud Data) format. #include #include #include -#include "pcl_ros/transforms.h" +#include "pcl_ros/transforms.hpp" #include #include diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index 03638183..02ef1bbc 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -54,7 +54,7 @@ #include #include -#include "pcl_ros/publisher.h" +#include "pcl_ros/publisher.hpp" using namespace std; From 63cee139f1950f021191a3b1fe0dc8aeedf61770 Mon Sep 17 00:00:00 2001 From: Sean Kelly Date: Thu, 6 Aug 2020 15:28:29 -0400 Subject: [PATCH 367/405] Apply ament_uncrustify --reformat (#297) Signed-off-by: Sean Kelly --- pcl_ros/include/pcl_ros/features/boundary.hpp | 67 +- pcl_ros/include/pcl_ros/features/feature.hpp | 351 ++++---- pcl_ros/include/pcl_ros/features/fpfh.hpp | 95 ++- pcl_ros/include/pcl_ros/features/fpfh_omp.hpp | 88 +- .../pcl_ros/features/moment_invariants.hpp | 61 +- .../include/pcl_ros/features/normal_3d.hpp | 68 +- .../pcl_ros/features/normal_3d_omp.hpp | 55 +- .../pcl_ros/features/normal_3d_tbb.hpp | 57 +- pcl_ros/include/pcl_ros/features/pfh.hpp | 95 ++- .../pcl_ros/features/principal_curvatures.hpp | 63 +- pcl_ros/include/pcl_ros/features/shot.hpp | 57 +- pcl_ros/include/pcl_ros/features/shot_omp.hpp | 54 +- pcl_ros/include/pcl_ros/features/vfh.hpp | 63 +- pcl_ros/include/pcl_ros/filters/crop_box.hpp | 98 +-- .../pcl_ros/filters/extract_indices.hpp | 83 +- pcl_ros/include/pcl_ros/filters/filter.hpp | 187 ++-- .../include/pcl_ros/filters/passthrough.hpp | 90 +- .../pcl_ros/filters/project_inliers.hpp | 112 +-- .../filters/radius_outlier_removal.hpp | 85 +- .../filters/statistical_outlier_removal.hpp | 98 +-- .../include/pcl_ros/filters/voxel_grid.hpp | 68 +- pcl_ros/include/pcl_ros/impl/transforms.hpp | 210 +++-- pcl_ros/include/pcl_ros/io/bag_io.hpp | 131 +-- .../include/pcl_ros/io/concatenate_data.hpp | 160 ++-- .../include/pcl_ros/io/concatenate_fields.hpp | 78 +- pcl_ros/include/pcl_ros/io/pcd_io.hpp | 152 ++-- pcl_ros/include/pcl_ros/pcl_nodelet.hpp | 309 +++---- pcl_ros/include/pcl_ros/point_cloud.hpp | 661 ++++++++------- pcl_ros/include/pcl_ros/publisher.hpp | 170 ++-- .../pcl_ros/segmentation/extract_clusters.hpp | 105 +-- .../extract_polygonal_prism_data.hpp | 132 +-- .../pcl_ros/segmentation/sac_segmentation.hpp | 422 +++++----- .../segmentation/segment_differences.hpp | 107 +-- .../include/pcl_ros/surface/convex_hull.hpp | 75 +- .../pcl_ros/surface/moving_least_squares.hpp | 170 ++-- pcl_ros/include/pcl_ros/transforms.hpp | 232 ++--- pcl_ros/src/pcl_ros/features/boundary.cpp | 29 +- pcl_ros/src/pcl_ros/features/feature.cpp | 690 ++++++++------- pcl_ros/src/pcl_ros/features/fpfh.cpp | 34 +- pcl_ros/src/pcl_ros/features/fpfh_omp.cpp | 34 +- .../pcl_ros/features/moment_invariants.cpp | 30 +- pcl_ros/src/pcl_ros/features/normal_3d.cpp | 30 +- .../src/pcl_ros/features/normal_3d_omp.cpp | 30 +- .../src/pcl_ros/features/normal_3d_tbb.cpp | 34 +- pcl_ros/src/pcl_ros/features/pfh.cpp | 34 +- .../pcl_ros/features/principal_curvatures.cpp | 34 +- pcl_ros/src/pcl_ros/features/shot.cpp | 34 +- pcl_ros/src/pcl_ros/features/shot_omp.cpp | 34 +- pcl_ros/src/pcl_ros/features/vfh.cpp | 34 +- pcl_ros/src/pcl_ros/filters/crop_box.cpp | 70 +- .../src/pcl_ros/filters/extract_indices.cpp | 29 +- .../src/pcl_ros/filters/features/boundary.cpp | 39 +- .../src/pcl_ros/filters/features/feature.cpp | 604 +++++++------ pcl_ros/src/pcl_ros/filters/features/fpfh.cpp | 40 +- .../src/pcl_ros/filters/features/fpfh_omp.cpp | 40 +- .../filters/features/moment_invariants.cpp | 36 +- .../pcl_ros/filters/features/normal_3d.cpp | 36 +- .../filters/features/normal_3d_omp.cpp | 36 +- .../filters/features/normal_3d_tbb.cpp | 36 +- pcl_ros/src/pcl_ros/filters/features/pfh.cpp | 40 +- .../filters/features/principal_curvatures.cpp | 40 +- pcl_ros/src/pcl_ros/filters/features/vfh.cpp | 40 +- pcl_ros/src/pcl_ros/filters/filter.cpp | 194 +++-- pcl_ros/src/pcl_ros/filters/passthrough.cpp | 79 +- .../src/pcl_ros/filters/project_inliers.cpp | 132 +-- .../filters/radius_outlier_removal.cpp | 40 +- .../filters/statistical_outlier_removal.cpp | 51 +- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 94 ++- pcl_ros/src/pcl_ros/io/bag_io.cpp | 83 +- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 326 +++---- pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 119 +-- pcl_ros/src/pcl_ros/io/io.cpp | 8 +- pcl_ros/src/pcl_ros/io/pcd_io.cpp | 163 ++-- .../pcl_ros/segmentation/extract_clusters.cpp | 239 +++--- .../extract_polygonal_prism_data.cpp | 222 ++--- .../pcl_ros/segmentation/sac_segmentation.cpp | 795 ++++++++++-------- .../segmentation/segment_differences.cpp | 130 +-- .../src/pcl_ros/segmentation/segmentation.cpp | 2 - pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 164 ++-- .../pcl_ros/surface/moving_least_squares.cpp | 225 ++--- pcl_ros/src/pcl_ros/surface/surface.cpp | 2 - .../src/test/test_tf_message_filter_pcl.cpp | 72 +- pcl_ros/src/transforms.cpp | 372 +++++--- pcl_ros/tools/bag_to_pcd.cpp | 101 +-- pcl_ros/tools/convert_pcd_to_image.cpp | 38 +- pcl_ros/tools/convert_pointcloud_to_image.cpp | 46 +- pcl_ros/tools/pcd_to_pointcloud.cpp | 167 ++-- pcl_ros/tools/pointcloud_to_pcd.cpp | 197 +++-- 88 files changed, 6019 insertions(+), 5318 deletions(-) diff --git a/pcl_ros/include/pcl_ros/features/boundary.hpp b/pcl_ros/include/pcl_ros/features/boundary.hpp index 350426e8..e226359e 100644 --- a/pcl_ros/include/pcl_ros/features/boundary.hpp +++ b/pcl_ros/include/pcl_ros/features/boundary.hpp @@ -45,44 +45,43 @@ namespace pcl_ros { - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle - * criterion. The code makes use of the estimated surface normals at each point in the input dataset. - * - * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at - * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. - * \author Radu Bogdan Rusu - */ - class BoundaryEstimation: public FeatureFromNormals +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle + * criterion. The code makes use of the estimated surface normals at each point in the input dataset. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. + * \author Radu Bogdan Rusu + */ +class BoundaryEstimation : public FeatureFromNormals +{ +private: + pcl::BoundaryEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::BoundaryEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_BOUNDARY_H_ - - diff --git a/pcl_ros/include/pcl_ros/features/feature.hpp b/pcl_ros/include/pcl_ros/features/feature.hpp index 155e6b61..bf1a9d83 100644 --- a/pcl_ros/include/pcl_ros/features/feature.hpp +++ b/pcl_ros/include/pcl_ros/features/feature.hpp @@ -54,198 +54,209 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - /////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Feature represents the base feature class. Some generic 3D operations that - * are applicable to all features are defined here as static methods. - * \author Radu Bogdan Rusu +/////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Feature represents the base feature class. Some generic 3D operations that + * are applicable to all features are defined here as static methods. + * \author Radu Bogdan Rusu + */ +class Feature : public PCLNodelet +{ +public: + typedef pcl::KdTree KdTree; + typedef pcl::KdTree::Ptr KdTreePtr; + + typedef pcl::PointCloud PointCloudIn; + typedef boost::shared_ptr PointCloudInPtr; + typedef boost::shared_ptr PointCloudInConstPtr; + + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; + + /** \brief Empty constructor. */ + Feature() + : /*input_(), indices_(), surface_(), */ tree_(), k_(0), search_radius_(0), + use_surface_(false), spatial_locator_type_(-1) + {} + +protected: + /** \brief The input point cloud dataset. */ + //PointCloudInConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + //IndicesConstPtr indices_; + + /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ + //PointCloudInConstPtr surface_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The number of K nearest neighbors to use for each point. */ + int k_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + // ROS nodelet attributes + /** \brief The surface PointCloud subscriber filter. */ + message_filters::Subscriber sub_surface_filter_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */ + bool use_surface_; + + /** \brief Parameter for the spatial locator tree. By convention, the values represent: + * 0: ANN (Approximate Nearest Neigbor library) kd-tree + * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree + * 2: Organized spatial dataset index */ - class Feature : public PCLNodelet + int spatial_locator_type_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; + + /** \brief Child initialization routine. Internal method. */ + virtual bool childInit(ros::NodeHandle & nh) = 0; + + /** \brief Publish an empty point cloud of the feature output type. */ + virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0; + + /** \brief Compute the feature and publish it. Internal method. */ + virtual void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) = 0; + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback(FeatureConfig & config, uint32_t level); + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_pi_; + message_filters::PassThrough nf_pc_; + + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. + */ + inline void + input_callback(const PointCloudInConstPtr & input) { - public: - typedef pcl::KdTree KdTree; - typedef pcl::KdTree::Ptr KdTreePtr; + PointIndices indices; + indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp; + PointCloudIn cloud; + cloud.header.stamp = input->header.stamp; + nf_pc_.add(ros_ptr(cloud.makeShared())); + nf_pi_.add(boost::make_shared(indices)); + } - typedef pcl::PointCloud PointCloudIn; - typedef boost::shared_ptr PointCloudInPtr; - typedef boost::shared_ptr PointCloudInConstPtr; +private: + /** \brief Synchronized input, surface, and point indices.*/ + boost::shared_ptr>> sync_input_surface_indices_a_; + boost::shared_ptr>> sync_input_surface_indices_e_; - typedef pcl::IndicesPtr IndicesPtr; - typedef pcl::IndicesConstPtr IndicesConstPtr; + /** \brief Nodelet initialization routine. */ + virtual void onInit(); - /** \brief Empty constructor. */ - Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0), - use_surface_(false), spatial_locator_type_(-1) - {}; + /** \brief NodeletLazy connection routine. */ + virtual void subscribe(); + virtual void unsubscribe(); - protected: - /** \brief The input point cloud dataset. */ - //PointCloudInConstPtr input_; + /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. + * \param cloud the pointer to the input point cloud + * \param cloud_surface the pointer to the surface point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_surface_indices_callback( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & cloud_surface, + const PointIndicesConstPtr & indices); - /** \brief A pointer to the vector of point indices to use. */ - //IndicesConstPtr indices_; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; - /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ - //PointCloudInConstPtr surface_; +////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// +class FeatureFromNormals : public Feature +{ +public: + typedef sensor_msgs::PointCloud2 PointCloud2; - /** \brief A pointer to the spatial search object. */ - KdTreePtr tree_; + typedef pcl::PointCloud PointCloudN; + typedef boost::shared_ptr PointCloudNPtr; + typedef boost::shared_ptr PointCloudNConstPtr; - /** \brief The number of K nearest neighbors to use for each point. */ - int k_; + FeatureFromNormals() + : normals_() {} - /** \brief The nearest neighbors search radius for each point. */ - double search_radius_; +protected: + /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */ + PointCloudNConstPtr normals_; - // ROS nodelet attributes - /** \brief The surface PointCloud subscriber filter. */ - message_filters::Subscriber sub_surface_filter_; - - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + /** \brief Child initialization routine. Internal method. */ + virtual bool childInit(ros::NodeHandle & nh) = 0; - /** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */ - bool use_surface_; + /** \brief Publish an empty point cloud of the feature output type. */ + virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0; - /** \brief Parameter for the spatial locator tree. By convention, the values represent: - * 0: ANN (Approximate Nearest Neigbor library) kd-tree - * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree - * 2: Organized spatial dataset index - */ - int spatial_locator_type_; + /** \brief Compute the feature and publish it. */ + virtual void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) = 0; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; +private: + /** \brief The normals PointCloud subscriber filter. */ + message_filters::Subscriber sub_normals_filter_; - /** \brief Child initialization routine. Internal method. */ - virtual bool childInit (ros::NodeHandle &nh) = 0; + /** \brief Synchronized input, normals, surface, and point indices.*/ + boost::shared_ptr>> sync_input_normals_surface_indices_a_; + boost::shared_ptr>> sync_input_normals_surface_indices_e_; - /** \brief Publish an empty point cloud of the feature output type. */ - virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0; + /** \brief Internal method. */ + void computePublish( + const PointCloudInConstPtr &, + const PointCloudInConstPtr &, + const IndicesPtr &) {} // This should never be called - /** \brief Compute the feature and publish it. Internal method. */ - virtual void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) = 0; + /** \brief Nodelet initialization routine. */ + virtual void onInit(); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback (FeatureConfig &config, uint32_t level); + /** \brief NodeletLazy connection routine. */ + virtual void subscribe(); + virtual void unsubscribe(); - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_pi_; - message_filters::PassThrough nf_pc_; + /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. + * \param cloud the pointer to the input point cloud + * \param cloud_normals the pointer to the input point cloud normals + * \param cloud_surface the pointer to the surface point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_normals_surface_indices_callback( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & cloud_normals, + const PointCloudInConstPtr & cloud_surface, + const PointIndicesConstPtr & indices); - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback (const PointCloudInConstPtr &input) - { - PointIndices indices; - indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp; - PointCloudIn cloud; - cloud.header.stamp = input->header.stamp; - nf_pc_.add (ros_ptr(cloud.makeShared ())); - nf_pi_.add (boost::make_shared (indices)); - } - - private: - /** \brief Synchronized input, surface, and point indices.*/ - boost::shared_ptr > > sync_input_surface_indices_a_; - boost::shared_ptr > > sync_input_surface_indices_e_; - - /** \brief Nodelet initialization routine. */ - virtual void onInit (); - - /** \brief NodeletLazy connection routine. */ - virtual void subscribe (); - virtual void unsubscribe (); - - /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. - * \param cloud the pointer to the input point cloud - * \param cloud_surface the pointer to the surface point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_surface_indices_callback (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &cloud_surface, - const PointIndicesConstPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - ////////////////////////////////////////////////////////////////////////////////////////// - ////////////////////////////////////////////////////////////////////////////////////////// - ////////////////////////////////////////////////////////////////////////////////////////// - class FeatureFromNormals : public Feature - { - public: - typedef sensor_msgs::PointCloud2 PointCloud2; - - typedef pcl::PointCloud PointCloudN; - typedef boost::shared_ptr PointCloudNPtr; - typedef boost::shared_ptr PointCloudNConstPtr; - - FeatureFromNormals () : normals_() {}; - - protected: - /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */ - PointCloudNConstPtr normals_; - - /** \brief Child initialization routine. Internal method. */ - virtual bool childInit (ros::NodeHandle &nh) = 0; - - /** \brief Publish an empty point cloud of the feature output type. */ - virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0; - - /** \brief Compute the feature and publish it. */ - virtual void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) = 0; - - private: - /** \brief The normals PointCloud subscriber filter. */ - message_filters::Subscriber sub_normals_filter_; - - /** \brief Synchronized input, normals, surface, and point indices.*/ - boost::shared_ptr > > sync_input_normals_surface_indices_a_; - boost::shared_ptr > > sync_input_normals_surface_indices_e_; - - /** \brief Internal method. */ - void computePublish (const PointCloudInConstPtr &, - const PointCloudInConstPtr &, - const IndicesPtr &) {} // This should never be called - - /** \brief Nodelet initialization routine. */ - virtual void onInit (); - - /** \brief NodeletLazy connection routine. */ - virtual void subscribe (); - virtual void unsubscribe (); - - /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. - * \param cloud the pointer to the input point cloud - * \param cloud_normals the pointer to the input point cloud normals - * \param cloud_surface the pointer to the surface point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &cloud_normals, - const PointCloudInConstPtr &cloud_surface, - const PointIndicesConstPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FEATURE_H_ diff --git a/pcl_ros/include/pcl_ros/features/fpfh.hpp b/pcl_ros/include/pcl_ros/features/fpfh.hpp index 8e3647a0..98d45ab6 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh.hpp +++ b/pcl_ros/include/pcl_ros/features/fpfh.hpp @@ -43,58 +43,57 @@ namespace pcl_ros { - /** \brief @b FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud - * dataset containing points and normals. - * - * @note If you use this code in any academic work, please cite: - * - *
    - *
  • R.B. Rusu, N. Blodow, M. Beetz. - * Fast Point Feature Histograms (FPFH) for 3D Registration. - * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), - * Kobe, Japan, May 12-17 2009. - *
    • - *
  • R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. - * Fast Geometric Point Labeling using Conditional Random Fields. - * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), - * St. Louis, MO, USA, October 11-15 2009. - *
    • - *
- * - * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at - * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). - * \author Radu Bogdan Rusu - */ - class FPFHEstimation : public FeatureFromNormals +/** \brief @b FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud + * dataset containing points and normals. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • R.B. Rusu, N. Blodow, M. Beetz. + * Fast Point Feature Histograms (FPFH) for 3D Registration. + * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), + * Kobe, Japan, May 12-17 2009. + *
    • + *
  • R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. + * Fast Geometric Point Labeling using Conditional Random Fields. + * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * St. Louis, MO, USA, October 11-15 2009. + *
    • + *
+ * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * \author Radu Bogdan Rusu + */ +class FPFHEstimation : public FeatureFromNormals +{ +private: + pcl::FPFHEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::FPFHEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_FPFH_H_ - - diff --git a/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp b/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp index 5710120a..c3c88cf0 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp @@ -43,54 +43,54 @@ namespace pcl_ros { - /** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud - * dataset containing points and normals, in parallel, using the OpenMP standard. - * - * @note If you use this code in any academic work, please cite: - * - *
    - *
  • R.B. Rusu, N. Blodow, M. Beetz. - * Fast Point Feature Histograms (FPFH) for 3D Registration. - * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), - * Kobe, Japan, May 12-17 2009. - *
    • - *
  • R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. - * Fast Geometric Point Labeling using Conditional Random Fields. - * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), - * St. Louis, MO, USA, October 11-15 2009. - *
    • - *
- * \author Radu Bogdan Rusu - */ - class FPFHEstimationOMP : public FeatureFromNormals +/** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud + * dataset containing points and normals, in parallel, using the OpenMP standard. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • R.B. Rusu, N. Blodow, M. Beetz. + * Fast Point Feature Histograms (FPFH) for 3D Registration. + * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), + * Kobe, Japan, May 12-17 2009. + *
    • + *
  • R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. + * Fast Geometric Point Labeling using Conditional Random Fields. + * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * St. Louis, MO, USA, October 11-15 2009. + *
    • + *
+ * \author Radu Bogdan Rusu + */ +class FPFHEstimationOMP : public FeatureFromNormals +{ +private: + pcl::FPFHEstimationOMP impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::FPFHEstimationOMP impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FPFH_OMP_H_ - diff --git a/pcl_ros/include/pcl_ros/features/moment_invariants.hpp b/pcl_ros/include/pcl_ros/features/moment_invariants.hpp index c5b255c6..1c48ec47 100644 --- a/pcl_ros/include/pcl_ros/features/moment_invariants.hpp +++ b/pcl_ros/include/pcl_ros/features/moment_invariants.hpp @@ -43,41 +43,40 @@ namespace pcl_ros { - /** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. - * - * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at - * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. - * \author Radu Bogdan Rusu - */ - class MomentInvariantsEstimation: public Feature +/** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. + * \author Radu Bogdan Rusu + */ +class MomentInvariantsEstimation : public Feature +{ +private: + pcl::MomentInvariantsEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::MomentInvariantsEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ - - diff --git a/pcl_ros/include/pcl_ros/features/normal_3d.hpp b/pcl_ros/include/pcl_ros/features/normal_3d.hpp index 82ff188a..8c6d5626 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d.hpp @@ -43,44 +43,44 @@ namespace pcl_ros { - /** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and - * curvatures. - * - * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at - * \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations. - * \author Radu Bogdan Rusu - */ - class NormalEstimation: public Feature +/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and + * curvatures. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations. + * \author Radu Bogdan Rusu + */ +class NormalEstimation : public Feature +{ +private: + /** \brief PCL implementation object. */ + pcl::NormalEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - /** \brief PCL implementation object. */ - pcl::NormalEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. + * \param cloud the input point cloud to copy the header from. + */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. - * \param cloud the input point cloud to copy the header from. - */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_NORMAL_3D_H_ - diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp b/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp index 624d780f..4b96a87c 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp @@ -43,37 +43,38 @@ namespace pcl_ros { - /** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and - * curvatures, in parallel, using the OpenMP standard. - * \author Radu Bogdan Rusu - */ - class NormalEstimationOMP: public Feature +/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and + * curvatures, in parallel, using the OpenMP standard. + * \author Radu Bogdan Rusu + */ +class NormalEstimationOMP : public Feature +{ +private: + pcl::NormalEstimationOMP impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::NormalEstimationOMP impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_ diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp index 15282141..fcc41029 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp @@ -46,41 +46,40 @@ namespace pcl_ros { - /** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and - * curvatures, in parallel, using Intel's Threading Building Blocks library. - * \author Radu Bogdan Rusu - */ - class NormalEstimationTBB: public Feature +/** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and + * curvatures, in parallel, using Intel's Threading Building Blocks library. + * \author Radu Bogdan Rusu + */ +class NormalEstimationTBB : public Feature +{ +private: + pcl::NormalEstimationTBB impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::NormalEstimationTBB impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } //#endif // HAVE_TBB #endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_ - - diff --git a/pcl_ros/include/pcl_ros/features/pfh.hpp b/pcl_ros/include/pcl_ros/features/pfh.hpp index 9ce4d101..ff6490dc 100644 --- a/pcl_ros/include/pcl_ros/features/pfh.hpp +++ b/pcl_ros/include/pcl_ros/features/pfh.hpp @@ -43,58 +43,57 @@ namespace pcl_ros { - /** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset - * containing points and normals. - * - * @note If you use this code in any academic work, please cite: - * - *
    - *
  • R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz. - * Aligning Point Cloud Views using Persistent Feature Histograms. - * In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), - * Nice, France, September 22-26 2008. - *
    • - *
  • R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz. - * Learning Informative Point Classes for the Acquisition of Object Model Maps. - * In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV), - * Hanoi, Vietnam, December 17-20 2008. - *
    • - *
- * - * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at - * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). - * \author Radu Bogdan Rusu - */ - class PFHEstimation : public FeatureFromNormals +/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset + * containing points and normals. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz. + * Aligning Point Cloud Views using Persistent Feature Histograms. + * In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * Nice, France, September 22-26 2008. + *
    • + *
  • R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz. + * Learning Informative Point Classes for the Acquisition of Object Model Maps. + * In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV), + * Hanoi, Vietnam, December 17-20 2008. + *
    • + *
+ * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * \author Radu Bogdan Rusu + */ +class PFHEstimation : public FeatureFromNormals +{ +private: + pcl::PFHEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::PFHEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_PFH_H_ - - diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp b/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp index 98cd4134..e150189d 100644 --- a/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp +++ b/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp @@ -44,41 +44,42 @@ namespace pcl_ros { - /** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of - * principal surface curvatures for a given point cloud dataset containing points and normals. - * - * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at - * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. - * \author Radu Bogdan Rusu, Jared Glover - */ - class PrincipalCurvaturesEstimation : public FeatureFromNormals +/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of + * principal surface curvatures for a given point cloud dataset containing points and normals. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. + * \author Radu Bogdan Rusu, Jared Glover + */ +class PrincipalCurvaturesEstimation : public FeatureFromNormals +{ +private: + pcl::PrincipalCurvaturesEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::PrincipalCurvaturesEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ diff --git a/pcl_ros/include/pcl_ros/features/shot.hpp b/pcl_ros/include/pcl_ros/features/shot.hpp index 214736be..a3cdb3e1 100644 --- a/pcl_ros/include/pcl_ros/features/shot.hpp +++ b/pcl_ros/include/pcl_ros/features/shot.hpp @@ -42,39 +42,38 @@ namespace pcl_ros { - /** \brief @b SHOTEstimation estimates SHOT descriptor. - * - */ - class SHOTEstimation : public FeatureFromNormals +/** \brief @b SHOTEstimation estimates SHOT descriptor. + * + */ +class SHOTEstimation : public FeatureFromNormals +{ +private: + pcl::SHOTEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::SHOTEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_SHOT_H_ - - diff --git a/pcl_ros/include/pcl_ros/features/shot_omp.hpp b/pcl_ros/include/pcl_ros/features/shot_omp.hpp index c02ca498..77d6a3f8 100644 --- a/pcl_ros/include/pcl_ros/features/shot_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/shot_omp.hpp @@ -42,37 +42,37 @@ namespace pcl_ros { - /** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP. - */ - class SHOTEstimationOMP : public FeatureFromNormals +/** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP. + */ +class SHOTEstimationOMP : public FeatureFromNormals +{ +private: + pcl::SHOTEstimationOMP impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::SHOTEstimationOMP impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_SHOT_OMP_H_ - diff --git a/pcl_ros/include/pcl_ros/features/vfh.hpp b/pcl_ros/include/pcl_ros/features/vfh.hpp index 526cdcdf..efe5d631 100644 --- a/pcl_ros/include/pcl_ros/features/vfh.hpp +++ b/pcl_ros/include/pcl_ros/features/vfh.hpp @@ -43,41 +43,42 @@ namespace pcl_ros { - /** \brief @b VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud - * dataset containing points and normals. - * - * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at - * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). - * \author Radu Bogdan Rusu - */ - class VFHEstimation : public FeatureFromNormals +/** \brief @b VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud + * dataset containing points and normals. + * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * \author Radu Bogdan Rusu + */ +class VFHEstimation : public FeatureFromNormals +{ +private: + pcl::VFHEstimation impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::VFHEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud PointCloudOut; + /** \brief Publish an empty point cloud of the feature output type. */ + void emptyPublish(const PointCloudInConstPtr & cloud); - /** \brief Child initialization routine. Internal method. */ - inline bool - childInit (ros::NodeHandle &nh) - { - // Create the output publisher - pub_output_ = advertise (nh, "output", max_queue_size_); - return (true); - } + /** \brief Compute the feature and publish it. */ + void computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices); - /** \brief Publish an empty point cloud of the feature output type. */ - void emptyPublish (const PointCloudInConstPtr &cloud); - - /** \brief Compute the feature and publish it. */ - void computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FEATURES_VFH_H_ diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/pcl_ros/include/pcl_ros/filters/crop_box.hpp index cb376caf..234a9a07 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.hpp +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -1,5 +1,5 @@ /* - * + * * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. @@ -32,7 +32,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * $Id: cropbox.cpp + * $Id: cropbox.cpp * */ @@ -48,57 +48,59 @@ namespace pcl_ros { - /** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box. - * - * \author Radu Bogdan Rusu - * \author Justin Rosen - * \author Marti Morta Garriga +/** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box. + * + * \author Radu Bogdan Rusu + * \author Justin Rosen + * \author Marti Morta Garriga + */ +class CropBox : public Filter +{ +protected: + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; // TODO + + /** \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 */ - class CropBox : public Filter + inline void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output) { - protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; // TODO + 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); + } - /** \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 - */ - 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); - } + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + bool + child_init(ros::NodeHandle & nh, bool & has_service); - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service - */ - bool - child_init (ros::NodeHandle &nh, bool &has_service); - - /** \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); + /** \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); - private: - /** \brief The PCL filter implementation used. */ - pcl::CropBox impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +private: + /** \brief The PCL filter implementation used. */ + pcl::CropBox impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_ diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp index a3b12678..1eeddb38 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -46,53 +46,54 @@ namespace pcl_ros { - /** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. - * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. - * \author Radu Bogdan Rusu +/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ +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 + * \param output the resultant filtered dataset */ - class ExtractIndices : public Filter + inline void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output) { - protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; + 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); + } - /** \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 - */ - 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); - } + /** \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); - /** \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); + /** \brief Dynamic reconfigure service callback. */ + void + config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level); - /** \brief Dynamic reconfigure service callback. */ - void - config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level); +private: + /** \brief The PCL filter implementation used. */ + pcl::ExtractIndices impl_; - private: - /** \brief The PCL filter implementation used. */ - pcl::ExtractIndices impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ - diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index cd622991..40955e25 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -48,110 +48,115 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - /** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters - * are defined here as static methods. - * \author Radu Bogdan Rusu +/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters + * are defined here as static methods. + * \author Radu Bogdan Rusu + */ +class Filter : public PCLNodelet +{ +public: + typedef sensor_msgs::PointCloud2 PointCloud2; + + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; + + Filter() {} + +protected: + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + message_filters::Subscriber sub_input_filter_; + + /** \brief The desired user filter field name. */ + std::string filter_field_name_; + + /** \brief The minimum allowed filter value a point will be considered from. */ + double filter_limit_min_; + + /** \brief The maximum allowed filter value a point will be considered from. */ + double filter_limit_max_; + + /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ + bool filter_limit_negative_; + + /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_input_frame_; + + /** \brief The original data input TF frame. */ + std::string tf_input_orig_frame_; + + /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + 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 */ - class Filter : public PCLNodelet + virtual bool + child_init(ros::NodeHandle & nh, bool & has_service) { - public: - typedef sensor_msgs::PointCloud2 PointCloud2; + has_service = false; + return true; + } - typedef pcl::IndicesPtr IndicesPtr; - typedef pcl::IndicesConstPtr IndicesConstPtr; + /** \brief Virtual abstract filter method. To be implemented by every child. + * \param input the input point cloud dataset. + * \param indices a pointer to the vector of point indices to use. + * \param output the resultant filtered PointCloud2 + */ + virtual void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output) = 0; - Filter () {} + /** \brief Lazy transport subscribe routine. */ + virtual void + subscribe(); - protected: - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + /** \brief Lazy transport unsubscribe routine. */ + virtual void + unsubscribe(); - message_filters::Subscriber sub_input_filter_; + /** \brief Nodelet initialization routine. */ + virtual void + onInit(); - /** \brief The desired user filter field name. */ - std::string filter_field_name_; + /** \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); - /** \brief The minimum allowed filter value a point will be considered from. */ - double filter_limit_min_; +private: + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; - /** \brief The maximum allowed filter value a point will be considered from. */ - double filter_limit_max_; + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr>> sync_input_indices_e_; + boost::shared_ptr>> sync_input_indices_a_; - /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ - bool filter_limit_negative_; + /** \brief Dynamic reconfigure service callback. */ + virtual void + config_callback(pcl_ros::FilterConfig & config, uint32_t level); - /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ - std::string tf_input_frame_; + /** \brief PointCloud2 + Indices data callback. */ + void + input_indices_callback( + const PointCloud2::ConstPtr & cloud, + const PointIndicesConstPtr & indices); - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - - /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ - 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); - } - - /** \brief Virtual abstract filter method. To be implemented by every child. - * \param input the input point cloud dataset. - * \param indices a pointer to the vector of point indices to use. - * \param output the resultant filtered PointCloud2 - */ - virtual void - filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, - PointCloud2 &output) = 0; - - /** \brief Lazy transport subscribe routine. */ - virtual void - subscribe(); - - /** \brief Lazy transport unsubscribe routine. */ - virtual void - unsubscribe(); - - /** \brief Nodelet initialization routine. */ - virtual void - onInit (); - - /** \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); - - private: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; - - /** \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 PointCloud2 + Indices data callback. */ - void - input_indices_callback (const PointCloud2::ConstPtr &cloud, - const PointIndicesConstPtr &indices); - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTER_H_ diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index 1be1ef97..515fb897 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.hpp +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -44,55 +44,57 @@ namespace pcl_ros { - /** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given - * constraints. - * \author Radu Bogdan Rusu +/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given + * constraints. + * \author Radu Bogdan Rusu + */ +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 + * \param output the resultant filtered dataset */ - class PassThrough : public Filter + inline void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output) { - protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; + 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); + } - /** \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 - */ - 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); - } + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + bool + child_init(ros::NodeHandle & nh, bool & has_service); - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service - */ - bool - child_init (ros::NodeHandle &nh, bool &has_service); - - /** \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); + /** \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); - private: - /** \brief The PCL filter implementation used. */ - pcl::PassThrough impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +private: + /** \brief The PCL filter implementation used. */ + pcl::PassThrough impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp index f72a8c2d..d17d2466 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -46,69 +46,75 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - /** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a - * separate PointCloud. - * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. - * \author Radu Bogdan Rusu +/** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a + * separate PointCloud. + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ +class ProjectInliers : public Filter +{ +public: + ProjectInliers() + : model_() {} + +protected: + /** \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 */ - class ProjectInliers : public Filter + inline void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output) { - public: - ProjectInliers () : model_ () {} + 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); + } - protected: - /** \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 - */ - 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); - } +private: + /** \brief A pointer to the vector of model coefficients. */ + ModelCoefficientsConstPtr model_; - private: - /** \brief A pointer to the vector of model coefficients. */ - ModelCoefficientsConstPtr model_; + /** \brief The message filter subscriber for model coefficients. */ + message_filters::Subscriber sub_model_; - /** \brief The message filter subscriber for model coefficients. */ - 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 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 Nodelet initialization routine. */ - virtual void - onInit (); + /** \brief NodeletLazy connection routine. */ + void subscribe(); + void unsubscribe(); - /** \brief NodeletLazy connection routine. */ - void subscribe (); - void unsubscribe (); + /** \brief PointCloud2 + Indices + Model data callback. */ + void + input_indices_model_callback( + const PointCloud2::ConstPtr & cloud, + const PointIndicesConstPtr & indices, + const ModelCoefficientsConstPtr & model); - /** \brief PointCloud2 + Indices + Model data callback. */ - void - input_indices_model_callback (const PointCloud2::ConstPtr &cloud, - const PointIndicesConstPtr &indices, - const ModelCoefficientsConstPtr &model); - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_ 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 3b1120ad..c773ccc4 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -47,54 +47,55 @@ namespace pcl_ros { - /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain - * search radius is smaller than a given K. - * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. - * \author Radu Bogdan Rusu +/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain + * search radius is smaller than a given K. + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ +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 + * \param output the resultant filtered dataset */ - class RadiusOutlierRemoval : public Filter + inline void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output) { - protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; + 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); + } - /** \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 - */ - 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); - } + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + virtual inline bool child_init(ros::NodeHandle & nh, bool & has_service); - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service - */ - virtual inline bool child_init (ros::NodeHandle &nh, bool &has_service); + /** \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); - /** \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); +private: + /** \brief The PCL filter implementation used. */ + pcl::RadiusOutlierRemoval impl_; - - private: - /** \brief The PCL filter implementation used. */ - pcl::RadiusOutlierRemoval impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_ 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 6776cc19..2ed4d167 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -47,60 +47,62 @@ namespace pcl_ros { - /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more - * information check: - *
    - *
  • R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. - * Towards 3D Point Cloud Based Object Maps for Household Environments - * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. - *
- * - * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. - * \author Radu Bogdan Rusu +/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more + * information check: + *
    + *
  • R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. + * Towards 3D Point Cloud Based Object Maps for Household Environments + * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. + *
+ * + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ +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 + * \param output the resultant filtered dataset */ - class StatisticalOutlierRemoval : public Filter + inline void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output) { - protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; + 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); + } - /** \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 - */ - 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); - } + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + bool child_init(ros::NodeHandle & nh, bool & has_service); - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service - */ - bool child_init (ros::NodeHandle &nh, bool &has_service); + /** \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); - /** \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); +private: + /** \brief The PCL filter implementation used. */ + pcl::StatisticalOutlierRemoval impl_; - private: - /** \brief The PCL filter implementation used. */ - pcl::StatisticalOutlierRemoval impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_ diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index 6f900f23..398be306 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -47,43 +47,45 @@ namespace pcl_ros { - /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. - * \author Radu Bogdan Rusu +/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. + * \author Radu Bogdan Rusu + */ +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 */ - class VoxelGrid : public Filter - { - protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; + virtual void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output); - /** \brief The PCL filter implementation used. */ - pcl::VoxelGrid impl_; + /** \brief Child initialization routine. + * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service + */ + bool + child_init(ros::NodeHandle & nh, bool & has_service); - /** \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 - filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, - PointCloud2 &output); + /** \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); - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service - */ - bool - child_init (ros::NodeHandle &nh, bool &has_service); - - /** \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); - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_ diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index a4ab2121..794a8d93 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -46,178 +46,176 @@ using pcl_conversions::toPCL; namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////// -template void -transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, const tf::Transform &transform) +template +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, const tf::Transform & transform) { // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. - tf::Quaternion q = transform.getRotation (); - Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w) - tf::Vector3 v = transform.getOrigin (); - Eigen::Vector3f origin (v.x (), v.y (), v.z ()); + tf::Quaternion q = transform.getRotation(); + Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) + tf::Vector3 v = transform.getOrigin(); + Eigen::Vector3f origin(v.x(), v.y(), v.z()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform //Eigen::Transform3f t; //t = translation * rotation; - transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation); + transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -transformPointCloud (const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, const tf::Transform &transform) +template +void +transformPointCloud( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, const tf::Transform & transform) { // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. - tf::Quaternion q = transform.getRotation (); - Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w) - tf::Vector3 v = transform.getOrigin (); - Eigen::Vector3f origin (v.x (), v.y (), v.z ()); + tf::Quaternion q = transform.getRotation(); + Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) + tf::Vector3 v = transform.getOrigin(); + Eigen::Vector3f origin(v.x(), v.y(), v.z()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform //Eigen::Transform3f t; //t = translation * rotation; - transformPointCloud (cloud_in, cloud_out, origin, rotation); + transformPointCloud(cloud_in, cloud_out, origin, rotation); } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -transformPointCloudWithNormals (const std::string &target_frame, - const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener) +template +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const tf::TransformListener & tf_listener) { - if (cloud_in.header.frame_id == target_frame) - { + if (cloud_in.header.frame_id == target_frame) { cloud_out = cloud_in; - return (true); + return true; } tf::StampedTransform transform; - try - { - tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform); - } - catch (tf::LookupException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); - } - catch (tf::ExtrapolationException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); + try { + tf_listener.lookupTransform( + target_frame, cloud_in.header.frame_id, fromPCL( + cloud_in.header).stamp, transform); + } catch (tf::LookupException & e) { + ROS_ERROR("%s", e.what()); + return false; + } catch (tf::ExtrapolationException & e) { + ROS_ERROR("%s", e.what()); + return false; } - transformPointCloudWithNormals (cloud_in, cloud_out, transform); + transformPointCloudWithNormals(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -transformPointCloud (const std::string &target_frame, - const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener) +template +bool +transformPointCloud( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const tf::TransformListener & tf_listener) { - if (cloud_in.header.frame_id == target_frame) - { + if (cloud_in.header.frame_id == target_frame) { cloud_out = cloud_in; - return (true); + return true; } tf::StampedTransform transform; - try - { - tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform); + try { + tf_listener.lookupTransform( + target_frame, cloud_in.header.frame_id, fromPCL( + cloud_in.header).stamp, transform); + } catch (tf::LookupException & e) { + ROS_ERROR("%s", e.what()); + return false; + } catch (tf::ExtrapolationException & e) { + ROS_ERROR("%s", e.what()); + return false; } - catch (tf::LookupException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); - } - catch (tf::ExtrapolationException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); - } - transformPointCloud (cloud_in, cloud_out, transform); + transformPointCloud(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -transformPointCloud (const std::string &target_frame, - const ros::Time & target_time, - const pcl::PointCloud &cloud_in, - const std::string &fixed_frame, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener) +template +bool +transformPointCloud( + const std::string & target_frame, + const ros::Time & target_time, + const pcl::PointCloud & cloud_in, + const std::string & fixed_frame, + pcl::PointCloud & cloud_out, + const tf::TransformListener & tf_listener) { tf::StampedTransform transform; - try - { - tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform); - } - catch (tf::LookupException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); - } - catch (tf::ExtrapolationException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); + try { + tf_listener.lookupTransform( + target_frame, target_time, cloud_in.header.frame_id, + fromPCL(cloud_in.header).stamp, fixed_frame, transform); + } catch (tf::LookupException & e) { + ROS_ERROR("%s", e.what()); + return false; + } catch (tf::ExtrapolationException & e) { + ROS_ERROR("%s", e.what()); + return false; } - transformPointCloud (cloud_in, cloud_out, transform); + transformPointCloud(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; std_msgs::Header header; header.stamp = target_time; cloud_out.header = toPCL(header); - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -transformPointCloudWithNormals (const std::string &target_frame, - const ros::Time & target_time, - const pcl::PointCloud &cloud_in, - const std::string &fixed_frame, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener) +template +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const ros::Time & target_time, + const pcl::PointCloud & cloud_in, + const std::string & fixed_frame, + pcl::PointCloud & cloud_out, + const tf::TransformListener & tf_listener) { tf::StampedTransform transform; - try - { - tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform); - } - catch (tf::LookupException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); - } - catch (tf::ExtrapolationException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); + try { + tf_listener.lookupTransform( + target_frame, target_time, cloud_in.header.frame_id, + fromPCL(cloud_in.header).stamp, fixed_frame, transform); + } catch (tf::LookupException & e) { + ROS_ERROR("%s", e.what()); + return false; + } catch (tf::ExtrapolationException & e) { + ROS_ERROR("%s", e.what()); + return false; } - transformPointCloudWithNormals (cloud_in, cloud_out, transform); + transformPointCloudWithNormals(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; std_msgs::Header header; header.stamp = target_time; cloud_out.header = toPCL(header); - return (true); + return true; } } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/io/bag_io.hpp b/pcl_ros/include/pcl_ros/io/bag_io.hpp index f526f2ea..17a936da 100644 --- a/pcl_ros/include/pcl_ros/io/bag_io.hpp +++ b/pcl_ros/include/pcl_ros/io/bag_io.hpp @@ -45,85 +45,86 @@ namespace pcl_ros { - //////////////////////////////////////////////////////////////////////////////////////////// - /** \brief BAG PointCloud file format reader. - * \author Radu Bogdan Rusu +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief BAG PointCloud file format reader. + * \author Radu Bogdan Rusu + */ +class BAGReader : public nodelet::Nodelet +{ +public: + typedef sensor_msgs::PointCloud2 PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + /** \brief Empty constructor. */ + BAGReader() + : publish_rate_(0), output_() /*, cloud_received_ (false)*/ {} + + /** \brief Set the publishing rate in seconds. + * \param publish_rate the publishing rate in seconds */ - class BAGReader: public nodelet::Nodelet + inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;} + + /** \brief Get the publishing rate in seconds. */ + inline double getPublishRate() {return publish_rate_;} + + /** \brief Get the next point cloud dataset in the BAG file. + * \return the next point cloud dataset as read from the file + */ + inline PointCloudConstPtr + getNextCloud() { - public: - typedef sensor_msgs::PointCloud2 PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; + if (it_ != view_.end()) { + output_ = it_->instantiate(); + ++it_; + } + return output_; + } - /** \brief Empty constructor. */ - BAGReader () : publish_rate_ (0), output_ ()/*, cloud_received_ (false)*/ {}; + /** \brief Open a BAG file for reading and select a specified topic + * \param file_name the BAG file to open + * \param topic_name the topic that we want to read data from + */ + bool open(const std::string & file_name, const std::string & topic_name); - /** \brief Set the publishing rate in seconds. - * \param publish_rate the publishing rate in seconds - */ - inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; } + /** \brief Close an open BAG file. */ + inline void + close() + { + bag_.close(); + } - /** \brief Get the publishing rate in seconds. */ - inline double getPublishRate () { return (publish_rate_); } + /** \brief Nodelet initialization routine. */ + virtual void onInit(); - /** \brief Get the next point cloud dataset in the BAG file. - * \return the next point cloud dataset as read from the file - */ - inline PointCloudConstPtr - getNextCloud () - { - if (it_ != view_.end ()) - { - output_ = it_->instantiate (); - ++it_; - } - return (output_); - } +private: + /** \brief The publishing interval in seconds. Set to 0 to publish once (default). */ + double publish_rate_; - /** \brief Open a BAG file for reading and select a specified topic - * \param file_name the BAG file to open - * \param topic_name the topic that we want to read data from - */ - bool open (const std::string &file_name, const std::string &topic_name); + /** \brief The BAG object. */ + rosbag::Bag bag_; - /** \brief Close an open BAG file. */ - inline void - close () - { - bag_.close (); - } + /** \brief The BAG view object. */ + rosbag::View view_; - /** \brief Nodelet initialization routine. */ - virtual void onInit (); + /** \brief The BAG view iterator object. */ + rosbag::View::iterator it_; - private: - /** \brief The publishing interval in seconds. Set to 0 to publish once (default). */ - double publish_rate_; + /** \brief The name of the topic that contains the PointCloud data. */ + std::string topic_name_; - /** \brief The BAG object. */ - rosbag::Bag bag_; + /** \brief The name of the BAG file that contains the PointCloud data. */ + std::string file_name_; - /** \brief The BAG view object. */ - rosbag::View view_; + /** \brief The output point cloud dataset containing the points loaded from the file. */ + PointCloudPtr output_; - /** \brief The BAG view iterator object. */ - rosbag::View::iterator it_; + /** \brief Signals that a new PointCloud2 message has been read from the file. */ + //bool cloud_received_; - /** \brief The name of the topic that contains the PointCloud data. */ - std::string topic_name_; - - /** \brief The name of the BAG file that contains the PointCloud data. */ - std::string file_name_; - - /** \brief The output point cloud dataset containing the points loaded from the file. */ - PointCloudPtr output_; - - /** \brief Signals that a new PointCloud2 message has been read from the file. */ - //bool cloud_received_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.hpp b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp index 89603d2a..00f54fdd 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_data.hpp +++ b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp @@ -49,88 +49,94 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data - * synchronizer: it listens for a set of input PointCloud messages on the same topic, - * checks their timestamps, and concatenates their fields together into a single - * PointCloud output message. - * \author Radu Bogdan Rusu +/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data + * synchronizer: it listens for a set of input PointCloud messages on the same topic, + * checks their timestamps, and concatenates their fields together into a single + * PointCloud output message. + * \author Radu Bogdan Rusu + */ +class PointCloudConcatenateDataSynchronizer : public nodelet_topic_tools::NodeletLazy +{ +public: + typedef sensor_msgs::PointCloud2 PointCloud2; + typedef PointCloud2::Ptr PointCloud2Ptr; + typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + + /** \brief Empty constructor. */ + PointCloudConcatenateDataSynchronizer() + : maximum_queue_size_(3) {} + + /** \brief Empty constructor. + * \param queue_size the maximum queue size */ - class PointCloudConcatenateDataSynchronizer: public nodelet_topic_tools::NodeletLazy + PointCloudConcatenateDataSynchronizer(int queue_size) + : maximum_queue_size_(queue_size), approximate_sync_(false) {} + + /** \brief Empty destructor. */ + virtual ~PointCloudConcatenateDataSynchronizer() {} + + void onInit(); + void subscribe(); + void unsubscribe(); + +private: + /** \brief The output PointCloud publisher. */ + ros::Publisher pub_output_; + + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_; + + /** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ + bool approximate_sync_; + + /** \brief A vector of message filters. */ + std::vector>> filters_; + + /** \brief Output TF frame the concatenated points should be transformed to. */ + std::string output_frame_; + + /** \brief Input point cloud topics. */ + XmlRpc::XmlRpcValue input_topics_; + + /** \brief TF listener object. */ + tf::TransformListener tf_; + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_; + + /** \brief Synchronizer. + * \note This will most likely be rewritten soon using the DynamicTimeSynchronizer. + */ + boost::shared_ptr>> ts_a_; + boost::shared_ptr>> ts_e_; + + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. + */ + inline void + input_callback(const PointCloud2ConstPtr & input) { - public: - typedef sensor_msgs::PointCloud2 PointCloud2; - typedef PointCloud2::Ptr PointCloud2Ptr; - typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + PointCloud2 cloud; + cloud.header.stamp = input->header.stamp; + nf_.add(boost::make_shared(cloud)); + } - /** \brief Empty constructor. */ - PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {}; + /** \brief Input callback for 8 synchronized topics. */ + void input( + const PointCloud2::ConstPtr & in1, const PointCloud2::ConstPtr & in2, + const PointCloud2::ConstPtr & in3, const PointCloud2::ConstPtr & in4, + const PointCloud2::ConstPtr & in5, const PointCloud2::ConstPtr & in6, + const PointCloud2::ConstPtr & in7, const PointCloud2::ConstPtr & in8); - /** \brief Empty constructor. - * \param queue_size the maximum queue size - */ - PointCloudConcatenateDataSynchronizer (int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {}; - - /** \brief Empty destructor. */ - virtual ~PointCloudConcatenateDataSynchronizer () {}; - - void onInit (); - void subscribe (); - void unsubscribe (); - - private: - /** \brief The output PointCloud publisher. */ - ros::Publisher pub_output_; - - /** \brief The maximum number of messages that we can store in the queue. */ - int maximum_queue_size_; - - /** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ - bool approximate_sync_; - - /** \brief A vector of message filters. */ - std::vector > > filters_; - - /** \brief Output TF frame the concatenated points should be transformed to. */ - std::string output_frame_; - - /** \brief Input point cloud topics. */ - XmlRpc::XmlRpcValue input_topics_; - - /** \brief TF listener object. */ - tf::TransformListener tf_; - - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_; - - /** \brief Synchronizer. - * \note This will most likely be rewritten soon using the DynamicTimeSynchronizer. - */ - boost::shared_ptr > > ts_a_; - boost::shared_ptr > > ts_e_; - - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback (const PointCloud2ConstPtr &input) - { - PointCloud2 cloud; - cloud.header.stamp = input->header.stamp; - nf_.add (boost::make_shared (cloud)); - } - - /** \brief Input callback for 8 synchronized topics. */ - void input (const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, - const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, - const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, - const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8); - - void combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out); - }; + void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out); +}; } #endif //#ifndef PCL_ROS_IO_CONCATENATE_H_ diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp index efa1c458..2b81af4c 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp @@ -49,53 +49,55 @@ namespace pcl_ros { - /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of - * input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into - * a single PointCloud output message. - * \author Radu Bogdan Rusu +/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of + * input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into + * a single PointCloud output message. + * \author Radu Bogdan Rusu + */ +class PointCloudConcatenateFieldsSynchronizer : public nodelet_topic_tools::NodeletLazy +{ +public: + typedef sensor_msgs::PointCloud2 PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + /** \brief Empty constructor. */ + PointCloudConcatenateFieldsSynchronizer() + : maximum_queue_size_(3), maximum_seconds_(0) {} + + /** \brief Empty constructor. + * \param queue_size the maximum queue size */ - class PointCloudConcatenateFieldsSynchronizer: public nodelet_topic_tools::NodeletLazy - { - public: - typedef sensor_msgs::PointCloud2 PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; + PointCloudConcatenateFieldsSynchronizer(int queue_size) + : maximum_queue_size_(queue_size), maximum_seconds_(0) {} - /** \brief Empty constructor. */ - PointCloudConcatenateFieldsSynchronizer () : maximum_queue_size_ (3), maximum_seconds_ (0) {}; + /** \brief Empty destructor. */ + virtual ~PointCloudConcatenateFieldsSynchronizer() {} - /** \brief Empty constructor. - * \param queue_size the maximum queue size - */ - PointCloudConcatenateFieldsSynchronizer (int queue_size) : maximum_queue_size_ (queue_size), maximum_seconds_ (0) {}; + void onInit(); + void subscribe(); + void unsubscribe(); + void input_callback(const PointCloudConstPtr & cloud); - /** \brief Empty destructor. */ - virtual ~PointCloudConcatenateFieldsSynchronizer () {}; +private: + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; - void onInit (); - void subscribe (); - void unsubscribe (); - void input_callback (const PointCloudConstPtr &cloud); + /** \brief The output PointCloud publisher. */ + ros::Publisher pub_output_; - private: - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + /** \brief The number of input messages that we expect on the input topic. */ + int input_messages_; - /** \brief The output PointCloud publisher. */ - ros::Publisher pub_output_; + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_; - /** \brief The number of input messages that we expect on the input topic. */ - int input_messages_; + /** \brief The maximum number of seconds to wait until we drop the synchronization. */ + double maximum_seconds_; - /** \brief The maximum number of messages that we can store in the queue. */ - int maximum_queue_size_; - - /** \brief The maximum number of seconds to wait until we drop the synchronization. */ - double maximum_seconds_; - - /** \brief A queue for messages. */ - std::map > queue_; - }; + /** \brief A queue for messages. */ + std::map> queue_; +}; } #endif //#ifndef PCL_IO_CONCATENATE_H_ diff --git a/pcl_ros/include/pcl_ros/io/pcd_io.hpp b/pcl_ros/include/pcl_ros/io/pcd_io.hpp index f551817b..afcde06e 100644 --- a/pcl_ros/include/pcl_ros/io/pcd_io.hpp +++ b/pcl_ros/include/pcl_ros/io/pcd_io.hpp @@ -43,90 +43,94 @@ namespace pcl_ros { - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief Point Cloud Data (PCD) file format reader. - * \author Radu Bogdan Rusu +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Point Cloud Data (PCD) file format reader. + * \author Radu Bogdan Rusu + */ +class PCDReader : public PCLNodelet +{ +public: + typedef sensor_msgs::PointCloud2 PointCloud2; + typedef PointCloud2::Ptr PointCloud2Ptr; + typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + + /** \brief Empty constructor. */ + PCDReader() + : publish_rate_(0), tf_frame_("/base_link") {} + + virtual void onInit(); + + /** \brief Set the publishing rate in seconds. + * \param publish_rate the publishing rate in seconds */ - class PCDReader : public PCLNodelet - { - public: - typedef sensor_msgs::PointCloud2 PointCloud2; - typedef PointCloud2::Ptr PointCloud2Ptr; - typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;} - /** \brief Empty constructor. */ - PCDReader () : publish_rate_ (0), tf_frame_ ("/base_link") {}; + /** \brief Get the publishing rate in seconds. */ + inline double getPublishRate() {return publish_rate_;} - virtual void onInit (); - - /** \brief Set the publishing rate in seconds. - * \param publish_rate the publishing rate in seconds - */ - inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; } - - /** \brief Get the publishing rate in seconds. */ - inline double getPublishRate () { return (publish_rate_); } - - /** \brief Set the TF frame the PointCloud will be published in. - * \param tf_frame the TF frame the PointCloud will be published in - */ - inline void setTFframe (std::string tf_frame) { tf_frame_ = tf_frame; } - - /** \brief Get the TF frame the PointCloud is published in. */ - inline std::string getTFframe () { return (tf_frame_); } - - protected: - /** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */ - double publish_rate_; - - /** \brief The TF frame the data should be published in ("/base_link" by default). */ - std::string tf_frame_; - - /** \brief The name of the file that contains the PointCloud data. */ - std::string file_name_; - - /** \brief The output point cloud dataset containing the points loaded from the file. */ - PointCloud2Ptr output_; - - private: - /** \brief The PCL implementation used. */ - pcl::PCDReader impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief Point Cloud Data (PCD) file format writer. - * \author Radu Bogdan Rusu + /** \brief Set the TF frame the PointCloud will be published in. + * \param tf_frame the TF frame the PointCloud will be published in */ - class PCDWriter : public PCLNodelet - { - public: - PCDWriter () : file_name_ (""), binary_mode_ (true) {} + inline void setTFframe(std::string tf_frame) {tf_frame_ = tf_frame;} - typedef sensor_msgs::PointCloud2 PointCloud2; - typedef PointCloud2::Ptr PointCloud2Ptr; - typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + /** \brief Get the TF frame the PointCloud is published in. */ + inline std::string getTFframe() {return tf_frame_;} - virtual void onInit (); - void input_callback (const PointCloud2ConstPtr &cloud); +protected: + /** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */ + double publish_rate_; - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + /** \brief The TF frame the data should be published in ("/base_link" by default). */ + std::string tf_frame_; - protected: - /** \brief The name of the file that contains the PointCloud data. */ - std::string file_name_; + /** \brief The name of the file that contains the PointCloud data. */ + std::string file_name_; - /** \brief Set to true if the output files should be saved in binary mode (true). */ - bool binary_mode_; + /** \brief The output point cloud dataset containing the points loaded from the file. */ + PointCloud2Ptr output_; - private: - /** \brief The PCL implementation used. */ - pcl::PCDWriter impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +private: + /** \brief The PCL implementation used. */ + pcl::PCDReader impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Point Cloud Data (PCD) file format writer. + * \author Radu Bogdan Rusu + */ +class PCDWriter : public PCLNodelet +{ +public: + PCDWriter() + : file_name_(""), binary_mode_(true) {} + + typedef sensor_msgs::PointCloud2 PointCloud2; + typedef PointCloud2::Ptr PointCloud2Ptr; + typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + + virtual void onInit(); + void input_callback(const PointCloud2ConstPtr & cloud); + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + +protected: + /** \brief The name of the file that contains the PointCloud data. */ + std::string file_name_; + + /** \brief Set to true if the output files should be saved in binary mode (true). */ + bool binary_mode_; + +private: + /** \brief The PCL implementation used. */ + pcl::PCDWriter impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_IO_PCD_IO_H_ diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp index fc4b64c8..28d8304a 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp @@ -66,168 +66,177 @@ 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 +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +/** \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") { - public: - typedef sensor_msgs::PointCloud2 PointCloud2; + 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()); - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + return false; + } + return true; + } - typedef pcl_msgs::PointIndices PointIndices; - typedef PointIndices::Ptr PointIndicesPtr; - typedef PointIndices::ConstPtr PointIndicesConstPtr; + /** \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()); - typedef pcl_msgs::ModelCoefficients ModelCoefficients; - typedef ModelCoefficients::Ptr ModelCoefficientsPtr; - typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; + return false; + } + return true; + } - typedef pcl::IndicesPtr IndicesPtr; - typedef pcl::IndicesConstPtr IndicesConstPtr; + /** \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 Empty constructor. */ - PCLNodelet () : use_indices_ (false), latched_indices_ (false), - max_queue_size_ (3), approximate_sync_ (false) {}; + /** \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; + } - 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 Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */ + virtual void subscribe() {} + virtual void unsubscribe() {} - /** \brief The message filter subscriber for PointCloud2. */ - message_filters::Subscriber sub_input_filter_; + /** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */ + virtual void + onInit() + { + nodelet_topic_tools::NodeletLazy::onInit(); - /** \brief The message filter subscriber for PointIndices. */ - message_filters::Subscriber sub_indices_filter_; + // Parameters that we care about only at startup + pnh_->getParam("max_queue_size", max_queue_size_); - /** \brief The output PointCloud publisher. */ - ros::Publisher pub_output_; + // ---[ Optional parameters + pnh_->getParam("use_indices", use_indices_); + pnh_->getParam("latched_indices", latched_indices_); + pnh_->getParam("approximate_sync", approximate_sync_); - /** \brief The maximum queue size (default: 3). */ - int max_queue_size_; + 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_); + } - /** \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 - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_NODELET_H_ diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index 32837d5c..b64b50de 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -11,286 +11,295 @@ #include #include -namespace pcl +namespace pcl { - namespace detail +namespace detail +{ +template +struct FieldStreamer +{ + FieldStreamer(Stream & stream) + : stream_(stream) {} + + template + void operator()() { - template - struct FieldStreamer - { - FieldStreamer(Stream& stream) : stream_(stream) {} + const char * name = traits::name::value; + std::uint32_t name_length = strlen(name); + stream_.next(name_length); + if (name_length > 0) { + memcpy(stream_.advance(name_length), name, name_length); + } - template void operator() () - { - const char* name = traits::name::value; - std::uint32_t name_length = strlen(name); - stream_.next(name_length); - if (name_length > 0) - memcpy(stream_.advance(name_length), name, name_length); + std::uint32_t offset = traits::offset::value; + stream_.next(offset); - std::uint32_t offset = traits::offset::value; - stream_.next(offset); + std::uint8_t datatype = traits::datatype::value; + stream_.next(datatype); - std::uint8_t datatype = traits::datatype::value; - stream_.next(datatype); + std::uint32_t count = traits::datatype::size; + stream_.next(count); + } - std::uint32_t count = traits::datatype::size; - stream_.next(count); - } + Stream & stream_; +}; - Stream& stream_; - }; +template +struct FieldsLength +{ + FieldsLength() + : length(0) {} - template - struct FieldsLength - { - FieldsLength() : length(0) {} + template + void operator()() + { + std::uint32_t name_length = strlen(traits::name::value); + length += name_length + 13; + } - template void operator() () - { - std::uint32_t name_length = strlen(traits::name::value); - length += name_length + 13; - } - - std::uint32_t length; - }; - } // namespace pcl::detail + std::uint32_t length; +}; +} // namespace pcl::detail } // namespace pcl -namespace ros +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. +// 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 ROS_VERSION_MINIMUM(1, 3, 1) - template - struct DefaultMessageCreator > - { - boost::shared_ptr mapping_; +template +struct DefaultMessageCreator> +{ + boost::shared_ptr mapping_; - DefaultMessageCreator() - : mapping_( boost::make_shared() ) - { - } - - boost::shared_ptr > operator() () - { - boost::shared_ptr > msg (new pcl::PointCloud ()); - pcl::detail::getMapping(*msg) = mapping_; - return msg; - } - }; + DefaultMessageCreator() + : mapping_(boost::make_shared() ) + { + } + + boost::shared_ptr> operator()() + { + boost::shared_ptr> msg(new pcl::PointCloud()); + pcl::detail::getMapping(*msg) = mapping_; + return msg; + } +}; #endif - namespace message_traits +namespace message_traits +{ +template +struct MD5Sum> +{ + static const char * value() {return MD5Sum::value();} + static const char * value(const pcl::PointCloud &) {return value();} + + static const uint64_t static_value1 = MD5Sum::static_value1; + static const uint64_t static_value2 = MD5Sum::static_value2; + + // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here. + ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL); + ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL); +}; + +template +struct DataType> +{ + static const char * value() {return DataType::value();} + static const char * value(const pcl::PointCloud &) {return value();} +}; + +template +struct Definition> +{ + static const char * value() {return Definition::value();} + static const char * value(const pcl::PointCloud &) {return value();} +}; + +// pcl point clouds message don't have a ROS compatible header +// the specialized meta functions below (TimeStamp and FrameId) +// can be used to get the header data. +template +struct HasHeader>: FalseType {}; + +template +struct TimeStamp> +{ + // This specialization could be dangerous, but it's the best I can do. + // If this TimeStamp struct is destroyed before they are done with the + // pointer returned by the first functions may go out of scope, but there + // isn't a lot I can do about that. This is a good reason to refuse to + // returning pointers like this... + static ros::Time * pointer(typename pcl::PointCloud & m) { - template struct MD5Sum > - { - static const char* value() { return MD5Sum::value(); } - static const char* value(const pcl::PointCloud&) { return value(); } - - static const uint64_t static_value1 = MD5Sum::static_value1; - static const uint64_t static_value2 = MD5Sum::static_value2; - - // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here. - ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL); - ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL); - }; - - template struct DataType > - { - static const char* value() { return DataType::value(); } - static const char* value(const pcl::PointCloud&) { return value(); } - }; - - template struct Definition > - { - static const char* value() { return Definition::value(); } - static const char* value(const pcl::PointCloud&) { return value(); } - }; - - // pcl point clouds message don't have a ROS compatible header - // the specialized meta functions below (TimeStamp and FrameId) - // can be used to get the header data. - template struct HasHeader > : FalseType {}; - - template - struct TimeStamp > - { - // This specialization could be dangerous, but it's the best I can do. - // If this TimeStamp struct is destroyed before they are done with the - // pointer returned by the first functions may go out of scope, but there - // isn't a lot I can do about that. This is a good reason to refuse to - // returning pointers like this... - static ros::Time* pointer(typename pcl::PointCloud &m) { - header_.reset(new std_msgs::Header()); - pcl_conversions::fromPCL(m.header, *(header_)); - return &(header_->stamp); - } - static ros::Time const* pointer(const typename pcl::PointCloud& m) { - header_const_.reset(new std_msgs::Header()); - pcl_conversions::fromPCL(m.header, *(header_const_)); - return &(header_const_->stamp); - } - static ros::Time value(const typename pcl::PointCloud& m) { - return pcl_conversions::fromPCL(m.header).stamp; - } - private: - static boost::shared_ptr header_; - static boost::shared_ptr header_const_; - }; - - template - struct FrameId > - { - static std::string* pointer(pcl::PointCloud& m) { return &m.header.frame_id; } - static std::string const* pointer(const pcl::PointCloud& m) { return &m.header.frame_id; } - static std::string value(const pcl::PointCloud& m) { return m.header.frame_id; } - }; - - } // namespace ros::message_traits - - namespace serialization + header_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_)); + return &(header_->stamp); + } + static ros::Time const * pointer(const typename pcl::PointCloud & m) { - template - struct Serializer > + header_const_.reset(new std_msgs::Header()); + pcl_conversions::fromPCL(m.header, *(header_const_)); + return &(header_const_->stamp); + } + static ros::Time value(const typename pcl::PointCloud & m) + { + return pcl_conversions::fromPCL(m.header).stamp; + } + +private: + static boost::shared_ptr header_; + static boost::shared_ptr header_const_; +}; + +template +struct FrameId> +{ + static std::string * pointer(pcl::PointCloud & m) {return &m.header.frame_id;} + static std::string const * pointer(const pcl::PointCloud & m) {return &m.header.frame_id;} + static std::string value(const pcl::PointCloud & m) {return m.header.frame_id;} +}; + +} // namespace ros::message_traits + +namespace serialization +{ +template +struct Serializer> +{ + template + inline static void write(Stream & stream, const pcl::PointCloud & m) + { + stream.next(m.header); + + // Ease the user's burden on specifying width/height for unorganized datasets + uint32_t height = m.height, width = m.width; + if (height == 0 && width == 0) { + width = m.points.size(); + height = 1; + } + stream.next(height); + stream.next(width); + + // Stream out point field metadata + typedef typename pcl::traits::fieldList::type FieldList; + uint32_t fields_size = boost::mpl::size::value; + stream.next(fields_size); + pcl::for_each_type(pcl::detail::FieldStreamer(stream)); + + // Assume little-endian... + uint8_t is_bigendian = false; + stream.next(is_bigendian); + + // Write out point data as binary blob + uint32_t point_step = sizeof(T); + stream.next(point_step); + uint32_t row_step = point_step * width; + stream.next(row_step); + uint32_t data_size = row_step * height; + stream.next(data_size); + memcpy(stream.advance(data_size), &m.points[0], data_size); + + uint8_t is_dense = m.is_dense; + stream.next(is_dense); + } + + template + inline static void read(Stream & stream, pcl::PointCloud & m) + { + std_msgs::Header header; + stream.next(header); + pcl_conversions::toPCL(header, m.header); + stream.next(m.height); + stream.next(m.width); + + /// @todo Check that fields haven't changed! + std::vector fields; + stream.next(fields); + + // Construct field mapping if deserializing for the first time + boost::shared_ptr & mapping_ptr = pcl::detail::getMapping(m); + if (!mapping_ptr) { + // This normally should get allocated by DefaultMessageCreator, but just in case + mapping_ptr = boost::make_shared(); + } + pcl::MsgFieldMap & mapping = *mapping_ptr; + if (mapping.empty()) { + pcl::createMapping(fields, mapping); + } + + uint8_t is_bigendian; + stream.next(is_bigendian); // ignoring... + uint32_t point_step, row_step; + stream.next(point_step); + stream.next(row_step); + + // Copy point data + uint32_t data_size; + stream.next(data_size); + assert(data_size == m.height * m.width * point_step); + m.points.resize(m.height * m.width); + uint8_t * m_data = reinterpret_cast(&m.points[0]); + // If the data layouts match, can copy a whole row in one memcpy + if (mapping.size() == 1 && + mapping[0].serialized_offset == 0 && + mapping[0].struct_offset == 0 && + point_step == sizeof(T)) { - template - inline static void write(Stream& stream, const pcl::PointCloud& m) - { - stream.next(m.header); - - // Ease the user's burden on specifying width/height for unorganized datasets - uint32_t height = m.height, width = m.width; - if (height == 0 && width == 0) { - width = m.points.size(); - height = 1; + uint32_t m_row_step = sizeof(T) * m.width; + // And if the row steps match, can copy whole point cloud in one memcpy + if (m_row_step == row_step) { + memcpy(m_data, stream.advance(data_size), data_size); + } else { + for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step) { + memcpy(m_data, stream.advance(row_step), m_row_step); } - stream.next(height); - stream.next(width); - - // Stream out point field metadata - typedef typename pcl::traits::fieldList::type FieldList; - uint32_t fields_size = boost::mpl::size::value; - stream.next(fields_size); - pcl::for_each_type(pcl::detail::FieldStreamer(stream)); - - // Assume little-endian... - uint8_t is_bigendian = false; - stream.next(is_bigendian); - - // Write out point data as binary blob - uint32_t point_step = sizeof(T); - stream.next(point_step); - uint32_t row_step = point_step * width; - stream.next(row_step); - uint32_t data_size = row_step * height; - stream.next(data_size); - memcpy(stream.advance(data_size), &m.points[0], data_size); - - uint8_t is_dense = m.is_dense; - stream.next(is_dense); } - - template - inline static void read(Stream& stream, pcl::PointCloud& m) - { - std_msgs::Header header; - stream.next(header); - pcl_conversions::toPCL(header, m.header); - stream.next(m.height); - stream.next(m.width); - - /// @todo Check that fields haven't changed! - std::vector fields; - stream.next(fields); - - // Construct field mapping if deserializing for the first time - boost::shared_ptr& mapping_ptr = pcl::detail::getMapping(m); - if (!mapping_ptr) - { - // This normally should get allocated by DefaultMessageCreator, but just in case - mapping_ptr = boost::make_shared(); - } - pcl::MsgFieldMap& mapping = *mapping_ptr; - if (mapping.empty()) - pcl::createMapping (fields, mapping); - - uint8_t is_bigendian; - stream.next(is_bigendian); // ignoring... - uint32_t point_step, row_step; - stream.next(point_step); - stream.next(row_step); - - // Copy point data - uint32_t data_size; - stream.next(data_size); - assert(data_size == m.height * m.width * point_step); - m.points.resize(m.height * m.width); - uint8_t* m_data = reinterpret_cast(&m.points[0]); - // If the data layouts match, can copy a whole row in one memcpy - if (mapping.size() == 1 && - mapping[0].serialized_offset == 0 && - mapping[0].struct_offset == 0 && - point_step == sizeof(T)) - { - uint32_t m_row_step = sizeof(T) * m.width; - // And if the row steps match, can copy whole point cloud in one memcpy - if (m_row_step == row_step) - { - memcpy (m_data, stream.advance(data_size), data_size); - } - else - { - for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step) - memcpy (m_data, stream.advance(row_step), m_row_step); + } else { + // If not, do a lot of memcpys to copy over the fields + for (uint32_t row = 0; row < m.height; ++row) { + const uint8_t * stream_data = stream.advance(row_step); + for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) { + BOOST_FOREACH(const pcl::detail::FieldMapping & fm, mapping) { + memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size); } + m_data += sizeof(T); } - else - { - // If not, do a lot of memcpys to copy over the fields - for (uint32_t row = 0; row < m.height; ++row) { - const uint8_t* stream_data = stream.advance(row_step); - for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) { - BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) { - memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size); - } - m_data += sizeof(T); - } - } - } - - uint8_t is_dense; - stream.next(is_dense); - m.is_dense = is_dense; } + } - inline static uint32_t serializedLength(const pcl::PointCloud& m) - { - uint32_t length = 0; + uint8_t is_dense; + stream.next(is_dense); + m.is_dense = is_dense; + } - length += serializationLength(m.header); - length += 8; // height/width + inline static uint32_t serializedLength(const pcl::PointCloud & m) + { + uint32_t length = 0; - pcl::detail::FieldsLength fl; - typedef typename pcl::traits::fieldList::type FieldList; - pcl::for_each_type(boost::ref(fl)); - length += 4; // size of 'fields' - length += fl.length; + length += serializationLength(m.header); + length += 8; // height/width - length += 1; // is_bigendian - length += 4; // point_step - length += 4; // row_step - length += 4; // size of 'data' - length += m.points.size() * sizeof(T); // data - length += 1; // is_dense + pcl::detail::FieldsLength fl; + typedef typename pcl::traits::fieldList::type FieldList; + pcl::for_each_type(boost::ref(fl)); + length += 4; // size of 'fields' + length += fl.length; - return length; - } - }; - } // namespace ros::serialization + length += 1; // is_bigendian + length += 4; // point_step + length += 4; // row_step + length += 4; // size of 'data' + length += m.points.size() * sizeof(T); // data + length += 1; // is_dense - /// @todo Printer specialization in message_operations + return length; + } +}; +} // namespace ros::serialization + +/// @todo Printer specialization in message_operations } // namespace ros @@ -315,103 +324,101 @@ namespace ros namespace pcl { - namespace detail - { +namespace detail +{ #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED #if PCL_VERSION_COMPARE(>=, 1, 10, 0) - template - constexpr static bool pcl_uses_boost = std::is_same, - pcl::shared_ptr>::value; +template +constexpr static bool pcl_uses_boost = std::is_same, + pcl::shared_ptr>::value; #else - template - constexpr static bool pcl_uses_boost = true; +template +constexpr static bool pcl_uses_boost = true; #endif - template struct Holder - { - SharedPointer p; +template +struct Holder +{ + SharedPointer p; - Holder(const SharedPointer &p) : p(p) {} - Holder(const Holder &other) : p(other.p) {} - Holder(Holder &&other) : p(std::move(other.p)) {} + Holder(const SharedPointer & p) + : p(p) {} + Holder(const Holder & other) + : p(other.p) {} + Holder(Holder && other) + : p(std::move(other.p)) {} - void operator () (...) { p.reset(); } - }; + void operator()(...) {p.reset();} +}; - template - inline std::shared_ptr to_std_ptr(const boost::shared_ptr &p) - { - typedef Holder> H; - if(H *h = boost::get_deleter(p)) - { - return h->p; - } - else - { - return std::shared_ptr(p.get(), Holder>(p)); - } - } +template +inline std::shared_ptr to_std_ptr(const boost::shared_ptr & p) +{ + typedef Holder> H; + if (H * h = boost::get_deleter(p)) { + return h->p; + } else { + return std::shared_ptr(p.get(), Holder>(p)); + } +} - template - inline boost::shared_ptr to_boost_ptr(const std::shared_ptr &p) - { - typedef Holder> H; - if(H * h = std::get_deleter(p)) - { - return h->p; - } - else - { - return boost::shared_ptr(p.get(), Holder>(p)); - } - } +template +inline boost::shared_ptr to_boost_ptr(const std::shared_ptr & p) +{ + typedef Holder> H; + if (H * h = std::get_deleter(p)) { + return h->p; + } else { + return boost::shared_ptr(p.get(), Holder>(p)); + } +} #endif - } // namespace pcl::detail +} // namespace pcl::detail // add functions to convert to smart pointer used by ROS - template - inline boost::shared_ptr ros_ptr(const boost::shared_ptr &p) - { - return p; - } +template +inline boost::shared_ptr ros_ptr(const boost::shared_ptr & p) +{ + return p; +} #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED - template - inline boost::shared_ptr ros_ptr(const std::shared_ptr &p) - { - return detail::to_boost_ptr(p); - } +template +inline boost::shared_ptr ros_ptr(const std::shared_ptr & p) +{ + return detail::to_boost_ptr(p); +} // add functions to convert to smart pointer used by PCL, based on PCL's own pointer - template >::type> - inline std::shared_ptr pcl_ptr(const std::shared_ptr &p) - { - return p; - } +template>::type> +inline std::shared_ptr pcl_ptr(const std::shared_ptr & p) +{ + return p; +} - template >::type> - inline std::shared_ptr pcl_ptr(const boost::shared_ptr &p) - { - return detail::to_std_ptr(p); - } +template>::type> +inline std::shared_ptr pcl_ptr(const boost::shared_ptr & p) +{ + return detail::to_std_ptr(p); +} - template >::type> - inline boost::shared_ptr pcl_ptr(const std::shared_ptr &p) - { - return detail::to_boost_ptr(p); - } +template>::type> +inline boost::shared_ptr pcl_ptr(const std::shared_ptr & p) +{ + return detail::to_boost_ptr(p); +} - template >::type> - inline boost::shared_ptr pcl_ptr(const boost::shared_ptr &p) - { - return p; - } +template>::type> +inline boost::shared_ptr pcl_ptr(const boost::shared_ptr & p) +{ + return p; +} #else - template - inline boost::shared_ptr pcl_ptr(const boost::shared_ptr &p) - { - return p; - } +template +inline boost::shared_ptr pcl_ptr(const boost::shared_ptr & p) +{ + return p; +} #endif } // namespace pcl diff --git a/pcl_ros/include/pcl_ros/publisher.hpp b/pcl_ros/include/pcl_ros/publisher.hpp index b752418c..9ef707af 100644 --- a/pcl_ros/include/pcl_ros/publisher.hpp +++ b/pcl_ros/include/pcl_ros/publisher.hpp @@ -38,7 +38,7 @@ \author Patrick Mihelich -@b Publisher represents a ROS publisher for the templated PointCloud implementation. +@b Publisher represents a ROS publisher for the templated PointCloud implementation. **/ @@ -51,99 +51,99 @@ #include -namespace pcl_ros +namespace pcl_ros { - class BasePublisher +class BasePublisher +{ +public: + void + advertise(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size) { - public: - void - advertise (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) - { - pub_ = nh.advertise(topic, queue_size); - } + pub_ = nh.advertise(topic, queue_size); + } - std::string - getTopic () - { - return (pub_.getTopic ()); - } - - uint32_t - getNumSubscribers () const - { - return (pub_.getNumSubscribers ()); - } - - void - shutdown () - { - pub_.shutdown (); - } - - operator void*() const - { - return (pub_) ? (void*)1 : (void*)0; - } - - protected: - ros::Publisher pub_; - }; - - template - class Publisher : public BasePublisher + std::string + getTopic() { - public: - Publisher () {} + return pub_.getTopic(); + } - Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) - { - advertise (nh, topic, queue_size); - } - - ~Publisher () {} - - inline void - publish (const boost::shared_ptr > &point_cloud) const - { - publish (*point_cloud); - } - - inline void - publish (const pcl::PointCloud& point_cloud) const - { - // Fill point cloud binary data - sensor_msgs::PointCloud2 msg; - pcl::toROSMsg (point_cloud, msg); - pub_.publish (boost::make_shared (msg)); - } - }; - - template <> - class Publisher : public BasePublisher + uint32_t + getNumSubscribers() const { - public: - Publisher () {} + return pub_.getNumSubscribers(); + } - Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) - { - advertise (nh, topic, queue_size); - } + void + shutdown() + { + pub_.shutdown(); + } - ~Publisher () {} + operator void *() const + { + return (pub_) ? (void *)1 : (void *)0; + } - void - publish (const sensor_msgs::PointCloud2Ptr& point_cloud) const - { - pub_.publish (point_cloud); - //pub_.publish (*point_cloud); - } +protected: + ros::Publisher pub_; +}; - void - publish (const sensor_msgs::PointCloud2& point_cloud) const - { - pub_.publish (boost::make_shared (point_cloud)); - //pub_.publish (point_cloud); - } - }; +template +class Publisher : public BasePublisher +{ +public: + Publisher() {} + + Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size) + { + advertise(nh, topic, queue_size); + } + + ~Publisher() {} + + inline void + publish(const boost::shared_ptr> & point_cloud) const + { + publish(*point_cloud); + } + + inline void + publish(const pcl::PointCloud & point_cloud) const + { + // Fill point cloud binary data + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(point_cloud, msg); + pub_.publish(boost::make_shared(msg)); + } +}; + +template<> +class Publisher: public BasePublisher +{ +public: + Publisher() {} + + Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size) + { + advertise(nh, topic, queue_size); + } + + ~Publisher() {} + + void + publish(const sensor_msgs::PointCloud2Ptr & point_cloud) const + { + pub_.publish(point_cloud); + //pub_.publish (*point_cloud); + } + + void + publish(const sensor_msgs::PointCloud2 & point_cloud) const + { + pub_.publish(boost::make_shared(point_cloud)); + //pub_.publish (point_cloud); + } +}; } #endif //#ifndef PCL_ROS_PUBLISHER_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp index b9c48425..cc189658 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp @@ -47,63 +47,68 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - //////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. - * \author Radu Bogdan Rusu +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. + * \author Radu Bogdan Rusu + */ +class EuclideanClusterExtraction : public PCLNodelet +{ +public: + /** \brief Empty constructor. */ + EuclideanClusterExtraction() + : publish_indices_(false), max_clusters_(std::numeric_limits::max()) {} + +protected: + // ROS nodelet attributes + /** \brief Publish indices or convert to PointCloud clusters. Default: false */ + bool publish_indices_; + + /** \brief Maximum number of clusters to publish. */ + int max_clusters_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; + + /** \brief Nodelet initialization routine. */ + void onInit(); + + /** \brief LazyNodelet connection routine. */ + void subscribe(); + void unsubscribe(); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level */ - class EuclideanClusterExtraction : public PCLNodelet - { - public: - /** \brief Empty constructor. */ - EuclideanClusterExtraction () : publish_indices_ (false), max_clusters_ (std::numeric_limits::max ()) {}; - - protected: - // ROS nodelet attributes - /** \brief Publish indices or convert to PointCloud clusters. Default: false */ - bool publish_indices_; + void config_callback(EuclideanClusterExtractionConfig & config, uint32_t level); - /** \brief Maximum number of clusters to publish. */ - int max_clusters_; + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_indices_callback( + const PointCloudConstPtr & cloud, + const PointIndicesConstPtr & indices); - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; +private: + /** \brief The PCL implementation used. */ + pcl::EuclideanClusterExtraction impl_; - /** \brief Nodelet initialization routine. */ - void onInit (); + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; - /** \brief LazyNodelet connection routine. */ - void subscribe (); - void unsubscribe (); + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr>> sync_input_indices_e_; + boost::shared_ptr>> sync_input_indices_a_; - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback (EuclideanClusterExtractionConfig &config, uint32_t level); - - /** \brief Input point cloud callback. - * \param cloud the pointer to the input point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices); - - private: - /** \brief The PCL implementation used. */ - pcl::EuclideanClusterExtraction impl_; - - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; - - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr > > sync_input_indices_e_; - boost::shared_ptr > > sync_input_indices_a_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ 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 458e565d..147f41d4 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 @@ -52,79 +52,83 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - /** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with - * a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying - * inside it. - * - * An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane). - * \author Radu Bogdan Rusu +/** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with + * a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying + * inside it. + * + * An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane). + * \author Radu Bogdan Rusu + */ +class ExtractPolygonalPrismData : public PCLNodelet +{ + typedef pcl::PointCloud PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; + +protected: + /** \brief The output PointIndices publisher. */ + ros::Publisher pub_output_; + + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_hull_filter_; + + /** \brief Synchronized input, planar hull, and indices.*/ + boost::shared_ptr>> sync_input_hull_indices_e_; + boost::shared_ptr>> sync_input_hull_indices_a_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_; + + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. */ - class ExtractPolygonalPrismData : public PCLNodelet + inline void + input_callback(const PointCloudConstPtr & input) { - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + PointIndices cloud; + cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp; + nf_.add(boost::make_shared(cloud)); + } - protected: - /** \brief The output PointIndices publisher. */ - ros::Publisher pub_output_; + /** \brief Nodelet initialization routine. */ + void onInit(); - /** \brief The message filter subscriber for PointCloud2. */ - message_filters::Subscriber sub_hull_filter_; + /** \brief LazyNodelet connection routine. */ + void subscribe(); + void unsubscribe(); - /** \brief Synchronized input, planar hull, and indices.*/ - boost::shared_ptr > > sync_input_hull_indices_e_; - boost::shared_ptr > > sync_input_hull_indices_a_; + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback(ExtractPolygonalPrismDataConfig & config, uint32_t level); - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; + /** \brief Input point cloud callback. Used when \a use_indices is set. + * \param cloud the pointer to the input point cloud + * \param hull the pointer to the planar hull point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_hull_indices_callback( + const PointCloudConstPtr & cloud, + const PointCloudConstPtr & hull, + const PointIndicesConstPtr & indices); - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_; +private: + /** \brief The PCL implementation used. */ + pcl::ExtractPolygonalPrismData impl_; - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback (const PointCloudConstPtr &input) - { - PointIndices cloud; - cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp; - nf_.add (boost::make_shared (cloud)); - } - - /** \brief Nodelet initialization routine. */ - void onInit (); - - /** \brief LazyNodelet connection routine. */ - void subscribe (); - void unsubscribe (); - - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level); - - /** \brief Input point cloud callback. Used when \a use_indices is set. - * \param cloud the pointer to the input point cloud - * \param hull the pointer to the planar hull point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_hull_indices_callback (const PointCloudConstPtr &cloud, - const PointCloudConstPtr &hull, - const PointIndicesConstPtr &indices); - - private: - /** \brief The PCL implementation used. */ - pcl::ExtractPolygonalPrismData impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp index e0f68690..475adf02 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -51,235 +51,243 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - //////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in - * the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. - * \author Radu Bogdan Rusu +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in + * the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. + * \author Radu Bogdan Rusu + */ +class SACSegmentation : public PCLNodelet +{ + typedef pcl::PointCloud PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; + +public: + /** \brief Constructor. */ + SACSegmentation() + : min_inliers_(0) {} + + /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. + * \param tf_frame the TF frame the input PointCloud should be transformed into before processing */ - class SACSegmentation : public PCLNodelet - { - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} - public: - /** \brief Constructor. */ - SACSegmentation () : min_inliers_(0) {} + /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ + inline std::string getInputTFframe() {return tf_input_frame_;} - /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. - * \param tf_frame the TF frame the input PointCloud should be transformed into before processing - */ - inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; } - - /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ - inline std::string getInputTFframe () { return (tf_input_frame_); } - - /** \brief Set the output TF frame the data should be transformed into after processing. - * \param tf_frame the TF frame the PointCloud should be transformed into after processing - */ - inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; } - - /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ - inline std::string getOutputTFframe () { return (tf_output_frame_); } - - protected: - // The minimum number of inliers a model must have in order to be considered valid. - int min_inliers_; - - // ROS nodelet attributes - /** \brief The output PointIndices publisher. */ - ros::Publisher pub_indices_; - - /** \brief The output ModelCoefficients publisher. */ - ros::Publisher pub_model_; - - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; - - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; - - /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ - std::string tf_input_frame_; - - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - - /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ - std::string tf_output_frame_; - - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_pi_; - - /** \brief Nodelet initialization routine. */ - virtual void onInit (); - - /** \brief LazyNodelet connection routine. */ - virtual void subscribe (); - virtual void unsubscribe (); - - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback (SACSegmentationConfig &config, uint32_t level); - - /** \brief Input point cloud callback. Used when \a use_indices is set. - * \param cloud the pointer to the input point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices); - - /** \brief Pointer to a set of indices stored internally. - * (used when \a latched_indices_ is set). - */ - PointIndices indices_; - - /** \brief Indices callback. Used when \a latched_indices_ is set. - * \param indices the pointer to the input point cloud indices - */ - inline void - indices_callback (const PointIndicesConstPtr &indices) - { - indices_ = *indices; - } - - /** \brief Input callback. Used when \a latched_indices_ is set. - * \param input the pointer to the input point cloud - */ - inline void - input_callback (const PointCloudConstPtr &input) - { - indices_.header = fromPCL(input->header); - PointIndicesConstPtr indices; - indices.reset (new PointIndices (indices_)); - nf_pi_.add (indices); - } - - private: - /** \brief Internal mutex. */ - boost::mutex mutex_; - - /** \brief The PCL implementation used. */ - pcl::SACSegmentation impl_; - - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr > > sync_input_indices_e_; - boost::shared_ptr > > sync_input_indices_a_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - //////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and - * models that require the use of surface normals for estimation. + /** \brief Set the output TF frame the data should be transformed into after processing. + * \param tf_frame the TF frame the PointCloud should be transformed into after processing */ - class SACSegmentationFromNormals: public SACSegmentation + inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} + + /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ + inline std::string getOutputTFframe() {return tf_output_frame_;} + +protected: + // The minimum number of inliers a model must have in order to be considered valid. + int min_inliers_; + + // ROS nodelet attributes + /** \brief The output PointIndices publisher. */ + ros::Publisher pub_indices_; + + /** \brief The output ModelCoefficients publisher. */ + ros::Publisher pub_model_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; + + /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_input_frame_; + + /** \brief The original data input TF frame. */ + std::string tf_input_orig_frame_; + + /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_output_frame_; + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_pi_; + + /** \brief Nodelet initialization routine. */ + virtual void onInit(); + + /** \brief LazyNodelet connection routine. */ + virtual void subscribe(); + virtual void unsubscribe(); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback(SACSegmentationConfig & config, uint32_t level); + + /** \brief Input point cloud callback. Used when \a use_indices is set. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_indices_callback( + const PointCloudConstPtr & cloud, + const PointIndicesConstPtr & indices); + + /** \brief Pointer to a set of indices stored internally. + * (used when \a latched_indices_ is set). + */ + PointIndices indices_; + + /** \brief Indices callback. Used when \a latched_indices_ is set. + * \param indices the pointer to the input point cloud indices + */ + inline void + indices_callback(const PointIndicesConstPtr & indices) { - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + indices_ = *indices; + } - typedef pcl::PointCloud PointCloudN; - typedef boost::shared_ptr PointCloudNPtr; - typedef boost::shared_ptr PointCloudNConstPtr; + /** \brief Input callback. Used when \a latched_indices_ is set. + * \param input the pointer to the input point cloud + */ + inline void + input_callback(const PointCloudConstPtr & input) + { + indices_.header = fromPCL(input->header); + PointIndicesConstPtr indices; + indices.reset(new PointIndices(indices_)); + nf_pi_.add(indices); + } - public: - /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. - * \param tf_frame the TF frame the input PointCloud should be transformed into before processing - */ - inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; } - - /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ - inline std::string getInputTFframe () { return (tf_input_frame_); } +private: + /** \brief Internal mutex. */ + boost::mutex mutex_; - /** \brief Set the output TF frame the data should be transformed into after processing. - * \param tf_frame the TF frame the PointCloud should be transformed into after processing - */ - inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; } - - /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ - inline std::string getOutputTFframe () { return (tf_output_frame_); } + /** \brief The PCL implementation used. */ + pcl::SACSegmentation impl_; - protected: - // ROS nodelet attributes - /** \brief The normals PointCloud subscriber filter. */ - message_filters::Subscriber sub_normals_filter_; + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr>> sync_input_indices_e_; + boost::shared_ptr>> sync_input_indices_a_; - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_axis_; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; - - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback (const PointCloudConstPtr &cloud) - { - PointIndices indices; - indices.header.stamp = fromPCL(cloud->header).stamp; - nf_.add (boost::make_shared (indices)); - } +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and + * models that require the use of surface normals for estimation. + */ +class SACSegmentationFromNormals : public SACSegmentation +{ + typedef pcl::PointCloud PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_; + typedef pcl::PointCloud PointCloudN; + typedef boost::shared_ptr PointCloudNPtr; + typedef boost::shared_ptr PointCloudNConstPtr; - /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ - std::string tf_input_frame_; - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ - std::string tf_output_frame_; +public: + /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. + * \param tf_frame the TF frame the input PointCloud should be transformed into before processing + */ + inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} - /** \brief Nodelet initialization routine. */ - virtual void onInit (); + /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ + inline std::string getInputTFframe() {return tf_input_frame_;} - /** \brief LazyNodelet connection routine. */ - virtual void subscribe (); - virtual void unsubscribe (); + /** \brief Set the output TF frame the data should be transformed into after processing. + * \param tf_frame the TF frame the PointCloud should be transformed into after processing + */ + inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} - /** \brief Model callback - * \param model the sample consensus model found - */ - void axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model); + /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ + inline std::string getOutputTFframe() {return tf_output_frame_;} - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level); +protected: + // ROS nodelet attributes + /** \brief The normals PointCloud subscriber filter. */ + message_filters::Subscriber sub_normals_filter_; - /** \brief Input point cloud callback. - * \param cloud the pointer to the input point cloud - * \param cloud_normals the pointer to the input point cloud normals - * \param indices the pointer to the input point cloud indices - */ - void input_normals_indices_callback (const PointCloudConstPtr &cloud, - const PointCloudNConstPtr &cloud_normals, - const PointIndicesConstPtr &indices); - - private: - /** \brief Internal mutex. */ - boost::mutex mutex_; + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_axis_; - /** \brief The PCL implementation used. */ - pcl::SACSegmentationFromNormals impl_; + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; - /** \brief Synchronized input, normals, and indices.*/ - boost::shared_ptr > > sync_input_normals_indices_a_; - boost::shared_ptr > > sync_input_normals_indices_e_; + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. + */ + inline void + input_callback(const PointCloudConstPtr & cloud) + { + PointIndices indices; + indices.header.stamp = fromPCL(cloud->header).stamp; + nf_.add(boost::make_shared(indices)); + } - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough nf_; + + /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_input_frame_; + /** \brief The original data input TF frame. */ + std::string tf_input_orig_frame_; + /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_output_frame_; + + /** \brief Nodelet initialization routine. */ + virtual void onInit(); + + /** \brief LazyNodelet connection routine. */ + virtual void subscribe(); + virtual void unsubscribe(); + + /** \brief Model callback + * \param model the sample consensus model found + */ + void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level); + + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param cloud_normals the pointer to the input point cloud normals + * \param indices the pointer to the input point cloud indices + */ + void input_normals_indices_callback( + const PointCloudConstPtr & cloud, + const PointCloudNConstPtr & cloud_normals, + const PointIndicesConstPtr & indices); + +private: + /** \brief Internal mutex. */ + boost::mutex mutex_; + + /** \brief The PCL implementation used. */ + pcl::SACSegmentationFromNormals impl_; + + /** \brief Synchronized input, normals, and indices.*/ + boost::shared_ptr>> sync_input_normals_indices_a_; + boost::shared_ptr>> sync_input_normals_indices_e_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp index ee7fb28c..ba0cd5b1 100644 --- a/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp @@ -48,63 +48,66 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - //////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the - * difference between them for a maximum given distance threshold. - * \author Radu Bogdan Rusu +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the + * difference between them for a maximum given distance threshold. + * \author Radu Bogdan Rusu + */ +class SegmentDifferences : public PCLNodelet +{ + typedef pcl::PointCloud PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; + +public: + /** \brief Empty constructor. */ + SegmentDifferences() {} + +protected: + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_target_filter_; + + /** \brief Synchronized input, and planar hull.*/ + boost::shared_ptr>> sync_input_target_e_; + boost::shared_ptr>> sync_input_target_a_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; + + /** \brief Nodelet initialization routine. */ + void onInit(); + + /** \brief LazyNodelet connection routine. */ + void subscribe(); + void unsubscribe(); + + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level */ - class SegmentDifferences : public PCLNodelet - { - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + void config_callback(SegmentDifferencesConfig & config, uint32_t level); - public: - /** \brief Empty constructor. */ - SegmentDifferences () {}; - - protected: - /** \brief The message filter subscriber for PointCloud2. */ - message_filters::Subscriber sub_target_filter_; + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param cloud_target the pointcloud that we want to segment \a cloud from + */ + void input_target_callback( + const PointCloudConstPtr & cloud, + const PointCloudConstPtr & cloud_target); - /** \brief Synchronized input, and planar hull.*/ - boost::shared_ptr > > sync_input_target_e_; - boost::shared_ptr > > sync_input_target_a_; +private: + /** \brief The PCL implementation used. */ + pcl::SegmentDifferences impl_; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; - - /** \brief Nodelet initialization routine. */ - void onInit (); - - /** \brief LazyNodelet connection routine. */ - void subscribe (); - void unsubscribe (); - - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback (SegmentDifferencesConfig &config, uint32_t level); - - /** \brief Input point cloud callback. - * \param cloud the pointer to the input point cloud - * \param cloud_target the pointcloud that we want to segment \a cloud from - */ - void input_target_callback (const PointCloudConstPtr &cloud, - const PointCloudConstPtr &cloud_target); - - private: - /** \brief The PCL implementation used. */ - pcl::SegmentDifferences impl_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.hpp b/pcl_ros/include/pcl_ros/surface/convex_hull.hpp index 066d7a16..726584a3 100644 --- a/pcl_ros/include/pcl_ros/surface/convex_hull.hpp +++ b/pcl_ros/include/pcl_ros/surface/convex_hull.hpp @@ -48,49 +48,52 @@ namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - /** \brief @b ConvexHull2D represents a 2D ConvexHull implementation. - * \author Radu Bogdan Rusu +/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation. + * \author Radu Bogdan Rusu + */ +class ConvexHull2D : public PCLNodelet +{ + typedef pcl::PointCloud PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; + +private: + /** \brief Nodelet initialization routine. */ + virtual void onInit(); + + /** \brief LazyNodelet connection routine. */ + virtual void subscribe(); + virtual void unsubscribe(); + + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices */ - class ConvexHull2D : public PCLNodelet - { - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + void input_indices_callback( + const PointCloudConstPtr & cloud, + const PointIndicesConstPtr & indices); - private: - /** \brief Nodelet initialization routine. */ - virtual void onInit (); +private: + /** \brief The PCL implementation used. */ + pcl::ConvexHull impl_; - /** \brief LazyNodelet connection routine. */ - virtual void subscribe (); - virtual void unsubscribe (); + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; - /** \brief Input point cloud callback. - * \param cloud the pointer to the input point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_indices_callback (const PointCloudConstPtr &cloud, - const PointIndicesConstPtr &indices); + /** \brief Publisher for PolygonStamped. */ + ros::Publisher pub_plane_; - private: - /** \brief The PCL implementation used. */ - pcl::ConvexHull impl_; + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr>> sync_input_indices_e_; + boost::shared_ptr>> sync_input_indices_a_; - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; - - /** \brief Publisher for PolygonStamped. */ - ros::Publisher pub_plane_; - - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr > > sync_input_indices_e_; - boost::shared_ptr > > sync_input_indices_a_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_ diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp b/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp index 36542087..0c0bb060 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp @@ -46,105 +46,107 @@ // Dynamic reconfigure #include -#include "pcl_ros/MLSConfig.h" +#include "pcl_ros/MLSConfig.h" namespace pcl_ros { - namespace sync_policies = message_filters::sync_policies; +namespace sync_policies = message_filters::sync_policies; - /** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation. - * The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters. - * Normals are estimated at each point as well and published on a separate topic. - * \author Radu Bogdan Rusu, Zoltan-Csaba Marton +/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation. + * The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters. + * Normals are estimated at each point as well and published on a separate topic. + * \author Radu Bogdan Rusu, Zoltan-Csaba Marton + */ +class MovingLeastSquares : public PCLNodelet +{ + typedef pcl::PointXYZ PointIn; + typedef pcl::PointNormal NormalOut; + + typedef pcl::PointCloud PointCloudIn; + typedef boost::shared_ptr PointCloudInPtr; + typedef boost::shared_ptr PointCloudInConstPtr; + typedef pcl::PointCloud NormalCloudOut; + + typedef pcl::KdTree KdTree; + typedef pcl::KdTree::Ptr KdTreePtr; + +protected: + /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ + PointCloudInConstPtr surface_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief The number of K nearest neighbors to use for each point. */ + //int k_; + + /** \brief Whether to use a polynomial fit. */ + bool use_polynomial_fit_; + + /** \brief The order of the polynomial to be fit. */ + int polynomial_order_; + + /** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */ + double gaussian_parameter_; + + // ROS nodelet attributes + /** \brief The surface PointCloud subscriber filter. */ + message_filters::Subscriber sub_surface_filter_; + + /** \brief Parameter for the spatial locator tree. By convention, the values represent: + * 0: ANN (Approximate Nearest Neigbor library) kd-tree + * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree + * 2: Organized spatial dataset index */ - class MovingLeastSquares : public PCLNodelet - { - typedef pcl::PointXYZ PointIn; - typedef pcl::PointNormal NormalOut; + int spatial_locator_type_; - typedef pcl::PointCloud PointCloudIn; - typedef boost::shared_ptr PointCloudInPtr; - typedef boost::shared_ptr PointCloudInConstPtr; - typedef pcl::PointCloud NormalCloudOut; + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; - typedef pcl::KdTree KdTree; - typedef pcl::KdTree::Ptr KdTreePtr; + /** \brief Dynamic reconfigure callback + * \param config the config object + * \param level the dynamic reconfigure level + */ + void config_callback(MLSConfig & config, uint32_t level); - protected: - /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ - PointCloudInConstPtr surface_; + /** \brief Nodelet initialization routine. */ + virtual void onInit(); - /** \brief A pointer to the spatial search object. */ - KdTreePtr tree_; + /** \brief LazyNodelet connection routine. */ + virtual void subscribe(); + virtual void unsubscribe(); - /** \brief The nearest neighbors search radius for each point. */ - double search_radius_; +private: + /** \brief Input point cloud callback. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_indices_callback( + const PointCloudInConstPtr & cloud, + const PointIndicesConstPtr & indices); - /** \brief The number of K nearest neighbors to use for each point. */ - //int k_; +private: + /** \brief The PCL implementation used. */ + pcl::MovingLeastSquares impl_; - /** \brief Whether to use a polynomial fit. */ - bool use_polynomial_fit_; + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; - /** \brief The order of the polynomial to be fit. */ - int polynomial_order_; + /** \brief The output PointCloud (containing the normals) publisher. */ + ros::Publisher pub_normals_; - /** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */ - double gaussian_parameter_; + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr>> sync_input_indices_e_; + boost::shared_ptr>> sync_input_indices_a_; - // ROS nodelet attributes - /** \brief The surface PointCloud subscriber filter. */ - message_filters::Subscriber sub_surface_filter_; - - /** \brief Parameter for the spatial locator tree. By convention, the values represent: - * 0: ANN (Approximate Nearest Neigbor library) kd-tree - * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree - * 2: Organized spatial dataset index - */ - int spatial_locator_type_; - - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; - - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback (MLSConfig &config, uint32_t level); - - /** \brief Nodelet initialization routine. */ - virtual void onInit (); - - /** \brief LazyNodelet connection routine. */ - virtual void subscribe (); - virtual void unsubscribe (); - - private: - /** \brief Input point cloud callback. - * \param cloud the pointer to the input point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_indices_callback (const PointCloudInConstPtr &cloud, - const PointIndicesConstPtr &indices); - - - private: - /** \brief The PCL implementation used. */ - pcl::MovingLeastSquares impl_; - - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; - - /** \brief The output PointCloud (containing the normals) publisher. */ - ros::Publisher pub_normals_; - - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr > > sync_input_indices_e_; - boost::shared_ptr > > sync_input_indices_a_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ diff --git a/pcl_ros/include/pcl_ros/transforms.hpp b/pcl_ros/include/pcl_ros/transforms.hpp index 36e5d805..28d85a62 100644 --- a/pcl_ros/include/pcl_ros/transforms.hpp +++ b/pcl_ros/include/pcl_ros/transforms.hpp @@ -44,124 +44,138 @@ namespace pcl_ros { - /** \brief Transform a point cloud and rotate its normals using an Eigen transform. - * \param cloud_in the input point cloud - * \param cloud_out the input point cloud - * \param transform a rigid transformation from tf - * \note calls the Eigen version - */ - template void - transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - const tf::Transform &transform); +/** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param transform a rigid transformation from tf + * \note calls the Eigen version + */ +template +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const tf::Transform & transform); - /** \brief Transforms a point cloud in a given target TF frame using a TransformListener - * \param target_frame the target TF frame the point cloud should be transformed to - * \param cloud_in the input point cloud - * \param cloud_out the input point cloud - * \param tf_listener a TF listener object - */ - template bool - transformPointCloudWithNormals (const std::string &target_frame, - const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener); +/** \brief Transforms a point cloud in a given target TF frame using a TransformListener + * \param target_frame the target TF frame the point cloud should be transformed to + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param tf_listener a TF listener object + */ +template +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const tf::TransformListener & tf_listener); - /** \brief Transforms a point cloud in a given target TF frame using a TransformListener - * \param target_frame the target TF frame the point cloud should be transformed to - * \param target_time the target timestamp - * \param cloud_in the input point cloud - * \param fixed_frame fixed TF frame - * \param cloud_out the input point cloud - * \param tf_listener a TF listener object - */ - template bool - transformPointCloudWithNormals (const std::string &target_frame, - const ros::Time & target_time, - const pcl::PointCloud &cloud_in, - const std::string &fixed_frame, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener); +/** \brief Transforms a point cloud in a given target TF frame using a TransformListener + * \param target_frame the target TF frame the point cloud should be transformed to + * \param target_time the target timestamp + * \param cloud_in the input point cloud + * \param fixed_frame fixed TF frame + * \param cloud_out the input point cloud + * \param tf_listener a TF listener object + */ +template +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const ros::Time & target_time, + const pcl::PointCloud & cloud_in, + const std::string & fixed_frame, + pcl::PointCloud & cloud_out, + const tf::TransformListener & tf_listener); - /** \brief Apply a rigid transform defined by a 3D offset and a quaternion - * \param cloud_in the input point cloud - * \param cloud_out the input point cloud - * \param transform a rigid transformation from tf - * \note calls the Eigen version - */ - template void - transformPointCloud (const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - const tf::Transform &transform); +/** \brief Apply a rigid transform defined by a 3D offset and a quaternion + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param transform a rigid transformation from tf + * \note calls the Eigen version + */ +template +void +transformPointCloud( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const tf::Transform & transform); - /** \brief Transforms a point cloud in a given target TF frame using a TransformListener - * \param target_frame the target TF frame the point cloud should be transformed to - * \param cloud_in the input point cloud - * \param cloud_out the input point cloud - * \param tf_listener a TF listener object - */ - template bool - transformPointCloud (const std::string &target_frame, - const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener); +/** \brief Transforms a point cloud in a given target TF frame using a TransformListener + * \param target_frame the target TF frame the point cloud should be transformed to + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param tf_listener a TF listener object + */ +template +bool +transformPointCloud( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const tf::TransformListener & tf_listener); - /** \brief Transforms a point cloud in a given target TF frame using a TransformListener - * \param target_frame the target TF frame the point cloud should be transformed to - * \param target_time the target timestamp - * \param cloud_in the input point cloud - * \param fixed_frame fixed TF frame - * \param cloud_out the input point cloud - * \param tf_listener a TF listener object - */ - template bool - transformPointCloud (const std::string &target_frame, const ros::Time & target_time, - const pcl::PointCloud &cloud_in, - const std::string &fixed_frame, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener); +/** \brief Transforms a point cloud in a given target TF frame using a TransformListener + * \param target_frame the target TF frame the point cloud should be transformed to + * \param target_time the target timestamp + * \param cloud_in the input point cloud + * \param fixed_frame fixed TF frame + * \param cloud_out the input point cloud + * \param tf_listener a TF listener object + */ +template +bool +transformPointCloud( + const std::string & target_frame, const ros::Time & target_time, + const pcl::PointCloud & cloud_in, + const std::string & fixed_frame, + pcl::PointCloud & cloud_out, + const tf::TransformListener & tf_listener); - /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. - * \param target_frame the target TF frame - * \param in the input PointCloud2 dataset - * \param out the resultant transformed PointCloud2 dataset - * \param tf_listener a TF listener object - */ - bool - transformPointCloud (const std::string &target_frame, - const sensor_msgs::PointCloud2 &in, - sensor_msgs::PointCloud2 &out, - const tf::TransformListener &tf_listener); +/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. + * \param target_frame the target TF frame + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + * \param tf_listener a TF listener object + */ +bool +transformPointCloud( + const std::string & target_frame, + const sensor_msgs::PointCloud2 & in, + sensor_msgs::PointCloud2 & out, + const tf::TransformListener & tf_listener); - /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. - * \param target_frame the target TF frame - * \param net_transform the TF transformer object - * \param in the input PointCloud2 dataset - * \param out the resultant transformed PointCloud2 dataset - */ - void - transformPointCloud (const std::string &target_frame, - const tf::Transform &net_transform, - const sensor_msgs::PointCloud2 &in, - sensor_msgs::PointCloud2 &out); +/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. + * \param target_frame the target TF frame + * \param net_transform the TF transformer object + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + */ +void +transformPointCloud( + const std::string & target_frame, + const tf::Transform & net_transform, + const sensor_msgs::PointCloud2 & in, + sensor_msgs::PointCloud2 & out); - /** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix. - * \param transform the transformation to use on the points - * \param in the input PointCloud2 dataset - * \param out the resultant transformed PointCloud2 dataset - */ - void - transformPointCloud (const Eigen::Matrix4f &transform, - const sensor_msgs::PointCloud2 &in, - sensor_msgs::PointCloud2 &out); +/** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix. + * \param transform the transformation to use on the points + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + */ +void +transformPointCloud( + const Eigen::Matrix4f & transform, + const sensor_msgs::PointCloud2 & in, + sensor_msgs::PointCloud2 & out); - /** \brief Obtain the transformation matrix from TF into an Eigen form - * \param bt the TF transformation - * \param out_mat the Eigen transformation - */ - void - transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat); +/** \brief Obtain the transformation matrix from TF into an Eigen form + * \param bt the TF transformation + * \param out_mat the Eigen transformation + */ +void +transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat); } #endif // PCL_ROS_TRANSFORMS_H_ - diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp index 6571ed30..6ac61f4a 100644 --- a/pcl_ros/src/pcl_ros/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -39,35 +39,36 @@ #include "pcl_ros/features/boundary.hpp" void -pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } void -pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +pcl_ros::BoundaryEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); - impl_.setInputNormals (pcl_ptr(normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(normals)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::BoundaryEstimation BoundaryEstimation; diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp index fcb9516d..146b20d0 100644 --- a/pcl_ros/src/pcl_ros/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -53,219 +53,263 @@ //////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Feature::onInit () +pcl_ros::Feature::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Call the child init - childInit (*pnh_); + childInit(*pnh_); // Allow each individual class that inherits from us to declare their own Publisher // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters - if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) - { - NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { + NODELET_ERROR( + "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", + getName().c_str()); return; } - if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) - { - NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) { + NODELET_ERROR( + "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", + getName().c_str()); return; } // ---[ Optional parameters - pnh_->getParam ("use_surface", use_surface_); + pnh_->getParam("use_surface", use_surface_); // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &Feature::config_callback, this, _1, _2); + srv_->setCallback(f); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName().c_str(), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); - onInitPostProcess (); + onInitPostProcess(); } //////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Feature::subscribe () +pcl_ros::Feature::subscribe() { // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages - if (use_indices_ || use_surface_) - { + if (use_indices_ || use_surface_) { // Create the objects here - if (approximate_sync_) - sync_input_surface_indices_a_ = boost::make_shared > >(max_queue_size_); - else - sync_input_surface_indices_e_ = boost::make_shared > >(max_queue_size_); + if (approximate_sync_) { + sync_input_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + } // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - if (use_indices_) - { + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + if (use_indices_) { // If indices are enabled, subscribe to the indices - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); - if (use_surface_) // Use both indices and surface - { + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + if (use_surface_) { // Use both indices and surface // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register - sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); - if (approximate_sync_) - sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); - else - sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); - } - else // Use only indices - { - sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); + sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); + if (approximate_sync_) { + sync_input_surface_indices_a_->connectInput( + sub_input_filter_, sub_surface_filter_, + sub_indices_filter_); + } else { + sync_input_surface_indices_e_->connectInput( + sub_input_filter_, sub_surface_filter_, + sub_indices_filter_); + } + } else { // Use only indices + sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1)); // surface not enabled, connect the input-indices duo and register - if (approximate_sync_) - sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); - else - sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); + if (approximate_sync_) { + sync_input_surface_indices_a_->connectInput( + sub_input_filter_, nf_pc_, + sub_indices_filter_); + } else { + sync_input_surface_indices_e_->connectInput( + sub_input_filter_, nf_pc_, + sub_indices_filter_); + } } - } - else // Use only surface - { - sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); + } else { // Use only surface + sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1)); // indices not enabled, connect the input-surface duo and register - sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); - if (approximate_sync_) - sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); - else - sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); + sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); + if (approximate_sync_) { + sync_input_surface_indices_a_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_); + } else { + sync_input_surface_indices_e_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_); + } } // Register callbacks - if (approximate_sync_) - sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); - else - sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); - } - else - // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); -} - -//////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Feature::unsubscribe () -{ - if (use_indices_ || use_surface_) - { - sub_input_filter_.unsubscribe (); - if (use_indices_) - { - sub_indices_filter_.unsubscribe (); - if (use_surface_) - sub_surface_filter_.unsubscribe (); + if (approximate_sync_) { + sync_input_surface_indices_a_->registerCallback( + bind( + &Feature::input_surface_indices_callback, + this, _1, _2, _3)); + } else { + sync_input_surface_indices_e_->registerCallback( + bind( + &Feature::input_surface_indices_callback, + this, _1, _2, _3)); } - else - sub_surface_filter_.unsubscribe (); + } else { + // Subscribe in an old fashion to input only (no filters) + sub_input_ = + pnh_->subscribe( + "input", max_queue_size_, + bind( + &Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(), + PointIndicesConstPtr())); } - else - sub_input_.shutdown (); } //////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level) +pcl_ros::Feature::unsubscribe() { - if (k_ != config.k_search) - { - k_ = config.k_search; - NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_); - } - if (search_radius_ != config.radius_search) - { - search_radius_ = config.radius_search; - NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_); + if (use_indices_ || use_surface_) { + sub_input_filter_.unsubscribe(); + if (use_indices_) { + sub_indices_filter_.unsubscribe(); + if (use_surface_) { + sub_surface_filter_.unsubscribe(); + } + } else { + sub_surface_filter_.unsubscribe(); + } + } else { + sub_input_.shutdown(); } } //////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) +pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level) +{ + if (k_ != config.k_search) { + k_ = config.k_search; + NODELET_DEBUG( + "[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_); + } + if (search_radius_ != config.radius_search) { + search_radius_ = config.radius_search; + NODELET_DEBUG( + "[config_callback] Setting the nearest neighbors search radius for each point: %f.", + search_radius_); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::input_surface_indices_callback( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0) + if (pub_output_.getNumSubscribers() <= 0) { return; + } // If cloud is given, check if it's valid - if (!isValid (cloud)) - { - NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ()); - emptyPublish (cloud); + if (!isValid(cloud)) { + NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str()); + emptyPublish(cloud); return; } // If surface is given, check if it's valid - if (cloud_surface && !isValid (cloud_surface, "surface")) - { - NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ()); - emptyPublish (cloud); + if (cloud_surface && !isValid(cloud_surface, "surface")) { + NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str()); + emptyPublish(cloud); return; } - + // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ()); - emptyPublish (cloud); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str()); + emptyPublish(cloud); return; } /// DEBUG - if (cloud_surface) - if (indices) - NODELET_DEBUG ("[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ()); - - else - if (indices) - NODELET_DEBUG ("[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + if (cloud_surface) { + if (indices) { + NODELET_DEBUG( + "[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList( + *cloud_surface).c_str(), fromPCL( + cloud_surface->header).stamp.toSec(), + cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList( + *cloud_surface).c_str(), fromPCL( + cloud_surface->header).stamp.toSec(), + cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str()); + } + } else if (indices) { + NODELET_DEBUG( + "[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str()); + } /// - if ((int)(cloud->width * cloud->height) < k_) - { - NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height)); - emptyPublish (cloud); + if ((int)(cloud->width * cloud->height) < k_) { + NODELET_ERROR( + "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, + (int)(cloud->width * cloud->height)); + emptyPublish(cloud); return; } // If indices given... IndicesPtr vindices; - if (indices && !indices->header.frame_id.empty ()) - vindices.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + vindices.reset(new std::vector(indices->indices)); + } - computePublish (cloud, cloud_surface, vindices); + computePublish(cloud, cloud_surface, vindices); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -274,223 +318,289 @@ pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cl ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::FeatureFromNormals::onInit () +pcl_ros::FeatureFromNormals::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Call the child init - childInit (*pnh_); + childInit(*pnh_); // Allow each individual class that inherits from us to declare their own Publisher // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters - if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) - { - NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { + NODELET_ERROR( + "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", + getName().c_str()); return; } - if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) - { - NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) { + NODELET_ERROR( + "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", + getName().c_str()); return; } // ---[ Optional parameters - pnh_->getParam ("use_surface", use_surface_); + pnh_->getParam("use_surface", use_surface_); // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &FeatureFromNormals::config_callback, this, _1, _2); + srv_->setCallback(f); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName().c_str(), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::FeatureFromNormals::subscribe () +pcl_ros::FeatureFromNormals::subscribe() { - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); // Create the objects here - if (approximate_sync_) - sync_input_normals_surface_indices_a_ = boost::make_shared > > (max_queue_size_); - else - sync_input_normals_surface_indices_e_ = boost::make_shared > > (max_queue_size_); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_normals_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + } // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages - if (use_indices_ || use_surface_) - { - if (use_indices_) - { + if (use_indices_ || use_surface_) { + if (use_indices_) { // If indices are enabled, subscribe to the indices - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); - if (use_surface_) // Use both indices and surface - { + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + if (use_surface_) { // Use both indices and surface // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register - sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); - if (approximate_sync_) - sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); - else - sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); - } - else // Use only indices - { - sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); - if (approximate_sync_) + sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_->connectInput( + sub_input_filter_, + sub_normals_filter_, + sub_surface_filter_, + sub_indices_filter_); + } else { + sync_input_normals_surface_indices_e_->connectInput( + sub_input_filter_, + sub_normals_filter_, + sub_surface_filter_, + sub_indices_filter_); + } + } else { // Use only indices + sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1)); + if (approximate_sync_) { // surface not enabled, connect the input-indices duo and register - sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); - else + sync_input_normals_surface_indices_a_->connectInput( + sub_input_filter_, + sub_normals_filter_, nf_pc_, + sub_indices_filter_); + } else { // surface not enabled, connect the input-indices duo and register - sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); + sync_input_normals_surface_indices_e_->connectInput( + sub_input_filter_, + sub_normals_filter_, nf_pc_, + sub_indices_filter_); + } } - } - else // Use only surface - { + } else { // Use only surface // indices not enabled, connect the input-surface duo and register - sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); - sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); - if (approximate_sync_) - sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); - else - sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); + sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1)); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_->connectInput( + sub_input_filter_, sub_normals_filter_, + sub_surface_filter_, nf_pi_); + } else { + sync_input_normals_surface_indices_e_->connectInput( + sub_input_filter_, sub_normals_filter_, + sub_surface_filter_, nf_pi_); + } } - } - else - { - sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); + } else { + sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1)); - if (approximate_sync_) - sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); - else - sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_->connectInput( + sub_input_filter_, sub_normals_filter_, + nf_pc_, nf_pi_); + } else { + sync_input_normals_surface_indices_e_->connectInput( + sub_input_filter_, sub_normals_filter_, + nf_pc_, nf_pi_); + } } // Register callbacks - if (approximate_sync_) - sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); - else - sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::FeatureFromNormals::unsubscribe () -{ - sub_input_filter_.unsubscribe (); - sub_normals_filter_.unsubscribe (); - if (use_indices_ || use_surface_) - { - if (use_indices_) - { - sub_indices_filter_.unsubscribe (); - if (use_surface_) - sub_surface_filter_.unsubscribe (); - } - else - sub_surface_filter_.unsubscribe (); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_->registerCallback( + bind( + &FeatureFromNormals:: + input_normals_surface_indices_callback, this, _1, _2, _3, _4)); + } else { + sync_input_normals_surface_indices_e_->registerCallback( + bind( + &FeatureFromNormals:: + input_normals_surface_indices_callback, this, _1, _2, _3, _4)); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( - const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, - const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) +pcl_ros::FeatureFromNormals::unsubscribe() +{ + sub_input_filter_.unsubscribe(); + sub_normals_filter_.unsubscribe(); + if (use_indices_ || use_surface_) { + if (use_indices_) { + sub_indices_filter_.unsubscribe(); + if (use_surface_) { + sub_surface_filter_.unsubscribe(); + } + } else { + sub_surface_filter_.unsubscribe(); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( + const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals, + const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0) + if (pub_output_.getNumSubscribers() <= 0) { return; + } // If cloud+normals is given, check if it's valid - if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) - { - NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ()); - emptyPublish (cloud); + if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals")) + NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str()); + emptyPublish(cloud); return; } // If surface is given, check if it's valid - if (cloud_surface && !isValid (cloud_surface, "surface")) - { - NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ()); - emptyPublish (cloud); + if (cloud_surface && !isValid(cloud_surface, "surface")) { + NODELET_ERROR( + "[%s::input_normals_surface_indices_callback] Invalid input surface!", + getName().c_str()); + emptyPublish(cloud); return; } - + // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ()); - emptyPublish (cloud); + if (indices && !isValid(indices)) { + NODELET_ERROR( + "[%s::input_normals_surface_indices_callback] Invalid input indices!", + getName().c_str()); + emptyPublish(cloud); return; } /// DEBUG - if (cloud_surface) - if (indices) - NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); - else - if (indices) - NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + if (cloud_surface) { + if (indices) { + NODELET_DEBUG( + "[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList( + *cloud_surface).c_str(), fromPCL( + cloud_surface->header).stamp.toSec(), + cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), fromPCL( + cloud_normals->header).stamp.toSec(), + cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList( + *cloud_surface).c_str(), fromPCL( + cloud_surface->header).stamp.toSec(), + cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), fromPCL( + cloud_normals->header).stamp.toSec(), + cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); + } + } else if (indices) { + NODELET_DEBUG( + "[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), fromPCL( + cloud_normals->header).stamp.toSec(), + cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), fromPCL( + cloud_normals->header).stamp.toSec(), + cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); + } /// - if ((int)(cloud->width * cloud->height) < k_) - { - NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height)); - emptyPublish (cloud); + if ((int)(cloud->width * cloud->height) < k_) { + NODELET_ERROR( + "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", + getName().c_str(), k_, (int)(cloud->width * cloud->height)); + emptyPublish(cloud); return; } // If indices given... IndicesPtr vindices; - if (indices && !indices->header.frame_id.empty ()) - vindices.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + vindices.reset(new std::vector(indices->indices)); + } - computePublish (cloud, cloud_normals, cloud_surface, vindices); + computePublish(cloud, cloud_normals, cloud_surface, vindices); } - diff --git a/pcl_ros/src/pcl_ros/features/fpfh.cpp b/pcl_ros/src/pcl_ros/features/fpfh.cpp index c1ac221a..e6c05b34 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh.cpp @@ -38,39 +38,39 @@ #include #include "pcl_ros/features/fpfh.hpp" -void -pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::FPFHEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); - impl_.setInputNormals (pcl_ptr(normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(normals)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::FPFHEstimation FPFHEstimation; PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp index 8dfaf044..d3e96fc4 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp @@ -38,39 +38,39 @@ #include #include "pcl_ros/features/fpfh_omp.hpp" -void -pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::FPFHEstimationOMP::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); - impl_.setInputNormals (pcl_ptr(normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(normals)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp index a6652dec..5a5c1452 100644 --- a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp @@ -38,37 +38,37 @@ #include #include "pcl_ros/features/moment_invariants.hpp" -void -pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::MomentInvariantsEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/features/normal_3d.cpp index 489fa473..7ab774bc 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d.cpp @@ -38,37 +38,37 @@ #include #include "pcl_ros/features/normal_3d.hpp" -void -pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::NormalEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::NormalEstimation NormalEstimation; PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp index b5aadc94..c7cd2124 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp @@ -38,37 +38,37 @@ #include #include "pcl_ros/features/normal_3d_omp.hpp" -void -pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::NormalEstimationOMP::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp index b76bf1aa..abb16d8b 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp @@ -40,42 +40,42 @@ #if defined HAVE_TBB -void -pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloud output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::NormalEstimationTBB::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet) #endif // HAVE_TBB - diff --git a/pcl_ros/src/pcl_ros/features/pfh.cpp b/pcl_ros/src/pcl_ros/features/pfh.cpp index b7210316..e55a7fea 100644 --- a/pcl_ros/src/pcl_ros/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/features/pfh.cpp @@ -38,39 +38,39 @@ #include #include "pcl_ros/features/pfh.hpp" -void -pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::PFHEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); - impl_.setInputNormals (pcl_ptr(normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(normals)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::PFHEstimation PFHEstimation; PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp index a5332a62..ff44b843 100644 --- a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp @@ -38,39 +38,39 @@ #include #include "pcl_ros/features/principal_curvatures.hpp" -void -pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::PrincipalCurvaturesEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); - impl_.setInputNormals (pcl_ptr(normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(normals)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/shot.cpp b/pcl_ros/src/pcl_ros/features/shot.cpp index eca60020..d0664724 100644 --- a/pcl_ros/src/pcl_ros/features/shot.cpp +++ b/pcl_ros/src/pcl_ros/features/shot.cpp @@ -37,39 +37,39 @@ #include #include "pcl_ros/features/shot.hpp" -void -pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::SHOTEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::SHOTEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); - impl_.setInputNormals (pcl_ptr(normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(normals)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::SHOTEstimation SHOTEstimation; PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/shot_omp.cpp b/pcl_ros/src/pcl_ros/features/shot_omp.cpp index 70ed3974..0d10be21 100644 --- a/pcl_ros/src/pcl_ros/features/shot_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/shot_omp.cpp @@ -37,39 +37,39 @@ #include #include "pcl_ros/features/shot_omp.hpp" -void -pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::SHOTEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::SHOTEstimationOMP::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); - impl_.setInputNormals (pcl_ptr(normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(normals)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP; PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/features/vfh.cpp b/pcl_ros/src/pcl_ros/features/vfh.cpp index c7cb6cc3..25c6911b 100644 --- a/pcl_ros/src/pcl_ros/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/features/vfh.cpp @@ -38,39 +38,39 @@ #include #include "pcl_ros/features/vfh.hpp" -void -pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } -void -pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::VFHEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Set the inputs - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices); - impl_.setSearchSurface (pcl_ptr(surface)); - impl_.setInputNormals (pcl_ptr(normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(normals)); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::VFHEstimation VFHEstimation; PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp index c7f1d517..83dec1eb 100644 --- a/pcl_ros/src/pcl_ros/filters/crop_box.cpp +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -1,5 +1,5 @@ /* - * + * * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. @@ -32,7 +32,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * $Id: cropbox.cpp + * $Id: cropbox.cpp * */ @@ -41,24 +41,25 @@ ////////////////////////////////////////////////////////////////////////////////////////////// bool -pcl_ros::CropBox::child_init (ros::NodeHandle &nh, bool &has_service) +pcl_ros::CropBox::child_init(ros::NodeHandle & nh, bool & has_service) { // Enable the dynamic reconfigure service has_service = true; - srv_ = boost::make_shared > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&CropBox::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &CropBox::config_callback, this, _1, _2); + srv_->setCallback(f); - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t level) +pcl_ros::CropBox::config_callback(pcl_ros::CropBoxConfig & config, uint32_t level) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - Eigen::Vector4f min_point,max_point; + Eigen::Vector4f min_point, max_point; min_point = impl_.getMin(); max_point = impl_.getMax(); @@ -67,49 +68,54 @@ pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t leve new_max_point << config.max_x, config.max_y, config.max_z, 0.0; // Check the current values for minimum point - if (min_point != new_min_point) - { - NODELET_DEBUG ("[%s::config_callback] Setting the minimum point to: %f %f %f.", getName ().c_str (), new_min_point(0),new_min_point(1),new_min_point(2)); + if (min_point != new_min_point) { + NODELET_DEBUG( + "[%s::config_callback] Setting the minimum point to: %f %f %f.", + getName().c_str(), new_min_point(0), new_min_point(1), new_min_point(2)); // Set the filter min point if different impl_.setMin(new_min_point); } - // Check the current values for the maximum point - if (max_point != new_max_point) - { - NODELET_DEBUG ("[%s::config_callback] Setting the maximum point to: %f %f %f.", getName ().c_str (), new_max_point(0),new_max_point(1),new_max_point(2)); + // Check the current values for the maximum point + if (max_point != new_max_point) { + NODELET_DEBUG( + "[%s::config_callback] Setting the maximum point to: %f %f %f.", + getName().c_str(), new_max_point(0), new_max_point(1), new_max_point(2)); // Set the filter max point if different impl_.setMax(new_max_point); } // Check the current value for keep_organized - if (impl_.getKeepOrganized () != config.keep_organized) - { - NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); + if (impl_.getKeepOrganized() != config.keep_organized) { + NODELET_DEBUG( + "[%s::config_callback] Setting the filter keep_organized value to: %s.", + getName().c_str(), config.keep_organized ? "true" : "false"); // Call the virtual method in the child - impl_.setKeepOrganized (config.keep_organized); + impl_.setKeepOrganized(config.keep_organized); } // Check the current value for the negative flag - if (impl_.getNegative() != config.negative) - { - NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.negative ? "true" : "false"); + if (impl_.getNegative() != config.negative) { + NODELET_DEBUG( + "[%s::config_callback] Setting the filter negative flag to: %s.", + getName().c_str(), config.negative ? "true" : "false"); // Call the virtual method in the child impl_.setNegative(config.negative); } // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL - if (tf_input_frame_ != config.input_frame) - { + if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; - NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); + NODELET_DEBUG( + "[%s::config_callback] Setting the input TF frame to: %s.", + getName().c_str(), tf_input_frame_.c_str()); } - if (tf_output_frame_ != config.output_frame) - { + if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; - NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); + NODELET_DEBUG( + "[%s::config_callback] Setting the output TF frame to: %s.", + getName().c_str(), tf_output_frame_.c_str()); } } typedef pcl_ros::CropBox CropBox; -PLUGINLIB_EXPORT_CLASS(CropBox,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp index 59554f0e..7ee29be0 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -39,32 +39,33 @@ #include "pcl_ros/filters/extract_indices.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::ExtractIndices::child_init (ros::NodeHandle &nh, bool &has_service) +bool +pcl_ros::ExtractIndices::child_init(ros::NodeHandle & nh, bool & has_service) { has_service = true; - srv_ = boost::make_shared > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&ExtractIndices::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &ExtractIndices::config_callback, this, _1, _2); + srv_->setCallback(f); use_indices_ = true; - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ExtractIndices::config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level) +pcl_ros::ExtractIndices::config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - if (impl_.getNegative () != config.negative) - { - impl_.setNegative (config.negative); - NODELET_DEBUG ("[%s::config_callback] Setting the extraction to: %s.", getName ().c_str (), (config.negative ? "indices" : "everything but the indices")); + if (impl_.getNegative() != config.negative) { + impl_.setNegative(config.negative); + NODELET_DEBUG( + "[%s::config_callback] Setting the extraction to: %s.", getName().c_str(), + (config.negative ? "indices" : "everything but the indices")); } } typedef pcl_ros::ExtractIndices ExtractIndices; -PLUGINLIB_EXPORT_CLASS(ExtractIndices,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(ExtractIndices, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp index 13a66c84..13a1139e 100644 --- a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp @@ -38,40 +38,41 @@ #include #include "pcl_ros/features/boundary.hpp" -void -pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::BoundaryEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); + impl_.setInputNormals(normals); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::BoundaryEstimation BoundaryEstimation; -PLUGINLIB_EXPORT_CLASS(BoundaryEstimation,nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/feature.cpp b/pcl_ros/src/pcl_ros/filters/features/feature.cpp index 1e36d828..85204ff9 100644 --- a/pcl_ros/src/pcl_ros/filters/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/feature.cpp @@ -53,192 +53,237 @@ //////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Feature::onInit () +pcl_ros::Feature::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Call the child init - childInit (*pnh_); + childInit(*pnh_); // Allow each individual class that inherits from us to declare their own Publisher // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters - if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) - { - NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { + NODELET_ERROR( + "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", + getName().c_str()); return; } - if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) - { - NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) { + NODELET_ERROR( + "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", + getName().c_str()); return; } // ---[ Optional parameters - pnh_->getParam ("use_surface", use_surface_); + pnh_->getParam("use_surface", use_surface_); // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &Feature::config_callback, this, _1, _2); + srv_->setCallback(f); // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages - if (use_indices_ || use_surface_) - { + if (use_indices_ || use_surface_) { // Create the objects here - if (approximate_sync_) - sync_input_surface_indices_a_ = boost::make_shared > >(max_queue_size_); - else - sync_input_surface_indices_e_ = boost::make_shared > >(max_queue_size_); + if (approximate_sync_) { + sync_input_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + } // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - if (use_indices_) - { + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + if (use_indices_) { // If indices are enabled, subscribe to the indices - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); - if (use_surface_) // Use both indices and surface - { + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + if (use_surface_) { // Use both indices and surface // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register - sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); - if (approximate_sync_) - sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); - else - sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); - } - else // Use only indices - { - sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); + sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); + if (approximate_sync_) { + sync_input_surface_indices_a_->connectInput( + sub_input_filter_, sub_surface_filter_, + sub_indices_filter_); + } else { + sync_input_surface_indices_e_->connectInput( + sub_input_filter_, sub_surface_filter_, + sub_indices_filter_); + } + } else { // Use only indices + sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1)); // surface not enabled, connect the input-indices duo and register - if (approximate_sync_) - sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); - else - sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); + if (approximate_sync_) { + sync_input_surface_indices_a_->connectInput( + sub_input_filter_, nf_pc_, + sub_indices_filter_); + } else { + sync_input_surface_indices_e_->connectInput( + sub_input_filter_, nf_pc_, + sub_indices_filter_); + } } - } - else // Use only surface - { - sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); + } else { // Use only surface + sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1)); // indices not enabled, connect the input-surface duo and register - sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); - if (approximate_sync_) - sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); - else - sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); + sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); + if (approximate_sync_) { + sync_input_surface_indices_a_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_); + } else { + sync_input_surface_indices_e_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_); + } } // Register callbacks - if (approximate_sync_) - sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); - else - sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); - } - else + if (approximate_sync_) { + sync_input_surface_indices_a_->registerCallback( + bind( + &Feature::input_surface_indices_callback, + this, _1, _2, _3)); + } else { + sync_input_surface_indices_e_->registerCallback( + bind( + &Feature::input_surface_indices_callback, + this, _1, _2, _3)); + } + } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); + sub_input_ = + pnh_->subscribe( + "input", max_queue_size_, + bind( + &Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(), + PointIndicesConstPtr())); + } - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName().c_str(), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); } //////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level) +pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level) { - if (k_ != config.k_search) - { + if (k_ != config.k_search) { k_ = config.k_search; - NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_); + NODELET_DEBUG( + "[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_); } - if (search_radius_ != config.radius_search) - { + if (search_radius_ != config.radius_search) { search_radius_ = config.radius_search; - NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_); + NODELET_DEBUG( + "[config_callback] Setting the nearest neighbors search radius for each point: %f.", + search_radius_); } } //////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) +pcl_ros::Feature::input_surface_indices_callback( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0) + if (pub_output_.getNumSubscribers() <= 0) { return; + } // If cloud is given, check if it's valid - if (!isValid (cloud)) - { - NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ()); - emptyPublish (cloud); + if (!isValid(cloud)) { + NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str()); + emptyPublish(cloud); return; } // If surface is given, check if it's valid - if (cloud_surface && !isValid (cloud_surface, "surface")) - { - NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ()); - emptyPublish (cloud); + if (cloud_surface && !isValid(cloud_surface, "surface")) { + NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str()); + emptyPublish(cloud); return; } - + // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ()); - emptyPublish (cloud); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str()); + emptyPublish(cloud); return; } /// DEBUG - if (cloud_surface) - if (indices) - NODELET_DEBUG ("[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ()); - - else - if (indices) - NODELET_DEBUG ("[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + if (cloud_surface) { + if (indices) { + NODELET_DEBUG( + "[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList( + *cloud_surface).c_str(), + cloud_surface->header.stamp.toSec(), + cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList( + *cloud_surface).c_str(), + cloud_surface->header.stamp.toSec(), + cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str()); + } + } else if (indices) { + NODELET_DEBUG( + "[input_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str()); + } /// - if ((int)(cloud->width * cloud->height) < k_) - { - NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height)); - emptyPublish (cloud); + if ((int)(cloud->width * cloud->height) < k_) { + NODELET_ERROR( + "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, + (int)(cloud->width * cloud->height)); + emptyPublish(cloud); return; } // If indices given... IndicesPtr vindices; - if (indices && !indices->header.frame_id.empty ()) - vindices.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + vindices.reset(new std::vector(indices->indices)); + } - computePublish (cloud, cloud_surface, vindices); + computePublish(cloud, cloud_surface, vindices); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -247,197 +292,264 @@ pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cl ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::FeatureFromNormals::onInit () +pcl_ros::FeatureFromNormals::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Call the child init - childInit (*pnh_); + childInit(*pnh_); // Allow each individual class that inherits from us to declare their own Publisher // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters - if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) - { - NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { + NODELET_ERROR( + "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", + getName().c_str()); return; } - if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) - { - NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) { + NODELET_ERROR( + "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", + getName().c_str()); return; } // ---[ Optional parameters - pnh_->getParam ("use_surface", use_surface_); + pnh_->getParam("use_surface", use_surface_); - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &FeatureFromNormals::config_callback, this, _1, _2); + srv_->setCallback(f); // Create the objects here - if (approximate_sync_) - sync_input_normals_surface_indices_a_ = boost::make_shared > > (max_queue_size_); - else - sync_input_normals_surface_indices_e_ = boost::make_shared > > (max_queue_size_); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_normals_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + } // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages - if (use_indices_ || use_surface_) - { - if (use_indices_) - { + if (use_indices_ || use_surface_) { + if (use_indices_) { // If indices are enabled, subscribe to the indices - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); - if (use_surface_) // Use both indices and surface - { + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + if (use_surface_) { // Use both indices and surface // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register - sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); - if (approximate_sync_) - sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); - else - sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); - } - else // Use only indices - { - sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); - if (approximate_sync_) + sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_->connectInput( + sub_input_filter_, + sub_normals_filter_, + sub_surface_filter_, + sub_indices_filter_); + } else { + sync_input_normals_surface_indices_e_->connectInput( + sub_input_filter_, + sub_normals_filter_, + sub_surface_filter_, + sub_indices_filter_); + } + } else { // Use only indices + sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1)); + if (approximate_sync_) { // surface not enabled, connect the input-indices duo and register - sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); - else + sync_input_normals_surface_indices_a_->connectInput( + sub_input_filter_, + sub_normals_filter_, nf_pc_, + sub_indices_filter_); + } else { // surface not enabled, connect the input-indices duo and register - sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); + sync_input_normals_surface_indices_e_->connectInput( + sub_input_filter_, + sub_normals_filter_, nf_pc_, + sub_indices_filter_); + } } - } - else // Use only surface - { + } else { // Use only surface // indices not enabled, connect the input-surface duo and register - sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); + sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); - sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); - if (approximate_sync_) - sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); - else - sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); + sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1)); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_->connectInput( + sub_input_filter_, sub_normals_filter_, + sub_surface_filter_, nf_pi_); + } else { + sync_input_normals_surface_indices_e_->connectInput( + sub_input_filter_, sub_normals_filter_, + sub_surface_filter_, nf_pi_); + } } - } - else - { - sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); + } else { + sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1)); - if (approximate_sync_) - sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); - else - sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_->connectInput( + sub_input_filter_, sub_normals_filter_, + nf_pc_, nf_pi_); + } else { + sync_input_normals_surface_indices_e_->connectInput( + sub_input_filter_, sub_normals_filter_, + nf_pc_, nf_pi_); + } } // Register callbacks - if (approximate_sync_) - sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); - else - sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_->registerCallback( + bind( + &FeatureFromNormals:: + input_normals_surface_indices_callback, this, _1, _2, _3, _4)); + } else { + sync_input_normals_surface_indices_e_->registerCallback( + bind( + &FeatureFromNormals:: + input_normals_surface_indices_callback, this, _1, _2, _3, _4)); + } - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName().c_str(), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( - const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, - const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) +pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( + const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals, + const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0) + if (pub_output_.getNumSubscribers() <= 0) { return; + } // If cloud+normals is given, check if it's valid - if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) - { - NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ()); - emptyPublish (cloud); + if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals")) + NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str()); + emptyPublish(cloud); return; } // If surface is given, check if it's valid - if (cloud_surface && !isValid (cloud_surface, "surface")) - { - NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ()); - emptyPublish (cloud); + if (cloud_surface && !isValid(cloud_surface, "surface")) { + NODELET_ERROR( + "[%s::input_normals_surface_indices_callback] Invalid input surface!", + getName().c_str()); + emptyPublish(cloud); return; } - + // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ()); - emptyPublish (cloud); + if (indices && !isValid(indices)) { + NODELET_ERROR( + "[%s::input_normals_surface_indices_callback] Invalid input indices!", + getName().c_str()); + emptyPublish(cloud); return; } /// DEBUG - if (cloud_surface) - if (indices) - NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); - else - if (indices) - NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + if (cloud_surface) { + if (indices) { + NODELET_DEBUG( + "[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList( + *cloud_surface).c_str(), + cloud_surface->header.stamp.toSec(), + cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), + cloud_normals->header.stamp.toSec(), + cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_surface->width * cloud_surface->height, pcl::getFieldsList( + *cloud_surface).c_str(), + cloud_surface->header.stamp.toSec(), + cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), + cloud_normals->header.stamp.toSec(), + cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); + } + } else if (indices) { + NODELET_DEBUG( + "[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), + cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName( + "normals").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_normals_surface_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), + cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName( + "normals").c_str()); + } /// - if ((int)(cloud->width * cloud->height) < k_) - { - NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height)); - emptyPublish (cloud); + if ((int)(cloud->width * cloud->height) < k_) { + NODELET_ERROR( + "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", + getName().c_str(), k_, (int)(cloud->width * cloud->height)); + emptyPublish(cloud); return; } // If indices given... IndicesPtr vindices; - if (indices && !indices->header.frame_id.empty ()) - vindices.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + vindices.reset(new std::vector(indices->indices)); + } - computePublish (cloud, cloud_normals, cloud_surface, vindices); + computePublish(cloud, cloud_normals, cloud_surface, vindices); } - diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp index 728f0d50..dac0f28f 100644 --- a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp @@ -38,42 +38,42 @@ #include #include "pcl_ros/features/fpfh.hpp" -void -pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::FPFHEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); + impl_.setInputNormals(normals); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::FPFHEstimation FPFHEstimation; -PLUGINLIB_EXPORT_CLASS(FPFHEstimation,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp index 822817a2..80cdde16 100644 --- a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp @@ -38,42 +38,42 @@ #include #include "pcl_ros/features/fpfh_omp.hpp" -void -pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::FPFHEstimationOMP::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); + impl_.setInputNormals(normals); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; -PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp index d8be14ac..7391856b 100644 --- a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp @@ -38,40 +38,40 @@ #include #include "pcl_ros/features/moment_invariants.hpp" -void -pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::MomentInvariantsEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; -PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp index 4d62f381..1e393358 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp @@ -38,40 +38,40 @@ #include #include "pcl_ros/features/normal_3d.hpp" -void -pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::NormalEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::NormalEstimation NormalEstimation; -PLUGINLIB_EXPORT_CLASS(NormalEstimation,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp index c64e85bd..688782ba 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp @@ -38,40 +38,40 @@ #include #include "pcl_ros/features/normal_3d_omp.hpp" -void -pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::NormalEstimationOMP::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; -PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp index 5874eea8..72f94c7f 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp @@ -40,42 +40,42 @@ #if defined HAVE_TBB -void -pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloud output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::NormalEstimationTBB::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; -PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB,nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet); #endif // HAVE_TBB - diff --git a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp index 01b1c8f2..12f4a7c5 100644 --- a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp @@ -38,42 +38,42 @@ #include #include "pcl_ros/features/pfh.hpp" -void -pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::PFHEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); + impl_.setInputNormals(normals); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::PFHEstimation PFHEstimation; -PLUGINLIB_EXPORT_CLASS(PFHEstimation,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp index cb7ce548..0d5fa4e6 100644 --- a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp @@ -38,42 +38,42 @@ #include #include "pcl_ros/features/principal_curvatures.hpp" -void -pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::PrincipalCurvaturesEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); + impl_.setInputNormals(normals); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; -PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp index 5f77d67a..eb2f58cf 100644 --- a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp @@ -38,42 +38,42 @@ #include #include "pcl_ros/features/vfh.hpp" -void -pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +void +pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) { PointCloudOut output; output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } -void -pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, - const PointCloudNConstPtr &normals, - const PointCloudInConstPtr &surface, - const IndicesPtr &indices) +void +pcl_ros::VFHEstimation::computePublish( + const PointCloudInConstPtr & cloud, + const PointCloudNConstPtr & normals, + const PointCloudInConstPtr & surface, + const IndicesPtr & indices) { // Set the parameters - impl_.setKSearch (k_); - impl_.setRadiusSearch (search_radius_); + impl_.setKSearch(k_); + impl_.setRadiusSearch(search_radius_); // Initialize the spatial locator - initTree (spatial_locator_type_, tree_, k_); - impl_.setSearchMethod (tree_); + initTree(spatial_locator_type_, tree_, k_); + impl_.setSearchMethod(tree_); // Set the inputs - impl_.setInputCloud (cloud); - impl_.setIndices (indices); - impl_.setSearchSurface (surface); - impl_.setInputNormals (normals); + impl_.setInputCloud(cloud); + impl_.setIndices(indices); + impl_.setSearchSurface(surface); + impl_.setInputNormals(normals); // Estimate the feature PointCloudOut output; - impl_.compute (output); + impl_.compute(output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (output.makeShared ()); + pub_output_.publish(output.makeShared()); } typedef pcl_ros::VFHEstimation VFHEstimation; -PLUGINLIB_EXPORT_CLASS(VFHEstimation,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 2970504a..dfa6eef5 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -61,45 +61,52 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices) +pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices) { PointCloud2 output; // Call the virtual method in the child - filter (input, indices, output); + filter(input, indices, output); - PointCloud2::Ptr cloud_tf (new PointCloud2 (output)); // set the output by default + PointCloud2::Ptr cloud_tf(new PointCloud2(output)); // set the output by default // Check whether the user has given a different output TF frame - if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_) - { - NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ()); + if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) { + NODELET_DEBUG( + "[%s::computePublish] Transforming output dataset from %s to %s.", + getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); // Convert the cloud into the different frame PointCloud2 cloud_transformed; - if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_)) - { - NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ()); + if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_listener_)) { + NODELET_ERROR( + "[%s::computePublish] Error converting output dataset from %s to %s.", + getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); return; } - cloud_tf.reset (new PointCloud2 (cloud_transformed)); + cloud_tf.reset(new PointCloud2(cloud_transformed)); } - if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_) - // no tf_output_frame given, transform the dataset to its original frame - { - NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ()); + if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) { + // no tf_output_frame given, transform the dataset to its original frame + NODELET_DEBUG( + "[%s::computePublish] Transforming output dataset from %s back to %s.", + getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); // Convert the cloud into the different frame PointCloud2 cloud_transformed; - if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_)) + if (!pcl_ros::transformPointCloud( + tf_input_orig_frame_, output, cloud_transformed, + tf_listener_)) { - NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ()); + NODELET_ERROR( + "[%s::computePublish] Error converting output dataset from %s back to %s.", + getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); return; } - cloud_tf.reset (new PointCloud2 (cloud_transformed)); + cloud_tf.reset(new PointCloud2(cloud_transformed)); } // Copy timestamp to keep it cloud_tf->header.stamp = input->header.stamp; // Publish a boost shared ptr - pub_output_.publish (cloud_tf); + pub_output_.publish(cloud_tf); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -107,141 +114,152 @@ void pcl_ros::Filter::subscribe() { // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { + if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - if (approximate_sync_) - { - sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); - sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); + if (approximate_sync_) { + sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); + } else { + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); } - else - { - sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); - } - } - else + } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ())); + sub_input_ = + pnh_->subscribe( + "input", max_queue_size_, + bind(&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr())); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::unsubscribe() { - if (use_indices_) - { + if (use_indices_) { sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); - } - else + } else { sub_input_.shutdown(); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::onInit () +pcl_ros::Filter::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Call the child's local init bool has_service = false; - if (!child_init (*pnh_, has_service)) - { - NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ()); + if (!child_init(*pnh_, has_service)) { + NODELET_ERROR("[%s::onInit] Initialization failed.", getName().c_str()); return; } - pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*pnh_, "output", max_queue_size_); // Enable the dynamic reconfigure service - if (!has_service) - { - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2); - srv_->setCallback (f); + if (!has_service) { + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &Filter::config_callback, this, _1, _2); + srv_->setCallback(f); } - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ()); + NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str()); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level) +pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level) { // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL - if (tf_input_frame_ != config.input_frame) - { + if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; - NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); + NODELET_DEBUG( + "[%s::config_callback] Setting the input TF frame to: %s.", + getName().c_str(), tf_input_frame_.c_str()); } - if (tf_output_frame_ != config.output_frame) - { + if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; - NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); + NODELET_DEBUG( + "[%s::config_callback] Setting the output TF frame to: %s.", + getName().c_str(), tf_output_frame_.c_str()); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices) +pcl_ros::Filter::input_indices_callback( + const PointCloud2::ConstPtr & cloud, + const PointIndicesConstPtr & indices) { // If cloud is given, check if it's valid - if (!isValid (cloud)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + if (!isValid(cloud)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); return; } // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); return; } /// DEBUG - if (indices) - NODELET_DEBUG ("[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + if (indices) { + NODELET_DEBUG( + "[%s::input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", + getName().c_str(), cloud->width * cloud->height, + cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str()); + } /// // Check whether the user has given a different input TF frame tf_input_orig_frame_ = cloud->header.frame_id; PointCloud2::ConstPtr cloud_tf; - if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) - { - NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); + if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { + NODELET_DEBUG( + "[%s::input_indices_callback] Transforming input dataset from %s to %s.", + getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); // Save the original frame ID // Convert the cloud into the different frame PointCloud2 cloud_transformed; - if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) - { - NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); + if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) { + NODELET_ERROR( + "[%s::input_indices_callback] Error converting input dataset from %s to %s.", + getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); return; } - cloud_tf = boost::make_shared (cloud_transformed); - } - else + cloud_tf = boost::make_shared(cloud_transformed); + } else { cloud_tf = cloud; + } // Need setInputCloud () here because we have to extract x/y/z IndicesPtr vindices; - if (indices) - vindices.reset (new std::vector (indices->indices)); + if (indices) { + vindices.reset(new std::vector(indices->indices)); + } - computePublish (cloud_tf, vindices); + computePublish(cloud_tf, vindices); } - diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index 693442e8..f03ec564 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -40,81 +40,88 @@ ////////////////////////////////////////////////////////////////////////////////////////////// bool -pcl_ros::PassThrough::child_init (ros::NodeHandle &nh, bool &has_service) +pcl_ros::PassThrough::child_init(ros::NodeHandle & nh, bool & has_service) { // Enable the dynamic reconfigure service has_service = true; - srv_ = boost::make_shared > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &PassThrough::config_callback, this, _1, _2); + srv_->setCallback(f); - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t level) +pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t level) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); double filter_min, filter_max; - impl_.getFilterLimits (filter_min, filter_max); + impl_.getFilterLimits(filter_min, filter_max); // Check the current values for filter min-max - if (filter_min != config.filter_limit_min) - { + if (filter_min != config.filter_limit_min) { filter_min = config.filter_limit_min; - NODELET_DEBUG ("[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_min); + NODELET_DEBUG( + "[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", + getName().c_str(), filter_min); // Set the filter min-max if different - impl_.setFilterLimits (filter_min, filter_max); + impl_.setFilterLimits(filter_min, filter_max); } // Check the current values for filter min-max - if (filter_max != config.filter_limit_max) - { + if (filter_max != config.filter_limit_max) { filter_max = config.filter_limit_max; - NODELET_DEBUG ("[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_max); + NODELET_DEBUG( + "[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", + getName().c_str(), filter_max); // Set the filter min-max if different - impl_.setFilterLimits (filter_min, filter_max); + impl_.setFilterLimits(filter_min, filter_max); } // Check the current value for the filter field //std::string filter_field = impl_.getFilterFieldName (); - if (impl_.getFilterFieldName () != config.filter_field_name) - { + if (impl_.getFilterFieldName() != config.filter_field_name) { // Set the filter field if different - impl_.setFilterFieldName (config.filter_field_name); - NODELET_DEBUG ("[%s::config_callback] Setting the filter field name to: %s.", getName ().c_str (), config.filter_field_name.c_str ()); + impl_.setFilterFieldName(config.filter_field_name); + NODELET_DEBUG( + "[%s::config_callback] Setting the filter field name to: %s.", + getName().c_str(), config.filter_field_name.c_str()); } // Check the current value for keep_organized - if (impl_.getKeepOrganized () != config.keep_organized) - { - NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); + if (impl_.getKeepOrganized() != config.keep_organized) { + NODELET_DEBUG( + "[%s::config_callback] Setting the filter keep_organized value to: %s.", + getName().c_str(), config.keep_organized ? "true" : "false"); // Call the virtual method in the child - impl_.setKeepOrganized (config.keep_organized); + impl_.setKeepOrganized(config.keep_organized); } // Check the current value for the negative flag - if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) - { - NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); + if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) { + NODELET_DEBUG( + "[%s::config_callback] Setting the filter negative flag to: %s.", + getName().c_str(), config.filter_limit_negative ? "true" : "false"); // Call the virtual method in the child - impl_.setFilterLimitsNegative (config.filter_limit_negative); + impl_.setFilterLimitsNegative(config.filter_limit_negative); } // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL - if (tf_input_frame_ != config.input_frame) - { + if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; - NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); + NODELET_DEBUG( + "[%s::config_callback] Setting the input TF frame to: %s.", + getName().c_str(), tf_input_frame_.c_str()); } - if (tf_output_frame_ != config.output_frame) - { + if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; - NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); + NODELET_DEBUG( + "[%s::config_callback] Setting the output TF frame to: %s.", + getName().c_str(), tf_output_frame_.c_str()); } } typedef pcl_ros::PassThrough PassThrough; -PLUGINLIB_EXPORT_CLASS(PassThrough,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(PassThrough, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index 2065f415..f1b8525c 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -41,122 +41,134 @@ ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ProjectInliers::onInit () +pcl_ros::ProjectInliers::onInit() { - PCLNodelet::onInit (); + PCLNodelet::onInit(); // ---[ Mandatory parameters // The type of model to use (user given parameter). int model_type; - if (!pnh_->getParam ("model_type", model_type)) - { - NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("model_type", model_type)) { + NODELET_ERROR( + "[%s::onInit] Need a 'model_type' parameter to be set before continuing!", + getName().c_str()); return; } // ---[ Optional parameters // True if all data will be returned, false if only the projected inliers. Default: false. bool copy_all_data = false; - // True if all fields will be returned, false if only XYZ. Default: true. + // True if all fields will be returned, false if only XYZ. Default: true. bool copy_all_fields = true; - pnh_->getParam ("copy_all_data", copy_all_data); - pnh_->getParam ("copy_all_fields", copy_all_fields); + pnh_->getParam("copy_all_data", copy_all_data); + pnh_->getParam("copy_all_fields", copy_all_fields); - pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*pnh_, "output", max_queue_size_); // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - copy_all_data : %s\n" - " - copy_all_fields : %s", - getName ().c_str (), - model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - copy_all_data : %s\n" + " - copy_all_fields : %s", + getName().c_str(), + model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); // Set given parameters here - impl_.setModelType (model_type); - impl_.setCopyAllFields (copy_all_fields); - impl_.setCopyAllData (copy_all_data); + impl_.setModelType(model_type); + impl_.setCopyAllFields(copy_all_fields); + impl_.setCopyAllData(copy_all_data); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ProjectInliers::subscribe () +pcl_ros::ProjectInliers::subscribe() { /* TODO : implement use_indices_ if (use_indices_) {*/ - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - sub_model_.subscribe (*pnh_, "model", max_queue_size_); + sub_model_.subscribe(*pnh_, "model", max_queue_size_); - if (approximate_sync_) - { - sync_input_indices_model_a_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); - sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); - } - else - { - sync_input_indices_model_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); - sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); + if (approximate_sync_) { + sync_input_indices_model_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); + sync_input_indices_model_a_->registerCallback( + bind( + &ProjectInliers::input_indices_model_callback, + this, _1, _2, _3)); + } else { + sync_input_indices_model_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); + sync_input_indices_model_e_->registerCallback( + bind( + &ProjectInliers::input_indices_model_callback, + this, _1, _2, _3)); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ProjectInliers::unsubscribe () +pcl_ros::ProjectInliers::unsubscribe() { /* TODO : implement use_indices_ if (use_indices_) {*/ - sub_input_filter_.unsubscribe (); - sub_model_.unsubscribe (); + sub_input_filter_.unsubscribe(); + sub_model_.unsubscribe(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud, - const PointIndicesConstPtr &indices, - const ModelCoefficientsConstPtr &model) +pcl_ros::ProjectInliers::input_indices_model_callback( + const PointCloud2::ConstPtr & cloud, + const PointIndicesConstPtr & indices, + const ModelCoefficientsConstPtr & model) { - if (pub_output_.getNumSubscribers () <= 0) - return; - - if (!isValid (model) || !isValid (indices) || !isValid (cloud)) - { - NODELET_ERROR ("[%s::input_indices_model_callback] Invalid input!", getName ().c_str ()); + if (pub_output_.getNumSubscribers() <= 0) { return; } - NODELET_DEBUG ("[%s::input_indices_model_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" - " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (), - model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ()); + if (!isValid(model) || !isValid(indices) || !isValid(cloud)) { + NODELET_ERROR("[%s::input_indices_model_callback] Invalid input!", getName().c_str()); + return; + } + + NODELET_DEBUG( + "[%s::input_indices_model_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" + " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("inliers").c_str(), + model->values.size(), model->header.stamp.toSec(), + model->header.frame_id.c_str(), pnh_->resolveName("model").c_str()); tf_input_orig_frame_ = cloud->header.frame_id; IndicesPtr vindices; - if (indices) - vindices.reset (new std::vector (indices->indices)); + if (indices) { + vindices.reset(new std::vector(indices->indices)); + } - model_ = model; - computePublish (cloud, vindices); + model_ = model; + computePublish(cloud, vindices); } typedef pcl_ros::ProjectInliers ProjectInliers; -PLUGINLIB_EXPORT_CLASS(ProjectInliers,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp index 34e0240a..2125a669 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -40,38 +40,42 @@ ////////////////////////////////////////////////////////////////////////////////////////////// bool -pcl_ros::RadiusOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) +pcl_ros::RadiusOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service) { // Enable the dynamic reconfigure service has_service = true; - srv_ = boost::make_shared > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&RadiusOutlierRemoval::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &RadiusOutlierRemoval::config_callback, this, _1, _2); + srv_->setCallback(f); - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::RadiusOutlierRemoval::config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level) +pcl_ros::RadiusOutlierRemoval::config_callback( + pcl_ros::RadiusOutlierRemovalConfig & config, + uint32_t level) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - if (impl_.getMinNeighborsInRadius () != config.min_neighbors) - { - impl_.setMinNeighborsInRadius (config.min_neighbors); - NODELET_DEBUG ("[%s::config_callback] Setting the number of neighbors in radius: %d.", getName ().c_str (), config.min_neighbors); + if (impl_.getMinNeighborsInRadius() != config.min_neighbors) { + impl_.setMinNeighborsInRadius(config.min_neighbors); + NODELET_DEBUG( + "[%s::config_callback] Setting the number of neighbors in radius: %d.", + getName().c_str(), config.min_neighbors); } - if (impl_.getRadiusSearch () != config.radius_search) - { - impl_.setRadiusSearch (config.radius_search); - NODELET_DEBUG ("[%s::config_callback] Setting the radius to search neighbors: %f.", getName ().c_str (), config.radius_search); + if (impl_.getRadiusSearch() != config.radius_search) { + impl_.setRadiusSearch(config.radius_search); + NODELET_DEBUG( + "[%s::config_callback] Setting the radius to search neighbors: %f.", + getName().c_str(), config.radius_search); } - + } typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; -PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp index 282419c6..00803e82 100644 --- a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -40,42 +40,47 @@ ////////////////////////////////////////////////////////////////////////////////////////////// bool -pcl_ros::StatisticalOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) +pcl_ros::StatisticalOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service) { // Enable the dynamic reconfigure service has_service = true; - srv_ = boost::make_shared > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&StatisticalOutlierRemoval::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>( + nh); + dynamic_reconfigure::Server::CallbackType f = + boost::bind(&StatisticalOutlierRemoval::config_callback, this, _1, _2); + srv_->setCallback(f); - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level) +pcl_ros::StatisticalOutlierRemoval::config_callback( + pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - if (impl_.getMeanK () != config.mean_k) - { - impl_.setMeanK (config.mean_k); - NODELET_DEBUG ("[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", getName ().c_str (), config.mean_k); - } - - if (impl_.getStddevMulThresh () != config.stddev) - { - impl_.setStddevMulThresh (config.stddev); - NODELET_DEBUG ("[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", getName ().c_str (), config.stddev); + if (impl_.getMeanK() != config.mean_k) { + impl_.setMeanK(config.mean_k); + NODELET_DEBUG( + "[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", + getName().c_str(), config.mean_k); } - if (impl_.getNegative () != config.negative) - { - impl_.setNegative (config.negative); - NODELET_DEBUG ("[%s::config_callback] Returning only inliers: %s.", getName ().c_str (), config.negative ? "false" : "true"); + if (impl_.getStddevMulThresh() != config.stddev) { + impl_.setStddevMulThresh(config.stddev); + NODELET_DEBUG( + "[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", + getName().c_str(), config.stddev); + } + + if (impl_.getNegative() != config.negative) { + impl_.setNegative(config.negative); + NODELET_DEBUG( + "[%s::config_callback] Returning only inliers: %s.", + getName().c_str(), config.negative ? "false" : "true"); } } typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval; -PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index be00eaa5..37c3adbd 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -40,88 +40,92 @@ ////////////////////////////////////////////////////////////////////////////////////////////// bool -pcl_ros::VoxelGrid::child_init (ros::NodeHandle &nh, bool &has_service) +pcl_ros::VoxelGrid::child_init(ros::NodeHandle & nh, bool & has_service) { // Enable the dynamic reconfigure service has_service = true; - srv_ = boost::make_shared > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &VoxelGrid::config_callback, this, _1, _2); + srv_->setCallback(f); - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, - const IndicesPtr &indices, - PointCloud2 &output) +pcl_ros::VoxelGrid::filter( + const PointCloud2::ConstPtr & input, + const IndicesPtr & indices, + PointCloud2 & output) { - boost::mutex::scoped_lock lock (mutex_); + 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_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); pcl::PCLPointCloud2 pcl_output; - impl_.filter (pcl_output); + impl_.filter(pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level) +pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - Eigen::Vector3f leaf_size = impl_.getLeafSize (); + Eigen::Vector3f leaf_size = impl_.getLeafSize(); - if (leaf_size[0] != config.leaf_size) - { - leaf_size.setConstant (config.leaf_size); - NODELET_DEBUG ("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]); - impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]); + if (leaf_size[0] != config.leaf_size) { + leaf_size.setConstant(config.leaf_size); + NODELET_DEBUG("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]); + impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]); } double filter_min, filter_max; - impl_.getFilterLimits (filter_min, filter_max); - if (filter_min != config.filter_limit_min) - { + impl_.getFilterLimits(filter_min, filter_max); + if (filter_min != config.filter_limit_min) { filter_min = config.filter_limit_min; - NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min); + NODELET_DEBUG( + "[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", + filter_min); } - if (filter_max != config.filter_limit_max) - { + if (filter_max != config.filter_limit_max) { filter_max = config.filter_limit_max; - NODELET_DEBUG ("[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max); + NODELET_DEBUG( + "[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", + filter_max); } - impl_.setFilterLimits (filter_min, filter_max); + impl_.setFilterLimits(filter_min, filter_max); - if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) - { - impl_.setFilterLimitsNegative (config.filter_limit_negative); - NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); + if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) { + impl_.setFilterLimitsNegative(config.filter_limit_negative); + NODELET_DEBUG( + "[%s::config_callback] Setting the filter negative flag to: %s.", + getName().c_str(), config.filter_limit_negative ? "true" : "false"); } - if (impl_.getFilterFieldName () != config.filter_field_name) - { - impl_.setFilterFieldName (config.filter_field_name); - NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ()); + if (impl_.getFilterFieldName() != config.filter_field_name) { + impl_.setFilterFieldName(config.filter_field_name); + NODELET_DEBUG( + "[config_callback] Setting the filter field name to: %s.", + config.filter_field_name.c_str()); } // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter - if (tf_input_frame_ != config.input_frame) - { + if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; - NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ()); + NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); } - if (tf_output_frame_ != config.output_frame) - { + if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; - NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ()); + NODELET_DEBUG( + "[config_callback] Setting the output TF frame to: %s.", + tf_output_frame_.c_str()); } // ]--- } typedef pcl_ros::VoxelGrid VoxelGrid; -PLUGINLIB_EXPORT_CLASS(VoxelGrid,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/bag_io.cpp b/pcl_ros/src/pcl_ros/io/bag_io.cpp index 12930fc2..e0a62e86 100644 --- a/pcl_ros/src/pcl_ros/io/bag_io.cpp +++ b/pcl_ros/src/pcl_ros/io/bag_io.cpp @@ -39,75 +39,74 @@ #include "pcl_ros/io/bag_io.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name) +bool +pcl_ros::BAGReader::open(const std::string & file_name, const std::string & topic_name) { - try - { - bag_.open (file_name, rosbag::bagmode::Read); - view_.addQuery (bag_, rosbag::TopicQuery (topic_name)); + try { + bag_.open(file_name, rosbag::bagmode::Read); + view_.addQuery(bag_, rosbag::TopicQuery(topic_name)); - if (view_.size () == 0) - return (false); + if (view_.size() == 0) { + return false; + } - it_ = view_.begin (); + it_ = view_.begin(); + } catch (rosbag::BagException & e) { + return false; } - catch (rosbag::BagException &e) - { - return (false); - } - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::BAGReader::onInit () +pcl_ros::BAGReader::onInit() { boost::shared_ptr pnh_; - pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); + pnh_.reset(new ros::NodeHandle(getMTPrivateNodeHandle())); // ---[ Mandatory parameters - if (!pnh_->getParam ("file_name", file_name_)) - { - NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!"); + if (!pnh_->getParam("file_name", file_name_)) { + NODELET_ERROR("[onInit] Need a 'file_name' parameter to be set before continuing!"); return; } - if (!pnh_->getParam ("topic_name", topic_name_)) - { - NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!"); + if (!pnh_->getParam("topic_name", topic_name_)) { + NODELET_ERROR("[onInit] Need a 'topic_name' parameter to be set before continuing!"); return; } // ---[ Optional parameters int max_queue_size = 1; - pnh_->getParam ("publish_rate", publish_rate_); - pnh_->getParam ("max_queue_size", max_queue_size); + pnh_->getParam("publish_rate", publish_rate_); + pnh_->getParam("max_queue_size", max_queue_size); - ros::Publisher pub_output = pnh_->advertise ("output", max_queue_size); + ros::Publisher pub_output = pnh_->advertise("output", max_queue_size); - NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n" - " - file_name : %s\n" - " - topic_name : %s", - file_name_.c_str (), topic_name_.c_str ()); + NODELET_DEBUG( + "[onInit] Nodelet successfully created with the following parameters:\n" + " - file_name : %s\n" + " - topic_name : %s", + file_name_.c_str(), topic_name_.c_str()); - if (!open (file_name_, topic_name_)) + if (!open(file_name_, topic_name_)) { return; + } PointCloud output; - output_ = boost::make_shared (output); - output_->header.stamp = ros::Time::now (); + output_ = boost::make_shared(output); + output_->header.stamp = ros::Time::now(); // Continous publishing enabled? - while (pnh_->ok ()) - { - PointCloudConstPtr cloud = getNextCloud (); - NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ()); - output_->header.stamp = ros::Time::now (); + while (pnh_->ok()) { + PointCloudConstPtr cloud = getNextCloud(); + NODELET_DEBUG( + "Publishing data (%d points) on topic %s in frame %s.", + output_->width * output_->height, pnh_->resolveName( + "output").c_str(), output_->header.frame_id.c_str()); + output_->header.stamp = ros::Time::now(); - pub_output.publish (output_); + pub_output.publish(output_); - ros::Duration (publish_rate_).sleep (); - ros::spinOnce (); + ros::Duration(publish_rate_).sleep(); + ros::spinOnce(); } } typedef pcl_ros::BAGReader BAGReader; -PLUGINLIB_EXPORT_CLASS(BAGReader,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index 13027d51..00cea9d9 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -44,226 +44,260 @@ ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () +pcl_ros::PointCloudConcatenateDataSynchronizer::onInit() { - nodelet_topic_tools::NodeletLazy::onInit (); + nodelet_topic_tools::NodeletLazy::onInit(); // ---[ Mandatory parameters - pnh_->getParam ("output_frame", output_frame_); - pnh_->getParam ("approximate_sync", approximate_sync_); + pnh_->getParam("output_frame", output_frame_); + pnh_->getParam("approximate_sync", approximate_sync_); - if (output_frame_.empty ()) - { - NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!"); + if (output_frame_.empty()) { + NODELET_ERROR("[onInit] Need an 'output_frame' parameter to be set before continuing!"); return; } - if (!pnh_->getParam ("input_topics", input_topics_)) - { - NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!"); + if (!pnh_->getParam("input_topics", input_topics_)) { + NODELET_ERROR("[onInit] Need a 'input_topics' parameter to be set before continuing!"); return; } - if (input_topics_.getType () != XmlRpc::XmlRpcValue::TypeArray) - { - NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!"); + if (input_topics_.getType() != XmlRpc::XmlRpcValue::TypeArray) { + NODELET_ERROR("[onInit] Invalid 'input_topics' parameter given!"); return; } - if (input_topics_.size () == 1) - { - NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue."); + if (input_topics_.size() == 1) { + NODELET_ERROR("[onInit] Only one topic given. Need at least two topics to continue."); return; } - if (input_topics_.size () > 8) - { - NODELET_ERROR ("[onInit] More than 8 topics passed!"); + if (input_topics_.size() > 8) { + NODELET_ERROR("[onInit] More than 8 topics passed!"); return; } // ---[ Optional parameters - pnh_->getParam ("max_queue_size", maximum_queue_size_); + pnh_->getParam("max_queue_size", maximum_queue_size_); // Output - pub_output_ = advertise (*pnh_, "output", maximum_queue_size_); + pub_output_ = advertise(*pnh_, "output", maximum_queue_size_); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe () +pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe() { - ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:"); - for (int d = 0; d < input_topics_.size (); ++d) - ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d])); + ROS_INFO_STREAM("Subscribing to " << input_topics_.size() << " user given topics as inputs:"); + for (int d = 0; d < input_topics_.size(); ++d) { + ROS_INFO_STREAM(" - " << (std::string)(input_topics_[d])); + } // Subscribe to the filters - filters_.resize (input_topics_.size ()); + filters_.resize(input_topics_.size()); // 8 topics - if (approximate_sync_) - ts_a_.reset (new message_filters::Synchronizer - > (maximum_queue_size_)); - else - ts_e_.reset (new message_filters::Synchronizer - > (maximum_queue_size_)); + if (approximate_sync_) { + ts_a_.reset( + new message_filters::Synchronizer + >(maximum_queue_size_)); + } else { + ts_e_.reset( + new message_filters::Synchronizer + >(maximum_queue_size_)); + } // First input_topics_.size () filters are valid - for (int d = 0; d < input_topics_.size (); ++d) - { - filters_[d].reset (new message_filters::Subscriber ()); - filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_); + for (int d = 0; d < input_topics_.size(); ++d) { + filters_[d].reset(new message_filters::Subscriber()); + filters_[d]->subscribe(*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_); } // Bogus null filter - filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1)); + filters_[0]->registerCallback( + bind( + &PointCloudConcatenateDataSynchronizer::input_callback, this, + _1)); - switch (input_topics_.size ()) - { + switch (input_topics_.size()) { case 2: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); - break; - } + { + if (approximate_sync_) { + ts_a_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); + } else { + ts_e_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); + } + break; + } case 3: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); - break; - } + { + if (approximate_sync_) { + ts_a_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); + } else { + ts_e_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); + } + break; + } case 4: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); - break; - } + { + if (approximate_sync_) { + ts_a_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, + nf_); + } else { + ts_e_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, + nf_); + } + break; + } case 5: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); - break; - } + { + if (approximate_sync_) { + ts_a_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], + nf_, nf_, nf_); + } else { + ts_e_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], + nf_, nf_, nf_); + } + break; + } case 6: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); - break; - } + { + if (approximate_sync_) { + ts_a_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], + *filters_[5], nf_, nf_); + } else { + ts_e_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], + *filters_[5], nf_, nf_); + } + break; + } case 7: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); - break; - } + { + if (approximate_sync_) { + ts_a_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], + *filters_[5], *filters_[6], nf_); + } else { + ts_e_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], + *filters_[5], *filters_[6], nf_); + } + break; + } case 8: - { - if (approximate_sync_) - ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); - else - ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); - break; - } + { + if (approximate_sync_) { + ts_a_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], + *filters_[5], *filters_[6], *filters_[7]); + } else { + ts_e_->connectInput( + *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], + *filters_[5], *filters_[6], *filters_[7]); + } + break; + } default: - { - NODELET_FATAL ("Invalid 'input_topics' parameter given!"); - return; - } + { + NODELET_FATAL("Invalid 'input_topics' parameter given!"); + return; + } } - if (approximate_sync_) - ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8)); - else - ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8)); + if (approximate_sync_) { + ts_a_->registerCallback( + boost::bind( + &PointCloudConcatenateDataSynchronizer::input, this, _1, _2, + _3, _4, _5, _6, _7, _8)); + } else { + ts_e_->registerCallback( + boost::bind( + &PointCloudConcatenateDataSynchronizer::input, this, _1, _2, + _3, _4, _5, _6, _7, _8)); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe () +pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe() { - for (int d = 0; d < filters_.size (); ++d) - { - filters_[d]->unsubscribe (); + for (int d = 0; d < filters_.size(); ++d) { + filters_[d]->unsubscribe(); } } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out) +void +pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds( + const PointCloud2 & in1, + const PointCloud2 & in2, + PointCloud2 & out) { //ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ()); - PointCloud2::Ptr in1_t (new PointCloud2 ()); - PointCloud2::Ptr in2_t (new PointCloud2 ()); + PointCloud2::Ptr in1_t(new PointCloud2()); + PointCloud2::Ptr in2_t(new PointCloud2()); // Transform the point clouds into the specified output frame - if (output_frame_ != in1.header.frame_id) - pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_); - else - in1_t = boost::make_shared (in1); + if (output_frame_ != in1.header.frame_id) { + pcl_ros::transformPointCloud(output_frame_, in1, *in1_t, tf_); + } else { + in1_t = boost::make_shared(in1); + } - if (output_frame_ != in2.header.frame_id) - pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_); - else - in2_t = boost::make_shared (in2); + if (output_frame_ != in2.header.frame_id) { + pcl_ros::transformPointCloud(output_frame_, in2, *in2_t, tf_); + } else { + in2_t = boost::make_shared(in2); + } // Concatenate the results - pcl::concatenatePointCloud (*in1_t, *in2_t, out); + pcl::concatenatePointCloud(*in1_t, *in2_t, out); // Copy header out.header.stamp = in1.header.stamp; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::PointCloudConcatenateDataSynchronizer::input ( - const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, - const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, - const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, - const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8) +void +pcl_ros::PointCloudConcatenateDataSynchronizer::input( + const PointCloud2::ConstPtr & in1, const PointCloud2::ConstPtr & in2, + const PointCloud2::ConstPtr & in3, const PointCloud2::ConstPtr & in4, + const PointCloud2::ConstPtr & in5, const PointCloud2::ConstPtr & in6, + const PointCloud2::ConstPtr & in7, const PointCloud2::ConstPtr & in8) { - PointCloud2::Ptr out1 (new PointCloud2 ()); - PointCloud2::Ptr out2 (new PointCloud2 ()); - pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1); - if (in3 && in3->width * in3->height > 0) - { - pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2); + PointCloud2::Ptr out1(new PointCloud2()); + PointCloud2::Ptr out2(new PointCloud2()); + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*in1, *in2, *out1); + if (in3 && in3->width * in3->height > 0) { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in3, *out2); out1 = out2; - if (in4 && in4->width * in4->height > 0) - { - pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1); - if (in5 && in5->width * in5->height > 0) - { - pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2); + if (in4 && in4->width * in4->height > 0) { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in4, *out1); + if (in5 && in5->width * in5->height > 0) { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in5, *out2); out1 = out2; - if (in6 && in6->width * in6->height > 0) - { - pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1); - if (in7 && in7->width * in7->height > 0) - { - pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2); + if (in6 && in6->width * in6->height > 0) { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in6, *out1); + if (in7 && in7->width * in7->height > 0) { + pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in7, *out2); out1 = out2; - } + } } } } } - pub_output_.publish (boost::make_shared (*out1)); + pub_output_.publish(boost::make_shared(*out1)); } typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer; -PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index 35618ea1..99524dd0 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -45,126 +45,129 @@ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () +pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit() { - nodelet_topic_tools::NodeletLazy::onInit (); + nodelet_topic_tools::NodeletLazy::onInit(); // ---[ Mandatory parameters - if (!pnh_->getParam ("input_messages", input_messages_)) - { - NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!"); + if (!pnh_->getParam("input_messages", input_messages_)) { + NODELET_ERROR("[onInit] Need a 'input_messages' parameter to be set before continuing!"); return; } - if (input_messages_ < 2) - { - NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!"); + if (input_messages_ < 2) { + NODELET_ERROR("[onInit] Invalid 'input_messages' parameter given!"); return; } // ---[ Optional parameters - pnh_->getParam ("max_queue_size", maximum_queue_size_); - pnh_->getParam ("maximum_seconds", maximum_seconds_); - pub_output_ = advertise (*pnh_, "output", maximum_queue_size_); + pnh_->getParam("max_queue_size", maximum_queue_size_); + pnh_->getParam("maximum_seconds", maximum_seconds_); + pub_output_ = advertise(*pnh_, "output", maximum_queue_size_); - onInitPostProcess (); + onInitPostProcess(); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe () +pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe() { - sub_input_ = pnh_->subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this); + sub_input_ = pnh_->subscribe( + "input", maximum_queue_size_, + &PointCloudConcatenateFieldsSynchronizer::input_callback, this); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe () +pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe() { - sub_input_.shutdown (); + sub_input_.shutdown(); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud) +pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointCloudConstPtr & cloud) { - NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + NODELET_DEBUG( + "[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), + pnh_->resolveName("input").c_str()); // Erase old data in the queue - if (maximum_seconds_ > 0 && queue_.size () > 0) - { - while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0) + if (maximum_seconds_ > 0 && queue_.size() > 0) { + while (fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ) > maximum_seconds_ && + queue_.size() > 0) { - NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_, - (*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () )); - queue_.erase (queue_.begin ()); + NODELET_WARN( + "[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_, + (*queue_.begin()).first.toSec(), + fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() )); + queue_.erase(queue_.begin()); } } // Push back new data - queue_[cloud->header.stamp].push_back (cloud); - if ((int)queue_[cloud->header.stamp].size () >= input_messages_) - { + queue_[cloud->header.stamp].push_back(cloud); + if ((int)queue_[cloud->header.stamp].size() >= input_messages_) { // Concatenate together and publish - std::vector &clouds = queue_[cloud->header.stamp]; + std::vector & clouds = queue_[cloud->header.stamp]; PointCloud cloud_out = *clouds[0]; // Resize the output dataset - int data_size = cloud_out.data.size (); - int nr_fields = cloud_out.fields.size (); + int data_size = cloud_out.data.size(); + int nr_fields = cloud_out.fields.size(); int nr_points = cloud_out.width * cloud_out.height; - for (size_t i = 1; i < clouds.size (); ++i) - { - assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step); + for (size_t i = 1; i < clouds.size(); ++i) { + assert( + clouds[i]->data.size() / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step); - if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) - { - NODELET_ERROR ("[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!", - i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height); + if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) { + NODELET_ERROR( + "[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!", + i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height); break; } // Point step must increase with the length of each new field cloud_out.point_step += clouds[i]->point_step; // Resize data to hold all clouds - data_size += clouds[i]->data.size (); + data_size += clouds[i]->data.size(); // Concatenate fields - cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ()); - int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype); - for (size_t d = 0; d < clouds[i]->fields.size (); ++d) - { + cloud_out.fields.resize(nr_fields + clouds[i]->fields.size()); + int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize( + cloud_out.fields[nr_fields - 1].datatype); + for (size_t d = 0; d < clouds[i]->fields.size(); ++d) { cloud_out.fields[nr_fields + d] = clouds[i]->fields[d]; cloud_out.fields[nr_fields + d].offset += delta_offset; } - nr_fields += clouds[i]->fields.size (); + nr_fields += clouds[i]->fields.size(); } // Recalculate row_step cloud_out.row_step = cloud_out.point_step * cloud_out.width; - cloud_out.data.resize (data_size); + cloud_out.data.resize(data_size); // Iterate over each point and perform the appropriate memcpys int point_offset = 0; - for (int cp = 0; cp < nr_points; ++cp) - { - for (size_t i = 0; i < clouds.size (); ++i) - { + for (int cp = 0; cp < nr_points; ++cp) { + for (size_t i = 0; i < clouds.size(); ++i) { // Copy each individual point - memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step); + memcpy( + &cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], + clouds[i]->point_step); point_offset += clouds[i]->point_step; } } - pub_output_.publish (boost::make_shared (cloud_out)); - queue_.erase (cloud->header.stamp); + pub_output_.publish(boost::make_shared(cloud_out)); + queue_.erase(cloud->header.stamp); } // Clean the queue to avoid overflowing - if (maximum_queue_size_ > 0) - { - while ((int)queue_.size () > maximum_queue_size_) - queue_.erase (queue_.begin ()); + if (maximum_queue_size_ > 0) { + while ((int)queue_.size() > maximum_queue_size_) { + queue_.erase(queue_.begin()); + } } } typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer; -PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/io.cpp b/pcl_ros/src/pcl_ros/io/io.cpp index 48fa54d6..7cfde645 100644 --- a/pcl_ros/src/pcl_ros/io/io.cpp +++ b/pcl_ros/src/pcl_ros/io/io.cpp @@ -42,7 +42,8 @@ #include #include -typedef nodelet::NodeletMUX > NodeletMUX; +typedef nodelet::NodeletMUX> NodeletMUX; //typedef nodelet::NodeletDEMUX > NodeletDEMUX; typedef nodelet::NodeletDEMUX NodeletDEMUX; @@ -51,7 +52,6 @@ typedef nodelet::NodeletDEMUX NodeletDEMUX; //#include "concatenate_fields.cpp" //#include "concatenate_data.cpp" -PLUGINLIB_EXPORT_CLASS(NodeletMUX,nodelet::Nodelet); -PLUGINLIB_EXPORT_CLASS(NodeletDEMUX,nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet); //PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet); - diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index df1fc24a..487d8cb4 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -40,143 +40,150 @@ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PCDReader::onInit () +pcl_ros::PCDReader::onInit() { - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Provide a latched topic - ros::Publisher pub_output = pnh_->advertise ("output", max_queue_size_, true); + ros::Publisher pub_output = pnh_->advertise("output", max_queue_size_, true); - pnh_->getParam ("publish_rate", publish_rate_); - pnh_->getParam ("tf_frame", tf_frame_); + pnh_->getParam("publish_rate", publish_rate_); + pnh_->getParam("tf_frame", tf_frame_); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - publish_rate : %f\n" - " - tf_frame : %s", - getName ().c_str (), - publish_rate_, tf_frame_.c_str ()); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - publish_rate : %f\n" + " - tf_frame : %s", + getName().c_str(), + publish_rate_, tf_frame_.c_str()); PointCloud2Ptr output_new; - output_ = boost::make_shared (); - output_new = boost::make_shared (); + output_ = boost::make_shared(); + output_new = boost::make_shared(); // Wait in a loop until someone connects - do - { - ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ()); - ros::spinOnce (); - ros::Duration (0.01).sleep (); - } - while (pnh_->ok () && pub_output.getNumSubscribers () == 0); + do { + ROS_DEBUG_ONCE("[%s::onInit] Waiting for a client to connect...", getName().c_str()); + ros::spinOnce(); + ros::Duration(0.01).sleep(); + } while (pnh_->ok() && pub_output.getNumSubscribers() == 0); std::string file_name; - while (pnh_->ok ()) - { + while (pnh_->ok()) { // Get the current filename parameter. If no filename set, loop - if (!pnh_->getParam ("filename", file_name_) && file_name_.empty ()) - { - ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ()); - ros::Duration (0.01).sleep (); - ros::spinOnce (); + if (!pnh_->getParam("filename", file_name_) && file_name_.empty()) { + ROS_ERROR_ONCE( + "[%s::onInit] Need a 'filename' parameter to be set before continuing!", + getName().c_str()); + ros::Duration(0.01).sleep(); + ros::spinOnce(); continue; } // If the filename parameter holds a different value than the last one we read - if (file_name_.compare (file_name) != 0 && !file_name_.empty ()) - { - NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ()); + if (file_name_.compare(file_name) != 0 && !file_name_.empty()) { + NODELET_INFO("[%s::onInit] New file given: %s", getName().c_str(), file_name_.c_str()); file_name = file_name_; pcl::PCLPointCloud2 cloud; - if (impl_.read (file_name_, cloud) < 0) - { - NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ()); + if (impl_.read(file_name_, cloud) < 0) { + NODELET_ERROR("[%s::onInit] Error reading %s !", getName().c_str(), file_name_.c_str()); return; } pcl_conversions::moveFromPCL(cloud, *(output_)); - output_->header.stamp = ros::Time::now (); + output_->header.stamp = ros::Time::now(); output_->header.frame_id = tf_frame_; } // We do not publish empty data - if (output_->data.size () == 0) + if (output_->data.size() == 0) { continue; + } - if (publish_rate_ == 0) - { - if (output_ != output_new) - { - NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); - pub_output.publish (output_); + if (publish_rate_ == 0) { + if (output_ != output_new) { + NODELET_DEBUG( + "Publishing data once (%d points) on topic %s in frame %s.", + output_->width * output_->height, + getMTPrivateNodeHandle().resolveName("output").c_str(), output_->header.frame_id.c_str()); + pub_output.publish(output_); output_new = output_; } - ros::Duration (0.01).sleep (); - } - else - { - NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); - output_->header.stamp = ros::Time::now (); - pub_output.publish (output_); + ros::Duration(0.01).sleep(); + } else { + NODELET_DEBUG( + "Publishing data (%d points) on topic %s in frame %s.", + output_->width * output_->height, getMTPrivateNodeHandle().resolveName( + "output").c_str(), output_->header.frame_id.c_str()); + output_->header.stamp = ros::Time::now(); + pub_output.publish(output_); - ros::Duration (publish_rate_).sleep (); + ros::Duration(publish_rate_).sleep(); } - ros::spinOnce (); + ros::spinOnce(); // Update parameters from server - pnh_->getParam ("publish_rate", publish_rate_); - pnh_->getParam ("tf_frame", tf_frame_); + pnh_->getParam("publish_rate", publish_rate_); + pnh_->getParam("tf_frame", tf_frame_); } - onInitPostProcess (); + onInitPostProcess(); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PCDWriter::onInit () +pcl_ros::PCDWriter::onInit() { - PCLNodelet::onInit (); + PCLNodelet::onInit(); - sub_input_ = pnh_->subscribe ("input", 1, &PCDWriter::input_callback, this); + sub_input_ = pnh_->subscribe("input", 1, &PCDWriter::input_callback, this); // ---[ Optional parameters - pnh_->getParam ("filename", file_name_); - pnh_->getParam ("binary_mode", binary_mode_); + pnh_->getParam("filename", file_name_); + pnh_->getParam("binary_mode", binary_mode_); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - filename : %s\n" - " - binary_mode : %s", - getName ().c_str (), - file_name_.c_str (), (binary_mode_) ? "true" : "false"); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - filename : %s\n" + " - binary_mode : %s", + getName().c_str(), + file_name_.c_str(), (binary_mode_) ? "true" : "false"); - onInitPostProcess (); + onInitPostProcess(); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud) +pcl_ros::PCDWriter::input_callback(const PointCloud2ConstPtr & cloud) { - if (!isValid (cloud)) + if (!isValid(cloud)) { return; - - pnh_->getParam ("filename", file_name_); + } + + pnh_->getParam("filename", file_name_); + + NODELET_DEBUG( + "[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", + getName().c_str(), cloud->width * cloud->height, + cloud->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("input").c_str()); - NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); - std::string fname; - if (file_name_.empty ()) - fname = boost::lexical_cast (cloud->header.stamp.toSec ()) + ".pcd"; - else + if (file_name_.empty()) { + fname = boost::lexical_cast(cloud->header.stamp.toSec()) + ".pcd"; + } else { fname = file_name_; + } pcl::PCLPointCloud2 pcl_cloud; // It is safe to remove the const here because we are the only subscriber callback. - pcl_conversions::moveToPCL(*(const_cast(cloud.get())), pcl_cloud); - impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_); + pcl_conversions::moveToPCL(*(const_cast(cloud.get())), pcl_cloud); + impl_.write( + fname, pcl_cloud, Eigen::Vector4f::Zero(), + Eigen::Quaternionf::Identity(), binary_mode_); - NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ()); + NODELET_DEBUG("[%s::input_callback] Data saved to %s", getName().c_str(), fname.c_str()); } typedef pcl_ros::PCDReader PCDReader; typedef pcl_ros::PCDWriter PCDWriter; -PLUGINLIB_EXPORT_CLASS(PCDReader,nodelet::Nodelet); -PLUGINLIB_EXPORT_CLASS(PCDWriter,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(PCDReader, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(PCDWriter, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index b953497b..43170cea 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -48,139 +48,155 @@ using pcl_conversions::toPCL; ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::EuclideanClusterExtraction::onInit () +pcl_ros::EuclideanClusterExtraction::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // ---[ Mandatory parameters double cluster_tolerance; - if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance)) - { - NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("cluster_tolerance", cluster_tolerance)) { + NODELET_ERROR( + "[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", + getName().c_str()); return; } int spatial_locator; - if (!pnh_->getParam ("spatial_locator", spatial_locator)) - { - NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("spatial_locator", spatial_locator)) { + NODELET_ERROR( + "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", + getName().c_str()); return; } //private_nh.getParam ("use_indices", use_indices_); - pnh_->getParam ("publish_indices", publish_indices_); + pnh_->getParam("publish_indices", publish_indices_); - if (publish_indices_) - pub_output_ = advertise (*pnh_, "output", max_queue_size_); - else - pub_output_ = advertise (*pnh_, "output", max_queue_size_); + if (publish_indices_) { + pub_output_ = advertise(*pnh_, "output", max_queue_size_); + } else { + pub_output_ = advertise(*pnh_, "output", max_queue_size_); + } // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &EuclideanClusterExtraction::config_callback, this, _1, _2); + srv_->setCallback(f); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - max_queue_size : %d\n" - " - use_indices : %s\n" - " - cluster_tolerance : %f\n", - getName ().c_str (), - max_queue_size_, - (use_indices_) ? "true" : "false", cluster_tolerance); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - max_queue_size : %d\n" + " - use_indices : %s\n" + " - cluster_tolerance : %f\n", + getName().c_str(), + max_queue_size_, + (use_indices_) ? "true" : "false", cluster_tolerance); // Set given parameters here - impl_.setClusterTolerance (cluster_tolerance); + impl_.setClusterTolerance(cluster_tolerance); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::EuclideanClusterExtraction::subscribe () +pcl_ros::EuclideanClusterExtraction::subscribe() { // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { + if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - if (approximate_sync_) - { - sync_input_indices_a_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2)); + if (approximate_sync_) { + sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback( + bind( + &EuclideanClusterExtraction:: + input_indices_callback, this, _1, _2)); + } else { + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback( + bind( + &EuclideanClusterExtraction:: + input_indices_callback, this, _1, _2)); } - else - { - sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2)); - } - } - else + } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ())); + sub_input_ = + pnh_->subscribe( + "input", max_queue_size_, + bind(&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr())); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::EuclideanClusterExtraction::unsubscribe () +pcl_ros::EuclideanClusterExtraction::unsubscribe() { - if (use_indices_) - { - sub_input_filter_.unsubscribe (); - sub_indices_filter_.unsubscribe (); + if (use_indices_) { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } else { + sub_input_.shutdown(); } - else - sub_input_.shutdown (); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level) +pcl_ros::EuclideanClusterExtraction::config_callback( + EuclideanClusterExtractionConfig & config, + uint32_t level) { - if (impl_.getClusterTolerance () != config.cluster_tolerance) - { - impl_.setClusterTolerance (config.cluster_tolerance); - NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance); + if (impl_.getClusterTolerance() != config.cluster_tolerance) { + impl_.setClusterTolerance(config.cluster_tolerance); + NODELET_DEBUG( + "[%s::config_callback] Setting new clustering tolerance to: %f.", + getName().c_str(), config.cluster_tolerance); } - if (impl_.getMinClusterSize () != config.cluster_min_size) - { - impl_.setMinClusterSize (config.cluster_min_size); - NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size); + if (impl_.getMinClusterSize() != config.cluster_min_size) { + impl_.setMinClusterSize(config.cluster_min_size); + NODELET_DEBUG( + "[%s::config_callback] Setting the minimum cluster size to: %d.", + getName().c_str(), config.cluster_min_size); } - if (impl_.getMaxClusterSize () != config.cluster_max_size) - { - impl_.setMaxClusterSize (config.cluster_max_size); - NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size); + if (impl_.getMaxClusterSize() != config.cluster_max_size) { + impl_.setMaxClusterSize(config.cluster_max_size); + NODELET_DEBUG( + "[%s::config_callback] Setting the maximum cluster size to: %d.", + getName().c_str(), config.cluster_max_size); } - if (max_clusters_ != config.max_clusters) - { + if (max_clusters_ != config.max_clusters) { max_clusters_ = config.max_clusters; - NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters); + NODELET_DEBUG( + "[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", + getName().c_str(), config.max_clusters); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::EuclideanClusterExtraction::input_indices_callback ( - const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices) +pcl_ros::EuclideanClusterExtraction::input_indices_callback( + const PointCloudConstPtr & cloud, const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0) + if (pub_output_.getNumSubscribers() <= 0) { return; + } // If cloud is given, check if it's valid - if (!isValid (cloud)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + if (!isValid(cloud)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); return; } // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); return; } @@ -188,65 +204,72 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( if (indices) { std_msgs::Header cloud_header = fromPCL(cloud->header); std_msgs::Header indices_header = indices->header; - NODELET_DEBUG ("[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); + NODELET_DEBUG( + "[%s::input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud_header.stamp.toSec(), cloud_header.frame_id.c_str(), pnh_->resolveName("input").c_str(), + indices->indices.size(), indices_header.stamp.toSec(), + indices_header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); } else { - NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + NODELET_DEBUG( + "[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + getName().c_str(), cloud->width * cloud->height, fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str()); } /// IndicesPtr indices_ptr; - if (indices) - indices_ptr.reset (new std::vector (indices->indices)); + if (indices) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices_ptr); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices_ptr); std::vector clusters; - impl_.extract (clusters); + impl_.extract(clusters); - if (publish_indices_) - { - for (size_t i = 0; i < clusters.size (); ++i) - { - if ((int)i >= max_clusters_) + if (publish_indices_) { + for (size_t i = 0; i < clusters.size(); ++i) { + if ((int)i >= max_clusters_) { break; + } // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. pcl_msgs::PointIndices ros_pi; moveFromPCL(clusters[i], ros_pi); - ros_pi.header.stamp += ros::Duration (i * 0.001); - pub_output_.publish (ros_pi); + ros_pi.header.stamp += ros::Duration(i * 0.001); + pub_output_.publish(ros_pi); } - NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ()); - } - else - { - for (size_t i = 0; i < clusters.size (); ++i) - { - if ((int)i >= max_clusters_) + NODELET_DEBUG( + "[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", + clusters.size(), pnh_->resolveName("output").c_str()); + } else { + for (size_t i = 0; i < clusters.size(); ++i) { + if ((int)i >= max_clusters_) { break; + } PointCloud output; - copyPointCloud (*cloud, clusters[i].indices, output); + copyPointCloud(*cloud, clusters[i].indices, output); //PointCloud output_blob; // Convert from the templated output to the PointCloud blob //pcl::toROSMsg (output, output_blob); // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. std_msgs::Header header = fromPCL(output.header); - header.stamp += ros::Duration (i * 0.001); + header.stamp += ros::Duration(i * 0.001); toPCL(header, output.header); // Publish a Boost shared ptr const data - pub_output_.publish (ros_ptr(output.makeShared ())); - NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", - i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); + pub_output_.publish(ros_ptr(output.makeShared())); + NODELET_DEBUG( + "[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", + i, clusters[i].indices.size(), header.stamp.toSec(), pnh_->resolveName("output").c_str()); } } } typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction; PLUGINLIB_EXPORT_CLASS(EuclideanClusterExtraction, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index b05770b7..3dac6ab7 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -47,174 +47,206 @@ using pcl_conversions::moveToPCL; ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ExtractPolygonalPrismData::onInit () +pcl_ros::ExtractPolygonalPrismData::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &ExtractPolygonalPrismData::config_callback, this, _1, _2); + srv_->setCallback(f); // Advertise the output topics - pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*pnh_, "output", max_queue_size_); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ExtractPolygonalPrismData::subscribe () +pcl_ros::ExtractPolygonalPrismData::subscribe() { - sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_); - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_hull_filter_.subscribe(*pnh_, "planar_hull", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); // Create the objects here - if (approximate_sync_) - sync_input_hull_indices_a_ = boost::make_shared > > (max_queue_size_); - else - sync_input_hull_indices_e_ = boost::make_shared > > (max_queue_size_); - - if (use_indices_) - { - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); - if (approximate_sync_) - sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_); - else - sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_); + if (approximate_sync_) { + sync_input_hull_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_hull_indices_e_ = boost::make_shared>>(max_queue_size_); } - else - { - sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1)); - if (approximate_sync_) - sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_); - else - sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_); + if (use_indices_) { + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + if (approximate_sync_) { + sync_input_hull_indices_a_->connectInput( + sub_input_filter_, sub_hull_filter_, + sub_indices_filter_); + } else { + sync_input_hull_indices_e_->connectInput( + sub_input_filter_, sub_hull_filter_, + sub_indices_filter_); + } + } else { + sub_input_filter_.registerCallback(bind(&ExtractPolygonalPrismData::input_callback, this, _1)); + + if (approximate_sync_) { + sync_input_hull_indices_a_->connectInput(sub_input_filter_, sub_hull_filter_, nf_); + } else { + sync_input_hull_indices_e_->connectInput(sub_input_filter_, sub_hull_filter_, nf_); + } } // Register callbacks - if (approximate_sync_) - sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3)); - else - sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3)); + if (approximate_sync_) { + sync_input_hull_indices_a_->registerCallback( + bind( + &ExtractPolygonalPrismData:: + input_hull_indices_callback, this, _1, _2, _3)); + } else { + sync_input_hull_indices_e_->registerCallback( + bind( + &ExtractPolygonalPrismData:: + input_hull_indices_callback, this, _1, _2, _3)); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ExtractPolygonalPrismData::unsubscribe () +pcl_ros::ExtractPolygonalPrismData::unsubscribe() { - sub_hull_filter_.unsubscribe (); - sub_input_filter_.unsubscribe (); + sub_hull_filter_.unsubscribe(); + sub_input_filter_.unsubscribe(); - if (use_indices_) - sub_indices_filter_.unsubscribe (); + if (use_indices_) { + sub_indices_filter_.unsubscribe(); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level) +pcl_ros::ExtractPolygonalPrismData::config_callback( + ExtractPolygonalPrismDataConfig & config, + uint32_t level) { double height_min, height_max; - impl_.getHeightLimits (height_min, height_max); - if (height_min != config.height_min) - { + impl_.getHeightLimits(height_min, height_max); + if (height_min != config.height_min) { height_min = config.height_min; - NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min); - impl_.setHeightLimits (height_min, height_max); + NODELET_DEBUG( + "[%s::config_callback] Setting new minimum height to the planar model to: %f.", + getName().c_str(), height_min); + impl_.setHeightLimits(height_min, height_max); } - if (height_max != config.height_max) - { + if (height_max != config.height_max) { height_max = config.height_max; - NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max); - impl_.setHeightLimits (height_min, height_max); + NODELET_DEBUG( + "[%s::config_callback] Setting new maximum height to the planar model to: %f.", + getName().c_str(), height_max); + impl_.setHeightLimits(height_min, height_max); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( - const PointCloudConstPtr &cloud, - const PointCloudConstPtr &hull, - const PointIndicesConstPtr &indices) +pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback( + const PointCloudConstPtr & cloud, + const PointCloudConstPtr & hull, + const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0) + if (pub_output_.getNumSubscribers() <= 0) { return; + } // Copy the header (stamp + frame_id) pcl_msgs::PointIndices inliers; inliers.header = fromPCL(cloud->header); // If cloud is given, check if it's valid - if (!isValid (cloud) || !isValid (hull, "planar_hull")) - { - NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ()); - pub_output_.publish (inliers); + if (!isValid(cloud) || !isValid(hull, "planar_hull")) { + NODELET_ERROR("[%s::input_hull_indices_callback] Invalid input!", getName().c_str()); + pub_output_.publish(inliers); return; } // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ()); - pub_output_.publish (inliers); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_hull_indices_callback] Invalid indices!", getName().c_str()); + pub_output_.publish(inliers); return; } /// DEBUG - if (indices) - NODELET_DEBUG ("[%s::input_indices_hull_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_indices_hull_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ()); + if (indices) { + NODELET_DEBUG( + "[%s::input_indices_hull_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL( + hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName( + "planar_hull").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_indices_hull_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL( + hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName( + "planar_hull").c_str()); + } /// - if (cloud->header.frame_id != hull->header.frame_id) - { - NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ()); + if (cloud->header.frame_id != hull->header.frame_id) { + NODELET_DEBUG( + "[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", + getName().c_str(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str()); PointCloud planar_hull; - if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_)) - { + if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) { // Publish empty before return - pub_output_.publish (inliers); + pub_output_.publish(inliers); return; } - impl_.setInputPlanarHull (pcl_ptr(planar_hull.makeShared ())); + impl_.setInputPlanarHull(pcl_ptr(planar_hull.makeShared())); + } else { + impl_.setInputPlanarHull(pcl_ptr(hull)); } - else - impl_.setInputPlanarHull (pcl_ptr(hull)); IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty ()) - indices_ptr.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices_ptr); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty ()) { + if (!cloud->points.empty()) { pcl::PointIndices pcl_inliers; moveToPCL(inliers, pcl_inliers); - impl_.segment (pcl_inliers); + impl_.segment(pcl_inliers); moveFromPCL(pcl_inliers, inliers); } // Enforce that the TF frame and the timestamp are copied inliers.header = fromPCL(cloud->header); - pub_output_.publish (inliers); - NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ()); + pub_output_.publish(inliers); + NODELET_DEBUG( + "[%s::input_hull_callback] Publishing %zu indices.", + getName().c_str(), inliers.indices.size()); } typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData; PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index cfb46d9b..1defe32d 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -45,230 +45,243 @@ using pcl_conversions::fromPCL; ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentation::onInit () +pcl_ros::SACSegmentation::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Advertise the output topics - pub_indices_ = advertise (*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise (*pnh_, "model", max_queue_size_); + pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); + pub_model_ = advertise(*pnh_, "model", max_queue_size_); // ---[ Mandatory parameters int model_type; - if (!pnh_->getParam ("model_type", model_type)) - { - NODELET_ERROR ("[onInit] Need a 'model_type' parameter to be set before continuing!"); + if (!pnh_->getParam("model_type", model_type)) { + NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!"); return; } double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam ("distance_threshold", threshold)) - { - NODELET_ERROR ("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); + if (!pnh_->getParam("distance_threshold", threshold)) { + NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); return; } // ---[ Optional parameters int method_type = 0; - pnh_->getParam ("method_type", method_type); + pnh_->getParam("method_type", method_type); XmlRpc::XmlRpcValue axis_param; - pnh_->getParam ("axis", axis_param); - Eigen::Vector3f axis = Eigen::Vector3f::Zero (); + pnh_->getParam("axis", axis_param); + Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - switch (axis_param.getType ()) - { + switch (axis_param.getType()) { case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size () != 3) { - NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ()); - return; - } - for (int i = 0; i < 3; ++i) - { - if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble) - { - NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ()); + if (axis_param.size() != 3) { + NODELET_ERROR( + "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", + getName().c_str(), axis_param.size()); return; } - double value = axis_param[i]; axis[i] = value; + for (int i = 0; i < 3; ++i) { + if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { + NODELET_ERROR( + "[%s::onInit] Need floating point values for 'axis' parameter.", + getName().c_str()); + return; + } + double value = axis_param[i]; axis[i] = value; + } + break; } - break; - } default: - { - break; - } + { + break; + } } // Initialize the random number generator - srand (time (0)); + srand(time(0)); // Enable the dynamic reconfigure service - srv_ = boost::make_shared >(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentation::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &SACSegmentation::config_callback, this, _1, _2); + srv_->setCallback(f); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - method_type : %d\n" - " - model_threshold : %f\n" - " - axis : [%f, %f, %f]\n", - getName ().c_str (), model_type, method_type, threshold, - axis[0], axis[1], axis[2]); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - method_type : %d\n" + " - model_threshold : %f\n" + " - axis : [%f, %f, %f]\n", + getName().c_str(), model_type, method_type, threshold, + axis[0], axis[1], axis[2]); // Set given parameters here - impl_.setModelType (model_type); - impl_.setMethodType (method_type); - impl_.setAxis (axis); + impl_.setModelType(model_type); + impl_.setMethodType(method_type); + impl_.setAxis(axis); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentation::subscribe () +pcl_ros::SACSegmentation::subscribe() { // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { + if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); // when "use_indices" is set to true, and "latched_indices" is set to true, - // we'll subscribe and get a separate callback for PointIndices that will - // save the indices internally, and a PointCloud + PointIndices callback - // will take care of meshing the new PointClouds with the old saved indices. - if (latched_indices_) - { + // we'll subscribe and get a separate callback for PointIndices that will + // save the indices internally, and a PointCloud + PointIndices callback + // will take care of meshing the new PointClouds with the old saved indices. + if (latched_indices_) { // Subscribe to a callback that saves the indices - sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1)); + sub_indices_filter_.registerCallback(bind(&SACSegmentation::indices_callback, this, _1)); // Subscribe to a callback that sets the header of the saved indices to the cloud header - sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1)); + sub_input_filter_.registerCallback(bind(&SACSegmentation::input_callback, this, _1)); // Synchronize the two topics. No need for an approximate synchronizer here, as we'll // match the timestamps exactly - sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_); - sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, nf_pi_); + sync_input_indices_e_->registerCallback( + bind( + &SACSegmentation::input_indices_callback, this, + _1, _2)); } // "latched_indices" not set, proceed with regular pairs - else - { - if (approximate_sync_) - { - sync_input_indices_a_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); - } - else - { - sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + else { + if (approximate_sync_) { + sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback( + bind( + &SACSegmentation::input_indices_callback, this, + _1, _2)); + } else { + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback( + bind( + &SACSegmentation::input_indices_callback, this, + _1, _2)); } } - } - else + } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ())); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::unsubscribe () -{ - if (use_indices_) - { - sub_input_filter_.unsubscribe (); - sub_indices_filter_.unsubscribe (); + sub_input_ = + pnh_->subscribe( + "input", max_queue_size_, + bind(&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr())); } - else - sub_input_.shutdown (); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32_t level) +pcl_ros::SACSegmentation::unsubscribe() { - boost::mutex::scoped_lock lock (mutex_); + if (use_indices_) { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } else { + sub_input_.shutdown(); + } +} - if (impl_.getDistanceThreshold () != config.distance_threshold) - { +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32_t level) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (impl_.getDistanceThreshold() != config.distance_threshold) { //sac_->setDistanceThreshold (threshold_); - done in initSAC - impl_.setDistanceThreshold (config.distance_threshold); - NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold); + impl_.setDistanceThreshold(config.distance_threshold); + NODELET_DEBUG( + "[%s::config_callback] Setting new distance to model threshold to: %f.", + getName().c_str(), config.distance_threshold); } // The maximum allowed difference between the model normal and the given axis _in radians_ - if (impl_.getEpsAngle () != config.eps_angle) - { - impl_.setEpsAngle (config.eps_angle); - NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI); - } - - // Number of inliers - if (min_inliers_ != config.min_inliers) - { - min_inliers_ = config.min_inliers; - NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_); + if (impl_.getEpsAngle() != config.eps_angle) { + impl_.setEpsAngle(config.eps_angle); + NODELET_DEBUG( + "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", + getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); } - if (impl_.getMaxIterations () != config.max_iterations) - { + // Number of inliers + if (min_inliers_ != config.min_inliers) { + min_inliers_ = config.min_inliers; + NODELET_DEBUG( + "[%s::config_callback] Setting new minimum number of inliers to: %d.", + getName().c_str(), min_inliers_); + } + + if (impl_.getMaxIterations() != config.max_iterations) { //sac_->setMaxIterations (max_iterations_); - done in initSAC - impl_.setMaxIterations (config.max_iterations); - NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations); + impl_.setMaxIterations(config.max_iterations); + NODELET_DEBUG( + "[%s::config_callback] Setting new maximum number of iterations to: %d.", + getName().c_str(), config.max_iterations); } - if (impl_.getProbability () != config.probability) - { + if (impl_.getProbability() != config.probability) { //sac_->setProbability (probability_); - done in initSAC - impl_.setProbability (config.probability); - NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability); + impl_.setProbability(config.probability); + NODELET_DEBUG( + "[%s::config_callback] Setting new probability to: %f.", + getName().c_str(), config.probability); } - if (impl_.getOptimizeCoefficients () != config.optimize_coefficients) - { - impl_.setOptimizeCoefficients (config.optimize_coefficients); - NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false"); + if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { + impl_.setOptimizeCoefficients(config.optimize_coefficients); + NODELET_DEBUG( + "[%s::config_callback] Setting coefficient optimization to: %s.", + getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); } double radius_min, radius_max; - impl_.getRadiusLimits (radius_min, radius_max); - if (radius_min != config.radius_min) - { + impl_.getRadiusLimits(radius_min, radius_max); + if (radius_min != config.radius_min) { radius_min = config.radius_min; - NODELET_DEBUG ("[config_callback] Setting minimum allowable model radius to: %f.", radius_min); - impl_.setRadiusLimits (radius_min, radius_max); + NODELET_DEBUG("[config_callback] Setting minimum allowable model radius to: %f.", radius_min); + impl_.setRadiusLimits(radius_min, radius_max); } - if (radius_max != config.radius_max) - { + if (radius_max != config.radius_max) { radius_max = config.radius_max; - NODELET_DEBUG ("[config_callback] Setting maximum allowable model radius to: %f.", radius_max); - impl_.setRadiusLimits (radius_min, radius_max); + NODELET_DEBUG("[config_callback] Setting maximum allowable model radius to: %f.", radius_max); + impl_.setRadiusLimits(radius_min, radius_max); } - if (tf_input_frame_ != config.input_frame) - { + if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; - NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ()); - NODELET_WARN ("input_frame TF not implemented yet!"); + NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); + NODELET_WARN("input_frame TF not implemented yet!"); } - if (tf_output_frame_ != config.output_frame) - { + if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; - NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ()); - NODELET_WARN ("output_frame TF not implemented yet!"); + NODELET_DEBUG( + "[config_callback] Setting the output TF frame to: %s.", + tf_output_frame_.c_str()); + NODELET_WARN("output_frame TF not implemented yet!"); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &cloud, - const PointIndicesConstPtr &indices) +pcl_ros::SACSegmentation::input_indices_callback( + const PointCloudConstPtr & cloud, + const PointIndicesConstPtr & indices) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); pcl_msgs::PointIndices inliers; pcl_msgs::ModelCoefficients model; @@ -276,33 +289,39 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou inliers.header = model.header = fromPCL(cloud->header); // If cloud is given, check if it's valid - if (!isValid (cloud)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); - pub_indices_.publish (inliers); - pub_model_.publish (model); + if (!isValid(cloud)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); + pub_indices_.publish(inliers); + pub_model_.publish(model); return; } // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); - pub_indices_.publish (inliers); - pub_model_.publish (model); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); + pub_indices_.publish(inliers); + pub_model_.publish(model); return; } /// DEBUG - if (indices && !indices->header.frame_id.empty ()) - NODELET_DEBUG ("[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + if (indices && !indices->header.frame_id.empty()) { + NODELET_DEBUG( + "[%s::input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + getName().c_str(), cloud->width * cloud->height, fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str()); + } /// // Check whether the user has given a different input TF frame @@ -319,22 +338,23 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou cloud_tf.reset (new PointCloud (cloud_transformed)); } else*/ - cloud_tf = cloud; + cloud_tf = cloud; IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty ()) - indices_ptr.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setInputCloud (pcl_ptr(cloud_tf)); - impl_.setIndices (indices_ptr); + impl_.setInputCloud(pcl_ptr(cloud_tf)); + impl_.setIndices(indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty ()) { + if (!cloud->points.empty()) { pcl::PointIndices pcl_inliers; pcl::ModelCoefficients pcl_model; pcl_conversions::moveToPCL(inliers, pcl_inliers); pcl_conversions::moveToPCL(model, pcl_model); - impl_.segment (pcl_inliers, pcl_model); + impl_.segment(pcl_inliers, pcl_model); pcl_conversions::moveFromPCL(pcl_inliers, inliers); pcl_conversions::moveFromPCL(pcl_model, model); } @@ -342,355 +362,402 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou // Probably need to transform the model of the plane here // Check if we have enough inliers, clear inliers + model if not - if ((int)inliers.indices.size () <= min_inliers_) - { - inliers.indices.clear (); - model.values.clear (); + if ((int)inliers.indices.size() <= min_inliers_) { + inliers.indices.clear(); + model.values.clear(); } // Publish - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); - NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", - getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (), - model.values.size (), pnh_->resolveName ("model").c_str ()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); + NODELET_DEBUG( + "[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", + getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), + model.values.size(), pnh_->resolveName("model").c_str()); - if (inliers.indices.empty ()) - NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ()); + if (inliers.indices.empty()) { + NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::onInit () +pcl_ros::SACSegmentationFromNormals::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &SACSegmentationFromNormals::config_callback, this, _1, _2); + srv_->setCallback(f); // Advertise the output topics - pub_indices_ = advertise (*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise (*pnh_, "model", max_queue_size_); + pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); + pub_model_ = advertise(*pnh_, "model", max_queue_size_); // ---[ Mandatory parameters int model_type; - if (!pnh_->getParam ("model_type", model_type)) - { - NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("model_type", model_type)) { + NODELET_ERROR( + "[%s::onInit] Need a 'model_type' parameter to be set before continuing!", + getName().c_str()); return; } double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam ("distance_threshold", threshold)) - { - NODELET_ERROR ("[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("distance_threshold", threshold)) { + NODELET_ERROR( + "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", + getName().c_str()); return; } // ---[ Optional parameters int method_type = 0; - pnh_->getParam ("method_type", method_type); + pnh_->getParam("method_type", method_type); XmlRpc::XmlRpcValue axis_param; - pnh_->getParam ("axis", axis_param); - Eigen::Vector3f axis = Eigen::Vector3f::Zero (); + pnh_->getParam("axis", axis_param); + Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - switch (axis_param.getType ()) - { + switch (axis_param.getType()) { case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size () != 3) { - NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ()); - return; - } - for (int i = 0; i < 3; ++i) - { - if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble) - { - NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ()); + if (axis_param.size() != 3) { + NODELET_ERROR( + "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", + getName().c_str(), axis_param.size()); return; } - double value = axis_param[i]; axis[i] = value; + for (int i = 0; i < 3; ++i) { + if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { + NODELET_ERROR( + "[%s::onInit] Need floating point values for 'axis' parameter.", + getName().c_str()); + return; + } + double value = axis_param[i]; axis[i] = value; + } + break; } - break; - } default: - { - break; - } + { + break; + } } // Initialize the random number generator - srand (time (0)); + srand(time(0)); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - method_type : %d\n" - " - model_threshold : %f\n" - " - axis : [%f, %f, %f]\n", - getName ().c_str (), model_type, method_type, threshold, - axis[0], axis[1], axis[2]); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - method_type : %d\n" + " - model_threshold : %f\n" + " - axis : [%f, %f, %f]\n", + getName().c_str(), model_type, method_type, threshold, + axis[0], axis[1], axis[2]); // Set given parameters here - impl_.setModelType (model_type); - impl_.setMethodType (method_type); - impl_.setAxis (axis); + impl_.setModelType(model_type); + impl_.setMethodType(method_type); + impl_.setAxis(axis); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::subscribe () +pcl_ros::SACSegmentationFromNormals::subscribe() { // Subscribe to the input and normals using filters - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) - sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this); + sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this); - if (approximate_sync_) - sync_input_normals_indices_a_ = boost::make_shared > > (max_queue_size_); - else - sync_input_normals_indices_e_ = boost::make_shared > > (max_queue_size_); + if (approximate_sync_) { + sync_input_normals_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_normals_indices_e_ = boost::make_shared>>(max_queue_size_); + } // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { + if (use_indices_) { // Subscribe to the input using a filter - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - if (approximate_sync_) - sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); - else - sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); - } - else - { + if (approximate_sync_) { + sync_input_normals_indices_a_->connectInput( + sub_input_filter_, sub_normals_filter_, + sub_indices_filter_); + } else { + sync_input_normals_indices_e_->connectInput( + sub_input_filter_, sub_normals_filter_, + sub_indices_filter_); + } + } else { // Create a different callback for copying over the timestamp to fake indices - sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1)); + sub_input_filter_.registerCallback(bind(&SACSegmentationFromNormals::input_callback, this, _1)); - if (approximate_sync_) - sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); - else - sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); + if (approximate_sync_) { + sync_input_normals_indices_a_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); + } else { + sync_input_normals_indices_e_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); + } } - if (approximate_sync_) - sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); - else - sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); + if (approximate_sync_) { + sync_input_normals_indices_a_->registerCallback( + bind( + &SACSegmentationFromNormals:: + input_normals_indices_callback, this, _1, _2, _3)); + } else { + sync_input_normals_indices_e_->registerCallback( + bind( + &SACSegmentationFromNormals:: + input_normals_indices_callback, this, _1, _2, _3)); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::unsubscribe () +pcl_ros::SACSegmentationFromNormals::unsubscribe() { - sub_input_filter_.unsubscribe (); - sub_normals_filter_.unsubscribe (); + sub_input_filter_.unsubscribe(); + sub_normals_filter_.unsubscribe(); - sub_axis_.shutdown (); + sub_axis_.shutdown(); - if (use_indices_) - sub_indices_filter_.unsubscribe (); + if (use_indices_) { + sub_indices_filter_.unsubscribe(); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model) +pcl_ros::SACSegmentationFromNormals::axis_callback( + const pcl_msgs::ModelCoefficientsConstPtr & model) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - if (model->values.size () < 3) - { - NODELET_ERROR ("[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", getName ().c_str (), model->values.size (), pnh_->resolveName ("axis").c_str ()); + if (model->values.size() < 3) { + NODELET_ERROR( + "[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", + getName().c_str(), model->values.size(), pnh_->resolveName("axis").c_str()); return; } - NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]); + NODELET_DEBUG( + "[%s::axis_callback] Received axis direction: %f %f %f", + getName().c_str(), model->values[0], model->values[1], model->values[2]); - Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]); - impl_.setAxis (axis); + Eigen::Vector3f axis(model->values[0], model->values[1], model->values[2]); + impl_.setAxis(axis); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level) +pcl_ros::SACSegmentationFromNormals::config_callback( + SACSegmentationFromNormalsConfig & config, + uint32_t level) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - if (impl_.getDistanceThreshold () != config.distance_threshold) - { - impl_.setDistanceThreshold (config.distance_threshold); - NODELET_DEBUG ("[%s::config_callback] Setting distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold); + if (impl_.getDistanceThreshold() != config.distance_threshold) { + impl_.setDistanceThreshold(config.distance_threshold); + NODELET_DEBUG( + "[%s::config_callback] Setting distance to model threshold to: %f.", + getName().c_str(), config.distance_threshold); } // The maximum allowed difference between the model normal and the given axis _in radians_ - if (impl_.getEpsAngle () != config.eps_angle) - { - impl_.setEpsAngle (config.eps_angle); - NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI); + if (impl_.getEpsAngle() != config.eps_angle) { + impl_.setEpsAngle(config.eps_angle); + NODELET_DEBUG( + "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", + getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); } - if (impl_.getMaxIterations () != config.max_iterations) - { - impl_.setMaxIterations (config.max_iterations); - NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations); + if (impl_.getMaxIterations() != config.max_iterations) { + impl_.setMaxIterations(config.max_iterations); + NODELET_DEBUG( + "[%s::config_callback] Setting new maximum number of iterations to: %d.", + getName().c_str(), config.max_iterations); } - + // Number of inliers - if (min_inliers_ != config.min_inliers) - { + if (min_inliers_ != config.min_inliers) { min_inliers_ = config.min_inliers; - NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_); + NODELET_DEBUG( + "[%s::config_callback] Setting new minimum number of inliers to: %d.", + getName().c_str(), min_inliers_); } - if (impl_.getProbability () != config.probability) - { - impl_.setProbability (config.probability); - NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability); + if (impl_.getProbability() != config.probability) { + impl_.setProbability(config.probability); + NODELET_DEBUG( + "[%s::config_callback] Setting new probability to: %f.", + getName().c_str(), config.probability); } - if (impl_.getOptimizeCoefficients () != config.optimize_coefficients) - { - impl_.setOptimizeCoefficients (config.optimize_coefficients); - NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false"); + if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { + impl_.setOptimizeCoefficients(config.optimize_coefficients); + NODELET_DEBUG( + "[%s::config_callback] Setting coefficient optimization to: %s.", + getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); } - if (impl_.getNormalDistanceWeight () != config.normal_distance_weight) - { - impl_.setNormalDistanceWeight (config.normal_distance_weight); - NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight); + if (impl_.getNormalDistanceWeight() != config.normal_distance_weight) { + impl_.setNormalDistanceWeight(config.normal_distance_weight); + NODELET_DEBUG( + "[%s::config_callback] Setting new distance weight to: %f.", + getName().c_str(), config.normal_distance_weight); } double radius_min, radius_max; - impl_.getRadiusLimits (radius_min, radius_max); - if (radius_min != config.radius_min) - { + impl_.getRadiusLimits(radius_min, radius_max); + if (radius_min != config.radius_min) { radius_min = config.radius_min; - NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min); - impl_.setRadiusLimits (radius_min, radius_max); + NODELET_DEBUG( + "[%s::config_callback] Setting minimum allowable model radius to: %f.", + getName().c_str(), radius_min); + impl_.setRadiusLimits(radius_min, radius_max); } - if (radius_max != config.radius_max) - { + if (radius_max != config.radius_max) { radius_max = config.radius_max; - NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max); - impl_.setRadiusLimits (radius_min, radius_max); + NODELET_DEBUG( + "[%s::config_callback] Setting maximum allowable model radius to: %f.", + getName().c_str(), radius_max); + impl_.setRadiusLimits(radius_min, radius_max); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( - const PointCloudConstPtr &cloud, - const PointCloudNConstPtr &cloud_normals, - const PointIndicesConstPtr &indices - ) +pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( + const PointCloudConstPtr & cloud, + const PointCloudNConstPtr & cloud_normals, + const PointIndicesConstPtr & indices +) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); PointIndices inliers; ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied inliers.header = model.header = fromPCL(cloud->header); - if (impl_.getModelType () < 0) - { - NODELET_ERROR ("[%s::input_normals_indices_callback] Model type not set!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + if (impl_.getModelType() < 0) { + NODELET_ERROR("[%s::input_normals_indices_callback] Model type not set!", getName().c_str()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); return; } - if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) - { - NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid input!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals")) + NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); return; } // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid indices!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_normals_indices_callback] Invalid indices!", getName().c_str()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); return; } /// DEBUG - if (indices && !indices->header.frame_id.empty ()) - NODELET_DEBUG ("[%s::input_normals_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_normals_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); + if (indices && !indices->header.frame_id.empty()) { + NODELET_DEBUG( + "[%s::input_normals_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), fromPCL( + cloud_normals->header).stamp.toSec(), + cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_normals_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_normals->width * cloud_normals->height, pcl::getFieldsList( + *cloud_normals).c_str(), fromPCL( + cloud_normals->header).stamp.toSec(), + cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); + } /// // Extra checks for safety - int cloud_nr_points = cloud->width * cloud->height; + int cloud_nr_points = cloud->width * cloud->height; int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; - if (cloud_nr_points != cloud_normals_nr_points) - { - NODELET_ERROR ("[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", getName ().c_str (), cloud_nr_points, cloud_normals_nr_points); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + if (cloud_nr_points != cloud_normals_nr_points) { + NODELET_ERROR( + "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", + getName().c_str(), cloud_nr_points, cloud_normals_nr_points); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); return; } - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setInputNormals (pcl_ptr(cloud_normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setInputNormals(pcl_ptr(cloud_normals)); IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty ()) - indices_ptr.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setIndices (indices_ptr); + impl_.setIndices(indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty ()) { + if (!cloud->points.empty()) { pcl::PointIndices pcl_inliers; pcl::ModelCoefficients pcl_model; pcl_conversions::moveToPCL(inliers, pcl_inliers); pcl_conversions::moveToPCL(model, pcl_model); - impl_.segment (pcl_inliers, pcl_model); + impl_.segment(pcl_inliers, pcl_model); pcl_conversions::moveFromPCL(pcl_inliers, inliers); pcl_conversions::moveFromPCL(pcl_model, model); } // Check if we have enough inliers, clear inliers + model if not - if ((int)inliers.indices.size () <= min_inliers_) - { - inliers.indices.clear (); - model.values.clear (); + if ((int)inliers.indices.size() <= min_inliers_) { + inliers.indices.clear(); + model.values.clear(); } // Publish - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); - NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", - getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (), - model.values.size (), pnh_->resolveName ("model").c_str ()); - if (inliers.indices.empty ()) - NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); + NODELET_DEBUG( + "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", + getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), + model.values.size(), pnh_->resolveName("model").c_str()); + if (inliers.indices.empty()) { + NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); + } } typedef pcl_ros::SACSegmentation SACSegmentation; typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals; PLUGINLIB_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index 79d01d12..93bc8f8b 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -41,103 +41,119 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SegmentDifferences::onInit () +pcl_ros::SegmentDifferences::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); - pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*pnh_, "output", max_queue_size_); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - max_queue_size : %d", - getName ().c_str (), - max_queue_size_); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - max_queue_size : %d", + getName().c_str(), + max_queue_size_); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SegmentDifferences::subscribe () +pcl_ros::SegmentDifferences::subscribe() { // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_); + sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + sub_target_filter_.subscribe(*pnh_, "target", max_queue_size_); // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &SegmentDifferences::config_callback, this, _1, _2); + srv_->setCallback(f); - if (approximate_sync_) - { - sync_input_target_a_ = boost::make_shared > > (max_queue_size_); - sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_); - sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2)); - } - else - { - sync_input_target_e_ = boost::make_shared > > (max_queue_size_); - sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_); - sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2)); + if (approximate_sync_) { + sync_input_target_a_ = boost::make_shared>>(max_queue_size_); + sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_); + sync_input_target_a_->registerCallback( + bind( + &SegmentDifferences::input_target_callback, this, + _1, _2)); + } else { + sync_input_target_e_ = boost::make_shared>>(max_queue_size_); + sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_); + sync_input_target_e_->registerCallback( + bind( + &SegmentDifferences::input_target_callback, this, + _1, _2)); } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SegmentDifferences::unsubscribe () +pcl_ros::SegmentDifferences::unsubscribe() { - sub_input_filter_.unsubscribe (); - sub_target_filter_.unsubscribe (); + sub_input_filter_.unsubscribe(); + sub_target_filter_.unsubscribe(); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level) +pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level) { - if (impl_.getDistanceThreshold () != config.distance_threshold) - { - impl_.setDistanceThreshold (config.distance_threshold); - NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold); + if (impl_.getDistanceThreshold() != config.distance_threshold) { + impl_.setDistanceThreshold(config.distance_threshold); + NODELET_DEBUG( + "[%s::config_callback] Setting new distance threshold to: %f.", + getName().c_str(), config.distance_threshold); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud, - const PointCloudConstPtr &cloud_target) +pcl_ros::SegmentDifferences::input_target_callback( + const PointCloudConstPtr & cloud, + const PointCloudConstPtr & cloud_target) { - if (pub_output_.getNumSubscribers () <= 0) - return; - - if (!isValid (cloud) || !isValid (cloud_target, "target")) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); - PointCloud output; - output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + if (pub_output_.getNumSubscribers() <= 0) { return; } - NODELET_DEBUG ("[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ()); + if (!isValid(cloud) || !isValid(cloud_target, "target")) { + NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); + PointCloud output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(output.makeShared())); + return; + } - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setTargetCloud (pcl_ptr(cloud_target)); + NODELET_DEBUG( + "[%s::input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( + "input").c_str(), + cloud_target->width * cloud_target->height, pcl::getFieldsList(*cloud_target).c_str(), + fromPCL(cloud_target->header).stamp.toSec(), + cloud_target->header.frame_id.c_str(), pnh_->resolveName("target").c_str()); + + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setTargetCloud(pcl_ptr(cloud_target)); PointCloud output; - impl_.segment (output); + impl_.segment(output); - pub_output_.publish (ros_ptr(output.makeShared ())); - NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (), - output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ()); + pub_output_.publish(ros_ptr(output.makeShared())); + NODELET_DEBUG( + "[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", + getName().c_str(), + output.points.size(), fromPCL(output.header).stamp.toSec(), + pnh_->resolveName("output").c_str()); } typedef pcl_ros::SegmentDifferences SegmentDifferences; PLUGINLIB_EXPORT_CLASS(SegmentDifferences, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp index 8c261926..09fa772a 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp @@ -40,5 +40,3 @@ //#include "extract_polygonal_prism_data.cpp" //#include "sac_segmentation.cpp" //#include "segment_differences.cpp" - - diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index 22cc2b98..8a80217e 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -42,20 +42,21 @@ ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ConvexHull2D::onInit () +pcl_ros::ConvexHull2D::onInit() { - PCLNodelet::onInit (); + PCLNodelet::onInit(); - pub_output_ = advertise (*pnh_, "output", max_queue_size_); - pub_plane_ = advertise (*pnh_, "output_polygon", max_queue_size_); + pub_output_ = advertise(*pnh_, "output", max_queue_size_); + pub_plane_ = advertise(*pnh_, "output_polygon", max_queue_size_); // ---[ Optional parameters - pnh_->getParam ("use_indices", use_indices_); + pnh_->getParam("use_indices", use_indices_); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_indices : %s", - getName ().c_str (), - (use_indices_) ? "true" : "false"); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_indices : %s", + getName().c_str(), + (use_indices_) ? "true" : "false"); onInitPostProcess(); } @@ -65,139 +66,150 @@ void pcl_ros::ConvexHull2D::subscribe() { // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { + if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", 1); + sub_input_filter_.subscribe(*pnh_, "input", 1); // If indices are enabled, subscribe to the indices - sub_indices_filter_.subscribe (*pnh_, "indices", 1); + sub_indices_filter_.subscribe(*pnh_, "indices", 1); - if (approximate_sync_) - { - sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); + if (approximate_sync_) { + sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); // surface not enabled, connect the input-indices duo and register - sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2)); - } - else - { - sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback( + bind( + &ConvexHull2D::input_indices_callback, this, _1, + _2)); + } else { + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); // surface not enabled, connect the input-indices duo and register - sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2)); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback( + bind( + &ConvexHull2D::input_indices_callback, this, _1, + _2)); } - } - else + } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ())); + sub_input_ = + pnh_->subscribe( + "input", 1, + bind(&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr())); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ConvexHull2D::unsubscribe() { - if (use_indices_) - { + if (use_indices_) { sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); - } - else + } else { sub_input_.shutdown(); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void - pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud, - const PointIndicesConstPtr &indices) +pcl_ros::ConvexHull2D::input_indices_callback( + const PointCloudConstPtr & cloud, + const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0) + if (pub_output_.getNumSubscribers() <= 0 && pub_plane_.getNumSubscribers() <= 0) { return; + } PointCloud output; // If cloud is given, check if it's valid - if (!isValid (cloud)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + if (!isValid(cloud)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); // Publish an empty message output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); return; } // If indices are given, check if they are valid - if (indices && !isValid (indices, "indices")) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + if (indices && !isValid(indices, "indices")) { + NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); // Publish an empty message output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); return; } /// DEBUG - if (indices) - NODELET_DEBUG ("[%s::input_indices_model_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + if (indices) { + NODELET_DEBUG( + "[%s::input_indices_model_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), + getMTPrivateNodeHandle().resolveName("input").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + getName().c_str(), cloud->width * cloud->height, fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), + getMTPrivateNodeHandle().resolveName("input").c_str()); + } // Reset the indices and surface pointers IndicesPtr indices_ptr; - if (indices) - indices_ptr.reset (new std::vector (indices->indices)); + if (indices) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices_ptr); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices_ptr); // Estimate the feature - impl_.reconstruct (output); + impl_.reconstruct(output); // If more than 3 points are present, send a PolygonStamped hull too - if (output.points.size () >= 3) - { + if (output.points.size() >= 3) { geometry_msgs::PolygonStamped poly; poly.header = fromPCL(output.header); - poly.polygon.points.resize (output.points.size ()); + poly.polygon.points.resize(output.points.size()); // Get three consecutive points (without copying) - pcl::Vector4fMap O = output.points[1].getVector4fMap (); - pcl::Vector4fMap B = output.points[0].getVector4fMap (); - pcl::Vector4fMap A = output.points[2].getVector4fMap (); + pcl::Vector4fMap O = output.points[1].getVector4fMap(); + pcl::Vector4fMap B = output.points[0].getVector4fMap(); + pcl::Vector4fMap A = output.points[2].getVector4fMap(); // Check the direction of points -- polygon must have CCW direction Eigen::Vector4f OA = A - O; Eigen::Vector4f OB = B - O; - Eigen::Vector4f N = OA.cross3 (OB); - double theta = N.dot (O); + Eigen::Vector4f N = OA.cross3(OB); + double theta = N.dot(O); bool reversed = false; - if (theta < (M_PI / 2.0)) + if (theta < (M_PI / 2.0)) { reversed = true; - for (size_t i = 0; i < output.points.size (); ++i) - { - if (reversed) - { - size_t j = output.points.size () - i - 1; + } + for (size_t i = 0; i < output.points.size(); ++i) { + if (reversed) { + size_t j = output.points.size() - i - 1; poly.polygon.points[i].x = output.points[j].x; poly.polygon.points[i].y = output.points[j].y; poly.polygon.points[i].z = output.points[j].z; - } - else - { + } else { poly.polygon.points[i].x = output.points[i].x; poly.polygon.points[i].y = output.points[i].y; poly.polygon.points[i].z = output.points[i].z; } } - pub_plane_.publish (boost::make_shared (poly)); + pub_plane_.publish(boost::make_shared(poly)); } // Publish a Boost shared ptr const data output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); } typedef pcl_ros::ConvexHull2D ConvexHull2D; PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index 26b6f3f6..a7daf9e3 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -40,192 +40,211 @@ #include ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::MovingLeastSquares::onInit () +pcl_ros::MovingLeastSquares::onInit() { - PCLNodelet::onInit (); - + PCLNodelet::onInit(); + //ros::NodeHandle private_nh = getMTPrivateNodeHandle (); - pub_output_ = advertise (*pnh_, "output", max_queue_size_); - pub_normals_ = advertise (*pnh_, "normals", max_queue_size_); - - //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) - if (!pnh_->getParam ("search_radius", search_radius_)) - { + pub_output_ = advertise(*pnh_, "output", max_queue_size_); + pub_normals_ = advertise(*pnh_, "normals", max_queue_size_); + + //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) + if (!pnh_->getParam("search_radius", search_radius_)) { //NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); - NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ()); + NODELET_ERROR( + "[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", + getName().c_str()); return; } - if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) - { - NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) { + NODELET_ERROR( + "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", + getName().c_str()); return; } // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 ); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &MovingLeastSquares::config_callback, this, _1, _2); + srv_->setCallback(f); // ---[ Optional parameters - pnh_->getParam ("use_indices", use_indices_); + pnh_->getParam("use_indices", use_indices_); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_indices : %s", - getName ().c_str (), - (use_indices_) ? "true" : "false"); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_indices : %s", + getName().c_str(), + (use_indices_) ? "true" : "false"); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::MovingLeastSquares::subscribe () +pcl_ros::MovingLeastSquares::subscribe() { // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { + if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", 1); + sub_input_filter_.subscribe(*pnh_, "input", 1); // If indices are enabled, subscribe to the indices - sub_indices_filter_.subscribe (*pnh_, "indices", 1); + sub_indices_filter_.subscribe(*pnh_, "indices", 1); - if (approximate_sync_) - { - sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); + if (approximate_sync_) { + sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); // surface not enabled, connect the input-indices duo and register - sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2)); - } - else - { - sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback( + bind( + &MovingLeastSquares::input_indices_callback, + this, _1, _2)); + } else { + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); // surface not enabled, connect the input-indices duo and register - sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2)); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback( + bind( + &MovingLeastSquares::input_indices_callback, + this, _1, _2)); } - } - else + } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", 1, bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ())); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::MovingLeastSquares::unsubscribe () -{ - if (use_indices_) - { - sub_input_filter_.unsubscribe (); - sub_indices_filter_.unsubscribe (); + sub_input_ = + pnh_->subscribe( + "input", 1, + bind(&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr())); } - else - sub_input_.shutdown (); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud, - const PointIndicesConstPtr &indices) +pcl_ros::MovingLeastSquares::unsubscribe() +{ + if (use_indices_) { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } else { + sub_input_.shutdown(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::MovingLeastSquares::input_indices_callback( + const PointCloudInConstPtr & cloud, + const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0) + if (pub_output_.getNumSubscribers() <= 0 && pub_normals_.getNumSubscribers() <= 0) { return; + } // Output points have the same type as the input, they are only smoothed PointCloudIn output; - + // Normals are also estimated and published on a separate topic - NormalCloudOut::Ptr normals (new NormalCloudOut ()); + NormalCloudOut::Ptr normals(new NormalCloudOut()); // If cloud is given, check if it's valid - if (!isValid (cloud)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); + if (!isValid(cloud)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); return; } // If indices are given, check if they are valid - if (indices && !isValid (indices, "indices")) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); + if (indices && !isValid(indices, "indices")) { + NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); return; } /// DEBUG - if (indices) - NODELET_DEBUG ("[%s::input_indices_model_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", - getName ().c_str (), - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), - indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); - else - NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + if (indices) { + NODELET_DEBUG( + "[%s::input_indices_model_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + getName().c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), + getMTPrivateNodeHandle().resolveName("input").c_str(), + indices->indices.size(), indices->header.stamp.toSec(), + indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str()); + } else { + NODELET_DEBUG( + "[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + getName().c_str(), cloud->width * cloud->height, fromPCL( + cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), + getMTPrivateNodeHandle().resolveName("input").c_str()); + } /// // Reset the indices and surface pointers - impl_.setInputCloud (pcl_ptr(cloud)); + impl_.setInputCloud(pcl_ptr(cloud)); IndicesPtr indices_ptr; - if (indices) - indices_ptr.reset (new std::vector (indices->indices)); + if (indices) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setIndices (indices_ptr); + impl_.setIndices(indices_ptr); // Initialize the spatial locator - + // Do the reconstructon //impl_.process (output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied output.header = cloud->header; - pub_output_.publish (ros_ptr(output.makeShared ())); + pub_output_.publish(ros_ptr(output.makeShared())); normals->header = cloud->header; - pub_normals_.publish (ros_ptr(normals)); + pub_normals_.publish(ros_ptr(normals)); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level) +pcl_ros::MovingLeastSquares::config_callback(MLSConfig & config, uint32_t level) { // \Note Zoli, shouldn't this be implemented in MLS too? /*if (k_ != config.k_search) { k_ = config.k_search; - NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_); + NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_); }*/ - if (search_radius_ != config.search_radius) - { + if (search_radius_ != config.search_radius) { search_radius_ = config.search_radius; - NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_); - impl_.setSearchRadius (search_radius_); + NODELET_DEBUG("[config_callback] Setting the search radius: %f.", search_radius_); + impl_.setSearchRadius(search_radius_); } - if (spatial_locator_type_ != config.spatial_locator) - { + if (spatial_locator_type_ != config.spatial_locator) { spatial_locator_type_ = config.spatial_locator; - NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_); + NODELET_DEBUG( + "[config_callback] Setting the spatial locator to type: %d.", + spatial_locator_type_); } - if (use_polynomial_fit_ != config.use_polynomial_fit) - { + if (use_polynomial_fit_ != config.use_polynomial_fit) { use_polynomial_fit_ = config.use_polynomial_fit; - NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_); - impl_.setPolynomialFit (use_polynomial_fit_); + NODELET_DEBUG( + "[config_callback] Setting the use_polynomial_fit flag to: %d.", + use_polynomial_fit_); + impl_.setPolynomialFit(use_polynomial_fit_); } - if (polynomial_order_ != config.polynomial_order) - { + if (polynomial_order_ != config.polynomial_order) { polynomial_order_ = config.polynomial_order; - NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_); - impl_.setPolynomialOrder (polynomial_order_); + NODELET_DEBUG("[config_callback] Setting the polynomial order to: %d.", polynomial_order_); + impl_.setPolynomialOrder(polynomial_order_); } - if (gaussian_parameter_ != config.gaussian_parameter) - { + if (gaussian_parameter_ != config.gaussian_parameter) { gaussian_parameter_ = config.gaussian_parameter; - NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_); - impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_); + NODELET_DEBUG("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_); + impl_.setSqrGaussParam(gaussian_parameter_ * gaussian_parameter_); } } diff --git a/pcl_ros/src/pcl_ros/surface/surface.cpp b/pcl_ros/src/pcl_ros/surface/surface.cpp index 13976f90..a73b242d 100644 --- a/pcl_ros/src/pcl_ros/surface/surface.cpp +++ b/pcl_ros/src/pcl_ros/surface/surface.cpp @@ -44,5 +44,3 @@ // Include the implementations here instead of compiling them separately to speed up compile time //#include "convex_hull.cpp" //#include "moving_least_squares.cpp" - - diff --git a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp index e3f4930e..ba92d794 100644 --- a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp +++ b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -58,7 +58,7 @@ typedef pcl::PointCloud PCDType; /// Sets pcl_stamp from stamp, BUT alters stamp /// a little to round it to millisecond. This is because converting back /// and forth from pcd to ros time induces some rounding errors. -void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp) +void setStamp(ros::Time & stamp, std::uint64_t & pcl_stamp) { // round to millisecond static const uint32_t mult = 1e6; @@ -71,7 +71,7 @@ void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp) { ros::Time t; pcl_conversions::fromPCL(pcl_stamp, t); - ROS_ASSERT_MSG(t==stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec); + ROS_ASSERT_MSG(t == stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec); } } @@ -79,18 +79,18 @@ class Notification { public: Notification(int expected_count) - : count_(0) - , expected_count_(expected_count) - , failure_count_(0) + : count_(0), + expected_count_(expected_count), + failure_count_(0) { } - void notify(const PCDType::ConstPtr& message) + void notify(const PCDType::ConstPtr & message) { ++count_; } - void failure(const PCDType::ConstPtr& message, FilterFailureReason reason) + void failure(const PCDType::ConstPtr & message, FilterFailureReason reason) { ++failure_count_; } @@ -144,7 +144,11 @@ TEST(MessageFilter, preexistingTransforms) ros::Time stamp = ros::Time::now(); setStamp(stamp, msg->header.stamp); - tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf::StampedTransform transform(tf::Transform( + tf::Quaternion(0, 0, 0, 1), tf::Vector3( + 1, 2, + 3)), stamp, "frame1", + "frame2"); tf_client.setTransform(transform); msg->header.frame_id = "frame2"; @@ -169,7 +173,11 @@ TEST(MessageFilter, postTransforms) EXPECT_EQ(0, n.count_); - tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf::StampedTransform transform(tf::Transform( + tf::Quaternion(0, 0, 0, 1), tf::Vector3( + 1, 2, + 3)), stamp, "frame1", + "frame2"); tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); @@ -190,8 +198,7 @@ TEST(MessageFilter, queueSize) std::uint64_t pcl_stamp; setStamp(stamp, pcl_stamp); - for (int i = 0; i < 20; ++i) - { + for (int i = 0; i < 20; ++i) { PCDType::Ptr msg(new PCDType); msg->header.stamp = pcl_stamp; msg->header.frame_id = "frame2"; @@ -202,7 +209,11 @@ TEST(MessageFilter, queueSize) EXPECT_EQ(0, n.count_); EXPECT_EQ(10, n.failure_count_); - tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf::StampedTransform transform(tf::Transform( + tf::Quaternion(0, 0, 0, 1), tf::Vector3( + 1, 2, + 3)), stamp, "frame1", + "frame2"); tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); @@ -224,7 +235,11 @@ TEST(MessageFilter, setTargetFrame) setStamp(stamp, msg->header.stamp); msg->header.frame_id = "frame2"; - tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2"); + tf::StampedTransform transform(tf::Transform( + tf::Quaternion(0, 0, 0, 1), tf::Vector3( + 1, 2, + 3)), stamp, "frame1000", + "frame2"); tf_client.setTransform(transform); filter.add(msg); @@ -249,7 +264,11 @@ TEST(MessageFilter, multipleTargetFrames) PCDType::Ptr msg(new PCDType); setStamp(stamp, msg->header.stamp); - tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3"); + tf::StampedTransform transform(tf::Transform( + tf::Quaternion(0, 0, 0, 1), tf::Vector3( + 1, 2, + 3)), stamp, "frame1", + "frame3"); tf_client.setTransform(transform); msg->header.frame_id = "frame3"; @@ -283,7 +302,11 @@ TEST(MessageFilter, tolerance) ros::Time stamp = ros::Time::now(); std::uint64_t pcl_stamp; setStamp(stamp, pcl_stamp); - tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf::StampedTransform transform(tf::Transform( + tf::Quaternion(0, 0, 0, 1), tf::Vector3( + 1, 2, + 3)), stamp, "frame1", + "frame2"); tf_client.setTransform(transform); PCDType::Ptr msg(new PCDType); @@ -295,7 +318,7 @@ TEST(MessageFilter, tolerance) //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1)); - transform.stamp_ += offset*1.1; + transform.stamp_ += offset * 1.1; tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); @@ -323,7 +346,11 @@ TEST(MessageFilter, outTheBackFailure) PCDType::Ptr msg(new PCDType); setStamp(stamp, msg->header.stamp); - tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); + tf::StampedTransform transform(tf::Transform( + tf::Quaternion(0, 0, 0, 1), tf::Vector3( + 1, 2, + 3)), stamp, "frame1", + "frame2"); tf_client.setTransform(transform); transform.stamp_ = stamp + ros::Duration(10000); @@ -359,16 +386,17 @@ TEST(MessageFilter, removeCallback) // TF filters; no data needs to be published boost::scoped_ptr tf_listener; - boost::scoped_ptr > tf_filter; + boost::scoped_ptr> tf_filter; spinner.start(); for (int i = 0; i < 3; ++i) { tf_listener.reset(new tf::TransformListener()); // Have callback fire at high rate to increase chances of race condition tf_filter.reset( - new tf::MessageFilter(*tf_listener, - "map", 5, threaded_nh, - ros::Duration(0.000001))); + new tf::MessageFilter( + *tf_listener, + "map", 5, threaded_nh, + ros::Duration(0.000001))); // Sleep and reset; sleeping increases chances of race condition ros::Duration(0.001).sleep(); @@ -378,7 +406,7 @@ TEST(MessageFilter, removeCallback) spinner.stop(); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index 76a93c4e..4eb23c3d 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -45,159 +45,145 @@ namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool -transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, - sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener) +transformPointCloud( + const std::string & target_frame, const sensor_msgs::PointCloud2 & in, + sensor_msgs::PointCloud2 & out, const tf::TransformListener & tf_listener) { - if (in.header.frame_id == target_frame) - { + if (in.header.frame_id == target_frame) { out = in; - return (true); + return true; } // Get the TF transform tf::StampedTransform transform; - try - { - tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform); - } - catch (tf::LookupException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); - } - catch (tf::ExtrapolationException &e) - { - ROS_ERROR ("%s", e.what ()); - return (false); + try { + tf_listener.lookupTransform(target_frame, in.header.frame_id, in.header.stamp, transform); + } catch (tf::LookupException & e) { + ROS_ERROR("%s", e.what()); + return false; + } catch (tf::ExtrapolationException & e) { + ROS_ERROR("%s", e.what()); + return false; } // Convert the TF transform to Eigen format Eigen::Matrix4f eigen_transform; - transformAsMatrix (transform, eigen_transform); + transformAsMatrix(transform, eigen_transform); - transformPointCloud (eigen_transform, in, out); + transformPointCloud(eigen_transform, in, out); out.header.frame_id = target_frame; - return (true); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void -transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, - const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) +void +transformPointCloud( + const std::string & target_frame, const tf::Transform & net_transform, + const sensor_msgs::PointCloud2 & in, sensor_msgs::PointCloud2 & out) { - if (in.header.frame_id == target_frame) - { + if (in.header.frame_id == target_frame) { out = in; return; } // Get the transformation Eigen::Matrix4f transform; - transformAsMatrix (net_transform, transform); + transformAsMatrix(net_transform, transform); - transformPointCloud (transform, in, out); + transformPointCloud(transform, in, out); out.header.frame_id = target_frame; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void -transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, - sensor_msgs::PointCloud2 &out) +void +transformPointCloud( + const Eigen::Matrix4f & transform, const sensor_msgs::PointCloud2 & in, + sensor_msgs::PointCloud2 & out) { // Get X-Y-Z indices - int x_idx = pcl::getFieldIndex (in, "x"); - int y_idx = pcl::getFieldIndex (in, "y"); - int z_idx = pcl::getFieldIndex (in, "z"); + int x_idx = pcl::getFieldIndex(in, "x"); + int y_idx = pcl::getFieldIndex(in, "y"); + int z_idx = pcl::getFieldIndex(in, "z"); - if (x_idx == -1 || y_idx == -1 || z_idx == -1) - { - ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); + if (x_idx == -1 || y_idx == -1 || z_idx == -1) { + ROS_ERROR("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); return; } - if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || - in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || - in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) + if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || + in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || + in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) { - ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported."); + ROS_ERROR("X-Y-Z coordinates not floats. Currently only floats are supported."); return; } // Check if distance is available - int dist_idx = pcl::getFieldIndex (in, "distance"); + int dist_idx = pcl::getFieldIndex(in, "distance"); // Copy the other data - if (&in != &out) - { + if (&in != &out) { out.header = in.header; out.height = in.height; - out.width = in.width; + out.width = in.width; out.fields = in.fields; out.is_bigendian = in.is_bigendian; - out.point_step = in.point_step; - out.row_step = in.row_step; - out.is_dense = in.is_dense; - out.data.resize (in.data.size ()); + out.point_step = in.point_step; + out.row_step = in.row_step; + out.is_dense = in.is_dense; + out.data.resize(in.data.size()); // Copy everything as it's faster than copying individual elements - memcpy (&out.data[0], &in.data[0], in.data.size ()); + memcpy(&out.data[0], &in.data[0], in.data.size()); } - Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0); + Eigen::Array4i xyz_offset(in.fields[x_idx].offset, in.fields[y_idx].offset, + in.fields[z_idx].offset, 0); - for (size_t i = 0; i < in.width * in.height; ++i) - { - Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1); + for (size_t i = 0; i < in.width * in.height; ++i) { + Eigen::Vector4f pt(*(float *)&in.data[xyz_offset[0]], *(float *)&in.data[xyz_offset[1]], + *(float *)&in.data[xyz_offset[2]], 1); Eigen::Vector4f pt_out; - + bool max_range_point = false; - int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; - float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); - if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) - { - if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point - { + int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset; + float * distance_ptr = (dist_idx < 0 ? NULL : (float *)(&in.data[distance_ptr_offset])); + if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { + if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point pt_out = pt; - } - else // max range point - { + } else { // max range point pt[0] = *distance_ptr; // Replace x with the x value saved in distance pt_out = transform * pt; max_range_point = true; //std::cout << pt[0]<<","< "<::quiet_NaN(); } - memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float)); - memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float)); - memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float)); - - + memcpy(&out.data[xyz_offset[0]], &pt_out[0], sizeof(float)); + memcpy(&out.data[xyz_offset[1]], &pt_out[1], sizeof(float)); + memcpy(&out.data[xyz_offset[2]], &pt_out[2], sizeof(float)); + + xyz_offset += in.point_step; } // Check if the viewpoint information is present - int vp_idx = pcl::getFieldIndex (in, "vp_x"); - if (vp_idx != -1) - { + int vp_idx = pcl::getFieldIndex(in, "vp_x"); + if (vp_idx != -1) { // Transform the viewpoint info too - for (size_t i = 0; i < out.width * out.height; ++i) - { - float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset]; + for (size_t i = 0; i < out.width * out.height; ++i) { + float * pstep = (float *)&out.data[i * out.point_step + out.fields[vp_idx].offset]; // Assume vp_x, vp_y, vp_z are consecutive - Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1); + Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1); Eigen::Vector4f vp_out = transform * vp_in; pstep[0] = vp_out[0]; @@ -209,73 +195,193 @@ transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointC ////////////////////////////////////////////////////////////////////////////////////////////// void -transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat) +transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat) { double mv[12]; - bt.getBasis ().getOpenGLSubMatrix (mv); + bt.getBasis().getOpenGLSubMatrix(mv); - tf::Vector3 origin = bt.getOrigin (); + tf::Vector3 origin = bt.getOrigin(); - out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; - out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; - out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; - - out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1; - out_mat (0, 3) = origin.x (); - out_mat (1, 3) = origin.y (); - out_mat (2, 3) = origin.z (); + out_mat(0, 0) = mv[0]; out_mat(0, 1) = mv[4]; out_mat(0, 2) = mv[8]; + out_mat(1, 0) = mv[1]; out_mat(1, 1) = mv[5]; out_mat(1, 2) = mv[9]; + out_mat(2, 0) = mv[2]; out_mat(2, 1) = mv[6]; out_mat(2, 2) = mv[10]; + + out_mat(3, 0) = out_mat(3, 1) = out_mat(3, 2) = 0; out_mat(3, 3) = 1; + out_mat(0, 3) = origin.x(); + out_mat(1, 3) = origin.y(); + out_mat(2, 3) = origin.z(); } } // namespace pcl_ros ////////////////////////////////////////////////////////////////////////////////////////////// -template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); ////////////////////////////////////////////////////////////////////////////////////////////// -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); ////////////////////////////////////////////////////////////////////////////////////////////// -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, + pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, + pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); - +template bool pcl_ros::transformPointCloud( + const std::string &, const ros::Time &, + const pcl::PointCloud &, + const std::string &, + pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, const ros::Time &, + const pcl::PointCloud &, + const std::string &, + pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index d48f2c58..7dd0dec9 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -61,14 +61,15 @@ typedef PointCloud::ConstPtr PointCloudConstPtr; /* ---[ */ int - main (int argc, char** argv) +main(int argc, char ** argv) { - ros::init (argc, argv, "bag_to_pcd"); - if (argc < 4) - { - std::cerr << "Syntax is: " << argv[0] << " []" << std::endl; - std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl; - return (-1); + ros::init(argc, argv, "bag_to_pcd"); + if (argc < 4) { + std::cerr << "Syntax is: " << argv[0] << + " []" << std::endl; + std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << + std::endl; + return -1; } // TF @@ -79,87 +80,77 @@ int rosbag::View view; rosbag::View::iterator view_it; - try - { - bag.open (argv[1], rosbag::bagmode::Read); - } - catch (rosbag::BagException) - { + try { + bag.open(argv[1], rosbag::bagmode::Read); + } catch (rosbag::BagException) { std::cerr << "Error opening file " << argv[1] << std::endl; - return (-1); + return -1; } - view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2")); - view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage")); - view.addQuery (bag, rosbag::TypeQuery ("tf2_msgs/TFMessage")); - view_it = view.begin (); + view.addQuery(bag, rosbag::TypeQuery("sensor_msgs/PointCloud2")); + view.addQuery(bag, rosbag::TypeQuery("tf/tfMessage")); + view.addQuery(bag, rosbag::TypeQuery("tf2_msgs/TFMessage")); + view_it = view.begin(); - std::string output_dir = std::string (argv[3]); - boost::filesystem::path outpath (output_dir); - if (!boost::filesystem::exists (outpath)) - { - if (!boost::filesystem::create_directories (outpath)) - { + std::string output_dir = std::string(argv[3]); + boost::filesystem::path outpath(output_dir); + if (!boost::filesystem::exists(outpath)) { + if (!boost::filesystem::create_directories(outpath)) { std::cerr << "Error creating directory " << output_dir << std::endl; - return (-1); + return -1; } std::cerr << "Creating directory " << output_dir << std::endl; } // Add the PointCloud2 handler - std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl; + std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << + output_dir << std::endl; PointCloud cloud_t; - ros::Duration r (0.001); + ros::Duration r(0.001); // Loop over the whole bag file - while (view_it != view.end ()) - { + while (view_it != view.end()) { // Handle TF messages first - tf::tfMessage::ConstPtr tf = view_it->instantiate (); - if (tf != NULL) - { - tf_broadcaster.sendTransform (tf->transforms); - ros::spinOnce (); - r.sleep (); - } - else - { + tf::tfMessage::ConstPtr tf = view_it->instantiate(); + if (tf != NULL) { + tf_broadcaster.sendTransform(tf->transforms); + ros::spinOnce(); + r.sleep(); + } else { // Get the PointCloud2 message - PointCloudConstPtr cloud = view_it->instantiate (); - if (cloud == NULL) - { + PointCloudConstPtr cloud = view_it->instantiate(); + if (cloud == NULL) { ++view_it; continue; } // If a target_frame was specified - if(argc > 4) - { + if (argc > 4) { // Transform it - if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener)) - { - ++view_it; - continue; + if (!pcl_ros::transformPointCloud(argv[4], *cloud, cloud_t, tf_listener)) { + ++view_it; + continue; } - } - else - { + } else { // Else, don't transform it cloud_t = *cloud; } - std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl; + std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << + cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList(cloud_t) << + std::endl; std::stringstream ss; ss << output_dir << "/" << cloud_t.header.stamp << ".pcd"; - std::cerr << "Data saved to " << ss.str () << std::endl; - pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (), - Eigen::Quaternionf::Identity (), true); + std::cerr << "Data saved to " << ss.str() << std::endl; + pcl::io::savePCDFile( + ss.str(), cloud_t, Eigen::Vector4f::Zero(), + Eigen::Quaternionf::Identity(), true); } // Increment the iterator ++view_it; } - return (0); + return 0; } /* ]--- */ diff --git a/pcl_ros/tools/convert_pcd_to_image.cpp b/pcl_ros/tools/convert_pcd_to_image.cpp index ff8e28af..e14249f0 100644 --- a/pcl_ros/tools/convert_pcd_to_image.cpp +++ b/pcl_ros/tools/convert_pcd_to_image.cpp @@ -56,41 +56,37 @@ /* ---[ */ int -main (int argc, char **argv) +main(int argc, char ** argv) { - ros::init (argc, argv, "image_publisher"); + ros::init(argc, argv, "image_publisher"); ros::NodeHandle nh; - ros::Publisher image_pub = nh.advertise ("output", 1); + ros::Publisher image_pub = nh.advertise("output", 1); - if (argc != 2) - { + if (argc != 2) { std::cout << "usage:\n" << argv[0] << " cloud.pcd" << std::endl; return 1; } sensor_msgs::Image image; sensor_msgs::PointCloud2 cloud; - pcl::io::loadPCDFile (std::string (argv[1]), cloud); + pcl::io::loadPCDFile(std::string(argv[1]), cloud); - try - { - pcl::toROSMsg (cloud, image); //convert the cloud - } - catch (std::runtime_error &e) - { - ROS_ERROR_STREAM("Error in converting cloud to image message: " - << e.what()); + try { + pcl::toROSMsg(cloud, image); //convert the cloud + } catch (std::runtime_error & e) { + ROS_ERROR_STREAM( + "Error in converting cloud to image message: " << + e.what()); return 1; //fail! } - ros::Rate loop_rate (5); - while (nh.ok ()) - { - image_pub.publish (image); - ros::spinOnce (); - loop_rate.sleep (); + ros::Rate loop_rate(5); + while (nh.ok()) { + image_pub.publish(image); + ros::spinOnce(); + loop_rate.sleep(); } - return (0); + return 0; } /* ]--- */ diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp index a7a4e1c1..3db4d31f 100644 --- a/pcl_ros/tools/convert_pointcloud_to_image.cpp +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -54,37 +54,37 @@ class PointCloudToImage { public: void - cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud) + cloud_cb(const sensor_msgs::PointCloud2ConstPtr & cloud) { - if (cloud->height <= 1) - { + if (cloud->height <= 1) { ROS_ERROR("Input point cloud is not organized, ignoring!"); return; } - try - { - pcl::toROSMsg (*cloud, image_); //convert the cloud + try { + pcl::toROSMsg(*cloud, image_); //convert the cloud image_.header = cloud->header; - image_pub_.publish (image_); //publish our cloud image - } - catch (std::runtime_error &e) - { - ROS_ERROR_STREAM("Error in converting cloud to image message: " - << e.what()); + image_pub_.publish(image_); //publish our cloud image + } catch (std::runtime_error & e) { + ROS_ERROR_STREAM( + "Error in converting cloud to image message: " << + e.what()); } } - PointCloudToImage () : cloud_topic_("input"),image_topic_("output") + PointCloudToImage() + : cloud_topic_("input"), image_topic_("output") { - sub_ = nh_.subscribe (cloud_topic_, 30, - &PointCloudToImage::cloud_cb, this); - image_pub_ = nh_.advertise (image_topic_, 30); + sub_ = nh_.subscribe( + cloud_topic_, 30, + &PointCloudToImage::cloud_cb, this); + image_pub_ = nh_.advertise(image_topic_, 30); //print some info about the node - std::string r_ct = nh_.resolveName (cloud_topic_); - std::string r_it = nh_.resolveName (image_topic_); - ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct ); - ROS_INFO_STREAM("Publishing image on topic " << r_it ); + std::string r_ct = nh_.resolveName(cloud_topic_); + std::string r_it = nh_.resolveName(image_topic_); + ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct); + ROS_INFO_STREAM("Publishing image on topic " << r_it); } + private: ros::NodeHandle nh_; sensor_msgs::Image image_; //cache the image message @@ -95,10 +95,10 @@ private: }; int -main (int argc, char **argv) +main(int argc, char ** argv) { - ros::init (argc, argv, "convert_pointcloud_to_image"); + ros::init(argc, argv, "convert_pointcloud_to_image"); PointCloudToImage pci; //this loads up the node - ros::spin (); //where she stops nobody knows + ros::spin(); //where she stops nobody knows return 0; } diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index 02ef1bbc..27b5628e 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -60,113 +60,112 @@ using namespace std; class PCDGenerator { - protected: - string tf_frame_; - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; - public: +protected: + string tf_frame_; + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; - // ROS messages - sensor_msgs::PointCloud2 cloud_; +public: + // ROS messages + sensor_msgs::PointCloud2 cloud_; - string file_name_, cloud_topic_; - double wait_; + string file_name_, cloud_topic_; + double wait_; - pcl_ros::Publisher pub_; + pcl_ros::Publisher pub_; - //////////////////////////////////////////////////////////////////////////////// - PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~") - { - // Maximum number of outgoing messages to be queued for delivery to subscribers = 1 + //////////////////////////////////////////////////////////////////////////////// + PCDGenerator() + : tf_frame_("/base_link"), private_nh_("~") + { + // Maximum number of outgoing messages to be queued for delivery to subscribers = 1 - cloud_topic_ = "cloud_pcd"; - pub_.advertise (nh_, cloud_topic_.c_str (), 1); - private_nh_.param("frame_id", tf_frame_, std::string("/base_link")); - ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str()); + cloud_topic_ = "cloud_pcd"; + pub_.advertise(nh_, cloud_topic_.c_str(), 1); + private_nh_.param("frame_id", tf_frame_, std::string("/base_link")); + ROS_INFO( + "Publishing data on topic %s with frame_id %s.", nh_.resolveName( + cloud_topic_).c_str(), tf_frame_.c_str()); + } + + //////////////////////////////////////////////////////////////////////////////// + // Start + int + start() + { + if (file_name_ == "" || pcl::io::loadPCDFile(file_name_, cloud_) == -1) { + return -1; } + cloud_.header.frame_id = tf_frame_; + return 0; + } - //////////////////////////////////////////////////////////////////////////////// - // Start - int - start () - { - if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1) - return (-1); - cloud_.header.frame_id = tf_frame_; - return (0); - } + //////////////////////////////////////////////////////////////////////////////// + // Spin (!) + bool spin() + { + int nr_points = cloud_.width * cloud_.height; + string fields_list = pcl::getFieldsList(cloud_); + double interval = wait_ * 1e+6; + while (nh_.ok()) { + ROS_DEBUG_ONCE( + "Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, + fields_list.c_str(), nh_.resolveName(cloud_topic_).c_str(), cloud_.header.frame_id.c_str()); + cloud_.header.stamp = ros::Time::now(); - //////////////////////////////////////////////////////////////////////////////// - // Spin (!) - bool spin () - { - int nr_points = cloud_.width * cloud_.height; - string fields_list = pcl::getFieldsList (cloud_); - double interval = wait_ * 1e+6; - while (nh_.ok ()) - { - ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ()); - cloud_.header.stamp = ros::Time::now (); - - if (pub_.getNumSubscribers () > 0) - { - ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ()); - pub_.publish (cloud_); - } - else - { - // check once a second if there is any subscriber - ros::Duration (1).sleep (); - continue; - } - - std::this_thread::sleep_for(std::chrono::microseconds(static_cast(interval))); - - if (interval == 0) // We only publish once if a 0 seconds interval is given - { - // Give subscribers 3 seconds until point cloud decays... a little ugly! - ros::Duration (3.0).sleep (); - break; - } + if (pub_.getNumSubscribers() > 0) { + ROS_DEBUG("Publishing data to %d subscribers.", pub_.getNumSubscribers()); + pub_.publish(cloud_); + } else { + // check once a second if there is any subscriber + ros::Duration(1).sleep(); + continue; + } + + std::this_thread::sleep_for(std::chrono::microseconds(static_cast(interval))); + + if (interval == 0) { // We only publish once if a 0 seconds interval is given + // Give subscribers 3 seconds until point cloud decays... a little ugly! + ros::Duration(3.0).sleep(); + break; } - return (true); } + return true; + } }; /* ---[ */ int - main (int argc, char** argv) +main(int argc, char ** argv) { - if (argc < 2) - { - std::cerr << "Syntax is: " << argv[0] << " [publishing_interval (in seconds)]" << std::endl; - return (-1); + if (argc < 2) { + std::cerr << "Syntax is: " << argv[0] << " [publishing_interval (in seconds)]" << + std::endl; + return -1; } - ros::init (argc, argv, "pcd_to_pointcloud"); + ros::init(argc, argv, "pcd_to_pointcloud"); PCDGenerator c; - c.file_name_ = string (argv[1]); + c.file_name_ = string(argv[1]); // check if publishing interval is given - if (argc == 2) - { - c.wait_ = 0; - } - else - { - c.wait_ = atof (argv[2]); - } - - if (c.start () == -1) - { - ROS_ERROR ("Could not load file %s. Exiting.", argv[1]); - return (-1); + if (argc == 2) { + c.wait_ = 0; + } else { + c.wait_ = atof(argv[2]); } - ROS_INFO ("Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.", c.cloud_.width * c.cloud_.height, c.cloud_.data.size (), pcl::getFieldsList (c.cloud_).c_str ()); - c.spin (); - return (0); + if (c.start() == -1) { + ROS_ERROR("Could not load file %s. Exiting.", argv[1]); + return -1; + } + ROS_INFO( + "Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.", + c.cloud_.width * c.cloud_.height, c.cloud_.data.size(), pcl::getFieldsList(c.cloud_).c_str()); + c.spin(); + + return 0; } /* ]--- */ diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index 2044b880..e2fb436f 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -64,124 +64,121 @@ Cloud Data) file format. **/ class PointCloudToPCD { - protected: - ros::NodeHandle nh_; +protected: + ros::NodeHandle nh_; - private: - std::string prefix_; - bool binary_; - bool compressed_; - std::string fixed_frame_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; +private: + std::string prefix_; + bool binary_; + bool compressed_; + std::string fixed_frame_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; - public: - string cloud_topic_; +public: + string cloud_topic_; - ros::Subscriber sub_; + ros::Subscriber sub_; - //////////////////////////////////////////////////////////////////////////////// - // Callback - void - cloud_cb (const boost::shared_ptr& cloud) - { - if ((cloud->width * cloud->height) == 0) + //////////////////////////////////////////////////////////////////////////////// + // Callback + void + cloud_cb(const boost::shared_ptr & cloud) + { + if ((cloud->width * cloud->height) == 0) { + return; + } + + ROS_INFO( + "Received %d data points in frame %s with the following fields: %s", + (int)cloud->width * cloud->height, + cloud->header.frame_id.c_str(), + pcl::getFieldsList(*cloud).c_str()); + + Eigen::Vector4f v = Eigen::Vector4f::Zero(); + Eigen::Quaternionf q = Eigen::Quaternionf::Identity(); + if (!fixed_frame_.empty()) { + if (!tf_buffer_.canTransform( + fixed_frame_, cloud->header.frame_id, + pcl_conversions::fromPCL(cloud->header.stamp), ros::Duration(3.0))) + { + ROS_WARN("Could not get transform!"); return; - - ROS_INFO ("Received %d data points in frame %s with the following fields: %s", - (int)cloud->width * cloud->height, - cloud->header.frame_id.c_str (), - pcl::getFieldsList (*cloud).c_str ()); - - Eigen::Vector4f v = Eigen::Vector4f::Zero (); - Eigen::Quaternionf q = Eigen::Quaternionf::Identity (); - if (!fixed_frame_.empty ()) { - if (!tf_buffer_.canTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp), ros::Duration (3.0))) { - ROS_WARN("Could not get transform!"); - return; - } - - Eigen::Affine3d transform; - transform = tf2::transformToEigen (tf_buffer_.lookupTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp))); - v = Eigen::Vector4f::Zero (); - v.head<3> () = transform.translation ().cast (); - q = transform.rotation ().cast (); } - std::stringstream ss; - ss << prefix_ << cloud->header.stamp << ".pcd"; - ROS_INFO ("Data saved to %s", ss.str ().c_str ()); - - pcl::PCDWriter writer; - if(binary_) - { - if(compressed_) - { - writer.writeBinaryCompressed (ss.str (), *cloud, v, q); - } - else - { - writer.writeBinary (ss.str (), *cloud, v, q); - } - } - else - { - writer.writeASCII (ss.str (), *cloud, v, q, 8); - } - + Eigen::Affine3d transform; + transform = + tf2::transformToEigen( + tf_buffer_.lookupTransform( + fixed_frame_, cloud->header.frame_id, + pcl_conversions::fromPCL(cloud->header.stamp))); + v = Eigen::Vector4f::Zero(); + v.head<3>() = transform.translation().cast(); + q = transform.rotation().cast(); } - //////////////////////////////////////////////////////////////////////////////// - PointCloudToPCD () : binary_(false), compressed_(false), tf_listener_(tf_buffer_) - { - // Check if a prefix parameter is defined for output file names. - ros::NodeHandle priv_nh("~"); - if (priv_nh.getParam ("prefix", prefix_)) - { - ROS_INFO_STREAM ("PCD file prefix is: " << prefix_); - } - else if (nh_.getParam ("prefix", prefix_)) - { - ROS_WARN_STREAM ("Non-private PCD prefix parameter is DEPRECATED: " - << prefix_); - } + std::stringstream ss; + ss << prefix_ << cloud->header.stamp << ".pcd"; + ROS_INFO("Data saved to %s", ss.str().c_str()); - priv_nh.getParam ("fixed_frame", fixed_frame_); - priv_nh.getParam ("binary", binary_); - priv_nh.getParam ("compressed", compressed_); - if(binary_) - { - if(compressed_) - { - ROS_INFO_STREAM ("Saving as binary compressed PCD"); - } - else - { - ROS_INFO_STREAM ("Saving as binary PCD"); - } - } - else - { - ROS_INFO_STREAM ("Saving as binary PCD"); - } - - cloud_topic_ = "input"; - - sub_ = nh_.subscribe (cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this); - ROS_INFO ("Listening for incoming data on topic %s", - nh_.resolveName (cloud_topic_).c_str ()); + pcl::PCDWriter writer; + if (binary_) { + if (compressed_) { + writer.writeBinaryCompressed(ss.str(), *cloud, v, q); + } else { + writer.writeBinary(ss.str(), *cloud, v, q); + } + } else { + writer.writeASCII(ss.str(), *cloud, v, q, 8); } + + } + + //////////////////////////////////////////////////////////////////////////////// + PointCloudToPCD() + : binary_(false), compressed_(false), tf_listener_(tf_buffer_) + { + // Check if a prefix parameter is defined for output file names. + ros::NodeHandle priv_nh("~"); + if (priv_nh.getParam("prefix", prefix_)) { + ROS_INFO_STREAM("PCD file prefix is: " << prefix_); + } else if (nh_.getParam("prefix", prefix_)) { + ROS_WARN_STREAM( + "Non-private PCD prefix parameter is DEPRECATED: " << + prefix_); + } + + priv_nh.getParam("fixed_frame", fixed_frame_); + priv_nh.getParam("binary", binary_); + priv_nh.getParam("compressed", compressed_); + if (binary_) { + if (compressed_) { + ROS_INFO_STREAM("Saving as binary compressed PCD"); + } else { + ROS_INFO_STREAM("Saving as binary PCD"); + } + } else { + ROS_INFO_STREAM("Saving as binary PCD"); + } + + cloud_topic_ = "input"; + + sub_ = nh_.subscribe(cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this); + ROS_INFO( + "Listening for incoming data on topic %s", + nh_.resolveName(cloud_topic_).c_str()); + } }; /* ---[ */ int - main (int argc, char** argv) +main(int argc, char ** argv) { - ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName); + ros::init(argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName); PointCloudToPCD b; - ros::spin (); + ros::spin(); - return (0); + return 0; } /* ]--- */ From 9689971aee57e70f775a8342a4d2f8eec73920f5 Mon Sep 17 00:00:00 2001 From: Sean Kelly Date: Sun, 9 Aug 2020 19:47:01 -0400 Subject: [PATCH 368/405] Make ament_flake8 pass (#299) Set of hand-made changes to let ament_flake8 pass --- pcl_ros/cfg/SACSegmentation_common.py | 55 ++++++++++++++++++++------- pcl_ros/cfg/common.py | 41 ++++++++++++++------ 2 files changed, 71 insertions(+), 25 deletions(-) diff --git a/pcl_ros/cfg/SACSegmentation_common.py b/pcl_ros/cfg/SACSegmentation_common.py index d0938904..9bf8da1c 100644 --- a/pcl_ros/cfg/SACSegmentation_common.py +++ b/pcl_ros/cfg/SACSegmentation_common.py @@ -1,20 +1,47 @@ #! /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 str_t + # set up parameters that we care about PACKAGE = 'pcl_ros' -from dynamic_reconfigure.parameter_generator_catkin import *; - -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", 50, 0, 100000) - 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", 0.02, 0, 1.0) - 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)", 0.0, 0, 1.0) - 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.", 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.", 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.", "") +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", + 50, 0, 100000) + 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", + 0.02, 0, 1.0) + 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)", + 0.0, 0, 1.0) + 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."), + 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.", + 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."), + "") diff --git a/pcl_ros/cfg/common.py b/pcl_ros/cfg/common.py index e04347ab..43c49554 100644 --- a/pcl_ros/cfg/common.py +++ b/pcl_ros/cfg/common.py @@ -1,17 +1,36 @@ #! /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 + # set up parameters that we care about PACKAGE = 'pcl_ros' -from dynamic_reconfigure.parameter_generator_catkin import *; - -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", 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", 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].", 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.", 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.", "") +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", + 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", + 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]."), + 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."), + 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."), + "") From 420f5b032b165c8d1f53219d163b5371a5d9e3fa Mon Sep 17 00:00:00 2001 From: Sean Kelly Date: Sun, 9 Aug 2020 19:47:21 -0400 Subject: [PATCH 369/405] Make ament_cpplint pass (#298) Collection of hand-made changes to make ament_cpplint pass consisting of: - whitespace between comments - line length - header ordering - include guard formats - remove a couple `using namespace std;` - using c++ casts instead of c-style casts Signed-off-by: Sean Kelly --- pcl_ros/include/pcl_ros/features/boundary.hpp | 8 +- pcl_ros/include/pcl_ros/features/feature.hpp | 27 ++-- pcl_ros/include/pcl_ros/features/fpfh.hpp | 8 +- pcl_ros/include/pcl_ros/features/fpfh_omp.hpp | 8 +- .../pcl_ros/features/moment_invariants.hpp | 8 +- .../include/pcl_ros/features/normal_3d.hpp | 8 +- .../pcl_ros/features/normal_3d_omp.hpp | 8 +- .../pcl_ros/features/normal_3d_tbb.hpp | 14 +- pcl_ros/include/pcl_ros/features/pfh.hpp | 8 +- .../pcl_ros/features/principal_curvatures.hpp | 8 +- pcl_ros/include/pcl_ros/features/shot.hpp | 8 +- pcl_ros/include/pcl_ros/features/shot_omp.hpp | 8 +- pcl_ros/include/pcl_ros/features/vfh.hpp | 8 +- pcl_ros/include/pcl_ros/filters/crop_box.hpp | 10 +- .../pcl_ros/filters/extract_indices.hpp | 8 +- pcl_ros/include/pcl_ros/filters/filter.hpp | 30 ++-- .../include/pcl_ros/filters/passthrough.hpp | 8 +- .../pcl_ros/filters/project_inliers.hpp | 11 +- .../filters/radius_outlier_removal.hpp | 8 +- .../filters/statistical_outlier_removal.hpp | 8 +- .../include/pcl_ros/filters/voxel_grid.hpp | 8 +- pcl_ros/include/pcl_ros/impl/transforms.hpp | 20 +-- pcl_ros/include/pcl_ros/io/bag_io.hpp | 12 +- .../include/pcl_ros/io/concatenate_data.hpp | 16 +- .../include/pcl_ros/io/concatenate_fields.hpp | 13 +- pcl_ros/include/pcl_ros/io/pcd_io.hpp | 9 +- pcl_ros/include/pcl_ros/pcl_nodelet.hpp | 42 +++-- pcl_ros/include/pcl_ros/point_cloud.hpp | 99 ++++++++---- pcl_ros/include/pcl_ros/publisher.hpp | 16 +- .../pcl_ros/segmentation/extract_clusters.hpp | 15 +- .../extract_polygonal_prism_data.hpp | 14 +- .../pcl_ros/segmentation/sac_segmentation.hpp | 46 +++--- .../segmentation/segment_differences.hpp | 12 +- .../include/pcl_ros/surface/convex_hull.hpp | 14 +- .../pcl_ros/surface/moving_least_squares.hpp | 30 ++-- pcl_ros/include/pcl_ros/transforms.hpp | 9 +- pcl_ros/src/pcl_ros/features/feature.cpp | 139 ++++++++++------ .../src/pcl_ros/features/normal_3d_tbb.cpp | 2 +- pcl_ros/src/pcl_ros/filters/crop_box.cpp | 5 +- .../src/pcl_ros/filters/features/feature.cpp | 151 +++++++++++------- .../filters/features/normal_3d_tbb.cpp | 2 +- pcl_ros/src/pcl_ros/filters/filter.cpp | 37 +++-- pcl_ros/src/pcl_ros/filters/passthrough.cpp | 13 +- .../src/pcl_ros/filters/project_inliers.cpp | 24 ++- .../filters/radius_outlier_removal.cpp | 1 - .../filters/statistical_outlier_removal.cpp | 3 +- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 9 +- pcl_ros/src/pcl_ros/io/bag_io.cpp | 1 + pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 5 +- pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 25 +-- pcl_ros/src/pcl_ros/io/io.cpp | 15 +- pcl_ros/src/pcl_ros/io/pcd_io.cpp | 1 + .../pcl_ros/segmentation/extract_clusters.cpp | 39 +++-- .../extract_polygonal_prism_data.cpp | 37 +++-- .../pcl_ros/segmentation/sac_segmentation.cpp | 103 +++++++----- .../segmentation/segment_differences.cpp | 26 +-- .../src/pcl_ros/segmentation/segmentation.cpp | 8 +- pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 22 ++- .../pcl_ros/surface/moving_least_squares.cpp | 34 ++-- pcl_ros/src/pcl_ros/surface/surface.cpp | 4 +- .../src/test/test_tf_message_filter_pcl.cpp | 40 ++--- pcl_ros/src/transforms.cpp | 24 +-- pcl_ros/tools/bag_to_pcd.cpp | 7 +- pcl_ros/tools/convert_pcd_to_image.cpp | 5 +- pcl_ros/tools/convert_pointcloud_to_image.cpp | 28 ++-- pcl_ros/tools/pcd_to_pointcloud.cpp | 21 ++- pcl_ros/tools/pointcloud_to_pcd.cpp | 6 +- 67 files changed, 831 insertions(+), 593 deletions(-) diff --git a/pcl_ros/include/pcl_ros/features/boundary.hpp b/pcl_ros/include/pcl_ros/features/boundary.hpp index e226359e..5b9a574d 100644 --- a/pcl_ros/include/pcl_ros/features/boundary.hpp +++ b/pcl_ros/include/pcl_ros/features/boundary.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_BOUNDARY_H_ -#define PCL_ROS_BOUNDARY_H_ +#ifndef PCL_ROS__FEATURES__BOUNDARY_HPP_ +#define PCL_ROS__FEATURES__BOUNDARY_HPP_ #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true @@ -82,6 +82,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_BOUNDARY_H_ +#endif // PCL_ROS__FEATURES__BOUNDARY_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/feature.hpp b/pcl_ros/include/pcl_ros/features/feature.hpp index bf1a9d83..8497330a 100644 --- a/pcl_ros/include/pcl_ros/features/feature.hpp +++ b/pcl_ros/include/pcl_ros/features/feature.hpp @@ -35,23 +35,24 @@ * */ -#ifndef PCL_ROS_FEATURE_H_ -#define PCL_ROS_FEATURE_H_ +#ifndef PCL_ROS__FEATURES__FEATURE_HPP_ +#define PCL_ROS__FEATURES__FEATURE_HPP_ // PCL includes #include #include -#include "pcl_ros/pcl_nodelet.hpp" #include // Dynamic reconfigure #include -#include "pcl_ros/FeatureConfig.hpp" // PCL conversions #include +#include "pcl_ros/pcl_nodelet.hpp" +#include "pcl_ros/FeatureConfig.hpp" + namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; @@ -84,13 +85,15 @@ public: protected: /** \brief The input point cloud dataset. */ - //PointCloudInConstPtr input_; + // PointCloudInConstPtr input_; /** \brief A pointer to the vector of point indices to use. */ - //IndicesConstPtr indices_; + // IndicesConstPtr indices_; - /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ - //PointCloudInConstPtr surface_; + /** \brief An input point cloud describing the surface that is to be used + * for nearest neighbors estimation. + */ + // PointCloudInConstPtr surface_; /** \brief A pointer to the spatial search object. */ KdTreePtr tree_; @@ -108,7 +111,9 @@ protected: /** \brief The input PointCloud subscriber. */ ros::Subscriber sub_input_; - /** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */ + /** \brief Set to true if the nodelet needs to listen for incoming point + * clouds representing the search surface. + */ bool use_surface_; /** \brief Parameter for the spatial locator tree. By convention, the values represent: @@ -257,6 +262,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_FEATURE_H_ +#endif // PCL_ROS__FEATURES__FEATURE_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/fpfh.hpp b/pcl_ros/include/pcl_ros/features/fpfh.hpp index 98d45ab6..b112439c 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh.hpp +++ b/pcl_ros/include/pcl_ros/features/fpfh.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_FPFH_H_ -#define PCL_ROS_FPFH_H_ +#ifndef PCL_ROS__FEATURES__FPFH_HPP_ +#define PCL_ROS__FEATURES__FPFH_HPP_ #include #include "pcl_ros/features/pfh.hpp" @@ -94,6 +94,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_FPFH_H_ +#endif // PCL_ROS__FEATURES__FPFH_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp b/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp index c3c88cf0..6192b3f3 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_FPFH_OMP_H_ -#define PCL_ROS_FPFH_OMP_H_ +#ifndef PCL_ROS__FEATURES__FPFH_OMP_HPP_ +#define PCL_ROS__FEATURES__FPFH_OMP_HPP_ #include #include "pcl_ros/features/fpfh.hpp" @@ -91,6 +91,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_FPFH_OMP_H_ +#endif // PCL_ROS__FEATURES__FPFH_OMP_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/moment_invariants.hpp b/pcl_ros/include/pcl_ros/features/moment_invariants.hpp index 1c48ec47..0235d05d 100644 --- a/pcl_ros/include/pcl_ros/features/moment_invariants.hpp +++ b/pcl_ros/include/pcl_ros/features/moment_invariants.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ -#define PCL_ROS_MOMENT_INVARIANTS_H_ +#ifndef PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_ +#define PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_ #include #include "pcl_ros/features/feature.hpp" @@ -77,6 +77,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ +#endif // PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/normal_3d.hpp b/pcl_ros/include/pcl_ros/features/normal_3d.hpp index 8c6d5626..bf4c40f1 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_NORMAL_3D_H_ -#define PCL_ROS_NORMAL_3D_H_ +#ifndef PCL_ROS__FEATURES__NORMAL_3D_HPP_ +#define PCL_ROS__FEATURES__NORMAL_3D_HPP_ #include #include "pcl_ros/features/feature.hpp" @@ -81,6 +81,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_NORMAL_3D_H_ +#endif // PCL_ROS__FEATURES__NORMAL_3D_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp b/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp index 4b96a87c..26300446 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_NORMAL_3D_OMP_H_ -#define PCL_ROS_NORMAL_3D_OMP_H_ +#ifndef PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_ +#define PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_ #include #include "pcl_ros/features/normal_3d.hpp" @@ -75,6 +75,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_ +#endif // PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp index fcc41029..9ebb4e06 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp @@ -35,11 +35,11 @@ * */ -#ifndef PCL_ROS_NORMAL_3D_TBB_H_ -#define PCL_ROS_NORMAL_3D_TBB_H_ +#ifndef PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_ +#define PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_ -//#include "pcl_ros/pcl_ros_config.hpp" -//#if defined(HAVE_TBB) +// #include "pcl_ros/pcl_ros_config.hpp" +// #if defined(HAVE_TBB) #include #include "pcl_ros/features/normal_3d.hpp" @@ -78,8 +78,8 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -//#endif // HAVE_TBB +// #endif // HAVE_TBB -#endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_ +#endif // PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/pfh.hpp b/pcl_ros/include/pcl_ros/features/pfh.hpp index ff6490dc..41a32ab9 100644 --- a/pcl_ros/include/pcl_ros/features/pfh.hpp +++ b/pcl_ros/include/pcl_ros/features/pfh.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_PFH_H_ -#define PCL_ROS_PFH_H_ +#ifndef PCL_ROS__FEATURES__PFH_HPP_ +#define PCL_ROS__FEATURES__PFH_HPP_ #include #include "pcl_ros/features/feature.hpp" @@ -94,6 +94,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_PFH_H_ +#endif // PCL_ROS__FEATURES__PFH_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp b/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp index e150189d..08e89b9b 100644 --- a/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp +++ b/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ -#define PCL_ROS_PRINCIPAL_CURVATURES_H_ +#ifndef PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_ +#define PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_ #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true #include @@ -80,6 +80,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ +#endif // PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/shot.hpp b/pcl_ros/include/pcl_ros/features/shot.hpp index a3cdb3e1..35d6e6f5 100644 --- a/pcl_ros/include/pcl_ros/features/shot.hpp +++ b/pcl_ros/include/pcl_ros/features/shot.hpp @@ -34,8 +34,8 @@ * */ -#ifndef PCL_ROS_SHOT_H_ -#define PCL_ROS_SHOT_H_ +#ifndef PCL_ROS__FEATURES__SHOT_HPP_ +#define PCL_ROS__FEATURES__SHOT_HPP_ #include #include "pcl_ros/features/pfh.hpp" @@ -74,6 +74,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_SHOT_H_ +#endif // PCL_ROS__FEATURES__SHOT_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/shot_omp.hpp b/pcl_ros/include/pcl_ros/features/shot_omp.hpp index 77d6a3f8..af202b2a 100644 --- a/pcl_ros/include/pcl_ros/features/shot_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/shot_omp.hpp @@ -34,8 +34,8 @@ * */ -#ifndef PCL_ROS_SHOT_OMP_H_ -#define PCL_ROS_SHOT_OMP_H_ +#ifndef PCL_ROS__FEATURES__SHOT_OMP_HPP_ +#define PCL_ROS__FEATURES__SHOT_OMP_HPP_ #include #include "pcl_ros/features/shot.hpp" @@ -73,6 +73,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_SHOT_OMP_H_ +#endif // PCL_ROS__FEATURES__SHOT_OMP_HPP_ diff --git a/pcl_ros/include/pcl_ros/features/vfh.hpp b/pcl_ros/include/pcl_ros/features/vfh.hpp index efe5d631..d2e69670 100644 --- a/pcl_ros/include/pcl_ros/features/vfh.hpp +++ b/pcl_ros/include/pcl_ros/features/vfh.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_FEATURES_VFH_H_ -#define PCL_ROS_FEATURES_VFH_H_ +#ifndef PCL_ROS__FEATURES__VFH_HPP_ +#define PCL_ROS__FEATURES__VFH_HPP_ #include #include "pcl_ros/features/fpfh.hpp" @@ -79,6 +79,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_FEATURES_VFH_H_ +#endif // PCL_ROS__FEATURES__VFH_HPP_ diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/pcl_ros/include/pcl_ros/filters/crop_box.hpp index 234a9a07..3665521a 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.hpp +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -36,8 +36,8 @@ * */ -#ifndef PCL_ROS_FILTERS_CROPBOX_H_ -#define PCL_ROS_FILTERS_CROPBOX_H_ +#ifndef PCL_ROS__FILTERS__CROP_BOX_HPP_ +#define PCL_ROS__FILTERS__CROP_BOX_HPP_ // PCL includes #include @@ -58,7 +58,7 @@ class CropBox : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; // TODO + boost::shared_ptr> srv_; // TODO(xxx) /** \brief Call the actual filter. * \param input the input point cloud dataset @@ -101,6 +101,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_ +#endif // PCL_ROS__FILTERS__CROP_BOX_HPP_ diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp index 1eeddb38..95740f60 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ -#define PCL_ROS_FILTERS_EXTRACTINDICES_H_ +#ifndef PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ +#define PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ // PCL includes #include @@ -94,6 +94,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ +#endif // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index 40955e25..e589c0cb 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -35,23 +35,21 @@ * */ -#ifndef PCL_ROS_FILTER_H_ -#define PCL_ROS_FILTER_H_ +#ifndef PCL_ROS__FILTERS__FILTER_HPP_ +#define PCL_ROS__FILTERS__FILTER_HPP_ -// PCL includes #include -#include "pcl_ros/pcl_nodelet.hpp" - -// Dynamic reconfigure #include +#include +#include "pcl_ros/pcl_nodelet.hpp" #include "pcl_ros/FilterConfig.hpp" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; -/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters - * are defined here as static methods. +/** \brief @b Filter represents the base filter class. Some generic 3D operations that are + * applicable to all filters are defined here as static methods. * \author Radu Bogdan Rusu */ class Filter : public PCLNodelet @@ -79,16 +77,22 @@ protected: /** \brief The maximum allowed filter value a point will be considered from. */ double filter_limit_max_; - /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ + /** \brief Set to true if we want to return the data outside + * (\a filter_limit_min_;\a filter_limit_max_). Default: false. + */ bool filter_limit_negative_; - /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + /** \brief The input TF frame the data should be transformed into, + * if input.header.frame_id is different. + */ std::string tf_input_frame_; /** \brief The original data input TF frame. */ std::string tf_input_orig_frame_; - /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + /** \brief The output TF frame the data should be transformed into, + * if input.header.frame_id is different. + */ std::string tf_output_frame_; /** \brief Internal mutex. */ @@ -157,6 +161,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_FILTER_H_ +#endif // PCL_ROS__FILTERS__FILTER_HPP_ diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index 515fb897..f85d3810 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.hpp +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ -#define PCL_ROS_FILTERS_PASSTHROUGH_H_ +#ifndef PCL_ROS__FILTERS__PASSTHROUGH_HPP_ +#define PCL_ROS__FILTERS__PASSTHROUGH_HPP_ // PCL includes #include @@ -95,6 +95,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ +#endif // PCL_ROS__FILTERS__PASSTHROUGH_HPP_ diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp index d17d2466..3c25846e 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -35,14 +35,13 @@ * */ -#ifndef PCL_ROS_FILTERS_PROJECT_INLIERS_H_ -#define PCL_ROS_FILTERS_PROJECT_INLIERS_H_ +#ifndef PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ +#define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ -// PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -#include namespace pcl_ros { @@ -115,6 +114,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_ +#endif // PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ 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 c773ccc4..420f4df5 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_ -#define PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_ +#ifndef PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_ +#define PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_ // PCL includes #include @@ -96,6 +96,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_ +#endif // PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_ 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 2ed4d167..80f4ea64 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_ -#define PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_ +#ifndef PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_ +#define PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_ // PCL includes #include @@ -103,6 +103,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_ +#endif // PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_ diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index 398be306..54c7c459 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_ROS_FILTERS_VOXEL_H_ -#define PCL_ROS_FILTERS_VOXEL_H_ +#ifndef PCL_ROS__FILTERS__VOXEL_GRID_HPP_ +#define PCL_ROS__FILTERS__VOXEL_GRID_HPP_ // PCL includes #include @@ -86,6 +86,6 @@ protected: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_ +#endif // PCL_ROS__FILTERS__VOXEL_GRID_HPP_ diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index 794a8d93..2f2a7580 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -34,10 +34,11 @@ * */ -#ifndef pcl_ros_IMPL_TRANSFORMS_H_ -#define pcl_ros_IMPL_TRANSFORMS_H_ +#ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_ +#define PCL_ROS__IMPL__TRANSFORMS_HPP_ #include +#include #include "pcl_ros/transforms.hpp" using pcl_conversions::fromPCL; @@ -58,13 +59,13 @@ transformPointCloudWithNormals( // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. tf::Quaternion q = transform.getRotation(); - Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) + Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) tf::Vector3 v = transform.getOrigin(); Eigen::Vector3f origin(v.x(), v.y(), v.z()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform - //Eigen::Transform3f t; - //t = translation * rotation; + // Eigen::Transform3f t; + // t = translation * rotation; transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation); } @@ -81,13 +82,13 @@ transformPointCloud( // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. tf::Quaternion q = transform.getRotation(); - Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) + Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) tf::Vector3 v = transform.getOrigin(); Eigen::Vector3f origin(v.x(), v.y(), v.z()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform - //Eigen::Transform3f t; - //t = translation * rotation; + // Eigen::Transform3f t; + // t = translation * rotation; transformPointCloud(cloud_in, cloud_out, origin, rotation); } @@ -217,6 +218,5 @@ transformPointCloudWithNormals( cloud_out.header = toPCL(header); return true; } - } // namespace pcl_ros -#endif +#endif // PCL_ROS__IMPL__TRANSFORMS_HPP_ diff --git a/pcl_ros/include/pcl_ros/io/bag_io.hpp b/pcl_ros/include/pcl_ros/io/bag_io.hpp index 17a936da..6f5add21 100644 --- a/pcl_ros/include/pcl_ros/io/bag_io.hpp +++ b/pcl_ros/include/pcl_ros/io/bag_io.hpp @@ -35,13 +35,14 @@ * */ -#ifndef PCL_ROS_IO_BAG_IO_H_ -#define PCL_ROS_IO_BAG_IO_H_ +#ifndef PCL_ROS__IO__BAG_IO_HPP_ +#define PCL_ROS__IO__BAG_IO_HPP_ #include #include #include #include +#include namespace pcl_ros { @@ -120,12 +121,11 @@ private: PointCloudPtr output_; /** \brief Signals that a new PointCloud2 message has been read from the file. */ - //bool cloud_received_; + // bool cloud_received_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +} // namespace pcl_ros -} - -#endif //#ifndef PCL_ROS_IO_BAG_IO_H_ +#endif // PCL_ROS__IO__BAG_IO_HPP_ diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.hpp b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp index 00f54fdd..62aae16d 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_data.hpp +++ b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_IO_CONCATENATE_DATA_H_ -#define PCL_IO_CONCATENATE_DATA_H_ +#ifndef PCL_ROS__IO__CONCATENATE_DATA_HPP_ +#define PCL_ROS__IO__CONCATENATE_DATA_HPP_ // ROS includes #include @@ -46,6 +46,8 @@ #include #include #include +#include +#include namespace pcl_ros { @@ -71,7 +73,7 @@ public: /** \brief Empty constructor. * \param queue_size the maximum queue size */ - PointCloudConcatenateDataSynchronizer(int queue_size) + explicit PointCloudConcatenateDataSynchronizer(int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {} /** \brief Empty destructor. */ @@ -88,7 +90,9 @@ private: /** \brief The maximum number of messages that we can store in the queue. */ int maximum_queue_size_; - /** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ + /** \brief True if we use an approximate time synchronizer + * versus an exact one (false by default). + */ bool approximate_sync_; /** \brief A vector of message filters. */ @@ -137,6 +141,6 @@ private: void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out); }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_IO_CONCATENATE_H_ +#endif // PCL_ROS__IO__CONCATENATE_DATA_HPP_ diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp index 2b81af4c..00b53160 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp @@ -35,8 +35,8 @@ * */ -#ifndef PCL_IO_CONCATENATE_FIELDS_H_ -#define PCL_IO_CONCATENATE_FIELDS_H_ +#ifndef PCL_ROS__IO__CONCATENATE_FIELDS_HPP_ +#define PCL_ROS__IO__CONCATENATE_FIELDS_HPP_ // ROS includes #include @@ -47,6 +47,9 @@ #include +#include +#include + namespace pcl_ros { /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of @@ -68,7 +71,7 @@ public: /** \brief Empty constructor. * \param queue_size the maximum queue size */ - PointCloudConcatenateFieldsSynchronizer(int queue_size) + explicit PointCloudConcatenateFieldsSynchronizer(int queue_size) : maximum_queue_size_(queue_size), maximum_seconds_(0) {} /** \brief Empty destructor. */ @@ -98,6 +101,6 @@ private: /** \brief A queue for messages. */ std::map> queue_; }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_IO_CONCATENATE_H_ +#endif // PCL_ROS__IO__CONCATENATE_FIELDS_HPP_ diff --git a/pcl_ros/include/pcl_ros/io/pcd_io.hpp b/pcl_ros/include/pcl_ros/io/pcd_io.hpp index afcde06e..b65882a4 100644 --- a/pcl_ros/include/pcl_ros/io/pcd_io.hpp +++ b/pcl_ros/include/pcl_ros/io/pcd_io.hpp @@ -35,10 +35,11 @@ * */ -#ifndef PCL_ROS_IO_PCD_IO_H_ -#define PCL_ROS_IO_PCD_IO_H_ +#ifndef PCL_ROS__IO__PCD_IO_HPP_ +#define PCL_ROS__IO__PCD_IO_HPP_ #include +#include #include "pcl_ros/pcl_nodelet.hpp" namespace pcl_ros @@ -131,6 +132,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_IO_PCD_IO_H_ +#endif // PCL_ROS__IO__PCD_IO_HPP_ diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp index 28d8304a..4806338f 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp @@ -41,8 +41,8 @@ **/ -#ifndef PCL_NODELET_H_ -#define PCL_NODELET_H_ +#ifndef PCL_ROS__PCL_NODELET_HPP_ +#define PCL_ROS__PCL_NODELET_HPP_ #include // PCL includes @@ -51,7 +51,6 @@ #include #include #include -#include "pcl_ros/point_cloud.hpp" // ROS Nodelet includes #include #include @@ -62,6 +61,11 @@ // Include TF #include +// STL +#include + +#include "pcl_ros/point_cloud.hpp" + using pcl_conversions::fromPCL; namespace pcl_ros @@ -69,7 +73,9 @@ namespace pcl_ros //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */ +/** \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: @@ -128,7 +134,9 @@ protected: /** \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). */ + /** \brief True if we use an approximate time synchronizer + * versus an exact one (false by default). + **/ bool approximate_sync_; /** \brief TF listener object. */ @@ -143,7 +151,8 @@ protected: { 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!", + "[%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( @@ -163,7 +172,8 @@ protected: { 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!", + "[%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()); @@ -182,7 +192,10 @@ protected: { /*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 ()); + 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; @@ -197,13 +210,18 @@ protected: { /*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 ()); + 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. */ + /** \brief Lazy transport subscribe/unsubscribe routine. + * It is optional for backward compatibility. + **/ virtual void subscribe() {} virtual void unsubscribe() {} @@ -237,6 +255,6 @@ protected: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_NODELET_H_ +#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 b64b50de..cf14d3d7 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -1,15 +1,71 @@ -#ifndef pcl_ROS_POINT_CLOUD_H_ -#define pcl_ROS_POINT_CLOUD_H_ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of 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. + */ + +#ifndef PCL_ROS__POINT_CLOUD_HPP__ +#define PCL_ROS__POINT_CLOUD_HPP__ + +// test if testing machinery can be implemented +#if defined(__cpp_rvalue_references) && defined(__cpp_constexpr) +#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1 +#else +#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0 +#endif #include #include #include #include #include +#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED +#include +#if PCL_VERSION_COMPARE(>=, 1, 11, 0) +#include +#elif PCL_VERSION_COMPARE(>=, 1, 10, 0) +#include +#endif +#endif #include #include #include #include +#include +#include +#include +#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED +#include +#include +#endif namespace pcl { @@ -18,7 +74,7 @@ namespace detail template struct FieldStreamer { - FieldStreamer(Stream & stream) + explicit FieldStreamer(Stream & stream) : stream_(stream) {} template @@ -59,8 +115,8 @@ struct FieldsLength std::uint32_t length; }; -} // namespace pcl::detail -} // namespace pcl +} // namespace detail +} // namespace pcl namespace ros { @@ -161,7 +217,7 @@ struct FrameId> static std::string value(const pcl::PointCloud & m) {return m.header.frame_id;} }; -} // namespace ros::message_traits +} // namespace message_traits namespace serialization { @@ -297,30 +353,11 @@ struct Serializer> return length; } }; -} // namespace ros::serialization +} // namespace serialization /// @todo Printer specialization in message_operations -} // namespace ros - -// test if testing machinery can be implemented -#if defined(__cpp_rvalue_references) && defined(__cpp_constexpr) -#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1 -#else -#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0 -#endif - -#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED -#include // for std::is_same -#include // for std::shared_ptr - -#include -#if PCL_VERSION_COMPARE(>=, 1, 11, 0) -#include -#elif PCL_VERSION_COMPARE(>=, 1, 10, 0) -#include -#endif -#endif +} // namespace ros namespace pcl { @@ -341,7 +378,7 @@ struct Holder { SharedPointer p; - Holder(const SharedPointer & p) + explicit Holder(const SharedPointer & p) : p(p) {} Holder(const Holder & other) : p(other.p) {} @@ -373,7 +410,7 @@ inline boost::shared_ptr to_boost_ptr(const std::shared_ptr & p) } } #endif -} // namespace pcl::detail +} // namespace detail // add functions to convert to smart pointer used by ROS template @@ -420,6 +457,6 @@ inline boost::shared_ptr pcl_ptr(const boost::shared_ptr & p) return p; } #endif -} // namespace pcl +} // namespace pcl -#endif +#endif // PCL_ROS__POINT_CLOUD_HPP__ diff --git a/pcl_ros/include/pcl_ros/publisher.hpp b/pcl_ros/include/pcl_ros/publisher.hpp index 9ef707af..4de04e8f 100644 --- a/pcl_ros/include/pcl_ros/publisher.hpp +++ b/pcl_ros/include/pcl_ros/publisher.hpp @@ -42,8 +42,8 @@ **/ -#ifndef PCL_ROS_PUBLISHER_H_ -#define PCL_ROS_PUBLISHER_H_ +#ifndef PCL_ROS__PUBLISHER_HPP_ +#define PCL_ROS__PUBLISHER_HPP_ #include #include @@ -51,6 +51,8 @@ #include +#include + namespace pcl_ros { class BasePublisher @@ -82,7 +84,7 @@ public: operator void *() const { - return (pub_) ? (void *)1 : (void *)0; + return (pub_) ? reinterpret_cast(1) : reinterpret_cast(0); } protected: @@ -135,15 +137,15 @@ public: publish(const sensor_msgs::PointCloud2Ptr & point_cloud) const { pub_.publish(point_cloud); - //pub_.publish (*point_cloud); + // pub_.publish (*point_cloud); } void publish(const sensor_msgs::PointCloud2 & point_cloud) const { pub_.publish(boost::make_shared(point_cloud)); - //pub_.publish (point_cloud); + // pub_.publish (point_cloud); } }; -} -#endif //#ifndef PCL_ROS_PUBLISHER_H_ +} // namespace pcl_ros +#endif // PCL_ROS__PUBLISHER_HPP_ diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp index cc189658..581bab57 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp @@ -35,14 +35,13 @@ * */ -#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ -#define PCL_ROS_EXTRACT_CLUSTERS_H_ +#ifndef PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ +#define PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ -#include -#include "pcl_ros/pcl_nodelet.hpp" - -// Dynamic reconfigure #include +#include +#include +#include "pcl_ros/pcl_nodelet.hpp" #include "pcl_ros/EuclideanClusterExtractionConfig.hpp" namespace pcl_ros @@ -109,6 +108,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ +#endif // PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ 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 147f41d4..b60f7e40 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 @@ -35,20 +35,16 @@ * */ -#ifndef PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ -#define PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ +#ifndef PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ +#define PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ -#include "pcl_ros/pcl_nodelet.hpp" #include #include #include - -// PCL includes #include - -// Dynamic reconfigure #include #include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp" +#include "pcl_ros/pcl_nodelet.hpp" namespace pcl_ros { @@ -129,6 +125,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_ +#endif // PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp index 475adf02..4699eee6 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -35,17 +35,14 @@ * */ -#ifndef PCL_ROS_SAC_SEGMENTATION_H_ -#define PCL_ROS_SAC_SEGMENTATION_H_ +#ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ +#define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ -#include "pcl_ros/pcl_nodelet.hpp" #include - -// PCL includes #include - -// Dynamic reconfigure #include +#include +#include "pcl_ros/pcl_nodelet.hpp" #include "pcl_ros/SACSegmentationConfig.hpp" #include "pcl_ros/SACSegmentationFromNormalsConfig.hpp" @@ -54,8 +51,9 @@ namespace pcl_ros namespace sync_policies = message_filters::sync_policies; //////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in - * the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. +/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus + * methods and models, in the sense that it just creates a Nodelet wrapper for generic-purpose + * SAC-based segmentation. * \author Radu Bogdan Rusu */ class SACSegmentation : public PCLNodelet @@ -69,7 +67,8 @@ public: SACSegmentation() : min_inliers_(0) {} - /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. + /** \brief Set the input TF frame the data should be transformed into before processing, + * if input.header.frame_id is different. * \param tf_frame the TF frame the input PointCloud should be transformed into before processing */ inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} @@ -102,13 +101,17 @@ protected: /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr> srv_; - /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + /** \brief The input TF frame the data should be transformed into, + * if input.header.frame_id is different. + */ std::string tf_input_frame_; /** \brief The original data input TF frame. */ std::string tf_input_orig_frame_; - /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + /** \brief The output TF frame the data should be transformed into, + * if input.header.frame_id is different. + */ std::string tf_output_frame_; /** \brief Null passthrough filter, used for pushing empty elements in the @@ -180,8 +183,8 @@ public: }; //////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and - * models that require the use of surface normals for estimation. +/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for + * Sample Consensus methods and models that require the use of surface normals for estimation. */ class SACSegmentationFromNormals : public SACSegmentation { @@ -194,7 +197,8 @@ class SACSegmentationFromNormals : public SACSegmentation typedef boost::shared_ptr PointCloudNConstPtr; public: - /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. + /** \brief Set the input TF frame the data should be transformed into before processing, + * if input.header.frame_id is different. * \param tf_frame the TF frame the input PointCloud should be transformed into before processing */ inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} @@ -237,11 +241,15 @@ protected: * synchronizer */ message_filters::PassThrough nf_; - /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ + /** \brief The input TF frame the data should be transformed into, + * if input.header.frame_id is different. + */ std::string tf_input_frame_; /** \brief The original data input TF frame. */ std::string tf_input_orig_frame_; - /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ + /** \brief The output TF frame the data should be transformed into, + * if input.header.frame_id is different. + */ std::string tf_output_frame_; /** \brief Nodelet initialization routine. */ @@ -288,6 +296,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_ +#endif // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp index ba0cd5b1..9a4a9322 100644 --- a/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp @@ -35,15 +35,13 @@ * */ -#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ -#define PCL_ROS_SEGMENT_DIFFERENCES_H_ +#ifndef PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ +#define PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ #include -#include "pcl_ros/pcl_nodelet.hpp" - -// Dynamic reconfigure #include #include "pcl_ros/SegmentDifferencesConfig.hpp" +#include "pcl_ros/pcl_nodelet.hpp" namespace pcl_ros @@ -108,6 +106,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ +#endif // PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.hpp b/pcl_ros/include/pcl_ros/surface/convex_hull.hpp index 726584a3..230c9415 100644 --- a/pcl_ros/include/pcl_ros/surface/convex_hull.hpp +++ b/pcl_ros/include/pcl_ros/surface/convex_hull.hpp @@ -35,16 +35,12 @@ * */ -#ifndef PCL_ROS_CONVEX_HULL_2D_H_ -#define PCL_ROS_CONVEX_HULL_2D_H_ +#ifndef PCL_ROS__SURFACE__CONVEX_HULL_HPP_ +#define PCL_ROS__SURFACE__CONVEX_HULL_HPP_ -#include "pcl_ros/pcl_nodelet.hpp" - -// PCL includes #include - -// Dynamic reconfigure #include +#include "pcl_ros/pcl_nodelet.hpp" namespace pcl_ros { @@ -94,6 +90,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_ +#endif // PCL_ROS__SURFACE__CONVEX_HULL_HPP_ diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp b/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp index 0c0bb060..c6d59bf4 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp @@ -35,25 +35,21 @@ * */ -#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ -#define PCL_ROS_MOVING_LEAST_SQUARES_H_ +#ifndef PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_ +#define PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_ -#include "pcl_ros/pcl_nodelet.hpp" - -// PCL includes #include -#include // for KdTree - -// Dynamic reconfigure #include -#include "pcl_ros/MLSConfig.h" +#include "pcl_ros/pcl_nodelet.hpp" +#include "pcl_ros/MLSConfig.hpp" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; /** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation. - * The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters. + * The type of the output is the same as the input, it only smooths the XYZ coordinates according + * to the parameters. * Normals are estimated at each point as well and published on a separate topic. * \author Radu Bogdan Rusu, Zoltan-Csaba Marton */ @@ -71,7 +67,9 @@ class MovingLeastSquares : public PCLNodelet typedef pcl::KdTree::Ptr KdTreePtr; protected: - /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ + /** \brief An input point cloud describing the surface that is to be used for + * nearest neighbors estimation. + */ PointCloudInConstPtr surface_; /** \brief A pointer to the spatial search object. */ @@ -81,7 +79,7 @@ protected: double search_radius_; /** \brief The number of K nearest neighbors to use for each point. */ - //int k_; + // int k_; /** \brief Whether to use a polynomial fit. */ bool use_polynomial_fit_; @@ -89,7 +87,9 @@ protected: /** \brief The order of the polynomial to be fit. */ int polynomial_order_; - /** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */ + /** \brief How 'flat' should the neighbor weighting gaussian be + * the smaller, the more local the fit). + */ double gaussian_parameter_; // ROS nodelet attributes @@ -147,6 +147,6 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +} // namespace pcl_ros -#endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ +#endif // PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_ diff --git a/pcl_ros/include/pcl_ros/transforms.hpp b/pcl_ros/include/pcl_ros/transforms.hpp index 28d85a62..bd890749 100644 --- a/pcl_ros/include/pcl_ros/transforms.hpp +++ b/pcl_ros/include/pcl_ros/transforms.hpp @@ -34,13 +34,14 @@ * */ -#ifndef pcl_ROS_TRANSFORMS_H_ -#define pcl_ROS_TRANSFORMS_H_ +#ifndef PCL_ROS__TRANSFORMS_HPP_ +#define PCL_ROS__TRANSFORMS_HPP_ #include #include #include #include +#include namespace pcl_ros { @@ -176,6 +177,6 @@ transformPointCloud( */ void transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat); -} +} // namespace pcl_ros -#endif // PCL_ROS_TRANSFORMS_H_ +#endif // PCL_ROS__TRANSFORMS_HPP_ diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp index 146b20d0..9d8adf44 100644 --- a/pcl_ros/src/pcl_ros/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -35,21 +35,22 @@ * */ -//#include +// #include // Include the implementations here instead of compiling them separately to speed up compile time -//#include "normal_3d.cpp" -//#include "boundary.cpp" -//#include "fpfh.cpp" -//#include "fpfh_omp.cpp" -//#include "moment_invariants.cpp" -//#include "normal_3d_omp.cpp" -//#include "normal_3d_tbb.cpp" -//#include "pfh.cpp" -//#include "principal_curvatures.cpp" -//#include "vfh.cpp" +// #include "normal_3d.cpp" +// #include "boundary.cpp" +// #include "fpfh.cpp" +// #include "fpfh_omp.cpp" +// #include "moment_invariants.cpp" +// #include "normal_3d_omp.cpp" +// #include "normal_3d_tbb.cpp" +// #include "pfh.cpp" +// #include "principal_curvatures.cpp" +// #include "vfh.cpp" #include -#include "pcl_ros/features/feature.hpp" #include +#include +#include "pcl_ros/features/feature.hpp" //////////////////////////////////////////////////////////////////////////////////////////// void @@ -62,13 +63,15 @@ pcl_ros::Feature::onInit() childInit(*pnh_); // Allow each individual class that inherits from us to declare their own Publisher - // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc - //pub_output_ = pnh_->template advertise ("output", max_queue_size_); + // This is useful for Publisher >, as NormalEstimation can publish + // PointCloud, etc + // pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { NODELET_ERROR( - "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", + "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these " + "parameters before continuing.", getName().c_str()); return; } @@ -108,11 +111,14 @@ pcl_ros::Feature::subscribe() if (use_indices_ || use_surface_) { // Create the objects here if (approximate_sync_) { - sync_input_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_surface_indices_a_ = + boost::make_shared>>(max_queue_size_); } else { - sync_input_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_surface_indices_e_ = + boost::make_shared>>(max_queue_size_); } // Subscribe to the input using a filter @@ -121,7 +127,8 @@ pcl_ros::Feature::subscribe() // If indices are enabled, subscribe to the indices sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); if (use_surface_) { // Use both indices and surface - // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register + // If surface is enabled, subscribe to the surface, + // connect the input-indices-surface trio and register sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); if (approximate_sync_) { sync_input_surface_indices_a_->connectInput( @@ -251,9 +258,12 @@ pcl_ros::Feature::input_surface_indices_callback( if (indices) { NODELET_DEBUG( "[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, " + "and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, " + "and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str(), @@ -266,8 +276,10 @@ pcl_ros::Feature::input_surface_indices_callback( } else { NODELET_DEBUG( "[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.", cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str(), @@ -279,8 +291,10 @@ pcl_ros::Feature::input_surface_indices_callback( } else if (indices) { NODELET_DEBUG( "[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and frame " + "%s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on " + "topic %s received.", cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str(), @@ -288,16 +302,18 @@ pcl_ros::Feature::input_surface_indices_callback( indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); } else { NODELET_DEBUG( - "[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL( + "[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on " + "topic %s received.", cloud->width * cloud->height, fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str()); } /// - if ((int)(cloud->width * cloud->height) < k_) { + if (static_cast(cloud->width * cloud->height) < k_) { NODELET_ERROR( - "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, + "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger " + "than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height)); emptyPublish(cloud); return; @@ -327,13 +343,15 @@ pcl_ros::FeatureFromNormals::onInit() childInit(*pnh_); // Allow each individual class that inherits from us to declare their own Publisher - // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc - //pub_output_ = pnh_->template advertise ("output", max_queue_size_); + // This is useful for Publisher >, as NormalEstimation can publish + // PointCloud, etc + // pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { NODELET_ERROR( - "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", + "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these " + "parameters before continuing.", getName().c_str()); return; } @@ -373,10 +391,13 @@ pcl_ros::FeatureFromNormals::subscribe() // Create the objects here if (approximate_sync_) { - sync_input_normals_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_normals_surface_indices_a_ = + boost::make_shared>>(max_queue_size_); } else { - sync_input_normals_surface_indices_e_ = boost::make_shared>>(max_queue_size_); } @@ -386,7 +407,8 @@ pcl_ros::FeatureFromNormals::subscribe() // If indices are enabled, subscribe to the indices sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); if (use_surface_) { // Use both indices and surface - // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register + // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio + // and register sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); if (approximate_sync_) { sync_input_normals_surface_indices_a_->connectInput( @@ -490,7 +512,7 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( } // If cloud+normals is given, check if it's valid - if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals")) + if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals")) NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str()); emptyPublish(cloud); return; @@ -519,10 +541,14 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( if (indices) { NODELET_DEBUG( "[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, " + "stamp %f, and frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -540,9 +566,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( } else { NODELET_DEBUG( "[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -559,9 +588,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( } else if (indices) { NODELET_DEBUG( "[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, " + "stamp %f, and frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -575,8 +607,10 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( } else { NODELET_DEBUG( "[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -588,9 +622,10 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( } /// - if ((int)(cloud->width * cloud->height) < k_) { + if (static_cast(cloud->width * cloud->height) < k_) { NODELET_ERROR( - "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", + "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) " + "is larger than the PointCloud size (%d)!", getName().c_str(), k_, (int)(cloud->width * cloud->height)); emptyPublish(cloud); return; diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp index abb16d8b..0eea2235 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp @@ -78,4 +78,4 @@ pcl_ros::NormalEstimationTBB::computePublish( typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet) -#endif // HAVE_TBB +#endif // HAVE_TBB diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp index 83dec1eb..bcba2a98 100644 --- a/pcl_ros/src/pcl_ros/filters/crop_box.cpp +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -36,8 +36,8 @@ * */ -#include #include "pcl_ros/filters/crop_box.hpp" +#include ////////////////////////////////////////////////////////////////////////////////////////////// bool @@ -102,7 +102,8 @@ pcl_ros::CropBox::config_callback(pcl_ros::CropBoxConfig & config, uint32_t leve impl_.setNegative(config.negative); } - // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL + // The following parameters are updated automatically for all PCL_ROS Nodelet Filters + // as they are inexistent in PCL if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; NODELET_DEBUG( diff --git a/pcl_ros/src/pcl_ros/filters/features/feature.cpp b/pcl_ros/src/pcl_ros/filters/features/feature.cpp index 85204ff9..8b325dca 100644 --- a/pcl_ros/src/pcl_ros/filters/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/feature.cpp @@ -35,21 +35,22 @@ * */ -//#include +// #include // Include the implementations here instead of compiling them separately to speed up compile time -//#include "normal_3d.cpp" -//#include "boundary.cpp" -//#include "fpfh.cpp" -//#include "fpfh_omp.cpp" -//#include "moment_invariants.cpp" -//#include "normal_3d_omp.cpp" -//#include "normal_3d_tbb.cpp" -//#include "pfh.cpp" -//#include "principal_curvatures.cpp" -//#include "vfh.cpp" +// #include "normal_3d.cpp" +// #include "boundary.cpp" +// #include "fpfh.cpp" +// #include "fpfh_omp.cpp" +// #include "moment_invariants.cpp" +// #include "normal_3d_omp.cpp" +// #include "normal_3d_tbb.cpp" +// #include "pfh.cpp" +// #include "principal_curvatures.cpp" +// #include "vfh.cpp" #include -#include "pcl_ros/features/feature.hpp" #include +#include +#include "pcl_ros/features/feature.hpp" //////////////////////////////////////////////////////////////////////////////////////////// void @@ -62,13 +63,15 @@ pcl_ros::Feature::onInit() childInit(*pnh_); // Allow each individual class that inherits from us to declare their own Publisher - // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc - //pub_output_ = pnh_->template advertise ("output", max_queue_size_); + // This is useful for Publisher >, as NormalEstimation can publish + // PointCloud, etc + // pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { NODELET_ERROR( - "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", + "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these " + "parameters before continuing.", getName().c_str()); return; } @@ -92,11 +95,15 @@ pcl_ros::Feature::onInit() if (use_indices_ || use_surface_) { // Create the objects here if (approximate_sync_) { - sync_input_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_surface_indices_a_ = + boost::make_shared>>(max_queue_size_); } else { - sync_input_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_surface_indices_e_ = + boost::make_shared>>(max_queue_size_); } // Subscribe to the input using a filter @@ -105,7 +112,8 @@ pcl_ros::Feature::onInit() // If indices are enabled, subscribe to the indices sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); if (use_surface_) { // Use both indices and surface - // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register + // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio + // and register sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); if (approximate_sync_) { sync_input_surface_indices_a_->connectInput( @@ -225,9 +233,12 @@ pcl_ros::Feature::input_surface_indices_callback( if (indices) { NODELET_DEBUG( "[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, " + "and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, " + "and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, " + "and frame %s on topic %s received.", cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str(), @@ -240,8 +251,10 @@ pcl_ros::Feature::input_surface_indices_callback( } else { NODELET_DEBUG( "[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.", cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str(), @@ -253,8 +266,10 @@ pcl_ros::Feature::input_surface_indices_callback( } else if (indices) { NODELET_DEBUG( "[input_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str(), @@ -262,16 +277,18 @@ pcl_ros::Feature::input_surface_indices_callback( indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); } else { NODELET_DEBUG( - "[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, + "[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and " + "frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str()); } /// - if ((int)(cloud->width * cloud->height) < k_) { + if (static_cast(cloud->width * cloud->height) < k_) { NODELET_ERROR( - "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, + "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger " + "than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height)); emptyPublish(cloud); return; @@ -301,13 +318,15 @@ pcl_ros::FeatureFromNormals::onInit() childInit(*pnh_); // Allow each individual class that inherits from us to declare their own Publisher - // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc - //pub_output_ = pnh_->template advertise ("output", max_queue_size_); + // This is useful for Publisher >, as NormalEstimation can publish + // PointCloud, etc + // pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { NODELET_ERROR( - "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", + "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these " + "parameters before continuing.", getName().c_str()); return; } @@ -331,11 +350,15 @@ pcl_ros::FeatureFromNormals::onInit() // Create the objects here if (approximate_sync_) { - sync_input_normals_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_normals_surface_indices_a_ = + boost::make_shared>>(max_queue_size_); } else { - sync_input_normals_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_normals_surface_indices_e_ = + boost::make_shared>>(max_queue_size_); } // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages @@ -344,7 +367,8 @@ pcl_ros::FeatureFromNormals::onInit() // If indices are enabled, subscribe to the indices sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); if (use_surface_) { // Use both indices and surface - // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register + // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio + // and register sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); if (approximate_sync_) { sync_input_normals_surface_indices_a_->connectInput( @@ -439,7 +463,7 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( } // If cloud+normals is given, check if it's valid - if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals")) + if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals")) NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str()); emptyPublish(cloud); return; @@ -468,10 +492,14 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( if (indices) { NODELET_DEBUG( "[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, " + "stamp %f, and frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -489,9 +517,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( } else { NODELET_DEBUG( "[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -508,38 +539,44 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback( } else if (indices) { NODELET_DEBUG( "[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), " + "stamp %f, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, " + "stamp %f, and frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str(), cloud_normals->width * cloud_normals->height, pcl::getFieldsList( *cloud_normals).c_str(), - cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName( - "normals").c_str(), + cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), + pnh_->resolveName("normals").c_str(), indices->indices.size(), indices->header.stamp.toSec(), indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); } else { NODELET_DEBUG( "[%s::input_normals_surface_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str(), cloud_normals->width * cloud_normals->height, pcl::getFieldsList( *cloud_normals).c_str(), - cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName( - "normals").c_str()); + cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), + pnh_->resolveName("normals").c_str()); } /// - if ((int)(cloud->width * cloud->height) < k_) { + if (static_cast(cloud->width * cloud->height) < k_) { NODELET_ERROR( - "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", + "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) " + "is larger than the PointCloud size (%d)!", getName().c_str(), k_, (int)(cloud->width * cloud->height)); emptyPublish(cloud); return; diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp index 72f94c7f..eb002a47 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp @@ -78,4 +78,4 @@ pcl_ros::NormalEstimationTBB::computePublish( typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet); -#endif // HAVE_TBB +#endif // HAVE_TBB diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index dfa6eef5..7d011c02 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -35,9 +35,10 @@ * */ -#include -#include "pcl_ros/transforms.hpp" #include "pcl_ros/filters/filter.hpp" +#include +#include +#include "pcl_ros/transforms.hpp" /*//#include //#include @@ -48,18 +49,18 @@ */ // Include the implementations instead of compiling them separately to speed up compile time -//#include "extract_indices.cpp" -//#include "passthrough.cpp" -//#include "project_inliers.cpp" -//#include "radius_outlier_removal.cpp" -//#include "statistical_outlier_removal.cpp" -//#include "voxel_grid.cpp" +// #include "extract_indices.cpp" +// #include "passthrough.cpp" +// #include "project_inliers.cpp" +// #include "radius_outlier_removal.cpp" +// #include "statistical_outlier_removal.cpp" +// #include "voxel_grid.cpp" /*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet); //PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet); */ -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices) { @@ -120,12 +121,14 @@ pcl_ros::Filter::subscribe() sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); if (approximate_sync_) { - sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); } else { - sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); @@ -182,7 +185,8 @@ pcl_ros::Filter::onInit() void pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level) { - // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL + // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are + // inexistent in PCL if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; NODELET_DEBUG( @@ -218,8 +222,10 @@ pcl_ros::Filter::input_indices_callback( if (indices) { NODELET_DEBUG( "[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -228,7 +234,8 @@ pcl_ros::Filter::input_indices_callback( indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); } else { NODELET_DEBUG( - "[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", + "[%s::input_indices_callback] PointCloud with %d data points and frame %s on " + "topic %s received.", getName().c_str(), cloud->width * cloud->height, cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str()); } diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index f03ec564..8ee183d2 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -35,8 +35,8 @@ * */ -#include #include "pcl_ros/filters/passthrough.hpp" +#include ////////////////////////////////////////////////////////////////////////////////////////////// bool @@ -65,7 +65,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l if (filter_min != config.filter_limit_min) { filter_min = config.filter_limit_min; NODELET_DEBUG( - "[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", + "[%s::config_callback] Setting the minimum filtering value a point will be " + "considered from to: %f.", getName().c_str(), filter_min); // Set the filter min-max if different impl_.setFilterLimits(filter_min, filter_max); @@ -74,14 +75,15 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l if (filter_max != config.filter_limit_max) { filter_max = config.filter_limit_max; NODELET_DEBUG( - "[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", + "[%s::config_callback] Setting the maximum filtering value a point will be " + "considered from to: %f.", getName().c_str(), filter_max); // Set the filter min-max if different impl_.setFilterLimits(filter_min, filter_max); } // Check the current value for the filter field - //std::string filter_field = impl_.getFilterFieldName (); + // std::string filter_field = impl_.getFilterFieldName (); if (impl_.getFilterFieldName() != config.filter_field_name) { // Set the filter field if different impl_.setFilterFieldName(config.filter_field_name); @@ -108,7 +110,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l impl_.setFilterLimitsNegative(config.filter_limit_negative); } - // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL + // The following parameters are updated automatically for all PCL_ROS Nodelet Filters + // as they are inexistent in PCL if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; NODELET_DEBUG( diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index f1b8525c..23a3111d 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -35,9 +35,10 @@ * */ -#include #include "pcl_ros/filters/project_inliers.hpp" +#include #include +#include ////////////////////////////////////////////////////////////////////////////////////////////// void @@ -99,16 +100,20 @@ pcl_ros::ProjectInliers::subscribe() sub_model_.subscribe(*pnh_, "model", max_queue_size_); if (approximate_sync_) { - sync_input_indices_model_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_model_a_ = + boost::make_shared>>(max_queue_size_); sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_a_->registerCallback( bind( &ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); } else { - sync_input_indices_model_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_model_e_ = + boost::make_shared>>(max_queue_size_); sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_e_->registerCallback( bind( @@ -148,9 +153,12 @@ pcl_ros::ProjectInliers::input_indices_model_callback( NODELET_DEBUG( "[%s::input_indices_model_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" - " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.\n" + " - ModelCoefficients with %zu values, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str(), diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp index 2125a669..0a01b0b1 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -73,7 +73,6 @@ pcl_ros::RadiusOutlierRemoval::config_callback( "[%s::config_callback] Setting the radius to search neighbors: %f.", getName().c_str(), config.radius_search); } - } diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp index 00803e82..e7a48a9b 100644 --- a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -63,7 +63,8 @@ pcl_ros::StatisticalOutlierRemoval::config_callback( if (impl_.getMeanK() != config.mean_k) { impl_.setMeanK(config.mean_k); NODELET_DEBUG( - "[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", + "[%s::config_callback] Setting the number of points (k) to use for mean " + "distance estimation to: %d.", getName().c_str(), config.mean_k); } diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 37c3adbd..39b797a0 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -88,13 +88,15 @@ pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t if (filter_min != config.filter_limit_min) { filter_min = config.filter_limit_min; NODELET_DEBUG( - "[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", + "[config_callback] Setting the minimum filtering value a point will be considered " + "from to: %f.", filter_min); } if (filter_max != config.filter_limit_max) { filter_max = config.filter_limit_max; NODELET_DEBUG( - "[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", + "[config_callback] Setting the maximum filtering value a point will be considered " + "from to: %f.", filter_max); } impl_.setFilterLimits(filter_min, filter_max); @@ -113,7 +115,8 @@ pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t config.filter_field_name.c_str()); } - // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter + // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, + // we'll remove them and inherit from Filter if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); diff --git a/pcl_ros/src/pcl_ros/io/bag_io.cpp b/pcl_ros/src/pcl_ros/io/bag_io.cpp index e0a62e86..74ca8d6b 100644 --- a/pcl_ros/src/pcl_ros/io/bag_io.cpp +++ b/pcl_ros/src/pcl_ros/io/bag_io.cpp @@ -36,6 +36,7 @@ */ #include +#include #include "pcl_ros/io/bag_io.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index 00cea9d9..cde8113a 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -37,10 +37,11 @@ #include #include +#include +#include #include "pcl_ros/transforms.hpp" #include "pcl_ros/io/concatenate_data.hpp" -#include ////////////////////////////////////////////////////////////////////////////////////////////// void @@ -244,7 +245,7 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds( const PointCloud2 & in2, PointCloud2 & out) { - //ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ()); + // ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ()); PointCloud2::Ptr in1_t(new PointCloud2()); PointCloud2::Ptr in2_t(new PointCloud2()); diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index 99524dd0..1ab31693 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -39,11 +39,12 @@ #include #include +#include +#include #include "pcl_ros/io/concatenate_fields.hpp" -#include -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit() { @@ -66,7 +67,7 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit() onInitPostProcess(); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe() { @@ -75,19 +76,20 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe() &PointCloudConcatenateFieldsSynchronizer::input_callback, this); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe() { sub_input_.shutdown(); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointCloudConstPtr & cloud) { NODELET_DEBUG( - "[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + "[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on " + "topic %s received.", cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str()); @@ -98,7 +100,8 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou queue_.size() > 0) { NODELET_WARN( - "[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_, + "[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message " + "in queue with stamp %f.", maximum_seconds_, (*queue_.begin()).first.toSec(), fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() )); queue_.erase(queue_.begin()); @@ -107,7 +110,7 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou // Push back new data queue_[cloud->header.stamp].push_back(cloud); - if ((int)queue_[cloud->header.stamp].size() >= input_messages_) { + if (static_cast(queue_[cloud->header.stamp].size()) >= input_messages_) { // Concatenate together and publish std::vector & clouds = queue_[cloud->header.stamp]; PointCloud cloud_out = *clouds[0]; @@ -122,7 +125,8 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) { NODELET_ERROR( - "[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!", + "[input_callback] Width/height of pointcloud %zu (%dx%d) differs " + "from the others (%dx%d)!", i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height); break; } @@ -162,11 +166,10 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou // Clean the queue to avoid overflowing if (maximum_queue_size_ > 0) { - while ((int)queue_.size() > maximum_queue_size_) { + while (static_cast(queue_.size()) > maximum_queue_size_) { queue_.erase(queue_.begin()); } } - } typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer; diff --git a/pcl_ros/src/pcl_ros/io/io.cpp b/pcl_ros/src/pcl_ros/io/io.cpp index 7cfde645..6787b832 100644 --- a/pcl_ros/src/pcl_ros/io/io.cpp +++ b/pcl_ros/src/pcl_ros/io/io.cpp @@ -38,20 +38,21 @@ #include #include #include -//#include +// #include #include #include typedef nodelet::NodeletMUX> NodeletMUX; -//typedef nodelet::NodeletDEMUX > NodeletDEMUX; +// typedef nodelet::NodeletDEMUX > NodeletDEMUX; typedef nodelet::NodeletDEMUX NodeletDEMUX; -//#include "pcd_io.cpp" -//#include "bag_io.cpp" -//#include "concatenate_fields.cpp" -//#include "concatenate_data.cpp" +// #include "pcd_io.cpp" +// #include "bag_io.cpp" +// #include "concatenate_fields.cpp" +// #include "concatenate_data.cpp" PLUGINLIB_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet); -//PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet); +// PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index 487d8cb4..5f379342 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -37,6 +37,7 @@ #include #include +#include //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index 43170cea..ead3a56a 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -38,9 +38,10 @@ #include #include #include +#include +#include #include "pcl_ros/segmentation/extract_clusters.hpp" -#include using pcl_conversions::fromPCL; using pcl_conversions::moveFromPCL; @@ -69,7 +70,7 @@ pcl_ros::EuclideanClusterExtraction::onInit() return; } - //private_nh.getParam ("use_indices", use_indices_); + // private_nh.getParam ("use_indices", use_indices_); pnh_->getParam("publish_indices", publish_indices_); if (publish_indices_) { @@ -110,16 +111,19 @@ pcl_ros::EuclideanClusterExtraction::subscribe() sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); if (approximate_sync_) { - sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_a_ = + boost::make_shared>>(max_queue_size_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback( bind( &EuclideanClusterExtraction:: input_indices_callback, this, _1, _2)); } else { - sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_ = + boost::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback( bind( @@ -206,8 +210,10 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback( std_msgs::Header indices_header = indices->header; NODELET_DEBUG( "[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud_header.stamp.toSec(), cloud_header.frame_id.c_str(), pnh_->resolveName("input").c_str(), @@ -215,7 +221,8 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback( indices_header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); } else { NODELET_DEBUG( - "[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + "[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on " + "topic %s received.", getName().c_str(), cloud->width * cloud->height, fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str()); @@ -235,10 +242,11 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback( if (publish_indices_) { for (size_t i = 0; i < clusters.size(); ++i) { - if ((int)i >= max_clusters_) { + if (static_cast(i) >= max_clusters_) { break; } - // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. + // TODO(xxx): HACK!!! We need to change the PointCloud2 message to add for an incremental + // sequence ID number. pcl_msgs::PointIndices ros_pi; moveFromPCL(clusters[i], ros_pi); ros_pi.header.stamp += ros::Duration(i * 0.001); @@ -250,15 +258,16 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback( clusters.size(), pnh_->resolveName("output").c_str()); } else { for (size_t i = 0; i < clusters.size(); ++i) { - if ((int)i >= max_clusters_) { + if (static_cast(i) >= max_clusters_) { break; } PointCloud output; copyPointCloud(*cloud, clusters[i].indices, output); - //PointCloud output_blob; // Convert from the templated output to the PointCloud blob - //pcl::toROSMsg (output, output_blob); - // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. + // PointCloud output_blob; // Convert from the templated output to the PointCloud blob + // pcl::toROSMsg (output, output_blob); + // TODO(xxx): HACK!!! We need to change the PointCloud2 message to add for an incremental + // sequence ID number. std_msgs::Header header = fromPCL(output.header); header.stamp += ros::Duration(i * 0.001); toPCL(header, output.header); diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 3dac6ab7..96272d49 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -36,11 +36,11 @@ */ #include +#include +#include +#include #include "pcl_ros/transforms.hpp" #include "pcl_ros/segmentation/extract_polygonal_prism_data.hpp" -#include - -#include using pcl_conversions::moveFromPCL; using pcl_conversions::moveToPCL; @@ -73,11 +73,13 @@ pcl_ros::ExtractPolygonalPrismData::subscribe() // Create the objects here if (approximate_sync_) { - sync_input_hull_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_hull_indices_a_ = + boost::make_shared>>(max_queue_size_); } else { - sync_input_hull_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_hull_indices_e_ = + boost::make_shared>>(max_queue_size_); } if (use_indices_) { @@ -183,9 +185,12 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback( if (indices) { NODELET_DEBUG( "[%s::input_indices_hull_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -198,8 +203,10 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback( } else { NODELET_DEBUG( "[%s::input_indices_hull_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -212,7 +219,8 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback( if (cloud->header.frame_id != hull->header.frame_id) { NODELET_DEBUG( - "[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", + "[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input " + "point cloud (%s)! Using TF to transform.", getName().c_str(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str()); PointCloud planar_hull; if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) { @@ -233,7 +241,8 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback( impl_.setInputCloud(pcl_ptr(cloud)); impl_.setIndices(indices_ptr); - // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) + // Final check if the data is empty + // (remember that indices are set to the size of the data -- if indices* = NULL) if (!cloud->points.empty()) { pcl::PointIndices pcl_inliers; moveToPCL(inliers, pcl_inliers); diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index 1defe32d..b7fcd539 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -36,10 +36,10 @@ */ #include -#include "pcl_ros/segmentation/sac_segmentation.hpp" #include - #include +#include +#include "pcl_ros/segmentation/sac_segmentation.hpp" using pcl_conversions::fromPCL; @@ -61,7 +61,7 @@ pcl_ros::SACSegmentation::onInit() NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!"); return; } - double threshold; // unused - set via dynamic reconfigure in the callback + double threshold; // unused - set via dynamic reconfigure in the callback if (!pnh_->getParam("distance_threshold", threshold)) { NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); return; @@ -80,7 +80,8 @@ pcl_ros::SACSegmentation::onInit() { if (axis_param.size() != 3) { NODELET_ERROR( - "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", + "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) " + "than required (3)!", getName().c_str(), axis_param.size()); return; } @@ -149,27 +150,28 @@ pcl_ros::SACSegmentation::subscribe() // Synchronize the two topics. No need for an approximate synchronizer here, as we'll // match the timestamps exactly - sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_ = + boost::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, nf_pi_); sync_input_indices_e_->registerCallback( bind( &SACSegmentation::input_indices_callback, this, _1, _2)); - } - // "latched_indices" not set, proceed with regular pairs - else { + } else { // "latched_indices" not set, proceed with regular pairs if (approximate_sync_) { - sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_a_ = + boost::make_shared>>(max_queue_size_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback( bind( &SACSegmentation::input_indices_callback, this, _1, _2)); } else { - sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_ = + boost::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback( bind( @@ -205,7 +207,7 @@ pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32 boost::mutex::scoped_lock lock(mutex_); if (impl_.getDistanceThreshold() != config.distance_threshold) { - //sac_->setDistanceThreshold (threshold_); - done in initSAC + // sac_->setDistanceThreshold (threshold_); - done in initSAC impl_.setDistanceThreshold(config.distance_threshold); NODELET_DEBUG( "[%s::config_callback] Setting new distance to model threshold to: %f.", @@ -228,14 +230,14 @@ pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32 } if (impl_.getMaxIterations() != config.max_iterations) { - //sac_->setMaxIterations (max_iterations_); - done in initSAC + // sac_->setMaxIterations (max_iterations_); - done in initSAC impl_.setMaxIterations(config.max_iterations); NODELET_DEBUG( "[%s::config_callback] Setting new maximum number of iterations to: %d.", getName().c_str(), config.max_iterations); } if (impl_.getProbability() != config.probability) { - //sac_->setProbability (probability_); - done in initSAC + // sac_->setProbability (probability_); - done in initSAC impl_.setProbability(config.probability); NODELET_DEBUG( "[%s::config_callback] Setting new probability to: %f.", @@ -307,8 +309,10 @@ pcl_ros::SACSegmentation::input_indices_callback( if (indices && !indices->header.frame_id.empty()) { NODELET_DEBUG( "[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -317,7 +321,8 @@ pcl_ros::SACSegmentation::input_indices_callback( indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); } else { NODELET_DEBUG( - "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( "input").c_str()); @@ -329,11 +334,13 @@ pcl_ros::SACSegmentation::input_indices_callback( PointCloudConstPtr cloud_tf; /* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) { - NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); + NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", + // cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); // Save the original frame ID // Convert the cloud into the different frame PointCloud cloud_transformed; - if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, cloud_transformed, tf_listener_)) + if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, + cloud_transformed, tf_listener_)) return; cloud_tf.reset (new PointCloud (cloud_transformed)); } @@ -348,7 +355,8 @@ pcl_ros::SACSegmentation::input_indices_callback( impl_.setInputCloud(pcl_ptr(cloud_tf)); impl_.setIndices(indices_ptr); - // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) + // Final check if the data is empty + // (remember that indices are set to the size of the data -- if indices* = NULL) if (!cloud->points.empty()) { pcl::PointIndices pcl_inliers; pcl::ModelCoefficients pcl_model; @@ -362,7 +370,7 @@ pcl_ros::SACSegmentation::input_indices_callback( // Probably need to transform the model of the plane here // Check if we have enough inliers, clear inliers + model if not - if ((int)inliers.indices.size() <= min_inliers_) { + if (static_cast(inliers.indices.size()) <= min_inliers_) { inliers.indices.clear(); model.values.clear(); } @@ -371,7 +379,8 @@ pcl_ros::SACSegmentation::input_indices_callback( pub_indices_.publish(boost::make_shared(inliers)); pub_model_.publish(boost::make_shared(model)); NODELET_DEBUG( - "[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", + "[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, " + "and ModelCoefficients with %zu values on topic %s", getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), model.values.size(), pnh_->resolveName("model").c_str()); @@ -405,7 +414,7 @@ pcl_ros::SACSegmentationFromNormals::onInit() getName().c_str()); return; } - double threshold; // unused - set via dynamic reconfigure in the callback + double threshold; // unused - set via dynamic reconfigure in the callback if (!pnh_->getParam("distance_threshold", threshold)) { NODELET_ERROR( "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", @@ -426,7 +435,8 @@ pcl_ros::SACSegmentationFromNormals::onInit() { if (axis_param.size() != 3) { NODELET_ERROR( - "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", + "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than " + "required (3)!", getName().c_str(), axis_param.size()); return; } @@ -475,15 +485,18 @@ pcl_ros::SACSegmentationFromNormals::subscribe() sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); - // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) + // Subscribe to an axis direction along which the model search is to be constrained (the first + // 3 model coefficients will be checked) sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this); if (approximate_sync_) { - sync_input_normals_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_normals_indices_a_ = + boost::make_shared>>(max_queue_size_); } else { - sync_input_normals_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_normals_indices_e_ = + boost::make_shared>>(max_queue_size_); } // If we're supposed to look for PointIndices (indices) @@ -658,7 +671,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( return; } - if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals")) + if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals")) NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str()); pub_indices_.publish(boost::make_shared(inliers)); pub_model_.publish(boost::make_shared(model)); @@ -676,9 +689,12 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( if (indices && !indices->header.frame_id.empty()) { NODELET_DEBUG( "[%s::input_normals_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -692,8 +708,10 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( } else { NODELET_DEBUG( "[%s::input_normals_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( @@ -711,7 +729,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; if (cloud_nr_points != cloud_normals_nr_points) { NODELET_ERROR( - "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", + "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs " + "from the number of points in the normals (%d)!", getName().c_str(), cloud_nr_points, cloud_normals_nr_points); pub_indices_.publish(boost::make_shared(inliers)); pub_model_.publish(boost::make_shared(model)); @@ -728,7 +747,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( impl_.setIndices(indices_ptr); - // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) + // Final check if the data is empty + // (remember that indices are set to the size of the data -- if indices* = NULL) if (!cloud->points.empty()) { pcl::PointIndices pcl_inliers; pcl::ModelCoefficients pcl_model; @@ -740,7 +760,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( } // Check if we have enough inliers, clear inliers + model if not - if ((int)inliers.indices.size() <= min_inliers_) { + if (static_cast(inliers.indices.size()) <= min_inliers_) { inliers.indices.clear(); model.values.clear(); } @@ -749,7 +769,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( pub_indices_.publish(boost::make_shared(inliers)); pub_model_.publish(boost::make_shared(model)); NODELET_DEBUG( - "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", + "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and " + "ModelCoefficients with %zu values on topic %s", getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), model.values.size(), pnh_->resolveName("model").c_str()); if (inliers.indices.empty()) { diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index 93bc8f8b..b2a20b26 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -36,10 +36,10 @@ */ #include -#include "pcl_ros/segmentation/segment_differences.hpp" #include +#include "pcl_ros/segmentation/segment_differences.hpp" -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SegmentDifferences::onInit() { @@ -57,7 +57,7 @@ pcl_ros::SegmentDifferences::onInit() onInitPostProcess(); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SegmentDifferences::subscribe() { @@ -72,16 +72,18 @@ pcl_ros::SegmentDifferences::subscribe() srv_->setCallback(f); if (approximate_sync_) { - sync_input_target_a_ = boost::make_shared>>(max_queue_size_); + sync_input_target_a_ = + boost::make_shared>>(max_queue_size_); sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_); sync_input_target_a_->registerCallback( bind( &SegmentDifferences::input_target_callback, this, _1, _2)); } else { - sync_input_target_e_ = boost::make_shared>>(max_queue_size_); + sync_input_target_e_ = + boost::make_shared>>(max_queue_size_); sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_); sync_input_target_e_->registerCallback( bind( @@ -90,7 +92,7 @@ pcl_ros::SegmentDifferences::subscribe() } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SegmentDifferences::unsubscribe() { @@ -98,7 +100,7 @@ pcl_ros::SegmentDifferences::unsubscribe() sub_target_filter_.unsubscribe(); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level) { @@ -131,8 +133,10 @@ pcl_ros::SegmentDifferences::input_target_callback( NODELET_DEBUG( "[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( diff --git a/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp index 09fa772a..244ecc94 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp @@ -36,7 +36,7 @@ */ // Include the implementations here instead of compiling them separately to speed up compile time -//#include "extract_clusters.cpp" -//#include "extract_polygonal_prism_data.cpp" -//#include "sac_segmentation.cpp" -//#include "segment_differences.cpp" +// #include "extract_clusters.cpp" +// #include "extract_polygonal_prism_data.cpp" +// #include "sac_segmentation.cpp" +// #include "segment_differences.cpp" diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index 8a80217e..1edca75a 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -37,8 +37,9 @@ #include #include -#include "pcl_ros/surface/convex_hull.hpp" #include +#include +#include "pcl_ros/surface/convex_hull.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// void @@ -73,8 +74,9 @@ pcl_ros::ConvexHull2D::subscribe() sub_indices_filter_.subscribe(*pnh_, "indices", 1); if (approximate_sync_) { - sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_a_ = + boost::make_shared>>(max_queue_size_); // surface not enabled, connect the input-indices duo and register sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback( @@ -82,8 +84,9 @@ pcl_ros::ConvexHull2D::subscribe() &ConvexHull2D::input_indices_callback, this, _1, _2)); } else { - sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_ = + boost::make_shared>>(max_queue_size_); // surface not enabled, connect the input-indices duo and register sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback( @@ -146,8 +149,10 @@ pcl_ros::ConvexHull2D::input_indices_callback( if (indices) { NODELET_DEBUG( "[%s::input_indices_model_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), @@ -156,7 +161,8 @@ pcl_ros::ConvexHull2D::input_indices_callback( indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str()); } else { NODELET_DEBUG( - "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("input").c_str()); diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index a7daf9e3..f48391d4 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -36,21 +36,24 @@ */ #include -#include "pcl_ros/surface/moving_least_squares.hpp" #include +#include +#include "pcl_ros/surface/moving_least_squares.hpp" + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::MovingLeastSquares::onInit() { PCLNodelet::onInit(); - //ros::NodeHandle private_nh = getMTPrivateNodeHandle (); + // ros::NodeHandle private_nh = getMTPrivateNodeHandle (); pub_output_ = advertise(*pnh_, "output", max_queue_size_); pub_normals_ = advertise(*pnh_, "normals", max_queue_size_); - //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) + // if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) if (!pnh_->getParam("search_radius", search_radius_)) { - //NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + // NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set " + // "at least one of these parameters before continuing.", getName ().c_str ()); NODELET_ERROR( "[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName().c_str()); @@ -93,8 +96,10 @@ pcl_ros::MovingLeastSquares::subscribe() sub_indices_filter_.subscribe(*pnh_, "indices", 1); if (approximate_sync_) { - sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_a_ = + boost::make_shared>>(max_queue_size_); // surface not enabled, connect the input-indices duo and register sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback( @@ -102,8 +107,10 @@ pcl_ros::MovingLeastSquares::subscribe() &MovingLeastSquares::input_indices_callback, this, _1, _2)); } else { - sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_ = + boost::make_shared>>(max_queue_size_); // surface not enabled, connect the input-indices duo and register sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback( @@ -168,8 +175,10 @@ pcl_ros::MovingLeastSquares::input_indices_callback( if (indices) { NODELET_DEBUG( "[%s::input_indices_model_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", + " - PointCloud with %d data points (%s), stamp %f, and " + "frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %f, and " + "frame %s on topic %s received.", getName().c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), @@ -178,7 +187,8 @@ pcl_ros::MovingLeastSquares::input_indices_callback( indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str()); } else { NODELET_DEBUG( - "[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", + "[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on " + "topic %s received.", getName().c_str(), cloud->width * cloud->height, fromPCL( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("input").c_str()); @@ -198,7 +208,7 @@ pcl_ros::MovingLeastSquares::input_indices_callback( // Initialize the spatial locator // Do the reconstructon - //impl_.process (output); + // impl_.process (output); // Publish a Boost shared ptr const data // Enforce that the TF frame and the timestamp are copied diff --git a/pcl_ros/src/pcl_ros/surface/surface.cpp b/pcl_ros/src/pcl_ros/surface/surface.cpp index a73b242d..09e3393f 100644 --- a/pcl_ros/src/pcl_ros/surface/surface.cpp +++ b/pcl_ros/src/pcl_ros/surface/surface.cpp @@ -42,5 +42,5 @@ **/ // Include the implementations here instead of compiling them separately to speed up compile time -//#include "convex_hull.cpp" -//#include "moving_least_squares.cpp" +// #include "convex_hull.cpp" +// #include "moving_least_squares.cpp" diff --git a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp index ba92d794..980a7d6b 100644 --- a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp +++ b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -48,7 +48,9 @@ #include -using namespace tf; +#include +#include +#include // using a random point type, as we want to make sure that it does work with // other points than just XYZ @@ -78,7 +80,7 @@ void setStamp(ros::Time & stamp, std::uint64_t & pcl_stamp) class Notification { public: - Notification(int expected_count) + explicit Notification(int expected_count) : count_(0), expected_count_(expected_count), failure_count_(0) @@ -90,7 +92,7 @@ public: ++count_; } - void failure(const PCDType::ConstPtr & message, FilterFailureReason reason) + void failure(const PCDType::ConstPtr & message, tf::FilterFailureReason reason) { ++failure_count_; } @@ -104,7 +106,7 @@ TEST(MessageFilter, noTransforms) { tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "frame1", 1); + tf::MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); PCDType::Ptr msg(new PCDType); @@ -120,7 +122,7 @@ TEST(MessageFilter, noTransformsSameFrame) { tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "frame1", 1); + tf::MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); PCDType::Ptr msg(new PCDType); @@ -136,7 +138,7 @@ TEST(MessageFilter, preexistingTransforms) { tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "frame1", 1); + tf::MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); PCDType::Ptr msg(new PCDType); @@ -161,7 +163,7 @@ TEST(MessageFilter, postTransforms) { tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "frame1", 1); + tf::MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); ros::Time stamp = ros::Time::now(); @@ -190,7 +192,7 @@ TEST(MessageFilter, queueSize) { tf::TransformListener tf_client; Notification n(10); - MessageFilter filter(tf_client, "frame1", 10); + tf::MessageFilter filter(tf_client, "frame1", 10); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); @@ -226,7 +228,7 @@ TEST(MessageFilter, setTargetFrame) { tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "frame1", 1); + tf::MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.setTargetFrame("frame1000"); @@ -252,7 +254,7 @@ TEST(MessageFilter, multipleTargetFrames) { tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "", 1); + tf::MessageFilter filter(tf_client, "", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); std::vector target_frames; @@ -277,9 +279,9 @@ TEST(MessageFilter, multipleTargetFrames) ros::WallDuration(0.1).sleep(); ros::spinOnce(); - EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet) + EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet) - //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0)); + // ros::Time::setNow(ros::Time::now() + ros::Duration(1.0)); transform.child_frame_id_ = "frame2"; tf_client.setTransform(transform); @@ -295,7 +297,7 @@ TEST(MessageFilter, tolerance) ros::Duration offset(0.2); tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "frame1", 1); + tf::MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.setTolerance(offset); @@ -314,9 +316,9 @@ TEST(MessageFilter, tolerance) msg->header.stamp = pcl_stamp; filter.add(msg); - EXPECT_EQ(0, n.count_); //No return due to lack of space for offset + EXPECT_EQ(0, n.count_); // No return due to lack of space for offset - //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1)); + // ros::Time::setNow(ros::Time::now() + ros::Duration(0.1)); transform.stamp_ += offset * 1.1; tf_client.setTransform(transform); @@ -324,7 +326,7 @@ TEST(MessageFilter, tolerance) ros::WallDuration(0.1).sleep(); ros::spinOnce(); - EXPECT_EQ(1, n.count_); // Now have data for the message published earlier + EXPECT_EQ(1, n.count_); // Now have data for the message published earlier stamp += offset; setStamp(stamp, pcl_stamp); @@ -332,14 +334,14 @@ TEST(MessageFilter, tolerance) filter.add(msg); - EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset + EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset } TEST(MessageFilter, outTheBackFailure) { tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "frame1", 1); + tf::MessageFilter filter(tf_client, "frame1", 1); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp = ros::Time::now(); @@ -366,7 +368,7 @@ TEST(MessageFilter, emptyFrameIDFailure) { tf::TransformListener tf_client; Notification n(1); - MessageFilter filter(tf_client, "frame1", 1); + tf::MessageFilter filter(tf_client, "frame1", 1); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); PCDType::Ptr msg(new PCDType); diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index 4eb23c3d..fcea320b 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -38,6 +38,8 @@ #include #include #include +#include +#include #include "pcl_ros/transforms.hpp" #include "pcl_ros/impl/transforms.hpp" @@ -142,21 +144,24 @@ transformPointCloud( in.fields[z_idx].offset, 0); for (size_t i = 0; i < in.width * in.height; ++i) { - Eigen::Vector4f pt(*(float *)&in.data[xyz_offset[0]], *(float *)&in.data[xyz_offset[1]], - *(float *)&in.data[xyz_offset[2]], 1); + Eigen::Vector4f pt(*reinterpret_cast(&in.data[xyz_offset[0]]), + *reinterpret_cast(&in.data[xyz_offset[1]]), + *reinterpret_cast(&in.data[xyz_offset[2]], 1)); Eigen::Vector4f pt_out; bool max_range_point = false; int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset; - float * distance_ptr = (dist_idx < 0 ? NULL : (float *)(&in.data[distance_ptr_offset])); + float * distance_ptr = + (dist_idx < 0 ? NULL : reinterpret_cast(&in.data[distance_ptr_offset])); if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { - if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point + if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point pt_out = pt; - } else { // max range point + } else { // max range point pt[0] = *distance_ptr; // Replace x with the x value saved in distance pt_out = transform * pt; max_range_point = true; - //std::cout << pt[0]<<","< "< "<(&out.data[distance_ptr_offset]) = pt_out[0]; pt_out[0] = std::numeric_limits::quiet_NaN(); } @@ -181,7 +186,8 @@ transformPointCloud( if (vp_idx != -1) { // Transform the viewpoint info too for (size_t i = 0; i < out.width * out.height; ++i) { - float * pstep = (float *)&out.data[i * out.point_step + out.fields[vp_idx].offset]; + float * pstep = + reinterpret_cast(&out.data[i * out.point_step + out.fields[vp_idx].offset]); // Assume vp_x, vp_y, vp_z are consecutive Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1); Eigen::Vector4f vp_out = transform * vp_in; @@ -212,7 +218,7 @@ transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat) out_mat(2, 3) = origin.z(); } -} // namespace pcl_ros +} // namespace pcl_ros ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl_ros::transformPointCloudWithNormals( diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index 7dd0dec9..696ddb1b 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -44,16 +44,17 @@ Cloud Data) format. **/ -#include -#include #include #include #include #include #include -#include "pcl_ros/transforms.hpp" #include #include +#include +#include +#include +#include "pcl_ros/transforms.hpp" typedef sensor_msgs::PointCloud2 PointCloud; typedef PointCloud::Ptr PointCloudPtr; diff --git a/pcl_ros/tools/convert_pcd_to_image.cpp b/pcl_ros/tools/convert_pcd_to_image.cpp index e14249f0..d0d9b17e 100644 --- a/pcl_ros/tools/convert_pcd_to_image.cpp +++ b/pcl_ros/tools/convert_pcd_to_image.cpp @@ -53,6 +53,7 @@ #include #include #include +#include /* ---[ */ int @@ -72,12 +73,12 @@ main(int argc, char ** argv) pcl::io::loadPCDFile(std::string(argv[1]), cloud); try { - pcl::toROSMsg(cloud, image); //convert the cloud + pcl::toROSMsg(cloud, image); // convert the cloud } catch (std::runtime_error & e) { ROS_ERROR_STREAM( "Error in converting cloud to image message: " << e.what()); - return 1; //fail! + return 1; // fail! } ros::Rate loop_rate(5); while (nh.ok()) { diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp index 3db4d31f..c692f506 100644 --- a/pcl_ros/tools/convert_pointcloud_to_image.cpp +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -40,14 +40,14 @@ **/ // ROS core #include -//Image message +// Image message #include #include -//pcl::toROSMsg +// pcl::toROSMsg #include -//conversions from PCL custom types +// conversions from PCL custom types #include -//stl stuff +// stl stuff #include class PointCloudToImage @@ -61,9 +61,9 @@ public: return; } try { - pcl::toROSMsg(*cloud, image_); //convert the cloud + pcl::toROSMsg(*cloud, image_); // convert the cloud image_.header = cloud->header; - image_pub_.publish(image_); //publish our cloud image + image_pub_.publish(image_); // publish our cloud image } catch (std::runtime_error & e) { ROS_ERROR_STREAM( "Error in converting cloud to image message: " << @@ -78,7 +78,7 @@ public: &PointCloudToImage::cloud_cb, this); image_pub_ = nh_.advertise(image_topic_, 30); - //print some info about the node + // print some info about the node std::string r_ct = nh_.resolveName(cloud_topic_); std::string r_it = nh_.resolveName(image_topic_); ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct); @@ -87,18 +87,18 @@ public: private: ros::NodeHandle nh_; - sensor_msgs::Image image_; //cache the image message - std::string cloud_topic_; //default input - std::string image_topic_; //default output - ros::Subscriber sub_; //cloud subscriber - ros::Publisher image_pub_; //image message publisher + sensor_msgs::Image image_; // cache the image message + std::string cloud_topic_; // default input + std::string image_topic_; // default output + ros::Subscriber sub_; // cloud subscriber + ros::Publisher image_pub_; // image message publisher }; int main(int argc, char ** argv) { ros::init(argc, argv, "convert_pointcloud_to_image"); - PointCloudToImage pci; //this loads up the node - ros::spin(); //where she stops nobody knows + PointCloudToImage pci; // this loads up the node + ros::spin(); // where she stops nobody knows return 0; } diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index 27b5628e..3119f91e 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -43,10 +43,6 @@ **/ -// STL -#include -#include - // ROS core #include #include @@ -54,14 +50,17 @@ #include #include -#include "pcl_ros/publisher.hpp" +// STL +#include +#include +#include -using namespace std; +#include "pcl_ros/publisher.hpp" class PCDGenerator { protected: - string tf_frame_; + std::string tf_frame_; ros::NodeHandle nh_; ros::NodeHandle private_nh_; @@ -69,7 +68,7 @@ public: // ROS messages sensor_msgs::PointCloud2 cloud_; - string file_name_, cloud_topic_; + std::string file_name_, cloud_topic_; double wait_; pcl_ros::Publisher pub_; @@ -105,7 +104,7 @@ public: bool spin() { int nr_points = cloud_.width * cloud_.height; - string fields_list = pcl::getFieldsList(cloud_); + std::string fields_list = pcl::getFieldsList(cloud_); double interval = wait_ * 1e+6; while (nh_.ok()) { ROS_DEBUG_ONCE( @@ -132,8 +131,6 @@ public: } return true; } - - }; /* ---[ */ @@ -149,7 +146,7 @@ main(int argc, char ** argv) ros::init(argc, argv, "pcd_to_pointcloud"); PCDGenerator c; - c.file_name_ = string(argv[1]); + c.file_name_ = std::string(argv[1]); // check if publishing interval is given if (argc == 2) { c.wait_ = 0; diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index e2fb436f..6c9fbb30 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -53,7 +53,8 @@ #include -using namespace std; +// STL +#include /** \author Radu Bogdan Rusu @@ -76,7 +77,7 @@ private: tf2_ros::TransformListener tf_listener_; public: - string cloud_topic_; + std::string cloud_topic_; ros::Subscriber sub_; @@ -131,7 +132,6 @@ public: } else { writer.writeASCII(ss.str(), *cloud, v, q, 8); } - } //////////////////////////////////////////////////////////////////////////////// From d7a79b927f8e1c5e08e8f76a94c5dd058e86f177 Mon Sep 17 00:00:00 2001 From: Sean Kelly Date: Wed, 12 Aug 2020 15:12:51 -0400 Subject: [PATCH 370/405] Port transforms library to ROS2 (#301) * Port transforms library to ROS2 - Port the transforms library to ROS2 - Update CMakeLists - Update package.xml - Enable the package Signed-off-by: Sean Kelly * Feedback from PR --- pcl_ros/CMakeLists.txt | 381 +++++++++----------- pcl_ros/COLCON_IGNORE | 0 pcl_ros/include/pcl_ros/impl/transforms.hpp | 132 ++++--- pcl_ros/include/pcl_ros/transforms.hpp | 101 ++++-- pcl_ros/package.xml | 43 ++- pcl_ros/src/transforms.cpp | 250 ++++++++----- perception_pcl/package.xml | 2 +- 7 files changed, 517 insertions(+), 392 deletions(-) delete mode 100644 pcl_ros/COLCON_IGNORE diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 7d845687..7311251c 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -1,245 +1,198 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) project(pcl_ros) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + ## Find system dependencies -find_package(cmake_modules REQUIRED) -find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS core features filters io segmentation surface) +find_package(PCL REQUIRED QUIET COMPONENTS core features filters io segmentation surface) -if(NOT "${PCL_LIBRARIES}" STREQUAL "") - # For debian: https://github.com/ros-perception/perception_pcl/issues/139 - list(REMOVE_ITEM PCL_LIBRARIES - "vtkGUISupportQt" - "vtkGUISupportQtOpenGL" - "vtkGUISupportQtSQL" - "vtkGUISupportQtWebkit" - "vtkViewsQt" - "vtkRenderingQt") -endif() +## Find ROS package dependencies +find_package(ament_cmake_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) -# There is a bug in the Ubuntu Artful (17.10) version of the VTK package, -# where it includes /usr/include/*-linux-gnu/freetype2 in the include -# directories (which doesn't exist). This filters down to the PCL_INCLUDE_DIRS, -# and causes downstream projects trying to use these libraries to fail to -# configure properly. Here we remove those bogus entries so that downstream -# consumers of this package succeed. -if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") - foreach(item ${PCL_INCLUDE_DIRS}) - string(REGEX MATCH "/usr/include/.*-linux-gnu/freetype2" item ${item}) - if(item) - list(REMOVE_ITEM PCL_INCLUDE_DIRS ${item}) - endif() - endforeach() -endif() - -## Find catkin packages -find_package(catkin REQUIRED COMPONENTS - dynamic_reconfigure - message_filters - nodelet - nodelet_topic_tools +set(dependencies pcl_conversions - pcl_msgs - pluginlib - rosbag - rosconsole - roscpp - roslib + rclcpp sensor_msgs - std_msgs - tf - tf2_eigen + geometry_msgs + tf2 + tf2_geometry_msgs + tf2_ros + EIGEN3 + PCL ) -## Add include directories -include_directories(include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} +include_directories( + include ${PCL_INCLUDE_DIRS} ) -## Generate dynamic_reconfigure -generate_dynamic_reconfigure_options( - cfg/EuclideanClusterExtraction.cfg - cfg/ExtractIndices.cfg - cfg/ExtractPolygonalPrismData.cfg - cfg/Feature.cfg - cfg/Filter.cfg - cfg/MLS.cfg - cfg/SACSegmentation.cfg - cfg/SACSegmentationFromNormals.cfg - cfg/SegmentDifferences.cfg - cfg/StatisticalOutlierRemoval.cfg - cfg/RadiusOutlierRemoval.cfg - cfg/VoxelGrid.cfg - cfg/CropBox.cfg -) - -## Declare the catkin package -catkin_package( - INCLUDE_DIRS include - LIBRARIES - pcl_ros_filters - pcl_ros_io - pcl_ros_tf - CATKIN_DEPENDS - dynamic_reconfigure - message_filters - nodelet - nodelet_topic_tools - pcl_conversions - pcl_msgs - rosbag - roscpp - sensor_msgs - std_msgs - tf - DEPENDS - Boost - EIGEN3 - PCL -) - ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) -target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_tf ${catkin_EXPORTED_TARGETS}) - -## Nodelets - -## Declare the pcl_ros_io library -add_library(pcl_ros_io - src/pcl_ros/io/bag_io.cpp - src/pcl_ros/io/concatenate_data.cpp - src/pcl_ros/io/concatenate_fields.cpp - src/pcl_ros/io/io.cpp - src/pcl_ros/io/pcd_io.cpp +ament_target_dependencies(pcl_ros_tf + ${dependencies} ) -target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -class_loader_hide_library_symbols(pcl_ros_io) +target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES}) -## Declare the pcl_ros_features library -add_library(pcl_ros_features - src/pcl_ros/features/feature.cpp - # Compilation is much faster if we include all the following CPP files in feature.cpp - src/pcl_ros/features/boundary.cpp - src/pcl_ros/features/fpfh.cpp - src/pcl_ros/features/fpfh_omp.cpp - src/pcl_ros/features/shot.cpp - src/pcl_ros/features/shot_omp.cpp - src/pcl_ros/features/moment_invariants.cpp - src/pcl_ros/features/normal_3d.cpp - src/pcl_ros/features/normal_3d_omp.cpp - src/pcl_ros/features/pfh.cpp - src/pcl_ros/features/principal_curvatures.cpp - src/pcl_ros/features/vfh.cpp -) -target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) -class_loader_hide_library_symbols(pcl_ros_features) - - -## 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) - -## Declare the pcl_ros_segmentation library -add_library (pcl_ros_segmentation - src/pcl_ros/segmentation/extract_clusters.cpp - src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp - src/pcl_ros/segmentation/sac_segmentation.cpp - src/pcl_ros/segmentation/segment_differences.cpp - src/pcl_ros/segmentation/segmentation.cpp -) -target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) -class_loader_hide_library_symbols(pcl_ros_segmentation) - -## Declare the pcl_ros_surface library -add_library (pcl_ros_surface - src/pcl_ros/surface/surface.cpp - # Compilation is much faster if we include all the following CPP files in surface.cpp - src/pcl_ros/surface/convex_hull.cpp - src/pcl_ros/surface/moving_least_squares.cpp -) -target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) -class_loader_hide_library_symbols(pcl_ros_surface) - -## Tools - -add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) -target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) -target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(bag_to_pcd tools/bag_to_pcd.cpp) -target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) -target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) -target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -## Downloads - -catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd - DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data - MD5 546b5b4822fb1de21b0cf83d41ad6683 -) -add_custom_target(download ALL DEPENDS download_extra_data) +### Nodelets +# +### Declare the pcl_ros_io library +#add_library(pcl_ros_io +# src/pcl_ros/io/bag_io.cpp +# src/pcl_ros/io/concatenate_data.cpp +# src/pcl_ros/io/concatenate_fields.cpp +# src/pcl_ros/io/io.cpp +# src/pcl_ros/io/pcd_io.cpp +#) +#target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#class_loader_hide_library_symbols(pcl_ros_io) +# +### Declare the pcl_ros_features library +#add_library(pcl_ros_features +# src/pcl_ros/features/feature.cpp +# # Compilation is much faster if we include all the following CPP files in feature.cpp +# src/pcl_ros/features/boundary.cpp +# src/pcl_ros/features/fpfh.cpp +# src/pcl_ros/features/fpfh_omp.cpp +# src/pcl_ros/features/shot.cpp +# src/pcl_ros/features/shot_omp.cpp +# src/pcl_ros/features/moment_invariants.cpp +# src/pcl_ros/features/normal_3d.cpp +# src/pcl_ros/features/normal_3d_omp.cpp +# src/pcl_ros/features/pfh.cpp +# src/pcl_ros/features/principal_curvatures.cpp +# src/pcl_ros/features/vfh.cpp +#) +#target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) +#class_loader_hide_library_symbols(pcl_ros_features) +# +# +### 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) +# +### Declare the pcl_ros_segmentation library +#add_library (pcl_ros_segmentation +# src/pcl_ros/segmentation/extract_clusters.cpp +# src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +# src/pcl_ros/segmentation/sac_segmentation.cpp +# src/pcl_ros/segmentation/segment_differences.cpp +# src/pcl_ros/segmentation/segmentation.cpp +#) +#target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) +#class_loader_hide_library_symbols(pcl_ros_segmentation) +# +### Declare the pcl_ros_surface library +#add_library (pcl_ros_surface +# src/pcl_ros/surface/surface.cpp +# # Compilation is much faster if we include all the following CPP files in surface.cpp +# src/pcl_ros/surface/convex_hull.cpp +# src/pcl_ros/surface/moving_least_squares.cpp +#) +#target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) +#class_loader_hide_library_symbols(pcl_ros_surface) +# +### Tools +# +#add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) +#target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +#add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) +#target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +#add_executable(bag_to_pcd tools/bag_to_pcd.cpp) +#target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +#add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) +#target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +#add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) +#target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +### Downloads +# +#catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd +# DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data +# MD5 546b5b4822fb1de21b0cf83d41ad6683 +#) +#add_custom_target(download ALL DEPENDS download_extra_data) ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - 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) - add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false) - add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) - add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false) - add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false) -endif(CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + #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) + #add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false) + #add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) + #add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false) + #add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false) +endif() -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install( + DIRECTORY include/ + DESTINATION include +) install( TARGETS pcl_ros_tf - pcl_ros_io - pcl_ros_features - pcl_ros_filters - pcl_ros_surface - pcl_ros_segmentation - pcd_to_pointcloud - pointcloud_to_pcd - bag_to_pcd - convert_pcd_to_image - convert_pointcloud_to_image - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# pcl_ros_io +# pcl_ros_features +# pcl_ros_filters +# pcl_ros_surface +# pcl_ros_segmentation +# pcd_to_pointcloud +# pointcloud_to_pcd +# bag_to_pcd +# convert_pcd_to_image +# convert_pointcloud_to_image + EXPORT export_pcl_ros + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include ) install(DIRECTORY plugins samples - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + DESTINATION share/${PROJECT_NAME}) + +ament_export_include_directories(include) +ament_export_libraries(pcl_ros_tf) +ament_export_dependencies(${dependencies}) +ament_export_targets(export_pcl_ros HAS_LIBRARY_TARGET) +ament_package() diff --git a/pcl_ros/COLCON_IGNORE b/pcl_ros/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index 2f2a7580..81fb9169 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -37,9 +37,22 @@ #ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_ #define PCL_ROS__IMPL__TRANSFORMS_HPP_ -#include -#include #include "pcl_ros/transforms.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include using pcl_conversions::fromPCL; using pcl_conversions::toPCL; @@ -51,16 +64,16 @@ template void transformPointCloudWithNormals( const pcl::PointCloud & cloud_in, - pcl::PointCloud & cloud_out, const tf::Transform & transform) + pcl::PointCloud & cloud_out, const tf2::Transform & transform) { // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. - tf::Quaternion q = transform.getRotation(); + tf2::Quaternion q = transform.getRotation(); Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) - tf::Vector3 v = transform.getOrigin(); + tf2::Vector3 v = transform.getOrigin(); Eigen::Vector3f origin(v.x(), v.y(), v.z()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform @@ -69,21 +82,33 @@ transformPointCloudWithNormals( transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation); } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, const geometry_msgs::msg::TransformStamped & transform) +{ + tf2::Transform tf; + tf2::convert(transform.transform, tf); + transformPointCloudWithNormals(cloud_in, cloud_out, tf); +} + ////////////////////////////////////////////////////////////////////////////////////////////// template void transformPointCloud( const pcl::PointCloud & cloud_in, - pcl::PointCloud & cloud_out, const tf::Transform & transform) + pcl::PointCloud & cloud_out, const tf2::Transform & transform) { // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. - tf::Quaternion q = transform.getRotation(); + tf2::Quaternion q = transform.getRotation(); Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) - tf::Vector3 v = transform.getOrigin(); + tf2::Vector3 v = transform.getOrigin(); Eigen::Vector3f origin(v.x(), v.y(), v.z()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform @@ -92,6 +117,18 @@ transformPointCloud( transformPointCloud(cloud_in, cloud_out, origin, rotation); } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +transformPointCloud( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, const geometry_msgs::msg::TransformStamped & transform) +{ + tf2::Transform tf; + tf2::convert(transform.transform, tf); + transformPointCloud(cloud_in, cloud_out, tf); +} + ////////////////////////////////////////////////////////////////////////////////////////////// template bool @@ -99,23 +136,24 @@ transformPointCloudWithNormals( const std::string & target_frame, const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener) + const tf2_ros::Buffer & tf_buffer) { if (cloud_in.header.frame_id == target_frame) { cloud_out = cloud_in; return true; } - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform( - target_frame, cloud_in.header.frame_id, fromPCL( - cloud_in.header).stamp, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + transform = + tf_buffer.lookupTransform( + target_frame, cloud_in.header.frame_id, + fromPCL(cloud_in.header.stamp)); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } @@ -131,25 +169,27 @@ transformPointCloud( const std::string & target_frame, const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener) + const tf2_ros::Buffer & tf_buffer) { if (cloud_in.header.frame_id == target_frame) { cloud_out = cloud_in; return true; } - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform( - target_frame, cloud_in.header.frame_id, fromPCL( - cloud_in.header).stamp, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + transform = + tf_buffer.lookupTransform( + target_frame, cloud_in.header.frame_id, + fromPCL(cloud_in.header.stamp)); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } + transformPointCloud(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; return true; @@ -160,28 +200,28 @@ template bool transformPointCloud( const std::string & target_frame, - const ros::Time & target_time, + const rclcpp::Time & target_time, const pcl::PointCloud & cloud_in, const std::string & fixed_frame, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener) + const tf2_ros::Buffer & tf_buffer) { - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform( + transform = tf_buffer.lookupTransform( target_frame, target_time, cloud_in.header.frame_id, - fromPCL(cloud_in.header).stamp, fixed_frame, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + fromPCL(cloud_in.header.stamp), fixed_frame); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } transformPointCloud(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; - std_msgs::Header header; + std_msgs::msg::Header header; header.stamp = target_time; cloud_out.header = toPCL(header); return true; @@ -192,28 +232,28 @@ template bool transformPointCloudWithNormals( const std::string & target_frame, - const ros::Time & target_time, + const rclcpp::Time & target_time, const pcl::PointCloud & cloud_in, const std::string & fixed_frame, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener) + const tf2_ros::Buffer & tf_buffer) { - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform( + transform = tf_buffer.lookupTransform( target_frame, target_time, cloud_in.header.frame_id, - fromPCL(cloud_in.header).stamp, fixed_frame, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + fromPCL(cloud_in.header.stamp), fixed_frame); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } transformPointCloudWithNormals(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; - std_msgs::Header header; + std_msgs::msg::Header header; header.stamp = target_time; cloud_out.header = toPCL(header); return true; diff --git a/pcl_ros/include/pcl_ros/transforms.hpp b/pcl_ros/include/pcl_ros/transforms.hpp index bd890749..1b5312f3 100644 --- a/pcl_ros/include/pcl_ros/transforms.hpp +++ b/pcl_ros/include/pcl_ros/transforms.hpp @@ -37,10 +37,14 @@ #ifndef PCL_ROS__TRANSFORMS_HPP_ #define PCL_ROS__TRANSFORMS_HPP_ -#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include namespace pcl_ros @@ -56,13 +60,26 @@ void transformPointCloudWithNormals( const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::Transform & transform); + const tf2::Transform & transform); + +/** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param transform a rigid transformation from tf + * \note calls the Eigen version + */ +template +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const geometry_msgs::msg::TransformStamped & transform); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param cloud_in the input point cloud * \param cloud_out the input point cloud - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ template bool @@ -70,7 +87,7 @@ transformPointCloudWithNormals( const std::string & target_frame, const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener); + const tf2_ros::Buffer & tf_buffer); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to @@ -78,17 +95,17 @@ transformPointCloudWithNormals( * \param cloud_in the input point cloud * \param fixed_frame fixed TF frame * \param cloud_out the input point cloud - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ template bool transformPointCloudWithNormals( const std::string & target_frame, - const ros::Time & target_time, + const rclcpp::Time & target_time, const pcl::PointCloud & cloud_in, const std::string & fixed_frame, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener); + const tf2_ros::Buffer & tf_buffer); /** \brief Apply a rigid transform defined by a 3D offset and a quaternion * \param cloud_in the input point cloud @@ -101,13 +118,26 @@ void transformPointCloud( const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::Transform & transform); + const tf2::Transform & transform); + +/** \brief Apply a rigid transform defined by a 3D offset and a quaternion + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param transform a rigid transformation from tf + * \note calls the Eigen version + */ +template +void +transformPointCloud( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const geometry_msgs::msg::TransformStamped & transform); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param cloud_in the input point cloud * \param cloud_out the input point cloud - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ template bool @@ -115,7 +145,7 @@ transformPointCloud( const std::string & target_frame, const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener); + const tf2_ros::Buffer & tf_buffer); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to @@ -123,29 +153,30 @@ transformPointCloud( * \param cloud_in the input point cloud * \param fixed_frame fixed TF frame * \param cloud_out the input point cloud - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ template bool transformPointCloud( - const std::string & target_frame, const ros::Time & target_time, + const std::string & target_frame, + const rclcpp::Time & target_time, const pcl::PointCloud & cloud_in, const std::string & fixed_frame, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener); + const tf2_ros::Buffer & tf_buffer); /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. * \param target_frame the target TF frame * \param in the input PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ bool transformPointCloud( const std::string & target_frame, - const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out, - const tf::TransformListener & tf_listener); + const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out, + const tf2_ros::Buffer & tf_buffer); /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. * \param target_frame the target TF frame @@ -156,9 +187,22 @@ transformPointCloud( void transformPointCloud( const std::string & target_frame, - const tf::Transform & net_transform, - const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out); + const tf2::Transform & net_transform, + const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out); + +/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. + * \param target_frame the target TF frame + * \param net_transform the TF transformer object + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + */ +void +transformPointCloud( + const std::string & target_frame, + const geometry_msgs::msg::TransformStamped & net_transform, + const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out); /** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix. * \param transform the transformation to use on the points @@ -168,15 +212,22 @@ transformPointCloud( void transformPointCloud( const Eigen::Matrix4f & transform, - const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out); + const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out); /** \brief Obtain the transformation matrix from TF into an Eigen form * \param bt the TF transformation * \param out_mat the Eigen transformation */ void -transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat); +transformAsMatrix(const tf2::Transform & bt, Eigen::Matrix4f & out_mat); + +/** \brief Obtain the transformation matrix from TF into an Eigen form + * \param bt the TF transformation + * \param out_mat the Eigen transformation + */ +void +transformAsMatrix(const geometry_msgs::msg::TransformStamped & bt, Eigen::Matrix4f & out_mat); } // namespace pcl_ros #endif // PCL_ROS__TRANSFORMS_HPP_ diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 88d40c40..cf23b4f7 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,5 +1,6 @@ - + + pcl_ros 2.2.0 @@ -10,10 +11,6 @@ - Open Perception - Julius Kammerl - William Woodall - Paul Bovbel Steve Macenski Kentaro Wada @@ -24,35 +21,37 @@ https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl - catkin - cmake_modules - rosconsole - roslib + Open Perception + Julius Kammerl + William Woodall - dynamic_reconfigure + ament_cmake eigen - message_filters - nodelet - nodelet_topic_tools libpcl-all-dev pcl_conversions - pcl_msgs - pluginlib - rosbag - roscpp + rclcpp sensor_msgs - std_msgs - tf - tf2_eigen + geometry_msgs + tf2 + tf2_geometry_msgs + tf2_ros - rostest + ament_lint_auto + ament_lint_common + ament_cmake_gtest - + + + + ament_cmake diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index fcea320b..ffb74a00 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -34,22 +34,34 @@ * */ -#include -#include -#include -#include -#include -#include #include "pcl_ros/transforms.hpp" #include "pcl_ros/impl/transforms.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool transformPointCloud( - const std::string & target_frame, const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out, const tf::TransformListener & tf_listener) + const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out, const tf2_ros::Buffer & tf_buffer) { if (in.header.frame_id == target_frame) { out = in; @@ -57,14 +69,17 @@ transformPointCloud( } // Get the TF transform - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform(target_frame, in.header.frame_id, in.header.stamp, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + transform = + tf_buffer.lookupTransform( + target_frame, in.header.frame_id, tf2_ros::fromMsg( + in.header.stamp)); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } @@ -81,8 +96,8 @@ transformPointCloud( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void transformPointCloud( - const std::string & target_frame, const tf::Transform & net_transform, - const sensor_msgs::PointCloud2 & in, sensor_msgs::PointCloud2 & out) + const std::string & target_frame, const tf2::Transform & net_transform, + const sensor_msgs::msg::PointCloud2 & in, sensor_msgs::msg::PointCloud2 & out) { if (in.header.frame_id == target_frame) { out = in; @@ -98,11 +113,21 @@ transformPointCloud( out.header.frame_id = target_frame; } +void +transformPointCloud( + const std::string & target_frame, const geometry_msgs::msg::TransformStamped & net_transform, + const sensor_msgs::msg::PointCloud2 & in, sensor_msgs::msg::PointCloud2 & out) +{ + tf2::Transform transform; + tf2::convert(net_transform.transform, transform); + transformPointCloud(target_frame, transform, in, out); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void transformPointCloud( - const Eigen::Matrix4f & transform, const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out) + const Eigen::Matrix4f & transform, const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out) { // Get X-Y-Z indices int x_idx = pcl::getFieldIndex(in, "x"); @@ -110,15 +135,19 @@ transformPointCloud( int z_idx = pcl::getFieldIndex(in, "z"); if (x_idx == -1 || y_idx == -1 || z_idx == -1) { - ROS_ERROR("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); + RCLCPP_ERROR( + rclcpp::get_logger("pcl_ros"), + "Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); return; } - if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || - in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || - in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) + if (in.fields[x_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || + in.fields[y_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || + in.fields[z_idx].datatype != sensor_msgs::msg::PointField::FLOAT32) { - ROS_ERROR("X-Y-Z coordinates not floats. Currently only floats are supported."); + RCLCPP_ERROR( + rclcpp::get_logger("pcl_ros"), + "X-Y-Z coordinates not floats. Currently only floats are supported."); return; } @@ -144,15 +173,15 @@ transformPointCloud( in.fields[z_idx].offset, 0); for (size_t i = 0; i < in.width * in.height; ++i) { - Eigen::Vector4f pt(*reinterpret_cast(&in.data[xyz_offset[0]]), - *reinterpret_cast(&in.data[xyz_offset[1]]), - *reinterpret_cast(&in.data[xyz_offset[2]], 1)); + Eigen::Vector4f pt(*reinterpret_cast(&in.data[xyz_offset[0]]), + *reinterpret_cast(&in.data[xyz_offset[1]]), + *reinterpret_cast(&in.data[xyz_offset[2]]), 1); Eigen::Vector4f pt_out; bool max_range_point = false; int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset; - float * distance_ptr = - (dist_idx < 0 ? NULL : reinterpret_cast(&in.data[distance_ptr_offset])); + const float * distance_ptr = (dist_idx < 0 ? + NULL : reinterpret_cast(&in.data[distance_ptr_offset])); if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point pt_out = pt; @@ -160,8 +189,6 @@ transformPointCloud( pt[0] = *distance_ptr; // Replace x with the x value saved in distance pt_out = transform * pt; max_range_point = true; - // std::cout << pt[0]<<","< "<( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloudWithNormals( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloudWithNormals( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloudWithNormals( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloudWithNormals( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloud( - const std::string &, const ros::Time &, + const std::string &, const rclcpp::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( - const std::string &, const ros::Time &, + const std::string &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( - const std::string &, const ros::Time &, + const std::string &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( - const std::string &, const ros::Time &, + const std::string &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 2a68f39c..acfc5854 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -27,7 +27,7 @@ pcl_conversions pcl_msgs - + pcl_ros ament_cmake From 706c020528b66e772e3f86ff5acc88191b415419 Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Sat, 22 Aug 2020 20:04:21 +0200 Subject: [PATCH 371/405] Deprecate use_polynomial_fit (#305) The pcl function setPolynomialFit is deprecated, setPolynomialOrder should be used instead --- pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index f48391d4..c05cbbb6 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -244,7 +244,19 @@ pcl_ros::MovingLeastSquares::config_callback(MLSConfig & config, uint32_t level) NODELET_DEBUG( "[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_); +#if PCL_VERSION_COMPARE(<, 1, 9, 0) impl_.setPolynomialFit(use_polynomial_fit_); +#else + if (use_polynomial_fit_) { + NODELET_WARN( + "[config_callback] use_polynomial_fit is deprecated, use polynomial_order instead!"); + if (impl_.getPolynomialOrder() < 2) { + impl_.setPolynomialOrder(2); + } + } else { + impl_.setPolynomialOrder(0); + } +#endif } if (polynomial_order_ != config.polynomial_order) { polynomial_order_ = config.polynomial_order; From 2d214674230cd4183e8dfa9bbf069e847ec12ff8 Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Mon, 9 Nov 2020 20:09:17 +0100 Subject: [PATCH 372/405] Change conversions of Vertices for new PCL versions (#316) In https://github.com/PointCloudLibrary/pcl/commit/ad00c7bee2fad0391649479d90eee4461a2e74e7, the vertices field of pcl::Vertices changed from std::vector to std::vector, where index_t is an index type with configurable size (currently by default int). This commit makes conversions from and to pcl_msgs::Vertices possible again, moving the vector contents if possible. --- .../include/pcl_conversions/pcl_conversions.h | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/pcl_conversions/include/pcl_conversions/pcl_conversions.h b/pcl_conversions/include/pcl_conversions/pcl_conversions.h index 4f652bd2..6f5f20c6 100644 --- a/pcl_conversions/include/pcl_conversions/pcl_conversions.h +++ b/pcl_conversions/include/pcl_conversions/pcl_conversions.h @@ -356,10 +356,25 @@ namespace pcl_conversions { /** pcl::Vertices <=> pcl_msgs::Vertices **/ + namespace internal + { + template + inline void move(std::vector &a, std::vector &b) + { + b.swap(a); + } + + template + inline void move(std::vector &a, std::vector &b) + { + b.assign(a.cbegin(), a.cend()); + } + } + inline void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert) { - vert.vertices = pcl_vert.vertices; + vert.vertices.assign(pcl_vert.vertices.cbegin(), pcl_vert.vertices.cend()); } inline @@ -376,7 +391,7 @@ namespace pcl_conversions { inline void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert) { - vert.vertices.swap(pcl_vert.vertices); + internal::move(pcl_vert.vertices, vert.vertices); } inline @@ -393,7 +408,7 @@ namespace pcl_conversions { inline void toPCL(const pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert) { - pcl_vert.vertices = vert.vertices; + pcl_vert.vertices.assign(vert.vertices.cbegin(), vert.vertices.cend()); } inline @@ -410,7 +425,7 @@ namespace pcl_conversions { inline void moveToPCL(pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert) { - pcl_vert.vertices.swap(vert.vertices); + internal::move(vert.vertices, pcl_vert.vertices); } inline From 135a2c29e3f46904f586e963cc4a4ec3fe594a9c Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Fri, 4 Dec 2020 19:57:37 +0100 Subject: [PATCH 373/405] Correct conversion of time stamp (#319) * Add test to verify that stamps are correctly converted * Fix bug in fromPCL function for headers The time stamp was not set. --- pcl_conversions/include/pcl_conversions/pcl_conversions.h | 3 +-- pcl_conversions/test/test_pcl_conversions.cpp | 4 ++++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/pcl_conversions/include/pcl_conversions/pcl_conversions.h b/pcl_conversions/include/pcl_conversions/pcl_conversions.h index 6f5f20c6..9970ba7b 100644 --- a/pcl_conversions/include/pcl_conversions/pcl_conversions.h +++ b/pcl_conversions/include/pcl_conversions/pcl_conversions.h @@ -111,8 +111,7 @@ namespace pcl_conversions { inline void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::msg::Header &header) { - auto time_stamp = rclcpp::Time(header.stamp); - fromPCL(pcl_header.stamp, time_stamp); + header.stamp = fromPCL(pcl_header.stamp); header.frame_id = pcl_header.frame_id; } diff --git a/pcl_conversions/test/test_pcl_conversions.cpp b/pcl_conversions/test/test_pcl_conversions.cpp index f827ab01..83a3142e 100644 --- a/pcl_conversions/test/test_pcl_conversions.cpp +++ b/pcl_conversions/test/test_pcl_conversions.cpp @@ -9,6 +9,7 @@ namespace { class PCLConversionTests : public ::testing::Test { protected: virtual void SetUp() { + pcl_image.header.stamp = 3141592653; pcl_image.header.frame_id = "pcl"; pcl_image.height = 1; pcl_image.width = 2; @@ -19,6 +20,7 @@ protected: pcl_image.data[0] = 0x42; pcl_image.data[1] = 0x43; + pcl_pc2.header.stamp = 3141592653; pcl_pc2.header.frame_id = "pcl"; pcl_pc2.height = 1; pcl_pc2.width = 2; @@ -66,6 +68,7 @@ TEST_F(PCLConversionTests, imageConversion) { pcl::PCLImage pcl_image2; pcl_conversions::toPCL(image, pcl_image2); test_image(pcl_image2); + EXPECT_EQ(pcl_image.header.stamp, pcl_image2.header.stamp); } template @@ -96,6 +99,7 @@ TEST_F(PCLConversionTests, pointcloud2Conversion) { pcl::PCLPointCloud2 pcl_pc2_2; pcl_conversions::toPCL(pc2, pcl_pc2_2); test_pc(pcl_pc2_2); + EXPECT_EQ(pcl_pc2.header.stamp, pcl_pc2_2.header.stamp); } } // namespace From 487957aebc728fbb7c1339122e1b3a3d229e5ab7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 20 May 2021 13:26:52 -0700 Subject: [PATCH 374/405] bumping to 2.3.0 for release --- pcl_conversions/package.xml | 2 +- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index 367e1939..77a182d6 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 2.2.0 + 2.3.0 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index cf23b4f7..1eccc1ff 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -2,7 +2,7 @@ pcl_ros - 2.2.0 + 2.3.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index acfc5854..8df087ab 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -2,7 +2,7 @@ perception_pcl - 2.2.0 + 2.3.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From 4156c71c025d9111113673bc891aeab4c602086f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 20 May 2021 15:57:37 -0700 Subject: [PATCH 375/405] replace ament_cmake_ros with ament_cmake to build in build farm (#326) --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 7311251c..b5f8aa97 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED QUIET COMPONENTS core features filters io segmentation surface) ## Find ROS package dependencies -find_package(ament_cmake_ros REQUIRED) +find_package(ament_cmake REQUIRED) find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) From d63143da272cb909f2b7d8b501cb5a298df02c53 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 20 May 2021 15:58:46 -0700 Subject: [PATCH 376/405] bump to 2.3.1 for release updates based on build farm fixes --- pcl_conversions/package.xml | 2 +- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index 77a182d6..e17b4220 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 2.3.0 + 2.3.1 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 1eccc1ff..55064f3e 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -2,7 +2,7 @@ pcl_ros - 2.3.0 + 2.3.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 8df087ab..d7736e9c 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -2,7 +2,7 @@ perception_pcl - 2.3.0 + 2.3.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From 35a9f258487585e7a10e1d30a4d72f4b2f9bd16e Mon Sep 17 00:00:00 2001 From: Patrick Musau Date: Mon, 28 Jun 2021 11:37:50 -0500 Subject: [PATCH 377/405] shared library bug fix (#331) Signed-off-by: Patrick Musau --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index b5f8aa97..3fd34cae 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -44,7 +44,7 @@ add_library(pcl_ros_tf src/transforms.cpp) ament_target_dependencies(pcl_ros_tf ${dependencies} ) -target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_tf SHARED ${PCL_LIBRARIES}) ### Nodelets # From 461a66ca0877b99e5f0cc0d6e157753db33dc0cd Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Wed, 30 Jun 2021 21:44:28 +0200 Subject: [PATCH 378/405] Use complete namespace for traits members (#332) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This prevents errors like this: error: ‘name’ is not a member of ‘pcl::detail::traits’; did you mean ‘pcl::traits::name’? The error appears for newer PCL versions (everything after commit https://github.com/PointCloudLibrary/pcl/commit/d39d3d3300746b952997e5bd2742dac7482aa5ab), but this change should also be fully compatible with older PCL versions. --- pcl_ros/include/pcl_ros/point_cloud.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index cf14d3d7..c7904548 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -80,20 +80,20 @@ struct FieldStreamer template void operator()() { - const char * name = traits::name::value; + const char * name = pcl::traits::name::value; std::uint32_t name_length = strlen(name); stream_.next(name_length); if (name_length > 0) { memcpy(stream_.advance(name_length), name, name_length); } - std::uint32_t offset = traits::offset::value; + std::uint32_t offset = pcl::traits::offset::value; stream_.next(offset); - std::uint8_t datatype = traits::datatype::value; + std::uint8_t datatype = pcl::traits::datatype::value; stream_.next(datatype); - std::uint32_t count = traits::datatype::size; + std::uint32_t count = pcl::traits::datatype::size; stream_.next(count); } @@ -109,7 +109,7 @@ struct FieldsLength template void operator()() { - std::uint32_t name_length = strlen(traits::name::value); + std::uint32_t name_length = strlen(pcl::traits::name::value); length += name_length + 13; } From 5d63abd06edfb3e28865382cbbb89c2938926b34 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 1 Jul 2021 16:20:31 -0700 Subject: [PATCH 379/405] Revert "shared library bug fix (#331)" (#334) This reverts commit 35a9f258487585e7a10e1d30a4d72f4b2f9bd16e. --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 3fd34cae..b5f8aa97 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -44,7 +44,7 @@ add_library(pcl_ros_tf src/transforms.cpp) ament_target_dependencies(pcl_ros_tf ${dependencies} ) -target_link_libraries(pcl_ros_tf SHARED ${PCL_LIBRARIES}) +target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES}) ### Nodelets # From 051c6df956e88ecb92e118ef161a99d82af53e42 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 1 Jul 2021 16:54:20 -0700 Subject: [PATCH 380/405] adding -fPIC compiler flag per warning suggestion (#335) --- pcl_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index b5f8aa97..7340c7cb 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -5,7 +5,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -fPIC) endif() ## Find system dependencies From 1a214ccc6eea62c57bf359c6dac50452e0257ce4 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 31 Aug 2021 03:08:25 +0900 Subject: [PATCH 381/405] Fix obsolete header (#342) Signed-off-by: Kenji Miyake --- pcl_ros/include/pcl_ros/impl/transforms.hpp | 2 +- pcl_ros/src/transforms.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index 81fb9169..31c0e1fe 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index ffb74a00..fe191703 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include #include From 834eb111e86b53ab462fda816f3c23b702990e07 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 14 Sep 2021 14:21:14 -0700 Subject: [PATCH 382/405] bumping rolling from 2.3.1 to 2.4.0 --- pcl_conversions/package.xml | 2 +- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index e17b4220..8752c5c0 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 2.3.1 + 2.4.0 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 55064f3e..3083bb05 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -2,7 +2,7 @@ pcl_ros - 2.3.1 + 2.4.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index d7736e9c..7e2ec09d 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -2,7 +2,7 @@ perception_pcl - 2.3.1 + 2.4.0 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From 755f566356a1d34d4b914f477cdf3820aa1a6b07 Mon Sep 17 00:00:00 2001 From: Rich Mattes Date: Mon, 27 Sep 2021 17:28:26 -0400 Subject: [PATCH 383/405] pcl_ros: Use vec.data() instead of &vec[0] (#348) Update pcl_ros to use the data() member function of STL containers to get the pointer to the underlying storage, instead of dereferencing the zeroth element and taking its reference. When a container is of size 0, dereferencing element 0 with operator[]() is undefined behavior, and will trigger assertions when features like _GLIBCXX_ASSERTIONS are enabled. Fixes #333. --- pcl_ros/include/pcl_ros/point_cloud.hpp | 4 ++-- pcl_ros/src/transforms.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index c7904548..2b7ec855 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -255,7 +255,7 @@ struct Serializer> stream.next(row_step); uint32_t data_size = row_step * height; stream.next(data_size); - memcpy(stream.advance(data_size), &m.points[0], data_size); + memcpy(stream.advance(data_size), m.points.data(), data_size); uint8_t is_dense = m.is_dense; stream.next(is_dense); @@ -296,7 +296,7 @@ struct Serializer> stream.next(data_size); assert(data_size == m.height * m.width * point_step); m.points.resize(m.height * m.width); - uint8_t * m_data = reinterpret_cast(&m.points[0]); + uint8_t * m_data = reinterpret_cast(m.points.data()); // If the data layouts match, can copy a whole row in one memcpy if (mapping.size() == 1 && mapping[0].serialized_offset == 0 && diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index fe191703..2ca2462a 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -166,7 +166,7 @@ transformPointCloud( out.is_dense = in.is_dense; out.data.resize(in.data.size()); // Copy everything as it's faster than copying individual elements - memcpy(&out.data[0], &in.data[0], in.data.size()); + memcpy(out.data.data(), in.data.data(), in.data.size()); } Eigen::Array4i xyz_offset(in.fields[x_idx].offset, in.fields[y_idx].offset, From 8093d5b35df0c570efb918d170182b45eada78b0 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 3 Feb 2022 10:11:55 -0800 Subject: [PATCH 384/405] Install headers to include/${PROJECT_NAME} (#354) Signed-off-by: Shane Loretz --- pcl_conversions/CMakeLists.txt | 8 +++++--- pcl_ros/CMakeLists.txt | 22 ++++++++++++---------- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index 499ac38e..38a2e21d 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -33,8 +33,8 @@ include_directories( ${PCL_COMMON_INCLUDE_DIRS} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME}/ +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) # Add gtest based cpp test target @@ -48,6 +48,8 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}-test ${Boost_LIBRARIES} ${PCL_LIBRARIES}) endif() -ament_export_include_directories(include) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") + ament_export_dependencies(${dependencies}) ament_package() diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 7340c7cb..4441c1be 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -34,17 +34,15 @@ set(dependencies PCL ) -include_directories( - include - ${PCL_INCLUDE_DIRS} -) - ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) +target_include_directories(pcl_ros_tf PUBLIC + $ + $ +) ament_target_dependencies(pcl_ros_tf ${dependencies} ) -target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES}) ### Nodelets # @@ -165,7 +163,7 @@ endif() install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) install( @@ -185,14 +183,18 @@ install( RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - INCLUDES DESTINATION include ) install(DIRECTORY plugins samples DESTINATION share/${PROJECT_NAME}) -ament_export_include_directories(include) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(pcl_ros_tf) -ament_export_dependencies(${dependencies}) + +# Export modern CMake targets ament_export_targets(export_pcl_ros HAS_LIBRARY_TARGET) + +ament_export_dependencies(${dependencies}) + ament_package() From 2b82545af3fda19eaa9bbed2a0f5d517bf23103f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 8 Feb 2022 15:31:32 -0800 Subject: [PATCH 385/405] bumping versions to 2.4.1 (#357) --- pcl_conversions/package.xml | 2 +- pcl_ros/package.xml | 2 +- perception_pcl/package.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index 8752c5c0..06a2ebd9 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -1,7 +1,7 @@ pcl_conversions - 2.4.0 + 2.4.1 Provides conversions from PCL data types and ROS message types William Woodall diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 3083bb05..c736fcad 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -2,7 +2,7 @@ pcl_ros - 2.4.0 + 2.4.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 7e2ec09d..c0737fae 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -2,7 +2,7 @@ perception_pcl - 2.4.0 + 2.4.1 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry From 27448f757529620e0100c61ce1acb2b3f6c132cc Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 4 Apr 2022 16:54:29 -0300 Subject: [PATCH 386/405] Migrate pcd to pointcloud tool (#359) * Migrate pcl_ros pcd_to_pointcloud tool Signed-off-by: Ivan Santiago Paunovic * Remove log used for debugging Signed-off-by: Ivan Santiago Paunovic * Undo unnecessary changes Signed-off-by: Ivan Santiago Paunovic * Remove commented out code Signed-off-by: Ivan Santiago Paunovic * Address peer review comments Signed-off-by: Ivan Santiago Paunovic * rename node Signed-off-by: Ivan Santiago Paunovic --- pcl_ros/CMakeLists.txt | 20 +++- pcl_ros/package.xml | 1 + pcl_ros/tools/pcd_to_pointcloud.cpp | 141 ++++++++++------------------ 3 files changed, 67 insertions(+), 95 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 4441c1be..258f2366 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -10,12 +10,13 @@ endif() ## Find system dependencies find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED QUIET COMPONENTS core features filters io segmentation surface) +find_package(PCL REQUIRED QUIET COMPONENTS core common features filters io segmentation surface) ## Find ROS package dependencies find_package(ament_cmake REQUIRED) find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -118,8 +119,19 @@ ament_target_dependencies(pcl_ros_tf # ### Tools # -#add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) -#target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +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) +rclcpp_components_register_node(pcd_to_pointcloud_lib + PLUGIN "pcl_ros::PCDPublisher" + EXECUTABLE pcd_to_pointcloud) # #add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) #target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) @@ -169,12 +181,12 @@ install( install( TARGETS pcl_ros_tf + pcd_to_pointcloud_lib # pcl_ros_io # pcl_ros_features # pcl_ros_filters # pcl_ros_surface # pcl_ros_segmentation -# pcd_to_pointcloud # pointcloud_to_pcd # bag_to_pcd # convert_pcd_to_image diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index c736fcad..81b1c6cb 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -30,6 +30,7 @@ libpcl-all-dev pcl_conversions rclcpp + rclcpp_components sensor_msgs geometry_msgs tf2 diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index 3119f91e..5c51a774 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -43,126 +43,85 @@ **/ -// ROS core -#include -#include -#include -#include -#include - // STL #include #include #include -#include "pcl_ros/publisher.hpp" +// PCL +#include +#include +#include -class PCDGenerator +// ROS core +#include +#include "rclcpp_components/register_node_macro.hpp" +#include +#include + +namespace pcl_ros +{ +class PCDPublisher : public rclcpp::Node { protected: std::string tf_frame_; - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; public: // ROS messages - sensor_msgs::PointCloud2 cloud_; + sensor_msgs::msg::PointCloud2 cloud_; std::string file_name_, cloud_topic_; - double wait_; + size_t period_ms_; - pcl_ros::Publisher pub_; + std::shared_ptr> pub_; + rclcpp::TimerBase::SharedPtr timer_; //////////////////////////////////////////////////////////////////////////////// - PCDGenerator() - : tf_frame_("/base_link"), private_nh_("~") + explicit PCDPublisher(const rclcpp::NodeOptions & options) + : rclcpp::Node("pcd_publisher", options), tf_frame_("/base_link") { // Maximum number of outgoing messages to be queued for delivery to subscribers = 1 cloud_topic_ = "cloud_pcd"; - pub_.advertise(nh_, cloud_topic_.c_str(), 1); - private_nh_.param("frame_id", tf_frame_, std::string("/base_link")); - ROS_INFO( - "Publishing data on topic %s with frame_id %s.", nh_.resolveName( - cloud_topic_).c_str(), tf_frame_.c_str()); - } + tf_frame_ = this->declare_parameter("tf_frame", tf_frame_); + period_ms_ = this->declare_parameter("publishing_period_ms", 3000); + file_name_ = this->declare_parameter("file_name"); - //////////////////////////////////////////////////////////////////////////////// - // Start - int - start() - { if (file_name_ == "" || pcl::io::loadPCDFile(file_name_, cloud_) == -1) { - return -1; + RCLCPP_ERROR(this->get_logger(), "failed to open PCD file"); + throw std::runtime_error{"could not open pcd file"}; } cloud_.header.frame_id = tf_frame_; - return 0; + int nr_points = cloud_.width * cloud_.height; + + auto fields_list = pcl::getFieldsList(cloud_); + auto resolved_cloud_topic = + this->get_node_topics_interface()->resolve_topic_name(cloud_topic_); + + pub_ = this->create_publisher(cloud_topic_, 10); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(period_ms_), + [this]() { + this->publish(); + }); + + RCLCPP_INFO( + this->get_logger(), + "Publishing data with %d points (%s) on topic %s in frame %s.", + nr_points, + fields_list.c_str(), + resolved_cloud_topic.c_str(), + cloud_.header.frame_id.c_str()); } //////////////////////////////////////////////////////////////////////////////// - // Spin (!) - bool spin() + // Publish callback that is periodically called by a timer. + void publish() { - int nr_points = cloud_.width * cloud_.height; - std::string fields_list = pcl::getFieldsList(cloud_); - double interval = wait_ * 1e+6; - while (nh_.ok()) { - ROS_DEBUG_ONCE( - "Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, - fields_list.c_str(), nh_.resolveName(cloud_topic_).c_str(), cloud_.header.frame_id.c_str()); - cloud_.header.stamp = ros::Time::now(); - - if (pub_.getNumSubscribers() > 0) { - ROS_DEBUG("Publishing data to %d subscribers.", pub_.getNumSubscribers()); - pub_.publish(cloud_); - } else { - // check once a second if there is any subscriber - ros::Duration(1).sleep(); - continue; - } - - std::this_thread::sleep_for(std::chrono::microseconds(static_cast(interval))); - - if (interval == 0) { // We only publish once if a 0 seconds interval is given - // Give subscribers 3 seconds until point cloud decays... a little ugly! - ros::Duration(3.0).sleep(); - break; - } - } - return true; + cloud_.header.stamp = this->get_clock()->now(); + pub_->publish(cloud_); } }; +} // namespace pcl_ros -/* ---[ */ -int -main(int argc, char ** argv) -{ - if (argc < 2) { - std::cerr << "Syntax is: " << argv[0] << " [publishing_interval (in seconds)]" << - std::endl; - return -1; - } - - ros::init(argc, argv, "pcd_to_pointcloud"); - - PCDGenerator c; - c.file_name_ = std::string(argv[1]); - // check if publishing interval is given - if (argc == 2) { - c.wait_ = 0; - } else { - c.wait_ = atof(argv[2]); - } - - if (c.start() == -1) { - ROS_ERROR("Could not load file %s. Exiting.", argv[1]); - return -1; - } - ROS_INFO( - "Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.", - c.cloud_.width * c.cloud_.height, c.cloud_.data.size(), pcl::getFieldsList(c.cloud_).c_str()); - c.spin(); - - return 0; -} -/* ]--- */ +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::PCDPublisher) From bd7a060e98d2bb6720b3bc30794fd25c5ef43241 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 14 Apr 2022 05:17:03 +0900 Subject: [PATCH 387/405] use common header instead of io (#361) Signed-off-by: wep21 --- pcl_ros/src/pcl_ros/features/feature.cpp | 2 +- pcl_ros/src/pcl_ros/filters/features/feature.cpp | 2 +- pcl_ros/src/pcl_ros/filters/filter.cpp | 2 +- pcl_ros/src/pcl_ros/filters/project_inliers.cpp | 2 +- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 2 +- pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 2 +- pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp | 2 +- .../src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp | 2 +- pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp | 2 +- pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp | 2 +- pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp | 2 +- pcl_ros/tools/bag_to_pcd.cpp | 2 +- pcl_ros/tools/convert_pcd_to_image.cpp | 2 +- pcl_ros/tools/pcd_to_pointcloud.cpp | 2 +- pcl_ros/tools/pointcloud_to_pcd.cpp | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp index 9d8adf44..b0f0fc83 100644 --- a/pcl_ros/src/pcl_ros/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -47,7 +47,7 @@ // #include "pfh.cpp" // #include "principal_curvatures.cpp" // #include "vfh.cpp" -#include +#include #include #include #include "pcl_ros/features/feature.hpp" diff --git a/pcl_ros/src/pcl_ros/filters/features/feature.cpp b/pcl_ros/src/pcl_ros/filters/features/feature.cpp index 8b325dca..c8d1eabb 100644 --- a/pcl_ros/src/pcl_ros/filters/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/feature.cpp @@ -47,7 +47,7 @@ // #include "pfh.cpp" // #include "principal_curvatures.cpp" // #include "vfh.cpp" -#include +#include #include #include #include "pcl_ros/features/feature.hpp" diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 7d011c02..ad5fb75d 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -36,7 +36,7 @@ */ #include "pcl_ros/filters/filter.hpp" -#include +#include #include #include "pcl_ros/transforms.hpp" diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index 23a3111d..4fc3f70e 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -37,7 +37,7 @@ #include "pcl_ros/filters/project_inliers.hpp" #include -#include +#include #include ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index cde8113a..fe03b09b 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -36,7 +36,7 @@ */ #include -#include +#include #include #include #include "pcl_ros/transforms.hpp" diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index 1ab31693..8d933e25 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -38,7 +38,7 @@ /** \author Radu Bogdan Rusu */ #include -#include +#include #include #include #include "pcl_ros/io/concatenate_fields.hpp" diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index ead3a56a..da5e0d7a 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -36,7 +36,7 @@ */ #include -#include +#include #include #include #include diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 96272d49..bfd9fe55 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -36,7 +36,7 @@ */ #include -#include +#include #include #include #include "pcl_ros/transforms.hpp" diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index b7fcd539..bcc128c4 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -36,7 +36,7 @@ */ #include -#include +#include #include #include #include "pcl_ros/segmentation/sac_segmentation.hpp" diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index b2a20b26..0ee82452 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -36,7 +36,7 @@ */ #include -#include +#include #include "pcl_ros/segmentation/segment_differences.hpp" /////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index c05cbbb6..8701935c 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -36,7 +36,7 @@ */ #include -#include +#include #include #include "pcl_ros/surface/moving_least_squares.hpp" diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index 696ddb1b..c895b801 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -46,7 +46,7 @@ Cloud Data) format. #include #include -#include +#include #include #include #include diff --git a/pcl_ros/tools/convert_pcd_to_image.cpp b/pcl_ros/tools/convert_pcd_to_image.cpp index d0d9b17e..586c3b1a 100644 --- a/pcl_ros/tools/convert_pcd_to_image.cpp +++ b/pcl_ros/tools/convert_pcd_to_image.cpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index 5c51a774..f8368f3d 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -49,7 +49,7 @@ #include // PCL -#include +#include #include #include diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index 6c9fbb30..9f2f9c9a 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -45,7 +45,7 @@ #include // PCL includes -#include +#include #include #include From 3eecd4e37b9a4f1ca73a7cd09c0219332e7a18cb Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Thu, 7 Jul 2022 21:22:26 +0200 Subject: [PATCH 388/405] Add boost include, missing in upcoming PCL versions (ros2) (#374) * Remove name of nonexistent PCL component (core) Probably, common is meant, which is also requested * Add boost include, missing in upcoming PCL versions Was removed in pcl/conversion.h here: https://github.com/PointCloudLibrary/pcl/commit/292593abd3b69af315c7fe3379363bdce7800d5a --- pcl_ros/CMakeLists.txt | 2 +- pcl_ros/include/pcl_ros/point_cloud.hpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 258f2366..29efc189 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -10,7 +10,7 @@ endif() ## 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) diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index 2b7ec855..2b8fd6a4 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -57,6 +57,7 @@ #endif #include #include +#include // for BOOST_FOREACH #include #include #include From 389297dfadb92df6b1efb53958723c40c2a4556d Mon Sep 17 00:00:00 2001 From: ralwing <58466562+ralwing@users.noreply.github.com> Date: Thu, 3 Nov 2022 13:03:31 +0100 Subject: [PATCH 389/405] Fixing implicit conversion warnings #378 (#379) --- .../include/pcl_conversions/pcl_conversions.h | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/pcl_conversions/include/pcl_conversions/pcl_conversions.h b/pcl_conversions/include/pcl_conversions/pcl_conversions.h index 9970ba7b..257139d7 100644 --- a/pcl_conversions/include/pcl_conversions/pcl_conversions.h +++ b/pcl_conversions/include/pcl_conversions/pcl_conversions.h @@ -37,6 +37,7 @@ #ifndef PCL_CONVERSIONS_H__ #define PCL_CONVERSIONS_H__ +#include #include #include @@ -81,13 +82,14 @@ namespace pcl_conversions { inline void fromPCL(const std::uint64_t &pcl_stamp, rclcpp::Time &stamp) { - stamp = rclcpp::Time(pcl_stamp * 1000ull); // Convert from us to ns + stamp = rclcpp::Time( + static_cast(pcl_stamp * 1000ull)); // Convert from us to ns } inline void toPCL(const rclcpp::Time &stamp, std::uint64_t &pcl_stamp) { - pcl_stamp = stamp.nanoseconds() / 1000ull; // Convert from ns to us + pcl_stamp = static_cast(stamp.nanoseconds()) / 1000ull; // Convert from ns to us } inline @@ -211,7 +213,7 @@ namespace pcl_conversions { { pfs.resize(pcl_pfs.size()); std::vector::const_iterator it = pcl_pfs.begin(); - int i = 0; + size_t i = 0; for(; it != pcl_pfs.end(); ++it, ++i) { fromPCL(*(it), pfs[i]); } @@ -231,7 +233,7 @@ namespace pcl_conversions { { pcl_pfs.resize(pfs.size()); std::vector::const_iterator it = pfs.begin(); - int i = 0; + size_t i = 0; for(; it != pfs.end(); ++it, ++i) { toPCL(*(it), pcl_pfs[i]); } @@ -676,15 +678,16 @@ namespace pcl { { // Get the field sizes for the second cloud std::vector fields2; - std::vector fields2_sizes; + std::vector 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 * - pcl::getFieldSize (cloud2.fields[j].datatype)); - fields2.push_back (cloud2.fields[j]); + fields2_sizes.push_back( + cloud2.fields[j].count * + static_cast(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); @@ -692,7 +695,7 @@ namespace pcl { // Copy the second cloud for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp) { - int i = 0; + size_t i = 0; for (size_t j = 0; j < fields2.size (); ++j) { if (cloud1.fields[i].name == "_") @@ -737,7 +740,7 @@ namespace pcl { } // namespace pcl -/* TODO when ROS2 type masquareding is implemented */ +/* TODO when ROS2 type masquerading is implemented */ /** namespace ros { From 7c6a2a5a38709fe9aa31acdb443d7490f78f8cfe Mon Sep 17 00:00:00 2001 From: Aditya Ardiya Date: Sat, 19 Nov 2022 03:39:32 +0900 Subject: [PATCH 390/405] Avoid redundant boost::make_shared copy in pcl_ros::Publisher (#380) `boost make_shared` is actually making a copy. This PR introduces the `sensor_msgs::PointCloud2` as pointer directly so that it doesn't need a `boost::make_shared` convertsion to pointer anymore. --- pcl_ros/include/pcl_ros/publisher.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl_ros/include/pcl_ros/publisher.hpp b/pcl_ros/include/pcl_ros/publisher.hpp index 4de04e8f..9f92745f 100644 --- a/pcl_ros/include/pcl_ros/publisher.hpp +++ b/pcl_ros/include/pcl_ros/publisher.hpp @@ -114,9 +114,9 @@ public: publish(const pcl::PointCloud & point_cloud) const { // Fill point cloud binary data - sensor_msgs::PointCloud2 msg; - pcl::toROSMsg(point_cloud, msg); - pub_.publish(boost::make_shared(msg)); + sensor_msgs::PointCloud2::Ptr msg_ptr(new sensor_msgs::PointCloud2); + pcl::toROSMsg(point_cloud, *msg_ptr); + pub_.publish(msg_ptr); } }; From 0918531c177d0bcd1ee043eb9250f7d30c56b345 Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Sun, 20 Nov 2022 20:00:22 +0100 Subject: [PATCH 391/405] Add logic for missing pcl/point_traits.h in newer PCL versions (#382) point_traits.h is no longer available, starting with PCL 1.13.0 Since PCL 1.11.0, type_traits.h should be used instead --- pcl_ros/include/pcl_ros/point_cloud.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index 2b8fd6a4..b25697e1 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -44,11 +44,15 @@ #include #include +#include // for PCL_VERSION_COMPARE +#if PCL_VERSION_COMPARE(>=, 1, 11, 0) +#include +#else #include +#endif // PCL_VERSION_COMPARE(>=, 1, 11, 0) #include #include #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED -#include #if PCL_VERSION_COMPARE(>=, 1, 11, 0) #include #elif PCL_VERSION_COMPARE(>=, 1, 10, 0) From ca69652eb8cec020dc6eb43c3c16d3bc8224ff3f Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Tue, 29 Nov 2022 11:27:59 -0700 Subject: [PATCH 392/405] use modular pcl dependencies for bloom (#384) --- pcl_conversions/package.xml | 6 +++++- pcl_ros/package.xml | 11 ++++++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/pcl_conversions/package.xml b/pcl_conversions/package.xml index 06a2ebd9..a3662bac 100644 --- a/pcl_conversions/package.xml +++ b/pcl_conversions/package.xml @@ -21,14 +21,18 @@ ament_cmake + libpcl-all-dev + eigen - libpcl-all-dev message_filters pcl_msgs rclcpp sensor_msgs std_msgs + libpcl-common + libpcl-io + ament_cmake_gtest diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 81b1c6cb..566b1b6d 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -26,8 +26,10 @@ William Woodall ament_cmake + + libpcl-all-dev + eigen - libpcl-all-dev pcl_conversions rclcpp rclcpp_components @@ -37,6 +39,13 @@ tf2_geometry_msgs tf2_ros + libpcl-common + libpcl-features + libpcl-filters + libpcl-io + libpcl-segmentation + libpcl-surface + ament_lint_auto ament_lint_common ament_cmake_gtest From 5c5382eb5c85af9e00076395e84520a209b81673 Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Fri, 9 Dec 2022 14:12:01 -0500 Subject: [PATCH 393/405] migrate pcl_nodelet.hpp to pcl_node.hpp (#385) * migrate pcl_nodelet.hpp to pcl_node.hpp TODOs - lazy subscription - cpplint, uncrustify - type adaptation Signed-off-by: Daisuke Sato * rename latched_indices to transient_local_indices Signed-off-by: Daisuke Sato * - remove some TODOs - change pub_output_ type Signed-off-by: Daisuke Sato * remove TODOs Signed-off-by: Daisuke Sato * use template instead of PublisherBase Signed-off-by: Daisuke Sato Signed-off-by: Daisuke Sato --- .../pcl_ros/{pcl_nodelet.hpp => pcl_node.hpp} | 203 +++++++++++------- 1 file changed, 120 insertions(+), 83 deletions(-) rename pcl_ros/include/pcl_ros/{pcl_nodelet.hpp => pcl_node.hpp} (53%) diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp b/pcl_ros/include/pcl_ros/pcl_node.hpp similarity index 53% rename from pcl_ros/include/pcl_ros/pcl_nodelet.hpp rename to pcl_ros/include/pcl_ros/pcl_node.hpp index 4806338f..d80cb6f4 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp +++ b/pcl_ros/include/pcl_ros/pcl_node.hpp @@ -31,7 +31,7 @@ * 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 $ + * $Id: pcl_node.h 33238 2010-03-11 00:46:58Z rusu $ * */ @@ -41,30 +41,35 @@ **/ -#ifndef PCL_ROS__PCL_NODELET_HPP_ -#define PCL_ROS__PCL_NODELET_HPP_ +#ifndef PCL_ROS__PCL_NODE_HPP_ +#define PCL_ROS__PCL_NODE_HPP_ -#include +#include // PCL includes -#include -#include +#include +#include #include #include #include // ROS Nodelet includes -#include +// #include // TODO(daisukes): lazy subscription #include #include #include #include // Include TF -#include +#include +#include // STL +#include #include +#include +// ROS2 includes +#include -#include "pcl_ros/point_cloud.hpp" +// #include "pcl_ros/point_cloud.hpp" using pcl_conversions::fromPCL; @@ -73,33 +78,85 @@ 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 +/** \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::PointCloud2 PointCloud2; + typedef sensor_msgs::msg::PointCloud2 PointCloud2; typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; - typedef pcl_msgs::PointIndices PointIndices; - typedef PointIndices::Ptr PointIndicesPtr; - typedef PointIndices::ConstPtr PointIndicesConstPtr; + typedef pcl_msgs::msg::PointIndices PointIndices; + typedef PointIndices::SharedPtr PointIndicesPtr; + typedef PointIndices::ConstSharedPtr PointIndicesConstPtr; - typedef pcl_msgs::ModelCoefficients ModelCoefficients; - typedef ModelCoefficients::Ptr ModelCoefficientsPtr; - typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; + 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. */ - PCLNodelet() - : use_indices_(false), latched_indices_(false), - max_queue_size_(3), approximate_sync_(false) {} + 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 approximatly 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. @@ -110,26 +167,28 @@ protected: * 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. + * specified jitter. See also @ref transient_local_indices_ and approximate_sync. **/ bool use_indices_; - /** \brief Set to true if the indices topic is latched. + /** \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 latched_indices_; + bool transient_local_indices_; /** \brief The message filter subscriber for PointCloud2. */ - message_filters::Subscriber sub_input_filter_; + 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 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_; @@ -140,23 +199,24 @@ protected: bool approximate_sync_; /** \brief TF listener object. */ - tf::TransformListener tf_listener_; - + tf2_ros::TransformListener tf_listener_; + tf2_ros::Buffer tf_buffer_; + /** \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") + isValid(const PointCloud2::ConstSharedPtr & 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(), + 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.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - topic_name).c_str()); + cloud->header.stamp.sec, cloud->header.stamp.nanosec, + cloud->header.frame_id.c_str(), topic_name.c_str()); return false; } @@ -171,12 +231,13 @@ protected: 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()); + 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; } @@ -192,11 +253,13 @@ protected: { /*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); + 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; } @@ -210,10 +273,11 @@ protected: { /*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 ()); + 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; @@ -225,36 +289,9 @@ protected: 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_ +#endif // PCL_ROS__PCL_NODE_HPP_ From 387f5b158ba7772d96f883718eb22d311eaa201b Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Mon, 19 Dec 2022 14:13:06 -0500 Subject: [PATCH 394/405] migrate abstract filter node (#388) * migrate abstract filter node Signed-off-by: Daisuke Sato * remove use_frame_params from constructor and make a function for the logic Signed-off-by: Daisuke Sato Signed-off-by: Daisuke Sato --- pcl_ros/CMakeLists.txt | 14 +- pcl_ros/include/pcl_ros/filters/filter.hpp | 63 +++---- pcl_ros/src/pcl_ros/filters/filter.cpp | 202 +++++++++++---------- 3 files changed, 143 insertions(+), 136 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 29efc189..5952e859 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -80,19 +80,19 @@ ament_target_dependencies(pcl_ros_tf # # ### Declare the pcl_ros_filters library -#add_library(pcl_ros_filters +add_library(pcl_ros_filters SHARED # src/pcl_ros/filters/extract_indices.cpp -# src/pcl_ros/filters/filter.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) +) +target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES}) +ament_target_dependencies(pcl_ros_filters ${dependencies}) +class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library #add_library (pcl_ros_segmentation @@ -184,7 +184,7 @@ install( pcd_to_pointcloud_lib # pcl_ros_io # pcl_ros_features -# pcl_ros_filters + pcl_ros_filters # pcl_ros_surface # pcl_ros_segmentation # pointcloud_to_pcd diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index e589c0cb..a0cd0d20 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -39,10 +39,10 @@ #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 +52,27 @@ 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 The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + rclcpp::Subscription::SharedPtr sub_input_; message_filters::Subscriber sub_input_filter_; @@ -96,18 +104,7 @@ protected: 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 +113,7 @@ protected: */ 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 +124,34 @@ protected: virtual void unsubscribe(); - /** \brief Nodelet initialization routine. */ - virtual void - onInit(); - /** \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/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index ad5fb75d..5e60ed84 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -37,7 +37,6 @@ #include "pcl_ros/filters/filter.hpp" #include -#include #include "pcl_ros/transforms.hpp" /*//#include @@ -62,42 +61,42 @@ /////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices) +pcl_ros::Filter::computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices) { PointCloud2 output; // Call the virtual method in the child filter(input, indices, output); - PointCloud2::Ptr cloud_tf(new PointCloud2(output)); // set the output by default + PointCloud2::UniquePtr cloud_tf(new PointCloud2(output)); // set the output by default // Check whether the user has given a different output TF frame if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) { - NODELET_DEBUG( - "[%s::computePublish] Transforming output dataset from %s to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); + RCLCPP_DEBUG( + this->get_logger(), "Transforming output dataset from %s to %s.", + output.header.frame_id.c_str(), tf_output_frame_.c_str()); // Convert the cloud into the different frame PointCloud2 cloud_transformed; - if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_listener_)) { - NODELET_ERROR( - "[%s::computePublish] Error converting output dataset from %s to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); + if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), "Error converting output dataset from %s to %s.", + output.header.frame_id.c_str(), tf_output_frame_.c_str()); return; } cloud_tf.reset(new PointCloud2(cloud_transformed)); } if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) { // no tf_output_frame given, transform the dataset to its original frame - NODELET_DEBUG( - "[%s::computePublish] Transforming output dataset from %s back to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); + RCLCPP_DEBUG( + this->get_logger(), "Transforming output dataset from %s back to %s.", + output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); // Convert the cloud into the different frame PointCloud2 cloud_transformed; if (!pcl_ros::transformPointCloud( tf_input_orig_frame_, output, cloud_transformed, - tf_listener_)) + tf_buffer_)) { - NODELET_ERROR( - "[%s::computePublish] Error converting output dataset from %s back to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); + RCLCPP_ERROR( + this->get_logger(), "Error converting output dataset from %s back to %s.", + output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); return; } cloud_tf.reset(new PointCloud2(cloud_transformed)); @@ -106,8 +105,8 @@ pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const Indic // Copy timestamp to keep it cloud_tf->header.stamp = input->header.stamp; - // Publish a boost shared ptr - pub_output_.publish(cloud_tf); + // Publish the unique ptr + pub_output_->publish(move(cloud_tf)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -117,28 +116,32 @@ pcl_ros::Filter::subscribe() // If we're supposed to look for PointIndices (indices) if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + sub_input_filter_.subscribe(this, "input", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile()); + sub_indices_filter_.subscribe(this, "indices", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile()); if (approximate_sync_) { sync_input_indices_a_ = - boost::make_shared>>(max_queue_size_); + std::make_shared>>(max_queue_size_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); + sync_input_indices_a_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2)); } else { sync_input_indices_e_ = - boost::make_shared>>(max_queue_size_); + std::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); + sync_input_indices_e_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2)); } } else { + // Workaround for a callback with custom arguments ros2/rclcpp#766 + std::function callback = + std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, nullptr); + // Subscribe in an old fashion to input only (no filters) sub_input_ = - pnh_->subscribe( + this->create_subscription( "input", max_queue_size_, - bind(&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr())); + callback); } } @@ -150,114 +153,123 @@ pcl_ros::Filter::unsubscribe() sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); } else { - sub_input_.shutdown(); + sub_input_.reset(); } } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::onInit() +pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & options) +: PCLNode(node_name, options) { - // Call the super onInit () - PCLNodelet::onInit(); - - // Call the child's local init - bool has_service = false; - if (!child_init(*pnh_, has_service)) { - NODELET_ERROR("[%s::onInit] Initialization failed.", getName().c_str()); - return; - } - - pub_output_ = advertise(*pnh_, "output", max_queue_size_); - - // Enable the dynamic reconfigure service - if (!has_service) { - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &Filter::config_callback, this, _1, _2); - srv_->setCallback(f); - } - - NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str()); + pub_output_ = create_publisher("output", max_queue_size_); + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); + RCLCPP_DEBUG(this->get_logger(), "Node successfully created."); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level) +pcl_ros::Filter::use_frame_params() { - // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are - // inexistent in PCL - if (tf_input_frame_ != config.input_frame) { - tf_input_frame_ = config.input_frame; - NODELET_DEBUG( - "[%s::config_callback] Setting the input TF frame to: %s.", - getName().c_str(), tf_input_frame_.c_str()); + rcl_interfaces::msg::ParameterDescriptor input_frame_desc; + input_frame_desc.name = "input_frame"; + input_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + input_frame_desc.description = "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different."; + declare_parameter(input_frame_desc.name, rclcpp::ParameterValue(""), input_frame_desc); + + rcl_interfaces::msg::ParameterDescriptor output_frame_desc; + output_frame_desc.name = "output_frame"; + output_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + output_frame_desc.description = "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different."; + declare_parameter(output_frame_desc.name, rclcpp::ParameterValue(""), output_frame_desc); + + // Validate initial values using same callback + callback_handle_ = + add_on_set_parameters_callback(std::bind(&Filter::config_callback, this, std::placeholders::_1)); + + std::vector param_names{input_frame_desc.name, output_frame_desc.name}; + auto result = config_callback(get_parameters(param_names)); + if (!result.successful) { + throw std::runtime_error(result.reason); } - if (tf_output_frame_ != config.output_frame) { - tf_output_frame_ = config.output_frame; - NODELET_DEBUG( - "[%s::config_callback] Setting the output TF frame to: %s.", - getName().c_str(), tf_output_frame_.c_str()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::Filter::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "input_frame") { + if (tf_input_frame_ != param.as_string()) { + tf_input_frame_ = param.as_string(); + RCLCPP_DEBUG(get_logger(), "Setting the input frame to: %s.", tf_input_frame_.c_str()); + } + } + if (param.get_name() == "output_frame") { + if (tf_output_frame_ != param.as_string()) { + tf_output_frame_ = param.as_string(); + RCLCPP_DEBUG(get_logger(), "Setting the output frame to: %s.", tf_output_frame_.c_str()); + } + } } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::input_indices_callback( - const PointCloud2::ConstPtr & cloud, - const PointIndicesConstPtr & indices) + const PointCloud2::ConstSharedPtr & cloud, + const PointIndices::ConstSharedPtr & indices) { // If cloud is given, check if it's valid if (!isValid(cloud)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); + RCLCPP_ERROR(this->get_logger(), "Invalid input!"); return; } // If indices are given, check if they are valid if (indices && !isValid(indices)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); + RCLCPP_ERROR(this->get_logger(), "Invalid indices!"); return; } /// DEBUG if (indices) { - NODELET_DEBUG( - "[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), + RCLCPP_DEBUG( + this->get_logger(), "[input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %d.%09d, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %d.%09d, and frame %s on topic %s received.", cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), - cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - indices->indices.size(), indices->header.stamp.toSec(), - indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + cloud->header.stamp.sec, cloud->header.stamp.nanosec, cloud->header.frame_id.c_str(), "input", + indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec, + indices->header.frame_id.c_str(), "indices"); } else { - NODELET_DEBUG( - "[%s::input_indices_callback] PointCloud with %d data points and frame %s on " - "topic %s received.", - getName().c_str(), cloud->width * cloud->height, - cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str()); + RCLCPP_DEBUG( + this->get_logger(), "PointCloud with %d data points and frame %s on topic %s received.", + cloud->width * cloud->height, cloud->header.frame_id.c_str(), "input"); } /// // Check whether the user has given a different input TF frame tf_input_orig_frame_ = cloud->header.frame_id; - PointCloud2::ConstPtr cloud_tf; + PointCloud2::ConstSharedPtr cloud_tf; if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { - NODELET_DEBUG( - "[%s::input_indices_callback] Transforming input dataset from %s to %s.", - getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + RCLCPP_DEBUG( + this->get_logger(), "Transforming input dataset from %s to %s.", + cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); // Save the original frame ID // Convert the cloud into the different frame PointCloud2 cloud_transformed; - if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) { - NODELET_ERROR( - "[%s::input_indices_callback] Error converting input dataset from %s to %s.", - getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), "Error converting input dataset from %s to %s.", + cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); return; } - cloud_tf = boost::make_shared(cloud_transformed); + cloud_tf = std::make_shared(cloud_transformed); } else { cloud_tf = cloud; } From 3b0cfd8529a59fc135418c2a27e40acb926ad06a Mon Sep 17 00:00:00 2001 From: Marcel Zeilinger Date: Mon, 9 Jan 2023 19:17:28 +0100 Subject: [PATCH 395/405] Export PCL dependency in pcl_conversions (#392) Co-authored-by: Marcel Zeilinger --- pcl_conversions/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_conversions/CMakeLists.txt b/pcl_conversions/CMakeLists.txt index 38a2e21d..024b7948 100644 --- a/pcl_conversions/CMakeLists.txt +++ b/pcl_conversions/CMakeLists.txt @@ -51,5 +51,5 @@ endif() # Export old-style CMake variables ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_dependencies(${dependencies}) +ament_export_dependencies(${dependencies} PCL) ament_package() From 0c8e7dafce90ff7621267b38f7be543ab3218a19 Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Fri, 27 Jan 2023 18:50:45 -0500 Subject: [PATCH 396/405] Migrate extract_indices filter (#394) * - migrate extract_indices filter - add test to check if filter node/component output result Signed-off-by: Daisuke Sato * add test_depend to package.xml Signed-off-by: Daisuke Sato * add launch_testing_ros as test_depend too Signed-off-by: Daisuke Sato * fixed test not to depend on ros2cli Signed-off-by: Daisuke Sato --------- Signed-off-by: Daisuke Sato --- pcl_ros/CMakeLists.txt | 8 +- .../pcl_ros/filters/extract_indices.hpp | 26 ++--- pcl_ros/package.xml | 6 + .../src/pcl_ros/filters/extract_indices.cpp | 66 ++++++----- pcl_ros/tests/filters/CMakeLists.txt | 35 ++++++ pcl_ros/tests/filters/dummy_topics.cpp | 109 ++++++++++++++++++ .../tests/filters/test_filter_component.py | 90 +++++++++++++++ .../tests/filters/test_filter_executable.py | 60 ++++++++++ 8 files changed, 358 insertions(+), 42 deletions(-) create mode 100644 pcl_ros/tests/filters/CMakeLists.txt create mode 100644 pcl_ros/tests/filters/dummy_topics.cpp create mode 100644 pcl_ros/tests/filters/test_filter_component.py create mode 100644 pcl_ros/tests/filters/test_filter_executable.py diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 5952e859..5e79bf6b 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(tf2_ros REQUIRED) set(dependencies pcl_conversions rclcpp + rclcpp_components sensor_msgs geometry_msgs tf2 @@ -81,7 +82,7 @@ ament_target_dependencies(pcl_ros_tf # ### Declare the pcl_ros_filters library add_library(pcl_ros_filters SHARED -# src/pcl_ros/filters/extract_indices.cpp + 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 @@ -92,6 +93,10 @@ add_library(pcl_ros_filters SHARED ) target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES}) ament_target_dependencies(pcl_ros_filters ${dependencies}) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::ExtractIndices" + EXECUTABLE filter_extract_indices_node +) class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library @@ -163,6 +168,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) diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp index 95740f60..bb752f2d 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -40,9 +40,9 @@ // PCL includes #include - +#include +#include #include "pcl_ros/filters/filter.hpp" -#include "pcl_ros/ExtractIndicesConfig.hpp" namespace pcl_ros { @@ -53,9 +53,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,10 +60,10 @@ protected: */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, PointCloud2 & output) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*(input), *(pcl_input)); impl_.setInputCloud(pcl_input); @@ -76,16 +73,13 @@ protected: pcl_conversions::moveFromPCL(pcl_output, output); } - /** \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 +87,8 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit ExtractIndices(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 566b1b6d..12e1def6 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -49,6 +49,12 @@ ament_lint_auto ament_lint_common ament_cmake_gtest + ament_cmake_pytest + launch + launch_ros + launch_testing + launch_testing_ros + sensor_msgs