diff --git a/ros2_ws/src/perception_pcl/.gitignore b/ros2_ws/src/perception_pcl/.gitignore new file mode 100644 index 00000000..0d20b648 --- /dev/null +++ b/ros2_ws/src/perception_pcl/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/ros2_ws/src/perception_pcl/.travis.yml b/ros2_ws/src/perception_pcl/.travis.yml new file mode 100644 index 00000000..6558554a --- /dev/null +++ b/ros2_ws/src/perception_pcl/.travis.yml @@ -0,0 +1,27 @@ +language: generic + +services: + - docker + +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: + - 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 +script: + - .industrial_ci/travis.sh + +branches: + only: + - /.*-devel$/ diff --git a/ros2_ws/src/perception_pcl/README.md b/ros2_ws/src/perception_pcl/README.md new file mode 100644 index 00000000..8db6bf6d --- /dev/null +++ b/ros2_ws/src/perception_pcl/README.md @@ -0,0 +1,7 @@ +# 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 +processing in ROS. diff --git a/ros2_ws/src/perception_pcl/pcl_conversions/CHANGELOG.rst b/ros2_ws/src/perception_pcl/pcl_conversions/CHANGELOG.rst new file mode 100644 index 00000000..945c9860 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_conversions/CHANGELOG.rst @@ -0,0 +1,68 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pcl_conversions +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.6.2 (2018-05-20) +------------------ + +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 +* 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) +------------------ + +* 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 + 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 +* 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) + +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 +* Fixup tests and pcl usage in CMakeList.txt + +0.1.2 (2013-07-12) +------------------ +* small fix for conversion functions + +0.1.1 (2013-07-10) +------------------ +* Fix find_package bug with pcl + +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 diff --git a/ros2_ws/src/perception_pcl/pcl_conversions/CMakeLists.txt b/ros2_ws/src/perception_pcl/pcl_conversions/CMakeLists.txt new file mode 100644 index 00000000..024b7948 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_conversions/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5) +project(pcl_conversions) + +# Default to C++14 +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_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/ + DESTINATION include/${PROJECT_NAME} +) + +# Add gtest based cpp test target +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp) + ament_target_dependencies(${PROJECT_NAME}-test + ${dependencies} + ) + target_link_libraries(${PROJECT_NAME}-test ${Boost_LIBRARIES} ${PCL_LIBRARIES}) +endif() + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") + +ament_export_dependencies(${dependencies} PCL) +ament_package() diff --git a/ros2_ws/src/perception_pcl/pcl_conversions/README.rst b/ros2_ws/src/perception_pcl/pcl_conversions/README.rst new file mode 100644 index 00000000..2478c439 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_conversions/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/perception_pcl/issues | ++-----------------+------------------------------------------------------------+ +.. | Documentation | http://ros-perception.github.com/pcl_conversions/doc | +.. +-----------------+------------------------------------------------------------+ + diff --git a/ros2_ws/src/perception_pcl/pcl_conversions/include/pcl_conversions/pcl_conversions.h b/ros2_ws/src/perception_pcl/pcl_conversions/include/pcl_conversions/pcl_conversions.h new file mode 100644 index 00000000..b27bc25e --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_conversions/include/pcl_conversions/pcl_conversions.h @@ -0,0 +1,942 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation, Inc. + * Copyright (c) 2010-2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation, Inc. nor + * the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_CONVERSIONS_H__ +#define PCL_CONVERSIONS_H__ + +#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 + +namespace pcl_conversions { + + /** PCLHeader <=> Header **/ + + inline + void fromPCL(const std::uint64_t &pcl_stamp, rclcpp::Time &stamp) + { + 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 = static_cast(stamp.nanoseconds()) / 1000ull; // Convert from ns to us + } + + inline + rclcpp::Time fromPCL(const std::uint64_t &pcl_stamp) + { + rclcpp::Time stamp; + fromPCL(pcl_stamp, stamp); + return stamp; + } + + inline + std::uint64_t toPCL(const rclcpp::Time &stamp) + { + std::uint64_t pcl_stamp; + toPCL(stamp, pcl_stamp); + return pcl_stamp; + } + + /** PCLHeader <=> Header **/ + + inline + void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::msg::Header &header) + { + header.stamp = fromPCL(pcl_header.stamp); + header.frame_id = pcl_header.frame_id; + } + + inline + void toPCL(const std_msgs::msg::Header &header, pcl::PCLHeader &pcl_header) + { + toPCL(header.stamp, pcl_header.stamp); + // 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::msg::Header fromPCL(const pcl::PCLHeader &pcl_header) + { + std_msgs::msg::Header header; + fromPCL(pcl_header, header); + return header; + } + + inline + pcl::PCLHeader toPCL(const std_msgs::msg::Header &header) + { + pcl::PCLHeader pcl_header; + toPCL(header, pcl_header); + return pcl_header; + } + + /** PCLImage <=> Image **/ + + inline + void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image) + { + fromPCL(pcl_image.header, image.header); + image.height = pcl_image.height; + image.width = pcl_image.width; + image.encoding = pcl_image.encoding; + image.is_bigendian = pcl_image.is_bigendian; + image.step = pcl_image.step; + } + + inline + void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image) + { + copyPCLImageMetaData(pcl_image, image); + image.data = pcl_image.data; + } + + inline + void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image) + { + copyPCLImageMetaData(pcl_image, image); + image.data.swap(pcl_image.data); + } + + inline + void copyImageMetaData(const sensor_msgs::msg::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::msg::Image &image, pcl::PCLImage &pcl_image) + { + copyImageMetaData(image, pcl_image); + pcl_image.data = image.data; + } + + inline + void moveToPCL(sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image) + { + copyImageMetaData(image, pcl_image); + pcl_image.data.swap(image.data); + } + + /** PCLPointField <=> PointField **/ + + inline + void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::msg::PointField &pf) + { + pf.name = pcl_pf.name; + pf.offset = pcl_pf.offset; + pf.datatype = pcl_pf.datatype; + pf.count = pcl_pf.count; + } + + inline + void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) + { + pfs.resize(pcl_pfs.size()); + std::vector::const_iterator it = pcl_pfs.begin(); + size_t i = 0; + for(; it != pcl_pfs.end(); ++it, ++i) { + fromPCL(*(it), pfs[i]); + } + } + + inline + void toPCL(const sensor_msgs::msg::PointField &pf, pcl::PCLPointField &pcl_pf) + { + pcl_pf.name = pf.name; + pcl_pf.offset = pf.offset; + pcl_pf.datatype = pf.datatype; + pcl_pf.count = pf.count; + } + + inline + void toPCL(const std::vector &pfs, std::vector &pcl_pfs) + { + pcl_pfs.resize(pfs.size()); + std::vector::const_iterator it = pfs.begin(); + size_t i = 0; + for(; it != pfs.end(); ++it, ++i) { + toPCL(*(it), pcl_pfs[i]); + } + } + + /** PCLPointCloud2 <=> PointCloud2 **/ + + inline + void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2) + { + fromPCL(pcl_pc2.header, pc2.header); + pc2.height = pcl_pc2.height; + pc2.width = pcl_pc2.width; + fromPCL(pcl_pc2.fields, pc2.fields); + pc2.is_bigendian = pcl_pc2.is_bigendian; + pc2.point_step = pcl_pc2.point_step; + pc2.row_step = pcl_pc2.row_step; + pc2.is_dense = pcl_pc2.is_dense; + } + + inline + void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2) + { + copyPCLPointCloud2MetaData(pcl_pc2, pc2); + pc2.data = pcl_pc2.data; + } + + inline + void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2) + { + copyPCLPointCloud2MetaData(pcl_pc2, pc2); + pc2.data.swap(pcl_pc2.data); + } + + inline + void copyPointCloud2MetaData(const sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { + toPCL(pc2.header, pcl_pc2.header); + pcl_pc2.height = pc2.height; + pcl_pc2.width = pc2.width; + toPCL(pc2.fields, pcl_pc2.fields); + pcl_pc2.is_bigendian = pc2.is_bigendian; + pcl_pc2.point_step = pc2.point_step; + pcl_pc2.row_step = pc2.row_step; + pcl_pc2.is_dense = pc2.is_dense; + } + + inline + void toPCL(const sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { + copyPointCloud2MetaData(pc2, pcl_pc2); + pcl_pc2.data = pc2.data; + } + + inline + void moveToPCL(sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) + { + copyPointCloud2MetaData(pc2, pcl_pc2); + pcl_pc2.data.swap(pc2.data); + } + + /** pcl::PointIndices <=> pcl_msgs::PointIndices **/ + + inline + void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::msg::PointIndices &pi) + { + fromPCL(pcl_pi.header, pi.header); + pi.indices = pcl_pi.indices; + } + + inline + void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::msg::PointIndices &pi) + { + fromPCL(pcl_pi.header, pi.header); + pi.indices.swap(pcl_pi.indices); + } + + inline + void toPCL(const pcl_msgs::msg::PointIndices &pi, pcl::PointIndices &pcl_pi) + { + toPCL(pi.header, pcl_pi.header); + pcl_pi.indices = pi.indices; + } + + inline + void moveToPCL(pcl_msgs::msg::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::msg::ModelCoefficients &mc) + { + fromPCL(pcl_mc.header, mc.header); + mc.values = pcl_mc.values; + } + + inline + void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::msg::ModelCoefficients &mc) + { + fromPCL(pcl_mc.header, mc.header); + mc.values.swap(pcl_mc.values); + } + + inline + void toPCL(const pcl_msgs::msg::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc) + { + toPCL(mc.header, pcl_mc.header); + pcl_mc.values = mc.values; + } + + inline + void moveToPCL(pcl_msgs::msg::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc) + { + toPCL(mc.header, pcl_mc.header); + pcl_mc.values.swap(mc.values); + } + + /** 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.assign(pcl_vert.vertices.cbegin(), pcl_vert.vertices.cend()); + } + + 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::msg::Vertices &vert) + { + internal::move(pcl_vert.vertices, 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::msg::Vertices &vert, pcl::Vertices &pcl_vert) + { + pcl_vert.vertices.assign(vert.vertices.cbegin(), vert.vertices.cend()); + } + + inline + void toPCL(const std::vector &verts, std::vector &pcl_verts) + { + 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::msg::Vertices &vert, pcl::Vertices &pcl_vert) + { + internal::move(vert.vertices, pcl_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::msg::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::msg::PolygonMesh &mesh) + { + fromPCL(pcl_mesh.header, mesh.header); + moveFromPCL(pcl_mesh.cloud, mesh.cloud); + } + + inline + void toPCL(const pcl_msgs::msg::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh) + { + toPCL(mesh.header, pcl_mesh.header); + toPCL(mesh.cloud, pcl_mesh.cloud); + toPCL(mesh.polygons, pcl_mesh.polygons); + } + + inline + void moveToPCL(pcl_msgs::msg::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::msg::PointCloud2 &cloud, const std::string &field_name) + { + // Get the index we need + for (size_t d = 0; d < cloud.fields.size(); ++d) { + if (cloud.fields[d].name == field_name) { + return (static_cast(d)); + } + } + return (-1); + } + + /** Overload pcl::getFieldsList **/ + + inline std::string getFieldsList(const sensor_msgs::msg::PointCloud2 &cloud) + { + std::string result; + for (size_t i = 0; i < cloud.fields.size () - 1; ++i) { + result += cloud.fields[i].name + " "; + } + result += cloud.fields[cloud.fields.size () - 1].name; + return (result); + } + + /** Provide pcl::toROSMsg **/ + + inline + void toROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::Image &image) + { + pcl::PCLPointCloud2 pcl_cloud; + pcl_conversions::toPCL(cloud, pcl_cloud); + pcl::PCLImage pcl_image; + pcl::toPCLPointCloud2(pcl_cloud, pcl_image); + pcl_conversions::moveFromPCL(pcl_image, image); + } + + inline + void moveToROSMsg(sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::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); + } + + template void + 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) + { + 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 (std::uint8_t) * 3; + msg.data.resize (msg.step * msg.height); + for (size_t y = 0; y < cloud.height; y++) + { + for (size_t x = 0; x < cloud.width; x++) + { + std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); + memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(std::uint8_t)); + } + } + } + + /** Provide to/fromROSMsg for sensor_msgs::msg::PointCloud2 <=> pcl::PointCloud **/ + + template + void toROSMsg(const pcl::PointCloud &pcl_cloud, sensor_msgs::msg::PointCloud2 &cloud) + { + pcl::PCLPointCloud2 pcl_pc2; + pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2); + pcl_conversions::moveFromPCL(pcl_pc2, cloud); + } + + template + void fromROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + { + pcl::PCLPointCloud2 pcl_pc2; +#if PCL_VERSION_COMPARE(>=, 1, 13, 1) + pcl_conversions::copyPointCloud2MetaData(cloud, pcl_pc2); // Like pcl_conversions::toPCL, but does not copy the binary data + pcl::MsgFieldMap field_map; + pcl::createMapping (pcl_pc2.fields, field_map); + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud, field_map, &cloud.data[0]); +#else + pcl_conversions::toPCL(cloud, pcl_pc2); + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); +#endif + } + + template + void moveFromROSMsg(sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) + { + pcl::PCLPointCloud2 pcl_pc2; + pcl_conversions::moveToPCL(cloud, pcl_pc2); + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); + } + + /** 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::msg::PointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const bool binary_mode = false) + { + pcl::PCLPointCloud2 pcl_cloud; + pcl_conversions::toPCL(cloud, pcl_cloud); + return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode); + } + + inline int + destructiveSavePCDFile(const std::string &file_name, sensor_msgs::msg::PointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const bool binary_mode = false) + { + pcl::PCLPointCloud2 pcl_cloud; + pcl_conversions::moveToPCL(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::msg::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::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) + { + 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 * + 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); + + // Copy the second cloud + for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp) + { + size_t i = 0; + for (size_t j = 0; j < fields2.size (); ++j) + { + if (cloud1.fields[i].name == "_") + { + ++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 + +/* TODO when ROS2 type masquerading is implemented */ +/** +namespace ros +{ + template<> + struct DefaultMessageCreator + { + std::shared_ptr operator() () + { + 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(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. + static_assert(static_value1 == 0x1158d486dd51d683ULL); + 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 : 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::msg::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::msg::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.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::msg::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(std::uint8_t); + length += 1; // is_dense + + return length; + } + }; + **/ + /* + * 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 + { + template + inline static void write(Stream& stream, const pcl::PCLHeader& m) + { + std_msgs::msg::Header header; + pcl_conversions::fromPCL(m, header); + stream.next(header); + } + + template + inline static void read(Stream& stream, pcl::PCLHeader& m) + { + std_msgs::msg::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::msg::Header header; + pcl_conversions::fromPCL(m, header); + length += serializationLength(header); + + return length; + } + }; + } // namespace ros::serialization +} // namespace ros +**/ + +#endif /* PCL_CONVERSIONS_H__ */ diff --git a/ros2_ws/src/perception_pcl/pcl_conversions/package.xml b/ros2_ws/src/perception_pcl/pcl_conversions/package.xml new file mode 100644 index 00000000..a3662bac --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_conversions/package.xml @@ -0,0 +1,41 @@ + + + pcl_conversions + 2.4.1 + 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 + + http://wiki.ros.org/pcl_conversions + https://github.com/ros-perception/perception_pcl + https://github.com/ros-perception/perception_pcl/issues + + ament_cmake + + libpcl-all-dev + + eigen + message_filters + pcl_msgs + rclcpp + sensor_msgs + std_msgs + + libpcl-common + libpcl-io + + ament_cmake_gtest + + + ament_cmake + + diff --git a/ros2_ws/src/perception_pcl/pcl_conversions/test/test_pcl_conversions.cpp b/ros2_ws/src/perception_pcl/pcl_conversions/test/test_pcl_conversions.cpp new file mode 100644 index 00000000..83a3142e --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_conversions/test/test_pcl_conversions.cpp @@ -0,0 +1,158 @@ +#include + +#include "gtest/gtest.h" + +#include "pcl_conversions/pcl_conversions.h" + +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; + 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.stamp = 3141592653; + 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::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::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::PCLImage pcl_image; + sensor_msgs::msg::Image image; + + pcl::PCLPointCloud2 pcl_pc2; + sensor_msgs::msg::PointCloud2 pc2; +}; + +template +void test_image(T &image) { + EXPECT_EQ(std::string("pcl"), image.header.frame_id); + 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(2U, image.data.size()); + EXPECT_EQ(0x42, image.data[0]); + EXPECT_EQ(0x43, image.data[1]); +} + +TEST_F(PCLConversionTests, imageConversion) { + pcl_conversions::fromPCL(pcl_image, image); + test_image(image); + 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 +void test_pc(T &pc) { + EXPECT_EQ(std::string("pcl"), pc.header.frame_id); + 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(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(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]); +} + +TEST_F(PCLConversionTests, pointcloud2Conversion) { + pcl_conversions::fromPCL(pcl_pc2, pc2); + test_pc(pc2); + 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 + + +struct StampTestData +{ + const rclcpp::Time stamp_; + rclcpp::Time stamp2_; + + explicit StampTestData(const rclcpp::Time &stamp) + : stamp_(stamp) + { + std::uint64_t pcl_stamp; + pcl_conversions::toPCL(stamp_, pcl_stamp); + pcl_conversions::fromPCL(pcl_stamp, stamp2_); + } +}; + +TEST(PCLConversionStamp, Stamps) +{ + { + const StampTestData d(rclcpp::Time(1, 1000)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } + + { + const StampTestData d(rclcpp::Time(1, 999999000)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } + + { + const StampTestData d(rclcpp::Time(1, 999000000)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } + + { + const StampTestData d(rclcpp::Time(1423680574, 746000000)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } + + { + const StampTestData d(rclcpp::Time(1423680629, 901000000)); + EXPECT_TRUE(d.stamp_==d.stamp2_); + } +} + +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; +} diff --git a/ros2_ws/src/perception_pcl/pcl_ros/CHANGELOG.rst b/ros2_ws/src/perception_pcl/pcl_ros/CHANGELOG.rst new file mode 100644 index 00000000..af7b1788 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/CHANGELOG.rst @@ -0,0 +1,328 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pcl_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.6.2 (2018-05-20) +------------------ +* 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 +* Fix the use of Eigen3 in cmake +* Contributors: Kentaro Wada + +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 +* 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) + 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 +* Contributors: Kentaro Wada + +1.5.1 (2017-04-26) +------------------ +* Add my name as a maintainer +* Contributors: Kentaro Wada + +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 +* 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 +* 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 + 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) +------------------ + +1.2.5 (2015-01-20) +------------------ + +1.2.4 (2015-01-15) +------------------ + +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 + +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 + 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 +* merge pull request `#60 `_ +* Contributors: Paul Bovbel + +1.2.0 (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 +* adding FeatureConfig dependency too + +1.1.6 (2013-09-20) +------------------ +* add excplicit dependency on gencfg target + +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 +* 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 +* Make find_package on Eigen and PCL REQUIRED + +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 +* 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/ros2_ws/src/perception_pcl/pcl_ros/CMakeLists.txt b/ros2_ws/src/perception_pcl/pcl_ros/CMakeLists.txt new file mode 100644 index 00000000..577d239b --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/CMakeLists.txt @@ -0,0 +1,242 @@ +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 -fPIC) +endif() + +## Find system dependencies +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED QUIET COMPONENTS 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) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +set(dependencies + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + geometry_msgs + tf2 + tf2_geometry_msgs + tf2_ros + EIGEN3 + PCL +) + +## 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} +) + +### 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 SHARED + 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 ${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 +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::PassThrough" + EXECUTABLE filter_passthrough_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::ProjectInliers" + EXECUTABLE filter_project_inliers_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::RadiusOutlierRemoval" + EXECUTABLE filter_radius_outlier_removal_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::StatisticalOutlierRemoval" + EXECUTABLE filter_statistical_outlier_removal_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::CropBox" + EXECUTABLE filter_crop_box_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::VoxelGrid" + EXECUTABLE filter_voxel_grid_node +) +class_loader_hide_library_symbols(pcl_ros_filters) +# +### Declare the pcl_ros_segmentation library +#add_library (pcl_ros_segmentation +# 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_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}) +# +#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(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_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) + #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/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS + pcl_ros_tf + pcd_to_pointcloud_lib +# pcl_ros_io +# pcl_ros_features + pcl_ros_filters +# pcl_ros_surface +# pcl_ros_segmentation +# 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 +) + +install(DIRECTORY plugins samples + DESTINATION share/${PROJECT_NAME}) + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(pcl_ros_tf) + +# Export modern CMake targets +ament_export_targets(export_pcl_ros HAS_LIBRARY_TARGET) + +ament_export_dependencies(${dependencies}) + +ament_package() diff --git a/ros2_ws/src/perception_pcl/pcl_ros/cfg/CropBox.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/CropBox.cfg new file mode 100755 index 00000000..5f72d3fb --- /dev/null +++ b/ros2_ws/src/perception_pcl/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, -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.", "") +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/ros2_ws/src/perception_pcl/pcl_ros/cfg/EuclideanClusterExtraction.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/EuclideanClusterExtraction.cfg new file mode 100755 index 00000000..e9607f4e --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/EuclideanClusterExtraction.cfg @@ -0,0 +1,17 @@ +#! /usr/bin/env python + +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 ("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/ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractIndices.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractIndices.cfg new file mode 100755 index 00000000..debcb65d --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractIndices.cfg @@ -0,0 +1,12 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +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 ("negative", bool_t, 0, "Extract indices or the negative (all-indices)", False) + +exit (gen.generate (PACKAGE, "pcl_ros", "ExtractIndices")) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractPolygonalPrismData.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractPolygonalPrismData.cfg new file mode 100755 index 00000000..0705aeaa --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractPolygonalPrismData.cfg @@ -0,0 +1,13 @@ +#! /usr/bin/env python + +PACKAGE='pcl_ros' + +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 ("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/ros2_ws/src/perception_pcl/pcl_ros/cfg/Feature.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/Feature.cfg new file mode 100755 index 00000000..aaf9ee9e --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/Feature.cfg @@ -0,0 +1,17 @@ +#! /usr/bin/env python + +PACKAGE='pcl_ros' + +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 = ""): +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/ros2_ws/src/perception_pcl/pcl_ros/cfg/Filter.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/Filter.cfg new file mode 100755 index 00000000..398af1aa --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/Filter.cfg @@ -0,0 +1,17 @@ +#! /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 common import add_common_parameters + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator () +add_common_parameters (gen) + +exit (gen.generate (PACKAGE, "pcl_ros", "Filter")) + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/cfg/MLS.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/MLS.cfg new file mode 100755 index 00000000..7f9ff8f6 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/MLS.cfg @@ -0,0 +1,20 @@ +#! /usr/bin/env python + +PACKAGE='pcl_ros' + +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 = ""): +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/ros2_ws/src/perception_pcl/pcl_ros/cfg/RadiusOutlierRemoval.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/RadiusOutlierRemoval.cfg new file mode 100755 index 00000000..d54962e6 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation.cfg new file mode 100755 index 00000000..013e8ccf --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation.cfg @@ -0,0 +1,13 @@ +#! /usr/bin/env python + +PACKAGE='pcl_ros' + +from dynamic_reconfigure.parameter_generator_catkin import * + +import SACSegmentation_common as common + +gen = ParameterGenerator () +common.add_common_parameters (gen) + +exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentation")) + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentationFromNormals.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentationFromNormals.cfg new file mode 100755 index 00000000..e54cbf0c --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentationFromNormals.cfg @@ -0,0 +1,18 @@ +#! /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.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/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation_common.py b/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation_common.py new file mode 100644 index 00000000..0bef192b --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation_common.py @@ -0,0 +1,47 @@ +#! /usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import bool_t +from dynamic_reconfigure.parameter_generator_catkin import double_t +from dynamic_reconfigure.parameter_generator_catkin import int_t +from dynamic_reconfigure.parameter_generator_catkin import str_t + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + + +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/ros2_ws/src/perception_pcl/pcl_ros/cfg/SegmentDifferences.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/SegmentDifferences.cfg new file mode 100755 index 00000000..1a17a7c8 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/SegmentDifferences.cfg @@ -0,0 +1,16 @@ +#! /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 ("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/ros2_ws/src/perception_pcl/pcl_ros/cfg/StatisticalOutlierRemoval.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/StatisticalOutlierRemoval.cfg new file mode 100755 index 00000000..4640b838 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/StatisticalOutlierRemoval.cfg @@ -0,0 +1,17 @@ +#! /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 ("mean_k", int_t, 0, "The number of points (k) to use for mean distance estimation", 2, 2, 100) +gen.add ("stddev", double_t, 0, "The standard deviation multiplier threshold. All points outside the mean +- sigma * std_mul will be considered outliers.", 0.0, 0.0, 5.0) +gen.add ("negative", bool_t, 0, "Set whether the inliers should be returned (true) or the outliers (false)", False) + +exit (gen.generate (PACKAGE, "pcl_ros", "StatisticalOutlierRemoval")) + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/cfg/VoxelGrid.cfg b/ros2_ws/src/perception_pcl/pcl_ros/cfg/VoxelGrid.cfg new file mode 100755 index 00000000..332f1f0a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/VoxelGrid.cfg @@ -0,0 +1,18 @@ +#! /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 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) +add_common_parameters (gen) + +exit (gen.generate (PACKAGE, "pcl_ros", "VoxelGrid")) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/cfg/common.py b/ros2_ws/src/perception_pcl/pcl_ros/cfg/common.py new file mode 100644 index 00000000..27e47107 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/cfg/common.py @@ -0,0 +1,36 @@ +#! /usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import bool_t +from dynamic_reconfigure.parameter_generator_catkin import double_t +from dynamic_reconfigure.parameter_generator_catkin import str_t + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + + +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.'), + '') diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/boundary.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/boundary.hpp new file mode 100644 index 00000000..5b9a574d --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/boundary.hpp @@ -0,0 +1,87 @@ +/* + * 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__FEATURES__BOUNDARY_HPP_ +#define PCL_ROS__FEATURES__BOUNDARY_HPP_ + +#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true + +#include +#include "pcl_ros/features/feature.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__BOUNDARY_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/feature.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/feature.hpp new file mode 100644 index 00000000..8497330a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/feature.hpp @@ -0,0 +1,267 @@ +/* + * 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__FEATURES__FEATURE_HPP_ +#define PCL_ROS__FEATURES__FEATURE_HPP_ + +// PCL includes +#include +#include + +#include + +// Dynamic reconfigure +#include + +// PCL conversions +#include + +#include "pcl_ros/pcl_nodelet.hpp" +#include "pcl_ros/FeatureConfig.hpp" + +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 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 + */ + 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 = 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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__FEATURE_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh.hpp new file mode 100644 index 00000000..b112439c --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh.hpp @@ -0,0 +1,99 @@ +/* + * 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__FEATURES__FPFH_HPP_ +#define PCL_ROS__FEATURES__FPFH_HPP_ + +#include +#include "pcl_ros/features/pfh.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__FPFH_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp new file mode 100644 index 00000000..6192b3f3 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp @@ -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__FEATURES__FPFH_OMP_HPP_ +#define PCL_ROS__FEATURES__FPFH_OMP_HPP_ + +#include +#include "pcl_ros/features/fpfh.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__FPFH_OMP_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/moment_invariants.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/moment_invariants.hpp new file mode 100644 index 00000000..0235d05d --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/moment_invariants.hpp @@ -0,0 +1,82 @@ +/* + * 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__FEATURES__MOMENT_INVARIANTS_HPP_ +#define PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_ + +#include +#include "pcl_ros/features/feature.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d.hpp new file mode 100644 index 00000000..bf4c40f1 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d.hpp @@ -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__FEATURES__NORMAL_3D_HPP_ +#define PCL_ROS__FEATURES__NORMAL_3D_HPP_ + +#include +#include "pcl_ros/features/feature.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__NORMAL_3D_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp new file mode 100644 index 00000000..26300446 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp @@ -0,0 +1,80 @@ +/* + * 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__FEATURES__NORMAL_3D_OMP_HPP_ +#define PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_ + +#include +#include "pcl_ros/features/normal_3d.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp new file mode 100644 index 00000000..9ebb4e06 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp @@ -0,0 +1,85 @@ +/* + * 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__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 +#include "pcl_ros/features/normal_3d.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +// #endif // HAVE_TBB + +#endif // PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/pfh.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/pfh.hpp new file mode 100644 index 00000000..41a32ab9 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/pfh.hpp @@ -0,0 +1,99 @@ +/* + * 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__FEATURES__PFH_HPP_ +#define PCL_ROS__FEATURES__PFH_HPP_ + +#include +#include "pcl_ros/features/feature.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__PFH_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp new file mode 100644 index 00000000..08e89b9b --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp @@ -0,0 +1,85 @@ +/* + * 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__FEATURES__PRINCIPAL_CURVATURES_HPP_ +#define PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_ + +#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true +#include +#include "pcl_ros/features/feature.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot.hpp new file mode 100644 index 00000000..35d6e6f5 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot.hpp @@ -0,0 +1,79 @@ +/* + * 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__FEATURES__SHOT_HPP_ +#define PCL_ROS__FEATURES__SHOT_HPP_ + +#include +#include "pcl_ros/features/pfh.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__SHOT_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot_omp.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot_omp.hpp new file mode 100644 index 00000000..af202b2a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot_omp.hpp @@ -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__FEATURES__SHOT_OMP_HPP_ +#define PCL_ROS__FEATURES__SHOT_OMP_HPP_ + +#include +#include "pcl_ros/features/shot.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__SHOT_OMP_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/vfh.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/vfh.hpp new file mode 100644 index 00000000..d2e69670 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/vfh.hpp @@ -0,0 +1,84 @@ +/* + * 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_HPP_ +#define PCL_ROS__FEATURES__VFH_HPP_ + +#include +#include "pcl_ros/features/fpfh.hpp" + +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_ = advertise(nh, "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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FEATURES__VFH_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/crop_box.hpp new file mode 100644 index 00000000..36733776 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -0,0 +1,87 @@ +/* + * + * 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__CROP_BOX_HPP_ +#define PCL_ROS__FILTERS__CROP_BOX_HPP_ + +// PCL includes +#include +#include +#include "pcl_ros/filters/filter.hpp" + +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 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::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; + + /** \brief Parameter callback + * \param params parameter values to set + */ + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::CropBox impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit CropBox(const rclcpp::NodeOptions & options); +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FILTERS__CROP_BOX_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/extract_indices.hpp new file mode 100644 index 00000000..8f4bc55a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -0,0 +1,84 @@ +/* + * 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.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ +#define PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ + +// PCL includes +#include +#include +#include "pcl_ros/filters/filter.hpp" + +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 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::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; + + /** \brief Parameter callback + * \param params parameter values to set + */ + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::ExtractIndices impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit ExtractIndices(const rclcpp::NodeOptions & options); +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/filter.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/filter.hpp new file mode 100644 index 00000000..c4e88d7f --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -0,0 +1,163 @@ +/* + * 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__FILTERS__FILTER_HPP_ +#define PCL_ROS__FILTERS__FILTER_HPP_ + +#include +#include +#include +#include "pcl_ros/pcl_node.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. + * \author Radu Bogdan Rusu + */ +class Filter : public PCLNode +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; + + /** \brief Filter constructor + * \param node_name node name + * \param options node options + */ + Filter(std::string node_name, const rclcpp::NodeOptions & options); + +protected: + /** \brief declare and subscribe to param callback for input_frame and output_frame params */ + void + use_frame_params(); + + /** \brief Add common parameters */ + std::vector add_common_params(); + + /** \brief The input PointCloud subscriber. */ + rclcpp::Subscription::SharedPtr 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. */ + std::mutex mutex_; + + /** \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::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) = 0; + + /** \brief Lazy transport subscribe routine. */ + virtual void + subscribe(); + + /** \brief Lazy transport unsubscribe routine. */ + virtual void + unsubscribe(); + + /** \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::ConstSharedPtr & input, const IndicesPtr & indices); + +private: + /** \brief Pointer to parameters callback handle. */ + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + + /** \brief Synchronized input, and indices.*/ + std::shared_ptr>> sync_input_indices_e_; + std::shared_ptr>> sync_input_indices_a_; + + /** \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::ConstSharedPtr & cloud, + const PointIndices::ConstSharedPtr & indices); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FILTERS__FILTER_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/passthrough.hpp new file mode 100644 index 00000000..dd18bec8 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -0,0 +1,84 @@ +/* + * 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_HPP_ +#define PCL_ROS__FILTERS__PASSTHROUGH_HPP_ + +// PCL includes +#include +#include +#include "pcl_ros/filters/filter.hpp" + +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 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::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; + + /** \brief Parameter callback + * \param params parameter values to set + */ + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::PassThrough impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit PassThrough(const rclcpp::NodeOptions & options); +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FILTERS__PASSTHROUGH_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/project_inliers.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/project_inliers.hpp new file mode 100644 index 00000000..639289c4 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -0,0 +1,103 @@ +/* + * 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_HPP_ +#define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ + +// PCL includes +#include +#include +#include +#include "pcl_ros/filters/filter.hpp" + + +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: + explicit ProjectInliers(const rclcpp::NodeOptions & options); + +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::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; + +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.*/ + std::shared_ptr>> sync_input_indices_model_e_; + std::shared_ptr>> sync_input_indices_model_a_; + /** \brief The PCL filter implementation used. */ + pcl::ProjectInliers impl_; + + void subscribe() override; + void unsubscribe() override; + + /** \brief PointCloud2 + Indices + Model data callback. */ + void + input_indices_model_callback( + const PointCloud2::ConstSharedPtr & cloud, + const PointIndicesConstPtr & indices, + const ModelCoefficientsConstPtr & model); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp new file mode 100644 index 00000000..5f7333df --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -0,0 +1,85 @@ +/* + * 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__RADIUS_OUTLIER_REMOVAL_HPP_ +#define PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_ + +// PCL includes +#include +#include +#include "pcl_ros/filters/filter.hpp" + +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::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; + + /** \brief Parameter callback + * \param params parameter values to set + */ + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::RadiusOutlierRemoval impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit RadiusOutlierRemoval(const rclcpp::NodeOptions & options); +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp new file mode 100644 index 00000000..f8e14a2b --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -0,0 +1,91 @@ +/* + * 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__STATISTICAL_OUTLIER_REMOVAL_HPP_ +#define PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_ + +// PCL includes +#include +#include +#include "pcl_ros/filters/filter.hpp" + +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 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::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; + + /** \brief Parameter callback + * \param params parameter values to set + */ + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::StatisticalOutlierRemoval impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit StatisticalOutlierRemoval(const rclcpp::NodeOptions & options); +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp new file mode 100644 index 00000000..4b51113c --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -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: voxel_grid.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef PCL_ROS__FILTERS__VOXEL_GRID_HPP_ +#define PCL_ROS__FILTERS__VOXEL_GRID_HPP_ + +// PCL includes +#include +#include +#include "pcl_ros/filters/filter.hpp" + +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 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::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; + + /** \brief Parameter callback + * \param params parameter values to set + */ + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::VoxelGrid impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit VoxelGrid(const rclcpp::NodeOptions & options); +}; +} // namespace pcl_ros + +#endif // PCL_ROS__FILTERS__VOXEL_GRID_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/impl/transforms.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/impl/transforms.hpp new file mode 100644 index 00000000..0916c203 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -0,0 +1,266 @@ +/* + * 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_HPP_ +#define PCL_ROS__IMPL__TRANSFORMS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include "pcl_ros/transforms.hpp" + +using pcl_conversions::fromPCL; +using pcl_conversions::toPCL; + +namespace pcl_ros +{ +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + 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. + tf2::Quaternion q = transform.getRotation(); + Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) + tf2::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 +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 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. + tf2::Quaternion q = transform.getRotation(); + Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) + tf2::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 +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 +transformPointCloudWithNormals( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const tf2_ros::Buffer & tf_buffer) +{ + if (cloud_in.header.frame_id == target_frame) { + cloud_out = cloud_in; + return true; + } + + geometry_msgs::msg::TransformStamped transform; + try { + 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 (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; + return true; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +transformPointCloud( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const tf2_ros::Buffer & tf_buffer) +{ + if (cloud_in.header.frame_id == target_frame) { + cloud_out = cloud_in; + return true; + } + + geometry_msgs::msg::TransformStamped transform; + try { + 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 (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; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +transformPointCloud( + 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 tf2_ros::Buffer & tf_buffer) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer.lookupTransform( + target_frame, target_time, cloud_in.header.frame_id, + fromPCL(cloud_in.header.stamp), fixed_frame); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); + return false; + } 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::msg::Header header; + header.stamp = target_time; + cloud_out.header = toPCL(header); + return true; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +transformPointCloudWithNormals( + 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 tf2_ros::Buffer & tf_buffer) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer.lookupTransform( + target_frame, target_time, cloud_in.header.frame_id, + fromPCL(cloud_in.header.stamp), fixed_frame); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); + return false; + } 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::msg::Header header; + header.stamp = target_time; + cloud_out.header = toPCL(header); + return true; +} +} // namespace pcl_ros +#endif // PCL_ROS__IMPL__TRANSFORMS_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/bag_io.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/bag_io.hpp new file mode 100644 index 00000000..bad98c7d --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/bag_io.hpp @@ -0,0 +1,131 @@ +/* + * 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_HPP_ +#define PCL_ROS__IO__BAG_IO_HPP_ + +#include +#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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__IO__BAG_IO_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_data.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_data.hpp new file mode 100644 index 00000000..62aae16d --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_data.hpp @@ -0,0 +1,146 @@ +/* + * 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_ROS__IO__CONCATENATE_DATA_HPP_ +#define PCL_ROS__IO__CONCATENATE_DATA_HPP_ + +// ROS includes +#include +#include +#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_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 + */ + explicit 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); +}; +} // namespace pcl_ros + +#endif // PCL_ROS__IO__CONCATENATE_DATA_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp new file mode 100644 index 00000000..00b53160 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp @@ -0,0 +1,106 @@ +/* + * 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_ROS__IO__CONCATENATE_FIELDS_HPP_ +#define PCL_ROS__IO__CONCATENATE_FIELDS_HPP_ + +// ROS includes +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace pcl_ros +{ +/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of + * 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 + */ + explicit PointCloudConcatenateFieldsSynchronizer(int queue_size) + : maximum_queue_size_(queue_size), maximum_seconds_(0) {} + + /** \brief Empty destructor. */ + virtual ~PointCloudConcatenateFieldsSynchronizer() {} + + void onInit(); + void subscribe(); + void unsubscribe(); + void input_callback(const PointCloudConstPtr & cloud); + +private: + /** \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_; +}; +} // namespace pcl_ros + +#endif // PCL_ROS__IO__CONCATENATE_FIELDS_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/pcd_io.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/pcd_io.hpp new file mode 100644 index 00000000..b65882a4 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/pcd_io.hpp @@ -0,0 +1,137 @@ +/* + * 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_HPP_ +#define PCL_ROS__IO__PCD_IO_HPP_ + +#include +#include +#include "pcl_ros/pcl_nodelet.hpp" + +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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__IO__PCD_IO_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/pcl_node.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/pcl_node.hpp new file mode 100644 index 00000000..31be9944 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/pcl_node.hpp @@ -0,0 +1,296 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: pcl_node.h 33238 2010-03-11 00:46:58Z rusu $ + * + */ + +/** + +\author Radu Bogdan Rusu + +**/ + +#ifndef PCL_ROS__PCL_NODE_HPP_ +#define PCL_ROS__PCL_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +// #include // TODO(daisukes): lazy subscription + +#include +#include +#include + +#include +#include +#include +#include + +// #include "pcl_ros/point_cloud.hpp" + +using pcl_conversions::fromPCL; + +namespace pcl_ros +{ +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b PCLNode represents the base PCL Node class. All PCL node should inherit from + * this class. */ +template> +class PCLNode : public rclcpp::Node +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + typedef pcl_msgs::msg::PointIndices PointIndices; + typedef PointIndices::SharedPtr PointIndicesPtr; + typedef PointIndices::ConstSharedPtr PointIndicesConstPtr; + + typedef pcl_msgs::msg::ModelCoefficients ModelCoefficients; + typedef ModelCoefficients::SharedPtr ModelCoefficientsPtr; + typedef ModelCoefficients::ConstSharedPtr ModelCoefficientsConstPtr; + + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; + + /** \brief Empty constructor. */ + PCLNode(std::string node_name, const rclcpp::NodeOptions & options) + : rclcpp::Node(node_name, options), + use_indices_(false), transient_local_indices_(false), + max_queue_size_(3), approximate_sync_(false), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) + { + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "max_queue_size"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + desc.description = "QoS History depth"; + desc.read_only = true; + max_queue_size_ = declare_parameter(desc.name, max_queue_size_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "use_indices"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = "Only process a subset of the point cloud from an indices topic"; + desc.read_only = true; + use_indices_ = declare_parameter(desc.name, use_indices_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "transient_local_indices"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = "Does indices topic use transient local documentation"; + desc.read_only = true; + transient_local_indices_ = declare_parameter(desc.name, transient_local_indices_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "approximate_sync"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = + "Match indices and point cloud messages if time stamps are 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. + * + * When receiving a point cloud, if use_indices_ is false, the entire + * point cloud is processed for the given operation. If use_indices_ is + * true, then the ~indices topic is read to get the vector of point + * indices specifying the subset of the point cloud that will be used for + * the operation. In the case where use_indices_ is true, the ~input and + * ~indices topics must be synchronised in time, either exact or within a + * specified jitter. See also @ref transient_local_indices_ and approximate_sync. + **/ + bool use_indices_; + /** \brief Set to true if the indices topic has transient_local durability. + * + * If use_indices_ is true, the ~input and ~indices topics generally must + * be synchronised in time. By setting this flag to true, the most recent + * value from ~indices can be used instead of requiring a synchronised + * message. + **/ + bool transient_local_indices_; + + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_input_filter_; + + /** \brief The message filter subscriber for PointIndices. */ + message_filters::Subscriber sub_indices_filter_; + + /** \brief The output PointCloud publisher. Allow each individual class that inherits from this + * to declare the Publisher with their type. + */ + std::shared_ptr pub_output_; + + /** \brief The maximum queue size (default: 3). */ + int max_queue_size_; + + /** \brief True if we use an approximate time synchronizer + * versus an exact one (false by default). + **/ + bool approximate_sync_; + + /** \brief TF listener object. */ + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). + * \param cloud the point cloud to test + * \param topic_name an optional topic name (only used for printing, defaults to "input") + */ + inline bool + isValid(const PointCloud2::ConstSharedPtr & cloud, const std::string & topic_name = "input") + { + if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) { + RCLCPP_WARN( + this->get_logger(), + "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) " + "with stamp %d.%09d, and frame %s on topic %s received!", + cloud->data.size(), cloud->width, cloud->height, cloud->point_step, + cloud->header.stamp.sec, cloud->header.stamp.nanosec, + cloud->header.frame_id.c_str(), topic_name.c_str()); + + return false; + } + return true; + } + + /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). + * \param cloud the point cloud to test + * \param topic_name an optional topic name (only used for printing, defaults to "input") + */ + inline bool + isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input") + { + if (cloud->width * cloud->height != cloud->points.size()) { + RCLCPP_WARN( + this->get_logger(), + "Invalid PointCloud (points = %zu, width = %d, height = %d) " + "with stamp %d.%09d, and frame %s on topic %s received!", + cloud->points.size(), cloud->width, cloud->height, + fromPCL(cloud->header).stamp.sec, fromPCL(cloud->header).stamp.nanosec, + cloud->header.frame_id.c_str(), topic_name.c_str()); + + return false; + } + return true; + } + + /** \brief Test whether a given PointIndices message is "valid" (i.e., has values). + * \param indices the point indices message to test + * \param topic_name an optional topic name (only used for printing, defaults to "indices") + */ + inline bool + isValid( + const PointIndicesConstPtr & /*indices*/, + const std::string & /*topic_name*/ = "indices") + { + /*if (indices->indices.empty ()) + { + RCLCPP_WARN( + this->get_logger(), "Empty indices (values = %zu) with stamp %d.%09d, " + "and frame %s on topic %s received!", + indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec, + indices->header.frame_id.c_str(), topic_name.c_str()); + //return (true); // looks like it should be false + return false; + }*/ + return true; + } + + /** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values). + * \param model the model coefficients to test + * \param topic_name an optional topic name (only used for printing, defaults to "model") + */ + inline bool + isValid( + const ModelCoefficientsConstPtr & /*model*/, + const std::string & /*topic_name*/ = "model") + { + /*if (model->values.empty ()) + { + RCLCPP_WARN( + this->get_logger(), "Empty model (values = %zu) with stamp %d.%09d, " + "and frame %s on topic %s received!", + model->values.size(), model->header.stamp.sec, model->header.stamp.nanosec, + model->header.frame_id.c_str(), topic_name.c_str()); + return (false); + }*/ + return true; + } + + /** \brief Lazy transport subscribe/unsubscribe routine. + * It is optional for backward compatibility. + **/ + virtual void subscribe() {} + virtual void unsubscribe() {} + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace pcl_ros + +#endif // PCL_ROS__PCL_NODE_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/point_cloud.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/point_cloud.hpp new file mode 100644 index 00000000..b1da6b0a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -0,0 +1,467 @@ +/* + * 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 // 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 +#if PCL_VERSION_COMPARE(>=, 1, 11, 0) +#include +#elif PCL_VERSION_COMPARE(>=, 1, 10, 0) +#include +#endif +#endif +#include +#include +#include +#include +#include +#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED +#include +#include +#endif +#include // for BOOST_FOREACH +#include +#include + +namespace pcl +{ +namespace detail +{ +template +struct FieldStreamer +{ + explicit FieldStreamer(Stream & stream) + : stream_(stream) {} + + template + void operator()() + { + 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 = pcl::traits::offset::value; + stream_.next(offset); + + std::uint8_t datatype = pcl::traits::datatype::value; + stream_.next(datatype); + + std::uint32_t count = pcl::traits::datatype::size; + stream_.next(count); + } + + Stream & stream_; +}; + +template +struct FieldsLength +{ + FieldsLength() + : length(0) {} + + template + void operator()() + { + std::uint32_t name_length = strlen(pcl::traits::name::value); + length += name_length + 13; + } + + std::uint32_t length; +}; +} // namespace 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();} +}; + +// 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 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.data(), 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.data()); + // 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 serialization + +/// @todo Printer specialization in message_operations + +} // namespace ros + +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; + + explicit 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 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 // PCL_ROS__POINT_CLOUD_HPP__ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/publisher.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/publisher.hpp new file mode 100644 index 00000000..9f92745f --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/publisher.hpp @@ -0,0 +1,151 @@ +/* + * 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_HPP_ +#define PCL_ROS__PUBLISHER_HPP_ + +#include +#include +#include + +#include + +#include + +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_) ? reinterpret_cast(1) : reinterpret_cast(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::Ptr msg_ptr(new sensor_msgs::PointCloud2); + pcl::toROSMsg(point_cloud, *msg_ptr); + pub_.publish(msg_ptr); + } +}; + +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); + } +}; +} // namespace pcl_ros +#endif // PCL_ROS__PUBLISHER_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp new file mode 100644 index 00000000..581bab57 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp @@ -0,0 +1,113 @@ +/* + * 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__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ +#define PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ + +#include +#include +#include +#include "pcl_ros/pcl_nodelet.hpp" +#include "pcl_ros/EuclideanClusterExtractionConfig.hpp" + +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 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(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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp new file mode 100644 index 00000000..b60f7e40 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp @@ -0,0 +1,130 @@ +/* + * 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__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ +#define PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ + +#include +#include +#include +#include +#include +#include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp" +#include "pcl_ros/pcl_nodelet.hpp" + +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 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. + */ + 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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp new file mode 100644 index 00000000..4699eee6 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -0,0 +1,301 @@ +/* + * 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__SEGMENTATION__SAC_SEGMENTATION_HPP_ +#define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ + +#include +#include +#include +#include +#include "pcl_ros/pcl_nodelet.hpp" +#include "pcl_ros/SACSegmentationConfig.hpp" +#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp" + +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 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 + */ + 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. + */ +class SACSegmentationFromNormals : public SACSegmentation +{ + typedef pcl::PointCloud PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; + + typedef pcl::PointCloud PointCloudN; + 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. + * \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 = fromPCL(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 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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp new file mode 100644 index 00000000..9a4a9322 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp @@ -0,0 +1,111 @@ +/* + * 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__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ +#define PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ + +#include +#include +#include "pcl_ros/SegmentDifferencesConfig.hpp" +#include "pcl_ros/pcl_nodelet.hpp" + + +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 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 + */ + 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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/convex_hull.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/convex_hull.hpp new file mode 100644 index 00000000..230c9415 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/convex_hull.hpp @@ -0,0 +1,95 @@ +/* + * 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__SURFACE__CONVEX_HULL_HPP_ +#define PCL_ROS__SURFACE__CONVEX_HULL_HPP_ + +#include +#include +#include "pcl_ros/pcl_nodelet.hpp" + +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 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 + */ + 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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__SURFACE__CONVEX_HULL_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp new file mode 100644 index 00000000..c6d59bf4 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp @@ -0,0 +1,152 @@ +/* + * 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__SURFACE__MOVING_LEAST_SQUARES_HPP_ +#define PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_ + +#include +#include +#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. + * 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 + */ + 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 +}; +} // namespace pcl_ros + +#endif // PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_ diff --git a/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/transforms.hpp b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/transforms.hpp new file mode 100644 index 00000000..4ed335c1 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/transforms.hpp @@ -0,0 +1,233 @@ +/* + * 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_HPP_ +#define PCL_ROS__TRANSFORMS_HPP_ + +#include +#include +#include +#include +#include +#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 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_buffer a TF buffer object + */ +template +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + 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 + * \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_buffer a TF buffer object + */ +template +bool +transformPointCloudWithNormals( + 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 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 + * \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 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_buffer a TF buffer object + */ +template +bool +transformPointCloud( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + 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 + * \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_buffer a TF buffer object + */ +template +bool +transformPointCloud( + 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 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_buffer a TF buffer object + */ +bool +transformPointCloud( + const std::string & target_frame, + 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 + * \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 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 + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + */ +void +transformPointCloud( + const Eigen::Matrix4f & transform, + 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 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/ros2_ws/src/perception_pcl/pcl_ros/package.xml b/ros2_ws/src/perception_pcl/pcl_ros/package.xml new file mode 100644 index 00000000..12e1def6 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/package.xml @@ -0,0 +1,73 @@ + + + + pcl_ros + 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 + processing in ROS. + + + + Paul Bovbel + Steve Macenski + Kentaro Wada + + BSD + + http://ros.org/wiki/perception_pcl + https://github.com/ros-perception/perception_pcl/issues + https://github.com/ros-perception/perception_pcl + + Open Perception + Julius Kammerl + William Woodall + + ament_cmake + + libpcl-all-dev + + eigen + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + geometry_msgs + tf2 + 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 + ament_cmake_pytest + launch + launch_ros + launch_testing + launch_testing_ros + sensor_msgs + + + + + ament_cmake + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_features.xml b/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_features.xml new file mode 100644 index 00000000..2593eb83 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_filters.xml b/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_filters.xml new file mode 100644 index 00000000..15476eea --- /dev/null +++ b/ros2_ws/src/perception_pcl/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/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_io.xml b/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_io.xml new file mode 100644 index 00000000..498d5d8b --- /dev/null +++ b/ros2_ws/src/perception_pcl/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/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml b/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml new file mode 100644 index 00000000..845b74a7 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_surface.xml b/ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_surface.xml new file mode 100644 index 00000000..d7c6398b --- /dev/null +++ b/ros2_ws/src/perception_pcl/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. + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/samples/data/.gitignore b/ros2_ws/src/perception_pcl/pcl_ros/samples/data/.gitignore new file mode 100644 index 00000000..d6b7ef32 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/samples/data/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch new file mode 100644 index 00000000..01ad46de --- /dev/null +++ b/ros2_ws/src/perception_pcl/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 + + + + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz new file mode 100644 index 00000000..2c581d6a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/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/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/config/voxel_grid.rviz b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/config/voxel_grid.rviz new file mode 100644 index 00000000..e395065f --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/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/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_statistical_outlier_removal.launch b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_statistical_outlier_removal.launch new file mode 100644 index 00000000..1aeeed36 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_statistical_outlier_removal.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + mean_k: 10 + stddev: 1.0 + + + + + + topic: /statistical_outlier_removal/output + hz: 20 + hzerror: 15 + test_duration: 5.0 + + + + + + + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch new file mode 100644 index 00000000..45b61294 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + filter_field_name: '' + leaf_size: $(arg leaf_size) + + + + + + + topic: /voxel_grid/output + hz: 20 + hzerror: 15 + test_duration: 5.0 + + + + + + + + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch new file mode 100644 index 00000000..ac5943af --- /dev/null +++ b/ros2_ws/src/perception_pcl/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 + + + + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch new file mode 100644 index 00000000..35a474af --- /dev/null +++ b/ros2_ws/src/perception_pcl/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: 2400 + test_duration: 5.0 + + + + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch b/ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch new file mode 100644 index 00000000..50e54f80 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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 + + + + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/__init__.py b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/boundary.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/boundary.cpp new file mode 100644 index 00000000..6ac61f4a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -0,0 +1,75 @@ +/* + * 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.hpp" + +void +pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + 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); + + // Enforce that the TF frame and the timestamp are copied + output.header = cloud->header; + pub_output_.publish(ros_ptr(output.makeShared())); +} + +typedef pcl_ros::BoundaryEstimation BoundaryEstimation; +PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/feature.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/feature.cpp new file mode 100644 index 00000000..b0f0fc83 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/feature.cpp @@ -0,0 +1,641 @@ +/* + * 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 +#include +#include "pcl_ros/features/feature.hpp" + +//////////////////////////////////////////////////////////////////////////////////////////// +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); + + 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_) { + // 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())); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////// +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(); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////// +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(), 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 (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_, + (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_); + + // 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_); + } 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)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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(); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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(), 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 (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)!", + 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/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh.cpp new file mode 100644 index 00000000..e6c05b34 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh.cpp @@ -0,0 +1,76 @@ +/* + * 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.hpp" + +void +pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + 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); + + // 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())); +} + +typedef pcl_ros::FPFHEstimation FPFHEstimation; +PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp new file mode 100644 index 00000000..d3e96fc4 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp @@ -0,0 +1,76 @@ +/* + * 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.hpp" + +void +pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + 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); + + // 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())); +} + +typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; +PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/moment_invariants.cpp new file mode 100644 index 00000000..5a5c1452 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/moment_invariants.cpp @@ -0,0 +1,74 @@ +/* + * 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.hpp" + +void +pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(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(ros_ptr(output.makeShared())); +} + +typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; +PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d.cpp new file mode 100644 index 00000000..7ab774bc --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d.cpp @@ -0,0 +1,74 @@ +/* + * 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.hpp" + +void +pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(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(ros_ptr(output.makeShared())); +} + +typedef pcl_ros::NormalEstimation NormalEstimation; +PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp new file mode 100644 index 00000000..c7cd2124 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp @@ -0,0 +1,74 @@ +/* + * 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.hpp" + +void +pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(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(ros_ptr(output.makeShared())); +} + +typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; +PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp new file mode 100644 index 00000000..0eea2235 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +#if defined HAVE_TBB + +void +pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloud output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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(ros_ptr(output.makeShared())); +} + +typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; +PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet) + +#endif // HAVE_TBB diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/pfh.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/pfh.cpp new file mode 100644 index 00000000..e55a7fea --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/pfh.cpp @@ -0,0 +1,76 @@ +/* + * 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.hpp" + +void +pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + 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); + + // 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())); +} + +typedef pcl_ros::PFHEstimation PFHEstimation; +PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp new file mode 100644 index 00000000..ff44b843 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp @@ -0,0 +1,76 @@ +/* + * 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.hpp" + +void +pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + 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); + + // 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())); +} + +typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; +PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/shot.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/shot.cpp new file mode 100644 index 00000000..d0664724 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +void +pcl_ros::SHOTEstimation::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(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(ros_ptr(output.makeShared())); +} + +typedef pcl_ros::SHOTEstimation SHOTEstimation; +PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/shot_omp.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/shot_omp.cpp new file mode 100644 index 00000000..0d10be21 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +void +pcl_ros::SHOTEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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(pcl_ptr(cloud)); + impl_.setIndices(indices); + impl_.setSearchSurface(pcl_ptr(surface)); + impl_.setInputNormals(pcl_ptr(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(ros_ptr(output.makeShared())); +} + +typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP; +PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/vfh.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/vfh.cpp new file mode 100644 index 00000000..25c6911b --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/vfh.cpp @@ -0,0 +1,76 @@ +/* + * 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.hpp" + +void +pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish(ros_ptr(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_); + + // Set the inputs + 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); + + // 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())); +} + +typedef pcl_ros::VFHEstimation VFHEstimation; +PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/crop_box.cpp new file mode 100644 index 00000000..0b9889a0 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -0,0 +1,253 @@ +/* + * + * 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 "pcl_ros/filters/crop_box.hpp" + +pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options) +: Filter("CropBoxNode", options) +{ + // This both declares and initializes the input and output frames + use_frame_params(); + + rcl_interfaces::msg::ParameterDescriptor min_x_desc; + min_x_desc.name = "min_x"; + min_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + min_x_desc.description = + "Minimum x value below which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + min_x_desc.floating_point_range.push_back(float_range); + } + declare_parameter(min_x_desc.name, rclcpp::ParameterValue(-1.0), min_x_desc); + + rcl_interfaces::msg::ParameterDescriptor max_x_desc; + max_x_desc.name = "max_x"; + max_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + max_x_desc.description = + "Maximum x value above which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + max_x_desc.floating_point_range.push_back(float_range); + } + declare_parameter(max_x_desc.name, rclcpp::ParameterValue(1.0), max_x_desc); + + rcl_interfaces::msg::ParameterDescriptor min_y_desc; + min_y_desc.name = "min_y"; + min_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + min_y_desc.description = + "Minimum y value below which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + min_y_desc.floating_point_range.push_back(float_range); + } + declare_parameter(min_y_desc.name, rclcpp::ParameterValue(-1.0), min_y_desc); + + rcl_interfaces::msg::ParameterDescriptor max_y_desc; + max_y_desc.name = "max_y"; + max_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + max_y_desc.description = + "Maximum y value above which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + max_y_desc.floating_point_range.push_back(float_range); + } + declare_parameter(max_y_desc.name, rclcpp::ParameterValue(1.0), max_y_desc); + + rcl_interfaces::msg::ParameterDescriptor min_z_desc; + min_z_desc.name = "min_z"; + min_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + min_z_desc.description = + "Minimum z value below which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + min_z_desc.floating_point_range.push_back(float_range); + } + declare_parameter(min_z_desc.name, rclcpp::ParameterValue(-1.0), min_z_desc); + + rcl_interfaces::msg::ParameterDescriptor max_z_desc; + max_z_desc.name = "max_z"; + max_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + max_z_desc.description = + "Maximum z value above which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + max_z_desc.floating_point_range.push_back(float_range); + } + declare_parameter(max_z_desc.name, rclcpp::ParameterValue(1.0), max_z_desc); + + rcl_interfaces::msg::ParameterDescriptor keep_organized_desc; + keep_organized_desc.name = "keep_organized"; + keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + keep_organized_desc.description = + "Set whether the filtered points should be kept and set to NaN, " + "or removed from the PointCloud, thus potentially breaking its organized structure."; + declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc); + + rcl_interfaces::msg::ParameterDescriptor negative_desc; + negative_desc.name = "negative"; + negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + negative_desc.description = + "Set whether the inliers should be returned (true) or the outliers (false)."; + declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc); + + const std::vector param_names { + min_x_desc.name, + max_x_desc.name, + min_y_desc.name, + max_y_desc.name, + min_z_desc.name, + max_z_desc.name, + keep_organized_desc.name, + negative_desc.name, + }; + + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &CropBox::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); +} + +void +pcl_ros::CropBox::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard 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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// + +rcl_interfaces::msg::SetParametersResult +pcl_ros::CropBox::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); + + Eigen::Vector4f min_point, max_point; + min_point = impl_.getMin(); + max_point = impl_.getMax(); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "min_x") { + min_point(0) = param.as_double(); + } + if (param.get_name() == "max_x") { + max_point(0) = param.as_double(); + } + if (param.get_name() == "min_y") { + min_point(1) = param.as_double(); + } + if (param.get_name() == "max_y") { + max_point(1) = param.as_double(); + } + if (param.get_name() == "min_z") { + min_point(2) = param.as_double(); + } + if (param.get_name() == "max_z") { + max_point(2) = param.as_double(); + } + if (param.get_name() == "negative") { + // Check the current value for the negative flag + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter negative flag to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setNegative(param.as_bool()); + } + } + if (param.get_name() == "keep_organized") { + // Check the current value for keep_organized + if (impl_.getKeepOrganized() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter keep_organized value to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setKeepOrganized(param.as_bool()); + } + } + } + + // Check the current values for minimum point + if (min_point != impl_.getMin()) { + RCLCPP_DEBUG( + get_logger(), "Setting the minimum point to: %f %f %f.", + min_point(0), min_point(1), min_point(2)); + impl_.setMin(min_point); + } + + // Check the current values for the maximum point + if (max_point != impl_.getMax()) { + RCLCPP_DEBUG( + get_logger(), "Setting the maximum point to: %f %f %f.", + max_point(0), max_point(1), max_point(2)); + impl_.setMax(max_point); + } + + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::CropBox) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/extract_indices.cpp new file mode 100644 index 00000000..d6287a1b --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -0,0 +1,102 @@ +/* + * 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 "pcl_ros/filters/extract_indices.hpp" + +pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options) +: Filter("ExtractIndicesNode", options) +{ + rcl_interfaces::msg::ParameterDescriptor neg_desc; + neg_desc.name = "negative"; + neg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + neg_desc.description = "Extract indices or the negative (all-indices)"; + declare_parameter(neg_desc.name, rclcpp::ParameterValue(false), neg_desc); + + // Validate initial values using same callback + callback_handle_ = + add_on_set_parameters_callback( + std::bind(&ExtractIndices::config_callback, this, std::placeholders::_1)); + + std::vector param_names{neg_desc.name}; + auto result = config_callback(get_parameters(param_names)); + if (!result.successful) { + throw std::runtime_error(result.reason); + } + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); +} + +void +pcl_ros::ExtractIndices::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard 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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::ExtractIndices::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "negative") { + // Check the current value for the negative flag + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter negative flag to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setNegative(param.as_bool()); + } + } + } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ExtractIndices) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/boundary.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/boundary.cpp new file mode 100644 index 00000000..13a1139e --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/boundary.cpp @@ -0,0 +1,78 @@ +/* + * 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.hpp" + +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_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/feature.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/feature.cpp new file mode 100644 index 00000000..c8d1eabb --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/feature.cpp @@ -0,0 +1,592 @@ +/* + * 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 +#include +#include "pcl_ros/features/feature.hpp" + +//////////////////////////////////////////////////////////////////////////////////////////// +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 (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_, + (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 (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)!", + 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/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp new file mode 100644 index 00000000..dac0f28f --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +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_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp new file mode 100644 index 00000000..80cdde16 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +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_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp new file mode 100644 index 00000000..7391856b --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +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_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp new file mode 100644 index 00000000..1e393358 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +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_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp new file mode 100644 index 00000000..688782ba --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +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_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp new file mode 100644 index 00000000..eb002a47 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +#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_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet); + +#endif // HAVE_TBB diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/pfh.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/pfh.cpp new file mode 100644 index 00000000..12f4a7c5 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +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_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp new file mode 100644 index 00000000..0d5fa4e6 --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +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_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/vfh.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/vfh.cpp new file mode 100644 index 00000000..eb2f58cf --- /dev/null +++ b/ros2_ws/src/perception_pcl/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.hpp" + +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_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/filter.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/filter.cpp new file mode 100644 index 00000000..fb167bb4 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -0,0 +1,345 @@ +/* + * 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 "pcl_ros/filters/filter.hpp" +#include +#include "pcl_ros/transforms.hpp" + +/*//#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_EXPORT_CLASS(PixelGrid,nodelet::Nodelet); +//PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet); +*/ + +/////////////////////////////////////////////////////////////////////////////////////////////////// +void +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::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_) { + 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_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 + 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_buffer_)) + { + 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)); + } + + // Copy timestamp to keep it + cloud_tf->header.stamp = input->header.stamp; + + // Publish the unique ptr + pub_output_->publish(move(cloud_tf)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::subscribe() +{ + // If we're supposed to look for PointIndices (indices) + if (use_indices_) { + // Subscribe to the input using a filter + auto sensor_qos_profile = rclcpp::QoS( + rclcpp::KeepLast(max_queue_size_), + rmw_qos_profile_sensor_data).get_rmw_qos_profile(); + sub_input_filter_.subscribe(this, "input", sensor_qos_profile); + sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile); + + if (approximate_sync_) { + sync_input_indices_a_ = + std::make_shared>>(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback( + std::bind( + &Filter::input_indices_callback, this, + std::placeholders::_1, std::placeholders::_2)); + } else { + sync_input_indices_e_ = + std::make_shared>>(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + 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_ = + this->create_subscription( + "input", max_queue_size_, + callback); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::unsubscribe() +{ + if (use_indices_) { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } else { + sub_input_.reset(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & options) +: PCLNode(node_name, options) +{ + pub_output_ = create_publisher("output", max_queue_size_); + RCLCPP_DEBUG(this->get_logger(), "Node successfully created."); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::use_frame_params() +{ + 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); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +std::vector +pcl_ros::Filter::add_common_params() +{ + rcl_interfaces::msg::ParameterDescriptor ffn_desc; + ffn_desc.name = "filter_field_name"; + ffn_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + ffn_desc.description = "The field name used for filtering"; + declare_parameter(ffn_desc.name, rclcpp::ParameterValue("z"), ffn_desc); + + rcl_interfaces::msg::ParameterDescriptor flmin_desc; + flmin_desc.name = "filter_limit_min"; + flmin_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + flmin_desc.description = "The minimum allowed field value a point will be considered from"; + rcl_interfaces::msg::FloatingPointRange flmin_range; + flmin_range.from_value = -100000.0; + flmin_range.to_value = 100000.0; + flmin_desc.floating_point_range.push_back(flmin_range); + declare_parameter(flmin_desc.name, rclcpp::ParameterValue(0.0), flmin_desc); + + rcl_interfaces::msg::ParameterDescriptor flmax_desc; + flmax_desc.name = "filter_limit_max"; + flmax_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + flmax_desc.description = "The maximum allowed field value a point will be considered from"; + rcl_interfaces::msg::FloatingPointRange flmax_range; + flmax_range.from_value = -100000.0; + flmax_range.to_value = 100000.0; + flmax_desc.floating_point_range.push_back(flmax_range); + declare_parameter(flmax_desc.name, rclcpp::ParameterValue(1.0), flmax_desc); + + rcl_interfaces::msg::ParameterDescriptor flneg_desc; + flneg_desc.name = "filter_limit_negative"; + flneg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + flneg_desc.description = + "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max]."; + declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc); + + return std::vector { + ffn_desc.name, + flmin_desc.name, + flmax_desc.name, + flneg_desc.name + }; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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::ConstSharedPtr & cloud, + const PointIndices::ConstSharedPtr & indices) +{ + // If cloud is given, check if it's valid + if (!isValid(cloud)) { + RCLCPP_ERROR(this->get_logger(), "Invalid input!"); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid(indices)) { + RCLCPP_ERROR(this->get_logger(), "Invalid indices!"); + return; + } + + /// DEBUG + if (indices) { + 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.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 { + 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::ConstSharedPtr cloud_tf; + if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { + 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_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 = std::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/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/passthrough.cpp new file mode 100644 index 00000000..a068da77 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -0,0 +1,157 @@ +/* + * 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 "pcl_ros/filters/passthrough.hpp" + +pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options) +: Filter("PassThroughNode", options) +{ + use_frame_params(); + std::vector common_param_names = add_common_params(); + + rcl_interfaces::msg::ParameterDescriptor keep_organized_desc; + keep_organized_desc.name = "keep_organized"; + keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + keep_organized_desc.description = + "Set whether the filtered points should be kept and set to NaN, " + "or removed from the PointCloud, thus potentially breaking its organized structure."; + declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc); + + std::vector param_names { + keep_organized_desc.name, + }; + param_names.insert(param_names.end(), common_param_names.begin(), common_param_names.end()); + + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &PassThrough::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); +} + +void pcl_ros::PassThrough::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard 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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::PassThrough::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); + + double filter_min, filter_max; + impl_.getFilterLimits(filter_min, filter_max); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "filter_field_name") { + // Check the current value for the filter field + if (impl_.getFilterFieldName() != param.as_string()) { + // Set the filter field if different + impl_.setFilterFieldName(param.as_string()); + RCLCPP_DEBUG( + get_logger(), "Setting the filter field name to: %s.", + param.as_string().c_str()); + } + } + if (param.get_name() == "filter_limit_min") { + // Check the current values for filter min-max + if (filter_min != param.as_double()) { + filter_min = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum filtering value a point will be considered from to: %f.", + filter_min); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_max") { + // Check the current values for filter min-max + if (filter_max != param.as_double()) { + filter_max = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the maximum filtering value a point will be considered from to: %f.", + filter_max); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_negative") { + // Check the current value for the negative flag + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter negative flag to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setNegative(param.as_bool()); + } + } + if (param.get_name() == "keep_organized") { + // Check the current value for keep_organized + if (impl_.getKeepOrganized() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter keep_organized value to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setKeepOrganized(param.as_bool()); + } + } + } + // TODO(sloretz) constraint validation + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::PassThrough) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/project_inliers.cpp new file mode 100644 index 00000000..39804ba3 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -0,0 +1,196 @@ +/* + * 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 "pcl_ros/filters/project_inliers.hpp" + +////////////////////////////////////////////////////////////////////////////////////////////// +pcl_ros::ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options) +: Filter("ProjectInliersNode", options), model_() +{ + // ---[ Mandatory parameters + // The type of model to use (user given parameter). + declare_parameter("model_type", rclcpp::ParameterType::PARAMETER_INTEGER); + int model_type; + if (!get_parameter("model_type", model_type)) { + RCLCPP_ERROR( + get_logger(), + "[onConstruct] Need a 'model_type' parameter to be set before continuing!"); + return; + } + // ---[ Optional parameters + // True if all data will be returned, false if only the projected inliers. Default: false. + declare_parameter("copy_all_data", rclcpp::ParameterValue(false)); + bool copy_all_data = get_parameter("copy_all_data").as_bool(); + + // True if all fields will be returned, false if only XYZ. Default: true. + declare_parameter("copy_all_fields", rclcpp::ParameterValue(true)); + bool copy_all_fields = get_parameter("copy_all_fields").as_bool(); + + pub_output_ = create_publisher("output", max_queue_size_); + + RCLCPP_DEBUG( + this->get_logger(), + "[onConstruct] Node successfully created with the following parameters:\n" + " - model_type : %d\n" + " - copy_all_data : %s\n" + " - copy_all_fields : %s", + 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); + + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); +} + +void +pcl_ros::ProjectInliers::filter( + const PointCloud2::ConstSharedPtr & 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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ProjectInliers::subscribe() +{ + RCLCPP_DEBUG(get_logger(), "subscribe"); +/* + TODO : implement use_indices_ + if (use_indices_) + {*/ + + auto qos_profile = rclcpp::QoS( + rclcpp::KeepLast(max_queue_size_), + rmw_qos_profile_default).get_rmw_qos_profile(); + auto sensor_qos_profile = rclcpp::QoS( + rclcpp::KeepLast(max_queue_size_), + rmw_qos_profile_sensor_data).get_rmw_qos_profile(); + sub_input_filter_.subscribe(this, "input", sensor_qos_profile); + sub_indices_filter_.subscribe(this, "indices", qos_profile); + sub_model_.subscribe(this, "model", qos_profile); + + if (approximate_sync_) { + sync_input_indices_model_a_ = std::make_shared< + message_filters::Synchronizer< + message_filters::sync_policies::ApproximateTime< + PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_); + sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); + sync_input_indices_model_a_->registerCallback( + std::bind( + &ProjectInliers::input_indices_model_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } else { + sync_input_indices_model_e_ = std::make_shared< + message_filters::Synchronizer< + message_filters::sync_policies::ExactTime< + PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_); + sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); + sync_input_indices_model_e_->registerCallback( + std::bind( + &ProjectInliers::input_indices_model_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ProjectInliers::unsubscribe() +{ +/* + TODO : implement use_indices_ + if (use_indices_) + {*/ + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + sub_model_.unsubscribe(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ProjectInliers::input_indices_model_callback( + const PointCloud2::ConstSharedPtr & cloud, + const PointIndicesConstPtr & indices, + const ModelCoefficientsConstPtr & model) +{ + if (pub_output_->get_subscription_count() == 0) { + return; + } + + if (!isValid(model) || !isValid(indices) || !isValid(cloud)) { + RCLCPP_ERROR( + this->get_logger(), "[%s::input_indices_model_callback] Invalid input!", this->get_name()); + return; + } + + RCLCPP_DEBUG( + this->get_logger(), + "[%s::input_indices_model_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.\n" + " - ModelCoefficients with %zu values, stamp %d.%09d, and frame %s on topic %s received.", + this->get_name(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).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(), "inliers", model->values.size(), + model->header.stamp.sec, model->header.stamp.nanosec, model->header.frame_id.c_str(), "model"); + + tf_input_orig_frame_ = cloud->header.frame_id; + + IndicesPtr vindices; + if (indices) { + vindices.reset(new std::vector(indices->indices)); + } + + model_ = model; + computePublish(cloud, vindices); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ProjectInliers) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp new file mode 100644 index 00000000..77e22579 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -0,0 +1,133 @@ +/* + * 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 "pcl_ros/filters/radius_outlier_removal.hpp" + +pcl_ros::RadiusOutlierRemoval::RadiusOutlierRemoval(const rclcpp::NodeOptions & options) +: Filter("RadiusOutlierRemovalNode", options) +{ + rcl_interfaces::msg::ParameterDescriptor min_neighbors_desc; + min_neighbors_desc.name = "min_neighbors"; + min_neighbors_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + min_neighbors_desc.description = + "The number of neighbors that need to be present in order to be classified as an inlier."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 0; + int_range.to_value = 1000; + min_neighbors_desc.integer_range.push_back(int_range); + } + declare_parameter(min_neighbors_desc.name, rclcpp::ParameterValue(5), min_neighbors_desc); + + rcl_interfaces::msg::ParameterDescriptor radius_search_desc; + radius_search_desc.name = "radius_search"; + radius_search_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + radius_search_desc.description = + "Radius of the sphere that will determine which points are neighbors."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 10.0; + radius_search_desc.floating_point_range.push_back(float_range); + } + declare_parameter(radius_search_desc.name, rclcpp::ParameterValue(0.1), radius_search_desc); + + const std::vector param_names { + min_neighbors_desc.name, + radius_search_desc.name, + }; + + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &RadiusOutlierRemoval::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); +} + +void +pcl_ros::RadiusOutlierRemoval::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard 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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::RadiusOutlierRemoval::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "min_neighbors") { + if (impl_.getMinNeighborsInRadius() != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), "Setting the number of neighbors in radius: %ld.", + param.as_int()); + impl_.setMinNeighborsInRadius(param.as_int()); + } + } + if (param.get_name() == "radius_search") { + if (impl_.getRadiusSearch() != param.as_double()) { + RCLCPP_DEBUG( + get_logger(), "Setting the radius to search neighbors: %f.", + param.as_double()); + impl_.setRadiusSearch(param.as_double()); + } + } + } + + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::RadiusOutlierRemoval) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp new file mode 100644 index 00000000..0285d12a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -0,0 +1,153 @@ +/* + * 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 "pcl_ros/filters/statistical_outlier_removal.hpp" + +pcl_ros::StatisticalOutlierRemoval::StatisticalOutlierRemoval(const rclcpp::NodeOptions & options) +: Filter("StatisticalOutlierRemovalNode", options) +{ + rcl_interfaces::msg::ParameterDescriptor mean_k_desc; + mean_k_desc.name = "mean_k"; + mean_k_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + mean_k_desc.description = + "The number of points (k) to use for mean distance estimation."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 2; + int_range.to_value = 100; + mean_k_desc.integer_range.push_back(int_range); + } + declare_parameter(mean_k_desc.name, rclcpp::ParameterValue(2), mean_k_desc); + + rcl_interfaces::msg::ParameterDescriptor stddev_desc; + stddev_desc.name = "stddev"; + stddev_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + stddev_desc.description = + "The standard deviation multiplier threshold." + "All points outside the mean +- sigma * std_mul will be considered outliers."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 5.0; + stddev_desc.floating_point_range.push_back(float_range); + } + declare_parameter(stddev_desc.name, rclcpp::ParameterValue(0.0), stddev_desc); + + rcl_interfaces::msg::ParameterDescriptor negative_desc; + negative_desc.name = "negative"; + negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + negative_desc.description = + "Set whether the inliers should be returned (true) or the outliers (false)."; + declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc); + + const std::vector param_names { + mean_k_desc.name, + stddev_desc.name, + negative_desc.name, + }; + + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &StatisticalOutlierRemoval::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); +} + +void +pcl_ros::StatisticalOutlierRemoval::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard 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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::StatisticalOutlierRemoval::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "mean_k") { + if (impl_.getMeanK() != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the number of points (k) to use for mean distance estimation to: %ld.", + param.as_int()); + impl_.setMeanK(param.as_int()); + } + } + if (param.get_name() == "stddev") { + if (impl_.getStddevMulThresh() != param.as_double()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the standard deviation multiplier threshold to: %f.", + param.as_double()); + impl_.setStddevMulThresh(param.as_double()); + } + } + if (param.get_name() == "negative") { + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), + "Returning only inliers: %s.", + (param.as_bool() ? "false" : "true")); + impl_.setNegative(param.as_bool()); + } + } + } + + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::StatisticalOutlierRemoval) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp new file mode 100644 index 00000000..d8f5844c --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -0,0 +1,192 @@ +/* + * 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 "pcl_ros/filters/voxel_grid.hpp" + +////////////////////////////////////////////////////////////////////////////////////////////// + +pcl_ros::VoxelGrid::VoxelGrid(const rclcpp::NodeOptions & options) +: Filter("VoxelGridNode", options) +{ + std::vector common_param_names = add_common_params(); + + rcl_interfaces::msg::ParameterDescriptor leaf_size_desc; + leaf_size_desc.name = "leaf_size"; + leaf_size_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + leaf_size_desc.description = + "The size of a leaf (on x,y,z) used for downsampling"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 1.0; + leaf_size_desc.floating_point_range.push_back(float_range); + } + declare_parameter(leaf_size_desc.name, rclcpp::ParameterValue(0.01), leaf_size_desc); + + rcl_interfaces::msg::ParameterDescriptor min_points_per_voxel_desc; + min_points_per_voxel_desc.name = "min_points_per_voxel"; + min_points_per_voxel_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + min_points_per_voxel_desc.description = + "The minimum number of points required for a voxel to be used."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 1; + int_range.to_value = 100000; + min_points_per_voxel_desc.integer_range.push_back(int_range); + } + declare_parameter( + min_points_per_voxel_desc.name, rclcpp::ParameterValue(2), min_points_per_voxel_desc); + + std::vector param_names { + leaf_size_desc.name, + min_points_per_voxel_desc.name, + }; + param_names.insert(param_names.end(), common_param_names.begin(), common_param_names.end()); + + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &VoxelGrid::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); +} + +void +pcl_ros::VoxelGrid::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard 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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::VoxelGrid::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); + + double filter_min, filter_max; + impl_.getFilterLimits(filter_min, filter_max); + + Eigen::Vector3f leaf_size = impl_.getLeafSize(); + + unsigned int minPointsPerVoxel = impl_.getMinimumPointsNumberPerVoxel(); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "filter_field_name") { + // Check the current value for the filter field + if (impl_.getFilterFieldName() != param.as_string()) { + // Set the filter field if different + impl_.setFilterFieldName(param.as_string()); + RCLCPP_DEBUG( + get_logger(), "Setting the filter field name to: %s.", + param.as_string().c_str()); + } + } + if (param.get_name() == "filter_limit_min") { + // Check the current values for filter min-max + if (filter_min != param.as_double()) { + filter_min = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum filtering value a point will be considered from to: %f.", + filter_min); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_max") { + // Check the current values for filter min-max + if (filter_max != param.as_double()) { + filter_max = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the maximum filtering value a point will be considered from to: %f.", + filter_max); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_negative") { + bool new_filter_limits_negative = param.as_bool(); + if (impl_.getFilterLimitsNegative() != new_filter_limits_negative) { + RCLCPP_DEBUG( + get_logger(), + "Setting the filter negative flag to: %s.", + (new_filter_limits_negative ? "true" : "false")); + impl_.setFilterLimitsNegative(new_filter_limits_negative); + } + } + if (param.get_name() == "min_points_per_voxel") { + if (minPointsPerVoxel != ((unsigned int) param.as_int())) { + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum points per voxel to: %u.", + minPointsPerVoxel); + impl_.setMinimumPointsNumberPerVoxel(param.as_int()); + } + } + if (param.get_name() == "leaf_size") { + leaf_size.setConstant(param.as_double()); + if (impl_.getLeafSize() != leaf_size) { + RCLCPP_DEBUG( + get_logger(), "Setting the downsampling leaf size to: %f %f %f.", + leaf_size[0], leaf_size[1], leaf_size[2]); + impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]); + } + } + } + + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::VoxelGrid) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/bag_io.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/bag_io.cpp new file mode 100644 index 00000000..74ca8d6b --- /dev/null +++ b/ros2_ws/src/perception_pcl/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 +#include "pcl_ros/io/bag_io.hpp" + +////////////////////////////////////////////////////////////////////////////////////////////// +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_EXPORT_CLASS(BAGReader, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/concatenate_data.cpp new file mode 100644 index 00000000..fe03b09b --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -0,0 +1,304 @@ +/* + * 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 +#include +#include "pcl_ros/transforms.hpp" +#include "pcl_ros/io/concatenate_data.hpp" + + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateDataSynchronizer::onInit() +{ + nodelet_topic_tools::NodeletLazy::onInit(); + + // ---[ Mandatory parameters + 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!"); + return; + } + + 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!"); + 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 (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_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)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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) +{ + // 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); + // 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) +{ + 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); + 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); + out1 = out2; + } + } + } + } + } + pub_output_.publish(boost::make_shared(*out1)); +} + +typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer; +PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp new file mode 100644 index 00000000..8d933e25 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -0,0 +1,176 @@ +/* + * 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 +#include +#include "pcl_ros/io/concatenate_fields.hpp" + + +/////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateFieldsSynchronizer::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!"); + return; + } + 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_); + + 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(); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +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(), + 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) + { + 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 (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]; + + // 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 (static_cast(queue_.size()) > maximum_queue_size_) { + queue_.erase(queue_.begin()); + } + } +} + +typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer; +PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/io.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/io.cpp new file mode 100644 index 00000000..6787b832 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/io.cpp @@ -0,0 +1,58 @@ +/* + * 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_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet); +// PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/pcd_io.cpp new file mode 100644 index 00000000..33985cbb --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -0,0 +1,190 @@ +/* + * 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 +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PCDReader::onInit() +{ + PCLNodelet::onInit(); + // Provide a latched topic + ros::Publisher pub_output = pnh_->advertise("output", max_queue_size_, true); + + 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()); + + 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 (pnh_->ok() && pub_output.getNumSubscribers() == 0); + + std::string file_name; + + 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(); + 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_; + 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; + } + pcl_conversions::moveFromPCL(cloud, *(output_)); + 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 + pnh_->getParam("publish_rate", publish_rate_); + pnh_->getParam("tf_frame", tf_frame_); + } + + onInitPostProcess(); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PCDWriter::onInit() +{ + 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_); + + 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(); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PCDWriter::input_callback(const PointCloud2ConstPtr & cloud) +{ + if (!isValid(cloud)) { + return; + } + + 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()); + + std::string fname; + 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_); + + 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); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp new file mode 100644 index 00000000..da5e0d7a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -0,0 +1,284 @@ +/* + * 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 +#include +#include +#include "pcl_ros/segmentation/extract_clusters.hpp" + + +using pcl_conversions::fromPCL; +using pcl_conversions::moveFromPCL; +using pcl_conversions::toPCL; + +////////////////////////////////////////////////////////////////////////////////////////////// +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_ = 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); + + 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_) { + // 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())); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::EuclideanClusterExtraction::unsubscribe() +{ + if (use_indices_) { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } else { + sub_input_.shutdown(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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) { + 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, 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)); + } + + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices_ptr); + + std::vector clusters; + impl_.extract(clusters); + + if (publish_indices_) { + for (size_t i = 0; i < clusters.size(); ++i) { + if (static_cast(i) >= max_clusters_) { + break; + } + // 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); + 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 (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(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); + // 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()); + } + } +} + +typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction; +PLUGINLIB_EXPORT_CLASS(EuclideanClusterExtraction, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp new file mode 100644 index 00000000..bfd9fe55 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -0,0 +1,261 @@ +/* + * 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 +#include +#include +#include "pcl_ros/transforms.hpp" +#include "pcl_ros/segmentation/extract_polygonal_prism_data.hpp" + +using pcl_conversions::moveFromPCL; +using pcl_conversions::moveToPCL; + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ExtractPolygonalPrismData::onInit() +{ + // Call the super 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); + + // 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_); + } 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)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ExtractPolygonalPrismData::unsubscribe() +{ + sub_hull_filter_.unsubscribe(); + sub_input_filter_.unsubscribe(); + + if (use_indices_) { + sub_indices_filter_.unsubscribe(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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_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); + 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); + 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 (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(inliers); + return; + } + impl_.setInputPlanarHull(pcl_ptr(planar_hull.makeShared())); + } else { + 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(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()) { + pcl::PointIndices pcl_inliers; + moveToPCL(inliers, 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()); +} + +typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData; +PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp new file mode 100644 index 00000000..bcc128c4 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -0,0 +1,784 @@ +/* + * 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 +#include +#include +#include "pcl_ros/segmentation/sac_segmentation.hpp" + +using pcl_conversions::fromPCL; + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::onInit() +{ + // Call the super onInit () + PCLNodelet::onInit(); + + + // Advertise the output topics + 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!"); + 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); + + 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)); + } 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_->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(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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_); + + pcl_msgs::PointIndices inliers; + pcl_msgs::ModelCoefficients model; + // Enforce that the TF frame and the timestamp are copied + 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); + 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); + 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()); + } + /// + + // 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(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()) { + 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 + + // Check if we have enough inliers, clear inliers + model if not + if (static_cast(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); + + // Advertise the output topics + 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()); + 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); + + 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(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::axis_callback( + const pcl_msgs::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_); + + 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)); + 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(), 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_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(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)); + } + + 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()) { + 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 (static_cast(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_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp new file mode 100644 index 00000000..0ee82452 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -0,0 +1,163 @@ +/* + * 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 +#include "pcl_ros/segmentation/segment_differences.hpp" + +/////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SegmentDifferences::onInit() +{ + // Call the super onInit () + PCLNodelet::onInit(); + + 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_); + + // 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)); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SegmentDifferences::unsubscribe() +{ + sub_input_filter_.unsubscribe(); + sub_target_filter_.unsubscribe(); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +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(ros_ptr(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(), 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); + + 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/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp new file mode 100644 index 00000000..244ecc94 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp @@ -0,0 +1,42 @@ +/* + * 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/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/convex_hull.cpp new file mode 100644 index 00000000..1edca75a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -0,0 +1,221 @@ +/* + * 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 +#include +#include "pcl_ros/surface/convex_hull.hpp" + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ConvexHull2D::onInit() +{ + 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_); + + 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); + // 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())); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ConvexHull2D::unsubscribe() +{ + if (use_indices_) { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } else { + sub_input_.shutdown(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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(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()); + // Publish an empty message + output.header = cloud->header; + 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()); + } + + // Reset the indices and surface pointers + IndicesPtr indices_ptr; + if (indices) { + indices_ptr.reset(new std::vector(indices->indices)); + } + + impl_.setInputCloud(pcl_ptr(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 = fromPCL(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(ros_ptr(output.makeShared())); +} + +typedef pcl_ros::ConvexHull2D ConvexHull2D; +PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp new file mode 100644 index 00000000..8701935c --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -0,0 +1,274 @@ +/* + * 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 +#include +#include "pcl_ros/surface/moving_least_squares.hpp" + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::MovingLeastSquares::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_)) { + // 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_); + + 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_) { + // 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())); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +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) { + 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(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()); + output.header = cloud->header; + 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()); + } + /// + + // Reset the indices and surface pointers + impl_.setInputCloud(pcl_ptr(cloud)); + + IndicesPtr indices_ptr; + if (indices) { + indices_ptr.reset(new std::vector(indices->indices)); + } + + 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())); + normals->header = cloud->header; + pub_normals_.publish(ros_ptr(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_); +#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; + 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_EXPORT_CLASS(MovingLeastSquares, nodelet::Nodelet) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/surface.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/surface.cpp new file mode 100644 index 00000000..09e3393f --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/surface/surface.cpp @@ -0,0 +1,46 @@ +/* + * 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" diff --git a/ros2_ws/src/perception_pcl/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/test/test_tf_message_filter_pcl.cpp new file mode 100644 index 00000000..a32d7511 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -0,0 +1,423 @@ +/* + * 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 Brice Rebsamen + * Copied and adapted from geometry/test_message_filter.cpp + */ + +#include +#include + +#include +#include +#include + +#include + +#include + +#include + +#include +#include +#include + +#include +#include + +#include + +// 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, std::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: + explicit 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, tf::FilterFailureReason reason) + { + ++failure_count_; + } + + int count_; + int expected_count_; + int failure_count_; +}; + +TEST(MessageFilter, noTransforms) +{ + tf::TransformListener tf_client; + Notification n(1); + tf::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); + tf::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); + tf::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); + tf::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); + tf::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(); + std::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); + tf::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); + tf::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); + tf::MessageFilter filter(tf_client, "frame1", 1); + filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + filter.setTolerance(offset); + + 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_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); + tf::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); + tf::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/ros2_ws/src/perception_pcl/pcl_ros/src/transforms.cpp b/ros2_ws/src/perception_pcl/pcl_ros/src/transforms.cpp new file mode 100644 index 00000000..95279969 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/src/transforms.cpp @@ -0,0 +1,478 @@ +/* + * 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 "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::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out, const tf2_ros::Buffer & tf_buffer) +{ + if (in.header.frame_id == target_frame) { + out = in; + return true; + } + + // Get the TF transform + geometry_msgs::msg::TransformStamped transform; + try { + 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 (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%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 tf2::Transform & net_transform, + const sensor_msgs::msg::PointCloud2 & in, sensor_msgs::msg::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 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::msg::PointCloud2 & in, + sensor_msgs::msg::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) { + 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::msg::PointField::FLOAT32 || + in.fields[y_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || + in.fields[z_idx].datatype != sensor_msgs::msg::PointField::FLOAT32) + { + RCLCPP_ERROR( + rclcpp::get_logger("pcl_ros"), + "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.data(), in.data.data(), 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(*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; + 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; + } 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; + } + } else { + pt_out = transform * pt; + } + + if (max_range_point) { + // Save x value in distance again + *reinterpret_cast(&out.data[distance_ptr_offset]) = pt_out[0]; + pt_out[0] = std::numeric_limits::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 = + 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; + + pstep[0] = vp_out[0]; + pstep[1] = vp_out[1]; + pstep[2] = vp_out[2]; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +transformAsMatrix(const tf2::Transform & bt, Eigen::Matrix4f & out_mat) +{ + double mv[12]; + bt.getBasis().getOpenGLSubMatrix(mv); + + tf2::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(); +} + +void +transformAsMatrix(const geometry_msgs::msg::TransformStamped & bt, Eigen::Matrix4f & out_mat) +{ + tf2::Transform transform; + tf2::convert(bt.transform, transform); + transformAsMatrix(transform, out_mat); +} +} // namespace pcl_ros + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + 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 tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf2::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + 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 tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, + pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf2_ros::Buffer &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloud( + const std::string &, const rclcpp::Time &, + const pcl::PointCloud &, + const std::string &, + pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, + const std::string &, + pcl::PointCloud &, + const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const rclcpp::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf2_ros::Buffer &); diff --git a/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/CMakeLists.txt b/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/CMakeLists.txt new file mode 100644 index 00000000..669b94eb --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/CMakeLists.txt @@ -0,0 +1,112 @@ +find_package(ament_cmake_pytest REQUIRED) + +# build dummy publisher node and component +add_library(dummy_topics SHARED + dummy_topics.cpp +) +target_link_libraries(dummy_topics ${PCL_LIBRARIES}) +ament_target_dependencies(dummy_topics + rclcpp + rclcpp_components + pcl_conversions + sensor_msgs + PCL +) + +# generate test ament index to locate dummy_topics comonent from tests +set(components "") +set(components "${components}pcl_ros_tests_filters::DummyTopics;$\n") +file(GENERATE +OUTPUT +"${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/share/ament_index/resource_index/rclcpp_components/pcl_ros_tests_filters" +CONTENT "${components}") + +# test components +ament_add_pytest_test(test_pcl_ros::ExtractIndices + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::ExtractIndices + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::PassThrough + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::PassThrough + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::ProjectInliers + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::ProjectInliers + PARAMETERS={'model_type':0} + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::RadiusOutlierRemoval + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::RadiusOutlierRemoval + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::StatisticalOutlierRemoval + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::StatisticalOutlierRemoval + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::CropBox + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::CropBox + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::VoxelGrid + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::VoxelGrid + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) + +# test executables +ament_add_pytest_test(test_filter_extract_indices_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_extract_indices_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_passthrough_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_passthrough_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_project_inliers_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_project_inliers_node + PARAMETERS={'model_type':0} + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_radius_outlier_removal_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_radius_outlier_removal_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_statistical_outlier_removal_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_statistical_outlier_removal_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_crop_box_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_crop_box_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_voxel_grid_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_voxel_grid_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/dummy_topics.cpp b/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/dummy_topics.cpp new file mode 100644 index 00000000..0611ff8c --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/dummy_topics.cpp @@ -0,0 +1,109 @@ +/* + * Copyright (c) 2022 Carnegie Mellon University + * 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 Carnegie Mellon University 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 + +using namespace std::chrono_literals; + +namespace pcl_ros_tests_filters +{ +class DummyTopics : public rclcpp::Node +{ +public: + explicit DummyTopics(const rclcpp::NodeOptions & options); + +private: + void timer_callback(); + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr point_cloud2_pub_; + rclcpp::Publisher::SharedPtr indices_pub_; + rclcpp::Publisher::SharedPtr model_pub_; + size_t count_; +}; + +DummyTopics::DummyTopics(const rclcpp::NodeOptions & options) +: Node("dummy_point_cloud2_publisher", options), count_(0) +{ + point_cloud2_pub_ = this->create_publisher("point_cloud2", 10); + indices_pub_ = this->create_publisher("indices", 10); + model_pub_ = this->create_publisher("model", 10); + timer_ = this->create_wall_timer( + 100ms, std::bind(&DummyTopics::timer_callback, this)); +} + +void DummyTopics::timer_callback() +{ + builtin_interfaces::msg::Time now_msg; + now_msg = get_clock()->now(); + + sensor_msgs::msg::PointCloud2 point_cloud2_msg; + + pcl::PointCloud random_pcl; + + unsigned int global_seed = 100; + for (int v = 0; v < 1000; ++v) { + pcl::PointXYZ newPoint; + newPoint.x = (rand_r(&global_seed) * 100.0) / RAND_MAX; + newPoint.y = (rand_r(&global_seed) * 100.0) / RAND_MAX; + newPoint.z = (rand_r(&global_seed) * 100.0) / RAND_MAX; + random_pcl.points.push_back(newPoint); + } + + // publish point cloud + pcl::toROSMsg(random_pcl, point_cloud2_msg); + point_cloud2_msg.header.stamp = now_msg; + point_cloud2_pub_->publish(point_cloud2_msg); + + // publish indices + pcl_msgs::msg::PointIndices indices_msg; + indices_msg.header.stamp = now_msg; + indices_msg.indices.push_back(0); + indices_pub_->publish(indices_msg); + + // publish model + pcl_msgs::msg::ModelCoefficients model_msg; + model_msg.header.stamp = now_msg; + model_msg.values.push_back(0); + model_msg.values.push_back(0); + model_msg.values.push_back(1); + model_msg.values.push_back(0); + model_pub_->publish(model_msg); +} + +} // namespace pcl_ros_tests_filters + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros_tests_filters::DummyTopics) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/test_filter_component.py b/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/test_filter_component.py new file mode 100644 index 00000000..2e01e0e8 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/test_filter_component.py @@ -0,0 +1,85 @@ +# +# Copyright (c) 2022 Carnegie Mellon University +# 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 Carnegie Mellon University 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. +# + +import ast +import os +import unittest + +import launch +import launch.actions +import launch_ros.actions +import launch_testing.actions +import launch_testing.markers +from launch_testing_ros import WaitForTopics +import pytest +from sensor_msgs.msg import PointCloud2 + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + dummy_plugin = os.getenv('DUMMY_PLUGIN') + filter_plugin = os.getenv('FILTER_PLUGIN') + parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {} + + print(parameters) + + return launch.LaunchDescription([ + launch_ros.actions.ComposableNodeContainer( + name='filter_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package='pcl_ros_tests_filters', + plugin=dummy_plugin, + name='dummy_publisher', + ), + launch_ros.descriptions.ComposableNode( + package='pcl_ros', + plugin=filter_plugin, + name='filter_node', + remappings=[('/input', '/point_cloud2')], + parameters=[parameters], + ), + ], + output='screen', + ), + launch_testing.actions.ReadyToTest() + ]) + + +class TestFilter(unittest.TestCase): + + def test_filter_output(self): + wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0) + assert wait_for_topics.wait() + assert 'output' in wait_for_topics.topics_received(), "Didn't receive message" + wait_for_topics.shutdown() diff --git a/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/test_filter_executable.py b/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/test_filter_executable.py new file mode 100644 index 00000000..b5e8e7c9 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tests/filters/test_filter_executable.py @@ -0,0 +1,87 @@ +# +# Copyright (c) 2022 Carnegie Mellon University +# 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 Carnegie Mellon University 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. +# + +import ast +import os +import unittest + +import launch +import launch.actions +import launch_ros.actions +import launch_testing.actions +import launch_testing.markers +from launch_testing_ros import WaitForTopics +import pytest +from sensor_msgs.msg import PointCloud2 + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + dummy_plugin = os.getenv('DUMMY_PLUGIN') + filter_executable = os.getenv('FILTER_EXECUTABLE') + parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {} + + ros_arguments = ['-r', 'input:=point_cloud2'] + for key in parameters.keys(): + ros_arguments.extend(['-p', '{}:={}'.format(key, parameters[key])]) + + return launch.LaunchDescription([ + + launch_ros.actions.ComposableNodeContainer( + name='filter_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package='pcl_ros_tests_filters', + plugin=dummy_plugin, + name='dummy_publisher', + ), + ], + output='screen', + ), + launch_ros.actions.Node( + package='pcl_ros', + executable=filter_executable, + output='screen', + ros_arguments=ros_arguments + ), + launch_testing.actions.ReadyToTest() + ]) + + +class TestFilter(unittest.TestCase): + + def test_filter_output(self): + wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0) + assert wait_for_topics.wait() + assert 'output' in wait_for_topics.topics_received(), "Didn't receive message" + wait_for_topics.shutdown() diff --git a/ros2_ws/src/perception_pcl/pcl_ros/tests/test_tf_message_filter_pcl.launch b/ros2_ws/src/perception_pcl/pcl_ros/tests/test_tf_message_filter_pcl.launch new file mode 100644 index 00000000..501b0190 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tests/test_tf_message_filter_pcl.launch @@ -0,0 +1,3 @@ + + + diff --git a/ros2_ws/src/perception_pcl/pcl_ros/tools/bag_to_pcd.cpp b/ros2_ws/src/perception_pcl/pcl_ros/tools/bag_to_pcd.cpp new file mode 100644 index 00000000..a06d7305 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tools/bag_to_pcd.cpp @@ -0,0 +1,159 @@ +/* + * 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 + +#include +#include +#include +#include + +#include +#include "pcl_ros/transforms.hpp" + +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 /base_link" << + 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.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::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; + } + + // If a target_frame was specified + if (argc > 4) { + // 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; + + 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/ros2_ws/src/perception_pcl/pcl_ros/tools/convert_pcd_to_image.cpp b/ros2_ws/src/perception_pcl/pcl_ros/tools/convert_pcd_to_image.cpp new file mode 100644 index 00000000..586c3b1a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tools/convert_pcd_to_image.cpp @@ -0,0 +1,93 @@ +/* + * 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 +#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/ros2_ws/src/perception_pcl/pcl_ros/tools/convert_pointcloud_to_image.cpp b/ros2_ws/src/perception_pcl/pcl_ros/tools/convert_pointcloud_to_image.cpp new file mode 100644 index 00000000..c692f506 --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -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: surface_convex_hull.cpp 34612 2010-12-08 01:06:27Z rusu $ + * + */ + +/** + \author Ethan Rublee + **/ +// ROS core +#include +// Image message +#include +#include +// pcl::toROSMsg +#include +// conversions from PCL custom types +#include +// stl stuff +#include + +class PointCloudToImage +{ +public: + void + cloud_cb(const sensor_msgs::PointCloud2ConstPtr & cloud) + { + if (cloud->height <= 1) { + ROS_ERROR("Input point cloud is not organized, ignoring!"); + return; + } + 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()); + } + } + 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/ros2_ws/src/perception_pcl/pcl_ros/tools/pcd_to_pointcloud.cpp b/ros2_ws/src/perception_pcl/pcl_ros/tools/pcd_to_pointcloud.cpp new file mode 100644 index 00000000..cb21a24d --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tools/pcd_to_pointcloud.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: 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. + + **/ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include "rclcpp_components/register_node_macro.hpp" +#include + +namespace pcl_ros +{ +class PCDPublisher : public rclcpp::Node +{ +protected: + std::string tf_frame_; + +public: + // ROS messages + sensor_msgs::msg::PointCloud2 cloud_; + + std::string file_name_, cloud_topic_; + size_t period_ms_; + + std::shared_ptr> pub_; + rclcpp::TimerBase::SharedPtr timer_; + + //////////////////////////////////////////////////////////////////////////////// + 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"; + 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"); + + if (file_name_ == "" || pcl::io::loadPCDFile(file_name_, cloud_) == -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_; + 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()); + } + + //////////////////////////////////////////////////////////////////////////////// + // Publish callback that is periodically called by a timer. + void publish() + { + cloud_.header.stamp = this->get_clock()->now(); + pub_->publish(cloud_); + } +}; +} // namespace pcl_ros + +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::PCDPublisher) diff --git a/ros2_ws/src/perception_pcl/pcl_ros/tools/pointcloud_to_pcd.cpp b/ros2_ws/src/perception_pcl/pcl_ros/tools/pointcloud_to_pcd.cpp new file mode 100644 index 00000000..9f2f9c9a --- /dev/null +++ b/ros2_ws/src/perception_pcl/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -0,0 +1,184 @@ +/* + * 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 + +#include + +#include +#include +#include + +// PCL includes +#include +#include +#include + +#include + +#include + +// STL +#include + +/** +\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_; + bool binary_; + bool compressed_; + std::string fixed_frame_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + +public: + std::string cloud_topic_; + + ros::Subscriber sub_; + + //////////////////////////////////////////////////////////////////////////////// + // 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; + } + + 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); + } + } + + //////////////////////////////////////////////////////////////////////////////// + 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) +{ + ros::init(argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName); + + PointCloudToPCD b; + ros::spin(); + + return 0; +} +/* ]--- */ diff --git a/ros2_ws/src/perception_pcl/perception_pcl/CHANGELOG.rst b/ros2_ws/src/perception_pcl/perception_pcl/CHANGELOG.rst new file mode 100644 index 00000000..f5f3523c --- /dev/null +++ b/ros2_ws/src/perception_pcl/perception_pcl/CHANGELOG.rst @@ -0,0 +1,191 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package perception_pcl +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.6.2 (2018-05-20) +------------------ + +1.6.1 (2018-05-08) +------------------ +* Add 1.6.0 section to CHANGELOG.rst +* Contributors: Kentaro Wada + +1.6.0 (2018-04-30) +------------------ + +* Fix build and update maintainers +* Contributors: Paul Bovbel, Kentaro Wada + +1.5.4 (2018-03-31) +------------------ + +1.5.3 (2017-05-03) +------------------ + +1.5.2 (2017-04-29) +------------------ + +1.5.1 (2017-04-26) +------------------ +* Add my name as a maintainer +* Contributors: Kentaro Wada + +1.5.0 (2017-04-25) +------------------ + +1.4.0 (2016-04-22) +------------------ + +1.3.0 (2015-06-22) +------------------ +* Remove pointcloud_to_laserscan package +* Contributors: Paul Bovbel + +1.2.6 (2015-02-04) +------------------ + +1.2.5 (2015-01-20) +------------------ + +1.2.4 (2015-01-15) +------------------ + +1.2.3 (2015-01-10) +------------------ +* clean up package.xml +* Contributors: Paul Bovbel + +1.2.2 (2014-10-25) +------------------ + +1.2.1 (2014-09-13) +------------------ + +1.2.0 (2014-04-09) +------------------ +* Updated maintainership and added bugtracker/repository info to package.xml + +1.1.7 (2013-09-20) +------------------ + +1.1.6 (2013-09-20) +------------------ + +1.1.5 (2013-08-27) +------------------ +* Updated package.xml's to use new libpcl-all rosdep rules + +1.1.4 (2013-07-23) +------------------ + +1.1.2 (2013-07-19) +------------------ + +1.1.1 (2013-07-10) +------------------ +* No changes + +1.1.0 (2013-07-09) +------------------ +* No changes + +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 diff --git a/ros2_ws/src/perception_pcl/perception_pcl/CMakeLists.txt b/ros2_ws/src/perception_pcl/perception_pcl/CMakeLists.txt new file mode 100644 index 00000000..c97773ad --- /dev/null +++ b/ros2_ws/src/perception_pcl/perception_pcl/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 3.5) +project(perception_pcl) +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/ros2_ws/src/perception_pcl/perception_pcl/package.xml b/ros2_ws/src/perception_pcl/perception_pcl/package.xml new file mode 100644 index 00000000..c0737fae --- /dev/null +++ b/ros2_ws/src/perception_pcl/perception_pcl/package.xml @@ -0,0 +1,35 @@ + + + + perception_pcl + 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 + processing in ROS. + + + Open Perception + William Woodall + Julius Kammerl + + Paul Bovbel + Steve Macenski + Kentaro Wada + + BSD + + http://ros.org/wiki/perception_pcl + https://github.com/ros-perception/perception_pcl/issues + https://github.com/ros-perception/perception_pcl + + ament_cmake + + pcl_conversions + pcl_msgs + pcl_ros + + + ament_cmake + +