pax_global_header00006660000000000000000000000064142006106200014501gustar00rootroot0000000000000052 comment=7230ef005267fccb1305e4c8235e3d5a206abc33 perception_pcl-1.7.4/000077500000000000000000000000001420061062000145205ustar00rootroot00000000000000perception_pcl-1.7.4/.gitignore000066400000000000000000000000061420061062000165040ustar00rootroot00000000000000*.pyc perception_pcl-1.7.4/.travis.yml000066400000000000000000000012501420061062000166270ustar00rootroot00000000000000language: generic services: - docker cache: directories: - $HOME/.ccache env: global: - ROS_DISTRO="melodic" - ROS_REPO=ros - CCACHE_DIR=$HOME/.ccache # To avoid memory error on Travis. - ROS_PARALLEL_JOBS=-j1 - ROS_PARALLEL_TEST_JOBS=-j1 # travis build will time out with no output unless we use verbose output - VERBOSE_OUTPUT=true - VERBOSE_TESTS=true matrix: - OS_NAME=ubuntu OS_CODE_NAME=bionic - OS_NAME=debian OS_CODE_NAME=stretch install: - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci script: - .industrial_ci/travis.sh branches: only: - /.*-devel$/ perception_pcl-1.7.4/README.md000066400000000000000000000005111420061062000157740ustar00rootroot00000000000000# 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. perception_pcl-1.7.4/pcl_conversions/000077500000000000000000000000001420061062000177265ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_conversions/CHANGELOG.rst000066400000000000000000000050361420061062000217530ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package pcl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.7.0 (2019-07-03) ------------------ * [Windows][melodic-devel] Fix Boost linkage issue and binary install location. (`#238 `_) * fix Windows build break. (`#6 `_) * Fix install path. (`#7 `_) * Contributors: Sean Yen 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 perception_pcl-1.7.4/pcl_conversions/CMakeLists.txt000066400000000000000000000027351420061062000224750ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(pcl_conversions) find_package(catkin REQUIRED COMPONENTS) find_package(PCL REQUIRED COMPONENTS common io) find_package(Eigen3 REQUIRED) # There is a bug in the Ubuntu Artful (17.10) version of the VTK package, # where it includes /usr/include/*-linux-gnu/freetype2 in the include # directories (which doesn't exist). This filters down to the PCL_INCLUDE_DIRS, # and causes downstream projects trying to use these libraries to fail to # configure properly. Here we remove those bogus entries so that downstream # consumers of this package succeed. if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") foreach(item ${PCL_INCLUDE_DIRS}) string(REGEX MATCH "/usr/include/.*-linux-gnu/freetype2" item ${item}) if(item) list(REMOVE_ITEM PCL_INCLUDE_DIRS ${item}) endif() endforeach() endif() catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS pcl_msgs roscpp sensor_msgs std_msgs DEPENDS EIGEN3 PCL ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) if(CATKIN_ENABLE_TESTING) find_package(Boost REQUIRED COMPONENTS iostreams) find_package(catkin REQUIRED COMPONENTS roscpp pcl_msgs sensor_msgs std_msgs) include_directories( include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) catkin_add_gtest(test_pcl_conversions test/test_pcl_conversions.cpp) target_link_libraries(test_pcl_conversions ${catkin_LIBRARIES} ${Boost_LIBRARIES}) endif() perception_pcl-1.7.4/pcl_conversions/README.rst000066400000000000000000000016131420061062000214160ustar00rootroot00000000000000pcl_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 | .. +-----------------+------------------------------------------------------------+ perception_pcl-1.7.4/pcl_conversions/include/000077500000000000000000000000001420061062000213515ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_conversions/include/pcl_conversions/000077500000000000000000000000001420061062000245575ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_conversions/include/pcl_conversions/pcl_conversions.h000066400000000000000000000652701420061062000301500ustar00rootroot00000000000000/* * 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 namespace pcl_conversions { /** PCLHeader <=> Header **/ inline void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp) { stamp.fromNSec(pcl_stamp * 1000ull); // Convert from us to ns } inline void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp) { pcl_stamp = stamp.toNSec() / 1000ull; // Convert from ns to us } inline ros::Time fromPCL(const std::uint64_t &pcl_stamp) { ros::Time stamp; fromPCL(pcl_stamp, stamp); return stamp; } inline std::uint64_t toPCL(const ros::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::Header &header) { fromPCL(pcl_header.stamp, header.stamp); header.seq = pcl_header.seq; header.frame_id = pcl_header.frame_id; } inline void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) { toPCL(header.stamp, pcl_header.stamp); pcl_header.seq = header.seq; pcl_header.frame_id = header.frame_id; } inline std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) { std_msgs::Header header; fromPCL(pcl_header, header); return header; } inline pcl::PCLHeader toPCL(const std_msgs::Header &header) { pcl::PCLHeader pcl_header; toPCL(header, pcl_header); return pcl_header; } /** PCLImage <=> Image **/ inline void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) { fromPCL(pcl_image.header, image.header); image.height = pcl_image.height; image.width = pcl_image.width; image.encoding = pcl_image.encoding; image.is_bigendian = pcl_image.is_bigendian; image.step = pcl_image.step; } inline void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image) { copyPCLImageMetaData(pcl_image, image); image.data = pcl_image.data; } inline void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image) { copyPCLImageMetaData(pcl_image, image); image.data.swap(pcl_image.data); } inline void copyImageMetaData(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image) { toPCL(image.header, pcl_image.header); pcl_image.height = image.height; pcl_image.width = image.width; pcl_image.encoding = image.encoding; pcl_image.is_bigendian = image.is_bigendian; pcl_image.step = image.step; } inline void toPCL(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image) { copyImageMetaData(image, pcl_image); pcl_image.data = image.data; } inline void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image) { copyImageMetaData(image, pcl_image); pcl_image.data.swap(image.data); } /** PCLPointField <=> PointField **/ inline void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf) { pf.name = pcl_pf.name; pf.offset = pcl_pf.offset; pf.datatype = pcl_pf.datatype; pf.count = pcl_pf.count; } inline void fromPCL(const std::vector &pcl_pfs, std::vector &pfs) { pfs.resize(pcl_pfs.size()); std::vector::const_iterator it = pcl_pfs.begin(); int i = 0; for(; it != pcl_pfs.end(); ++it, ++i) { fromPCL(*(it), pfs[i]); } } inline void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf) { pcl_pf.name = pf.name; pcl_pf.offset = pf.offset; pcl_pf.datatype = pf.datatype; pcl_pf.count = pf.count; } inline void toPCL(const std::vector &pfs, std::vector &pcl_pfs) { pcl_pfs.resize(pfs.size()); std::vector::const_iterator it = pfs.begin(); int i = 0; for(; it != pfs.end(); ++it, ++i) { toPCL(*(it), pcl_pfs[i]); } } /** PCLPointCloud2 <=> PointCloud2 **/ inline void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) { fromPCL(pcl_pc2.header, pc2.header); pc2.height = pcl_pc2.height; pc2.width = pcl_pc2.width; fromPCL(pcl_pc2.fields, pc2.fields); pc2.is_bigendian = pcl_pc2.is_bigendian; pc2.point_step = pcl_pc2.point_step; pc2.row_step = pcl_pc2.row_step; pc2.is_dense = pcl_pc2.is_dense; } inline void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) { copyPCLPointCloud2MetaData(pcl_pc2, pc2); pc2.data = pcl_pc2.data; } inline void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2) { copyPCLPointCloud2MetaData(pcl_pc2, pc2); pc2.data.swap(pcl_pc2.data); } inline void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) { toPCL(pc2.header, pcl_pc2.header); pcl_pc2.height = pc2.height; pcl_pc2.width = pc2.width; 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::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) { copyPointCloud2MetaData(pc2, pcl_pc2); pcl_pc2.data = pc2.data; } inline void moveToPCL(sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) { 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::PointIndices &pi) { fromPCL(pcl_pi.header, pi.header); pi.indices = pcl_pi.indices; } inline void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi) { fromPCL(pcl_pi.header, pi.header); pi.indices.swap(pcl_pi.indices); } inline void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi) { toPCL(pi.header, pcl_pi.header); pcl_pi.indices = pi.indices; } inline void moveToPCL(pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi) { toPCL(pi.header, pcl_pi.header); pcl_pi.indices.swap(pi.indices); } /** pcl::ModelCoefficients <=> pcl_msgs::ModelCoefficients **/ inline void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc) { fromPCL(pcl_mc.header, mc.header); mc.values = pcl_mc.values; } inline void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc) { fromPCL(pcl_mc.header, mc.header); mc.values.swap(pcl_mc.values); } inline void toPCL(const pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc) { toPCL(mc.header, pcl_mc.header); pcl_mc.values = mc.values; } inline void moveToPCL(pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc) { toPCL(mc.header, pcl_mc.header); pcl_mc.values.swap(mc.values); } /** pcl::Vertices <=> pcl_msgs::Vertices **/ 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::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::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::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::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::PolygonMesh &mesh) { fromPCL(pcl_mesh.header, mesh.header); fromPCL(pcl_mesh.cloud, mesh.cloud); fromPCL(pcl_mesh.polygons, mesh.polygons); } inline void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh) { fromPCL(pcl_mesh.header, mesh.header); moveFromPCL(pcl_mesh.cloud, mesh.cloud); } inline void toPCL(const pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh) { toPCL(mesh.header, pcl_mesh.header); toPCL(mesh.cloud, pcl_mesh.cloud); toPCL(mesh.polygons, pcl_mesh.polygons); } inline void moveToPCL(pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh) { toPCL(mesh.header, pcl_mesh.header); moveToPCL(mesh.cloud, pcl_mesh.cloud); moveToPCL(mesh.polygons, pcl_mesh.polygons); } } // namespace pcl_conversions namespace pcl { /** Overload pcl::getFieldIndex **/ inline int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name) { // Get the index we need for (size_t d = 0; d < cloud.fields.size(); ++d) { if (cloud.fields[d].name == field_name) { return (static_cast(d)); } } return (-1); } /** Overload pcl::getFieldsList **/ inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud) { std::string result; for (size_t i = 0; i < cloud.fields.size () - 1; ++i) { result += cloud.fields[i].name + " "; } result += cloud.fields[cloud.fields.size () - 1].name; return (result); } /** Provide pcl::toROSMsg **/ inline void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) { pcl::PCLPointCloud2 pcl_cloud; pcl_conversions::toPCL(cloud, pcl_cloud); pcl::PCLImage pcl_image; pcl::toPCLPointCloud2(pcl_cloud, pcl_image); pcl_conversions::moveFromPCL(pcl_image, image); } inline void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image) { pcl::PCLPointCloud2 pcl_cloud; pcl_conversions::moveToPCL(cloud, pcl_cloud); pcl::PCLImage pcl_image; pcl::toPCLPointCloud2(pcl_cloud, pcl_image); pcl_conversions::moveFromPCL(pcl_image, image); } template void toROSMsg (const pcl::PointCloud &cloud, sensor_msgs::Image& msg) { // Ease the user's burden on specifying width/height for unorganized datasets if (cloud.width == 0 && cloud.height == 0) { throw std::runtime_error("Needs to be a dense like cloud!!"); } else { if (cloud.points.size () != cloud.width * cloud.height) throw std::runtime_error("The width and height do not match the cloud size!"); msg.height = cloud.height; msg.width = cloud.width; } // sensor_msgs::image_encodings::BGR8; msg.encoding = "bgr8"; msg.step = msg.width * sizeof (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::PointCloud2 <=> pcl::PointCloud **/ template void toROSMsg(const pcl::PointCloud &pcl_cloud, sensor_msgs::PointCloud2 &cloud) { pcl::PCLPointCloud2 pcl_pc2; pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2); pcl_conversions::moveFromPCL(pcl_pc2, cloud); } template void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud) { pcl::PCLPointCloud2 pcl_pc2; pcl_conversions::toPCL(cloud, pcl_pc2); pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); } template void moveFromROSMsg(sensor_msgs::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::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::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::PointCloud2 &cloud) { pcl::PCLPointCloud2 pcl_cloud; int ret = pcl::io::loadPCDFile(file_name, pcl_cloud); pcl_conversions::moveFromPCL(pcl_cloud, cloud); return ret; } } // namespace io /** Overload asdf **/ inline bool concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out) { //if one input cloud has no points, but the other input does, just return the cloud with points if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0) { cloud_out = cloud2; return (true); } else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0) { cloud_out = cloud1; return (true); } bool strip = false; for (size_t i = 0; i < cloud1.fields.size (); ++i) if (cloud1.fields[i].name == "_") strip = true; for (size_t i = 0; i < cloud2.fields.size (); ++i) if (cloud2.fields[i].name == "_") strip = true; if (!strip && cloud1.fields.size () != cloud2.fields.size ()) { PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ()); return (false); } // Copy cloud1 into cloud_out cloud_out = cloud1; size_t nrpts = cloud_out.data.size (); // Height = 1 => no more organized cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height; cloud_out.height = 1; if (!cloud1.is_dense || !cloud2.is_dense) cloud_out.is_dense = false; else cloud_out.is_dense = true; // We need to strip the extra padding fields if (strip) { // Get the field sizes for the second cloud std::vector fields2; std::vector fields2_sizes; for (size_t j = 0; j < cloud2.fields.size (); ++j) { if (cloud2.fields[j].name == "_") continue; fields2_sizes.push_back (cloud2.fields[j].count * pcl::getFieldSize (cloud2.fields[j].datatype)); fields2.push_back (cloud2.fields[j]); } cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step); // Copy the second cloud for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp) { int i = 0; for (size_t j = 0; j < fields2.size (); ++j) { if (cloud1.fields[i].name == "_") { ++i; continue; } // We're fine with the special RGB vs RGBA use case if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") || (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") || (cloud1.fields[i].name == fields2[j].name)) { memcpy (reinterpret_cast (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]), reinterpret_cast (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]), fields2_sizes[j]); ++i; // increment the field size i } } } } else { for (size_t i = 0; i < cloud1.fields.size (); ++i) { // We're fine with the special RGB vs RGBA use case if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") || (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb")) continue; // Otherwise we need to make sure the names are the same if (cloud1.fields[i].name != cloud2.fields[i].name) { PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ()); return (false); } } cloud_out.data.resize (nrpts + cloud2.data.size ()); memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ()); } return (true); } } // namespace pcl namespace ros { template<> struct DefaultMessageCreator { boost::shared_ptr operator() () { boost::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. ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL); ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL); }; template<> struct DataType { static const char* value() { return DataType::value(); } static const char* value(const pcl::PCLPointCloud2&) { return value(); } }; template<> struct Definition { static const char* value() { return Definition::value(); } static const char* value(const pcl::PCLPointCloud2&) { return value(); } }; template<> struct HasHeader : TrueType {}; } // namespace ros::message_traits namespace serialization { /* * Provide a custom serialization for pcl::PCLPointCloud2 */ template<> struct Serializer { template inline static void write(Stream& stream, const pcl::PCLPointCloud2& m) { std_msgs::Header header; pcl_conversions::fromPCL(m.header, header); stream.next(header); stream.next(m.height); stream.next(m.width); std::vector pfs; pcl_conversions::fromPCL(m.fields, pfs); stream.next(pfs); stream.next(m.is_bigendian); stream.next(m.point_step); stream.next(m.row_step); stream.next(m.data); stream.next(m.is_dense); } template inline static void read(Stream& stream, pcl::PCLPointCloud2& m) { std_msgs::Header header; stream.next(header); pcl_conversions::toPCL(header, m.header); stream.next(m.height); stream.next(m.width); std::vector pfs; stream.next(pfs); pcl_conversions::toPCL(pfs, m.fields); stream.next(m.is_bigendian); stream.next(m.point_step); stream.next(m.row_step); stream.next(m.data); stream.next(m.is_dense); } inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m) { uint32_t length = 0; std_msgs::Header header; pcl_conversions::fromPCL(m.header, header); length += serializationLength(header); length += 4; // height length += 4; // width std::vector pfs; pcl_conversions::fromPCL(m.fields, pfs); length += serializationLength(pfs); // fields length += 1; // is_bigendian length += 4; // point_step length += 4; // row_step length += 4; // data's size length += m.data.size() * sizeof(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::Header header; pcl_conversions::fromPCL(m, header); stream.next(header); } template inline static void read(Stream& stream, pcl::PCLHeader& m) { std_msgs::Header header; stream.next(header); pcl_conversions::toPCL(header, m); } inline static uint32_t serializedLength(const pcl::PCLHeader& m) { uint32_t length = 0; std_msgs::Header header; pcl_conversions::fromPCL(m, header); length += serializationLength(header); return length; } }; } // namespace ros::serialization } // namespace ros #endif /* PCL_CONVERSIONS_H__ */ perception_pcl-1.7.4/pcl_conversions/package.xml000066400000000000000000000024751420061062000220530ustar00rootroot00000000000000 pcl_conversions 1.7.4 Provides conversions from PCL data types and ROS message types William Woodall 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 catkin eigen libpcl-all-dev pcl_msgs roscpp sensor_msgs std_msgs eigen libpcl-all-dev pcl_msgs roscpp sensor_msgs std_msgs perception_pcl-1.7.4/pcl_conversions/test/000077500000000000000000000000001420061062000207055ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_conversions/test/test_pcl_conversions.cpp000066400000000000000000000073501420061062000256630ustar00rootroot00000000000000#include #include "gtest/gtest.h" #include "pcl_conversions/pcl_conversions.h" namespace { class PCLConversionTests : public ::testing::Test { protected: virtual void SetUp() { pcl_image.header.frame_id = "pcl"; pcl_image.height = 1; pcl_image.width = 2; pcl_image.step = 1; pcl_image.is_bigendian = true; pcl_image.encoding = "bgr8"; pcl_image.data.resize(2); pcl_image.data[0] = 0x42; pcl_image.data[1] = 0x43; pcl_pc2.header.frame_id = "pcl"; pcl_pc2.height = 1; pcl_pc2.width = 2; pcl_pc2.point_step = 1; pcl_pc2.row_step = 1; pcl_pc2.is_bigendian = true; pcl_pc2.is_dense = true; pcl_pc2.fields.resize(2); pcl_pc2.fields[0].name = "XYZ"; pcl_pc2.fields[0].datatype = pcl::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::Image image; pcl::PCLPointCloud2 pcl_pc2; sensor_msgs::PointCloud2 pc2; }; template void test_image(T &image) { EXPECT_EQ(std::string("pcl"), image.header.frame_id); EXPECT_EQ(1, image.height); EXPECT_EQ(2, image.width); EXPECT_EQ(1, image.step); EXPECT_TRUE(image.is_bigendian); EXPECT_EQ(std::string("bgr8"), image.encoding); EXPECT_EQ(2, image.data.size()); EXPECT_EQ(0x42, image.data[0]); EXPECT_EQ(0x43, image.data[1]); } TEST_F(PCLConversionTests, imageConversion) { pcl_conversions::fromPCL(pcl_image, image); test_image(image); pcl::PCLImage pcl_image2; pcl_conversions::toPCL(image, pcl_image2); test_image(pcl_image2); } template void test_pc(T &pc) { EXPECT_EQ(std::string("pcl"), pc.header.frame_id); EXPECT_EQ(1, pc.height); EXPECT_EQ(2, pc.width); EXPECT_EQ(1, pc.point_step); EXPECT_EQ(1, pc.row_step); EXPECT_TRUE(pc.is_bigendian); EXPECT_TRUE(pc.is_dense); EXPECT_EQ("XYZ", pc.fields[0].name); EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[0].datatype); EXPECT_EQ(3, pc.fields[0].count); EXPECT_EQ(0, pc.fields[0].offset); EXPECT_EQ("RGB", pc.fields[1].name); EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[1].datatype); EXPECT_EQ(3, pc.fields[1].count); EXPECT_EQ(8 * 3, pc.fields[1].offset); EXPECT_EQ(2, pc.data.size()); EXPECT_EQ(0x42, pc.data[0]); EXPECT_EQ(0x43, pc.data[1]); } TEST_F(PCLConversionTests, pointcloud2Conversion) { pcl_conversions::fromPCL(pcl_pc2, pc2); test_pc(pc2); pcl::PCLPointCloud2 pcl_pc2_2; pcl_conversions::toPCL(pc2, pcl_pc2_2); test_pc(pcl_pc2_2); } } // namespace struct StampTestData { const ros::Time stamp_; ros::Time stamp2_; explicit StampTestData(const ros::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(ros::Time(1.000001)); EXPECT_TRUE(d.stamp_==d.stamp2_); } { const StampTestData d(ros::Time(1.999999)); EXPECT_TRUE(d.stamp_==d.stamp2_); } { const StampTestData d(ros::Time(1.999)); EXPECT_TRUE(d.stamp_==d.stamp2_); } { const StampTestData d(ros::Time(1423680574, 746000000)); EXPECT_TRUE(d.stamp_==d.stamp2_); } { const StampTestData d(ros::Time(1423680629, 901000000)); EXPECT_TRUE(d.stamp_==d.stamp2_); } } int main(int argc, char **argv) { try { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } catch (std::exception &e) { std::cerr << "Unhandled Exception: " << e.what() << std::endl; } return 1; } perception_pcl-1.7.4/pcl_ros/000077500000000000000000000000001420061062000161615ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/CHANGELOG.rst000066400000000000000000000350501420061062000202050ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package pcl_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.7.0 (2019-07-03) ------------------ * Added option for fixed filename (via local parameter) (`#102 `_) (cherry picked from commit bd38533523fa5c00f320e6505a2c533e90f9d97e) * rewrote pcd_to_pointcloud to publish using a latched topic (`#213 `_) * pointcloud is published as a latched topic now * added functionality: ability to set config via parameters * Fix `#150 `_: Allow bag_to_pcd to properly respond to topic argument (`#215 `_) * Properly respond to topic argument fixing `#150 `_ Previously providing a topic name in argv[2], as indicated in the usage string, modified the printouts indicating to the user that they were going to be exporting the correct topic, but the rosbag::View::Query used still grabbed the first sensor_msgs/PointCloud2 topic which was not necessarily what they had specified. * Added more useful printouts and type-checking on input topic * Catch tf::TransformException in transformPointCloud (`#223 `_) * Catch TransformException in transformPointCloud This allows to catch all exceptions, not only tf::LookupException and tf::ExtrapolationException. Note that most of the code using transformPointCloud already (implicitly) assumes this function doesn't throw any exception, and they check the return value, which is false when an exception happened (or something went wrong). * Check for transformPointCloud return value In pcl_ros/src/pcl_ros/io/concatenate_data.cpp, which seems to be the only place where that check was missed. * Split off pcl_ros_filter into separate library (`#239 `_) * [Windows][melodic-devel] Fix Boost linkage issue and binary install location. (`#238 `_) * fix Windows build break. (`#6 `_) * Fix install path. (`#7 `_) * Wait for transform in transformPointCloud(). (`#214 `_) * Added tf2 versions of functions in transforms.h/hpp. (`#231 `_) * Switch to industrial_ci (`#233 `_) * Use industrial_ci * Run for all melodic target platforms * Artful EOL; verbose build and tests * Use travis_wait * Remove travis_wait * Download to devel space * Mark SYSTEM includes to avoid spurious warnings * Don't double-build PRs * use and on Windows (`#221 `_) * Changing from usleep to c++14 style sleep_for for Windows support * Contributors: Enrique Fernandez Perdomo, James Xu, Jarvis Schultz, Martin Pecka, Paul Bovbel, Sean Yen, Stefan Kohlbrecher, Wolfgang Merkt, moooeeeep 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 perception_pcl-1.7.4/pcl_ros/CMakeLists.txt000066400000000000000000000213451420061062000207260ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(pcl_ros) # CMake 3.1 added support for CMAKE_CXX_STANDARD to manage the C++ standard version # Use CMake C++ standard management where possible for better interoperability # with dependencies that export target standards and/or features if (${CMAKE_VERSION} VERSION_LESS "3.1") add_compile_options(-std=c++14) else() set(CMAKE_CXX_STANDARD 14) endif() ## Find catkin packages find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure geometry_msgs message_filters nodelet nodelet_topic_tools pcl_conversions pcl_msgs pluginlib rosbag rosconsole roscpp roslib sensor_msgs std_msgs tf tf2 tf2_eigen tf2_ros ) ## Find system dependencies find_package(PCL REQUIRED COMPONENTS common features filters io segmentation surface) find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) if(NOT "${PCL_LIBRARIES}" STREQUAL "") # For debian: https://github.com/ros-perception/perception_pcl/issues/139 list(REMOVE_ITEM PCL_LIBRARIES "vtkGUISupportQt" "vtkGUISupportQtOpenGL" "vtkGUISupportQtSQL" "vtkGUISupportQtWebkit" "vtkViewsQt" "vtkRenderingQt") endif() # There is a bug in the Ubuntu Artful (17.10) version of the VTK package, # where it includes /usr/include/*-linux-gnu/freetype2 in the include # directories (which doesn't exist). This filters down to the PCL_INCLUDE_DIRS, # and causes downstream projects trying to use these libraries to fail to # configure properly. Here we remove those bogus entries so that downstream # consumers of this package succeed. if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") foreach(item ${PCL_INCLUDE_DIRS}) string(REGEX MATCH "/usr/include/.*-linux-gnu/freetype2" item ${item}) if(item) list(REMOVE_ITEM PCL_INCLUDE_DIRS ${item}) endif() endforeach() endif() ## Add include directories include_directories(include) include_directories( SYSTEM ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) ## Generate dynamic_reconfigure generate_dynamic_reconfigure_options( cfg/EuclideanClusterExtraction.cfg cfg/ExtractIndices.cfg cfg/ExtractPolygonalPrismData.cfg cfg/Feature.cfg cfg/Filter.cfg cfg/MLS.cfg cfg/SACSegmentation.cfg cfg/SACSegmentationFromNormals.cfg cfg/SegmentDifferences.cfg cfg/StatisticalOutlierRemoval.cfg cfg/RadiusOutlierRemoval.cfg cfg/VoxelGrid.cfg cfg/CropBox.cfg ) ## Declare the catkin package catkin_package( INCLUDE_DIRS include LIBRARIES pcl_ros_filter pcl_ros_tf CATKIN_DEPENDS dynamic_reconfigure geometry_msgs message_filters nodelet nodelet_topic_tools pcl_conversions pcl_msgs rosbag roscpp sensor_msgs std_msgs tf tf2 tf2_eigen tf2_ros DEPENDS Boost EIGEN3 PCL ) ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_tf ${catkin_EXPORTED_TARGETS}) ## Nodelets ## Declare the pcl_ros_io library add_library(pcl_ros_io src/pcl_ros/io/bag_io.cpp src/pcl_ros/io/concatenate_data.cpp src/pcl_ros/io/concatenate_fields.cpp src/pcl_ros/io/io.cpp src/pcl_ros/io/pcd_io.cpp ) 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 library for base filter plugin add_library(pcl_ros_filter src/pcl_ros/filters/filter.cpp ) target_link_libraries(pcl_ros_filter pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_filter ${PROJECT_NAME}_gencfg) ## Declare the pcl_ros_filters library add_library(pcl_ros_filters src/pcl_ros/filters/extract_indices.cpp src/pcl_ros/filters/passthrough.cpp src/pcl_ros/filters/project_inliers.cpp src/pcl_ros/filters/radius_outlier_removal.cpp src/pcl_ros/filters/statistical_outlier_removal.cpp src/pcl_ros/filters/voxel_grid.cpp src/pcl_ros/filters/crop_box.cpp ) target_link_libraries(pcl_ros_filters pcl_ros_filter pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_filters) ## Declare the pcl_ros_segmentation library add_library (pcl_ros_segmentation src/pcl_ros/segmentation/extract_clusters.cpp src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp src/pcl_ros/segmentation/sac_segmentation.cpp src/pcl_ros/segmentation/segment_differences.cpp src/pcl_ros/segmentation/segmentation.cpp ) target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_segmentation) ## Declare the pcl_ros_surface library add_library (pcl_ros_surface src/pcl_ros/surface/surface.cpp # Compilation is much faster if we include all the following CPP files in surface.cpp src/pcl_ros/surface/convex_hull.cpp src/pcl_ros/surface/moving_least_squares.cpp ) target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) class_loader_hide_library_symbols(pcl_ros_surface) ## Tools add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(bag_to_pcd tools/bag_to_pcd.cpp) target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) ## Downloads ############# ## Testing ## ############# if(CATKIN_ENABLE_TESTING) catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/samples/data MD5 546b5b4822fb1de21b0cf83d41ad6683 ) add_custom_target(download ALL DEPENDS download_extra_data) find_package(rostest REQUIRED) add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp) target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false) add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false) add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false) add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false) endif(CATKIN_ENABLE_TESTING) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) install( TARGETS pcl_ros_tf pcl_ros_io pcl_ros_features pcl_ros_filter pcl_ros_filters pcl_ros_surface pcl_ros_segmentation RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) install( TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) install(DIRECTORY plugins samples DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) perception_pcl-1.7.4/pcl_ros/cfg/000077500000000000000000000000001420061062000167205ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/cfg/CropBox.cfg000077500000000000000000000031231420061062000207570ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/EuclideanClusterExtraction.cfg000077500000000000000000000015501420061062000247010ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/ExtractIndices.cfg000077500000000000000000000006641420061062000223230ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/ExtractPolygonalPrismData.cfg000077500000000000000000000011331420061062000245060ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/Feature.cfg000077500000000000000000000015031420061062000207760ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/Filter.cfg000077500000000000000000000005541420061062000206350ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/MLS.cfg000077500000000000000000000021671420061062000200450ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/RadiusOutlierRemoval.cfg000077500000000000000000000012221420061062000235220ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/SACSegmentation.cfg000077500000000000000000000004171420061062000223720ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/SACSegmentationFromNormals.cfg000077500000000000000000000012321420061062000245460ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/SACSegmentation_common.py000066400000000000000000000027251420061062000236340ustar00rootroot00000000000000#! /usr/bin/env python # set up parameters that we care about PACKAGE = 'pcl_ros' from dynamic_reconfigure.parameter_generator_catkin import *; def add_common_parameters (gen): # add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = "") gen.add ("max_iterations", int_t, 0, "The maximum number of iterations the algorithm will run for", 50, 0, 100000) gen.add ("probability", double_t, 0, "The desired probability of choosing at least one sample free from outliers", 0.99, 0.5, 0.99) gen.add ("distance_threshold", double_t, 0, "The distance to model threshold", 0.02, 0, 1.0) gen.add ("optimize_coefficients", bool_t, 0, "Model coefficient refinement", True) gen.add ("radius_min", double_t, 0, "The minimum allowed model radius (where applicable)", 0.0, 0, 1.0) gen.add ("radius_max", double_t, 0, "The maximum allowed model radius (where applicable)", 0.05, 0, 1.0) gen.add ("eps_angle", double_t, 0, "The maximum allowed difference between the model normal and the given axis in radians.", 0.17, 0.0, 1.5707) gen.add ("min_inliers", int_t, 0, "The minimum number of inliers a model must have in order to be considered valid.", 0, 0, 100000) gen.add ("input_frame", str_t, 0, "The input TF frame the data should be transformed into, if input.header.frame_id is different.", "") gen.add ("output_frame", str_t, 0, "The output TF frame the data should be transformed into, if input.header.frame_id is different.", "") perception_pcl-1.7.4/pcl_ros/cfg/SegmentDifferences.cfg000077500000000000000000000010361420061062000231440ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/StatisticalOutlierRemoval.cfg000077500000000000000000000014251420061062000245640ustar00rootroot00000000000000#! /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")) perception_pcl-1.7.4/pcl_ros/cfg/VoxelGrid.cfg000077500000000000000000000013021420061062000213030ustar00rootroot00000000000000#! /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) gen.add ("min_points_per_voxel", int_t, 0, "The minimum number of points required for a voxel to be used.", 1, 1, 100000) add_common_parameters (gen) exit (gen.generate (PACKAGE, "pcl_ros", "VoxelGrid")) perception_pcl-1.7.4/pcl_ros/cfg/common.py000066400000000000000000000024341420061062000205650ustar00rootroot00000000000000#! /usr/bin/env python # set up parameters that we care about PACKAGE = 'pcl_ros' from dynamic_reconfigure.parameter_generator_catkin import *; def add_common_parameters (gen): # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): gen.add ("filter_field_name", str_t, 0, "The field name used for filtering", "z") gen.add ("filter_limit_min", double_t, 0, "The minimum allowed field value a point will be considered from", 0.0, -100000.0, 100000.0) gen.add ("filter_limit_max", double_t, 0, "The maximum allowed field value a point will be considered from", 1.0, -100000.0, 100000.0) gen.add ("filter_limit_negative", bool_t, 0, "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max].", False) gen.add ("keep_organized", bool_t, 0, "Set whether the filtered points should be kept and set to NaN, or removed from the PointCloud, thus potentially breaking its organized structure.", False) gen.add ("input_frame", str_t, 0, "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.", "") gen.add ("output_frame", str_t, 0, "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different.", "") perception_pcl-1.7.4/pcl_ros/include/000077500000000000000000000000001420061062000176045ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/include/pcl_ros/000077500000000000000000000000001420061062000212455ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/000077500000000000000000000000001420061062000230635ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/boundary.h000066400000000000000000000070101420061062000250550ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: boundary.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_BOUNDARY_H_ #define PCL_ROS_BOUNDARY_H_ #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true #include #include "pcl_ros/features/feature.h" namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle * criterion. The code makes use of the estimated surface normals at each point in the input dataset. * * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. * \author Radu Bogdan Rusu */ class BoundaryEstimation: public FeatureFromNormals { private: pcl::BoundaryEstimation impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_BOUNDARY_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/feature.h000066400000000000000000000251641420061062000246770ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: feature.h 35422 2011-01-24 20:04:44Z rusu $ * */ #ifndef PCL_ROS_FEATURE_H_ #define PCL_ROS_FEATURE_H_ // PCL includes #include #include "pcl_ros/pcl_nodelet.h" #include // Dynamic reconfigure #include #include "pcl_ros/FeatureConfig.h" // PCL conversions #include 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 }; } #endif //#ifndef PCL_ROS_FEATURE_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/fpfh.h000066400000000000000000000076441420061062000241720ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: fpfh.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_FPFH_H_ #define PCL_ROS_FPFH_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud * dataset containing points and normals. * * @note If you use this code in any academic work, please cite: * *
    *
  • R.B. Rusu, N. Blodow, M. Beetz. * Fast Point Feature Histograms (FPFH) for 3D Registration. * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), * Kobe, Japan, May 12-17 2009. *
    • *
  • R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. * Fast Geometric Point Labeling using Conditional Random Fields. * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), * St. Louis, MO, USA, October 11-15 2009. *
    • *
* * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). * \author Radu Bogdan Rusu */ class FPFHEstimation : public FeatureFromNormals { private: pcl::FPFHEstimation impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_FPFH_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/fpfh_omp.h000066400000000000000000000073771420061062000250500ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: fpfh_omp.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_FPFH_OMP_H_ #define PCL_ROS_FPFH_OMP_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud * dataset containing points and normals, in parallel, using the OpenMP standard. * * @note If you use this code in any academic work, please cite: * *
    *
  • R.B. Rusu, N. Blodow, M. Beetz. * Fast Point Feature Histograms (FPFH) for 3D Registration. * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), * Kobe, Japan, May 12-17 2009. *
    • *
  • R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. * Fast Geometric Point Labeling using Conditional Random Fields. * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), * St. Louis, MO, USA, October 11-15 2009. *
    • *
* \author Radu Bogdan Rusu */ class FPFHEstimationOMP : public FeatureFromNormals { private: pcl::FPFHEstimationOMP impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_FPFH_OMP_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/moment_invariants.h000066400000000000000000000063241420061062000267760ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: moment_invariants.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_MOMENT_INVARIANTS_H_ #define PCL_ROS_MOMENT_INVARIANTS_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. * * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. * \author Radu Bogdan Rusu */ class MomentInvariantsEstimation: public Feature { private: pcl::MomentInvariantsEstimation impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/normal_3d.h000066400000000000000000000063721420061062000251220ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: normal_3d.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_NORMAL_3D_H_ #define PCL_ROS_NORMAL_3D_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and * curvatures. * * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations. * \author Radu Bogdan Rusu */ class NormalEstimation: public Feature { private: /** \brief PCL implementation object. */ pcl::NormalEstimation impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_NORMAL_3D_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/normal_3d_omp.h000066400000000000000000000057641420061062000260010ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: normal_3d_omp.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_NORMAL_3D_OMP_H_ #define PCL_ROS_NORMAL_3D_OMP_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and * curvatures, in parallel, using the OpenMP standard. * \author Radu Bogdan Rusu */ class NormalEstimationOMP: public Feature { private: pcl::NormalEstimationOMP impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h000066400000000000000000000061411420061062000257430ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: normal_3d_tbb.h 35661 2011-02-01 06:04:14Z rusu $ * */ #ifndef PCL_ROS_NORMAL_3D_TBB_H_ #define PCL_ROS_NORMAL_3D_TBB_H_ //#include "pcl_ros/pcl_ros_config.h" //#if defined(HAVE_TBB) #include #include "pcl_ros/features/normal_3d.h" namespace pcl_ros { /** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and * curvatures, in parallel, using Intel's Threading Building Blocks library. * \author Radu Bogdan Rusu */ class NormalEstimationTBB: public Feature { private: pcl::NormalEstimationTBB impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } //#endif // HAVE_TBB #endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/pfh.h000066400000000000000000000077131420061062000240210ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: pfh.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_PFH_H_ #define PCL_ROS_PFH_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset * containing points and normals. * * @note If you use this code in any academic work, please cite: * *
    *
  • R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz. * Aligning Point Cloud Views using Persistent Feature Histograms. * In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), * Nice, France, September 22-26 2008. *
    • *
  • R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz. * Learning Informative Point Classes for the Acquisition of Object Model Maps. * In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV), * Hanoi, Vietnam, December 17-20 2008. *
    • *
* * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). * \author Radu Bogdan Rusu */ class PFHEstimation : public FeatureFromNormals { private: pcl::PFHEstimation impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_PFH_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/principal_curvatures.h000066400000000000000000000070001420061062000274750ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: principal_curvatures.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ #define PCL_ROS_PRINCIPAL_CURVATURES_H_ #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of * principal surface curvatures for a given point cloud dataset containing points and normals. * * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. * \author Radu Bogdan Rusu, Jared Glover */ class PrincipalCurvaturesEstimation : public FeatureFromNormals { private: pcl::PrincipalCurvaturesEstimation impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/shot.h000066400000000000000000000055141420061062000242160ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2015, Ryohei Ueda, JSK Lab * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of JSK Lab. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * */ #ifndef PCL_ROS_SHOT_H_ #define PCL_ROS_SHOT_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b SHOTEstimation estimates SHOT descriptor. * */ class SHOTEstimation : public FeatureFromNormals { private: pcl::SHOTEstimation impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_SHOT_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/shot_omp.h000066400000000000000000000055541420061062000250750ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2015, Ryohei Ueda, JSK Lab * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of JSK Lab. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * */ #ifndef PCL_ROS_SHOT_OMP_H_ #define PCL_ROS_SHOT_OMP_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP. */ class SHOTEstimationOMP : public FeatureFromNormals { private: pcl::SHOTEstimationOMP impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_SHOT_OMP_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/features/vfh.h000066400000000000000000000064271420061062000240300ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: vfh.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_FEATURES_VFH_H_ #define PCL_ROS_FEATURES_VFH_H_ #include #include "pcl_ros/features/feature.h" namespace pcl_ros { /** \brief @b VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud * dataset containing points and normals. * * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). * \author Radu Bogdan Rusu */ class VFHEstimation : public FeatureFromNormals { private: pcl::VFHEstimation impl_; typedef pcl::PointCloud PointCloudOut; /** \brief Child initialization routine. Internal method. */ inline bool childInit (ros::NodeHandle &nh) { // Create the output publisher pub_output_ = 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 }; } #endif //#ifndef PCL_ROS_FEATURES_VFH_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/000077500000000000000000000000001420061062000227155ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/crop_box.h000066400000000000000000000074371420061062000247140ustar00rootroot00000000000000/* * * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: cropbox.cpp * */ #ifndef PCL_ROS_FILTERS_CROPBOX_H_ #define PCL_ROS_FILTERS_CROPBOX_H_ // PCL includes #include #include "pcl_ros/filters/filter.h" // Dynamic reconfigure #include "pcl_ros/CropBoxConfig.h" namespace pcl_ros { /** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box. * * \author Radu Bogdan Rusu * \author Justin Rosen * \author Marti Morta Garriga */ class CropBox : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr > srv_; // TODO /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ inline void filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL (*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); pcl::PCLPointCloud2 pcl_output; impl_.filter (pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. * \param nh ROS node handle * \param has_service set to true if the child has a Dynamic Reconfigure service */ bool child_init (ros::NodeHandle &nh, bool &has_service); /** \brief Dynamic reconfigure service callback. * \param config the dynamic reconfigure CropBoxConfig object * \param level the dynamic reconfigure level */ void config_callback (pcl_ros::CropBoxConfig &config, uint32_t level); private: /** \brief The PCL filter implementation used. */ pcl::CropBox impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/extract_indices.h000066400000000000000000000073751420061062000262520ustar00rootroot00000000000000/* * 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_EXTRACTINDICES_H_ #define PCL_ROS_FILTERS_EXTRACTINDICES_H_ // PCL includes #include #include "pcl_ros/filters/filter.h" #include "pcl_ros/ExtractIndicesConfig.h" namespace pcl_ros { /** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu */ class ExtractIndices : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr > srv_; /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ inline void filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); pcl::PCLPointCloud2 pcl_output; impl_.filter (pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. * \param nh ROS node handle * \param has_service set to true if the child has a Dynamic Reconfigure service */ virtual bool child_init (ros::NodeHandle &nh, bool &has_service); /** \brief Dynamic reconfigure service callback. */ void config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level); private: /** \brief The PCL filter implementation used. */ pcl::ExtractIndices impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/filter.h000066400000000000000000000133671420061062000243650ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: filter.h 35876 2011-02-09 01:04:36Z rusu $ * */ #ifndef PCL_ROS_FILTER_H_ #define PCL_ROS_FILTER_H_ // PCL includes #include "pcl_ros/pcl_nodelet.h" // Dynamic reconfigure #include #include "pcl_ros/FilterConfig.h" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; /** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters * are defined here as static methods. * \author Radu Bogdan Rusu */ class Filter : public PCLNodelet { public: typedef sensor_msgs::PointCloud2 PointCloud2; typedef pcl::IndicesPtr IndicesPtr; typedef pcl::IndicesConstPtr IndicesConstPtr; Filter () {} protected: /** \brief The input PointCloud subscriber. */ ros::Subscriber sub_input_; message_filters::Subscriber sub_input_filter_; /** \brief The desired user filter field name. */ std::string filter_field_name_; /** \brief The minimum allowed filter value a point will be considered from. */ double filter_limit_min_; /** \brief The maximum allowed filter value a point will be considered from. */ double filter_limit_max_; /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ bool filter_limit_negative_; /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ std::string tf_input_frame_; /** \brief The original data input TF frame. */ std::string tf_input_orig_frame_; /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ std::string tf_output_frame_; /** \brief Internal mutex. */ boost::mutex mutex_; /** \brief Child initialization routine. * \param nh ROS node handle * \param has_service set to true if the child has a Dynamic Reconfigure service */ virtual bool child_init (ros::NodeHandle &/*nh*/, bool &has_service) { has_service = false; return (true); } /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. * \param indices a pointer to the vector of point indices to use. * \param output the resultant filtered PointCloud2 */ virtual void filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) = 0; /** \brief Lazy transport subscribe routine. */ virtual void subscribe(); /** \brief Lazy transport unsubscribe routine. */ virtual void unsubscribe(); /** \brief Nodelet initialization routine. */ virtual void onInit (); /** \brief Call the child filter () method, optionally transform the result, and publish it. * \param input the input point cloud dataset. * \param indices a pointer to the vector of point indices to use. */ void computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices); private: /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr > srv_; /** \brief Synchronized input, and indices.*/ boost::shared_ptr > > sync_input_indices_e_; boost::shared_ptr > > sync_input_indices_a_; /** \brief Dynamic reconfigure service callback. */ virtual void config_callback (pcl_ros::FilterConfig &config, uint32_t level); /** \brief PointCloud2 + Indices data callback. */ void input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices); public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_ROS_FILTER_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/passthrough.h000066400000000000000000000073541420061062000254460ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: passthrough.h 35876 2011-02-09 01:04:36Z rusu $ * */ #ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ #define PCL_ROS_FILTERS_PASSTHROUGH_H_ // PCL includes #include #include "pcl_ros/filters/filter.h" namespace pcl_ros { /** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given * constraints. * \author Radu Bogdan Rusu */ class PassThrough : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr > srv_; /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ inline void filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL (*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); pcl::PCLPointCloud2 pcl_output; impl_.filter (pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. * \param nh ROS node handle * \param has_service set to true if the child has a Dynamic Reconfigure service */ bool child_init (ros::NodeHandle &nh, bool &has_service); /** \brief Dynamic reconfigure service callback. * \param config the dynamic reconfigure FilterConfig object * \param level the dynamic reconfigure level */ void config_callback (pcl_ros::FilterConfig &config, uint32_t level); private: /** \brief The PCL filter implementation used. */ pcl::PassThrough impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/project_inliers.h000066400000000000000000000111211420061062000262550ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: project_inliers.h 35876 2011-02-09 01:04:36Z rusu $ * */ #ifndef PCL_ROS_FILTERS_PROJECT_INLIERS_H_ #define PCL_ROS_FILTERS_PROJECT_INLIERS_H_ // PCL includes #include #include "pcl_ros/filters/filter.h" #include namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; /** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a * separate PointCloud. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu */ class ProjectInliers : public Filter { public: ProjectInliers () : model_ () {} protected: /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ inline void filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL (*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); pcl_conversions::toPCL(*(model_), *(pcl_model)); impl_.setModelCoefficients (pcl_model); pcl::PCLPointCloud2 pcl_output; impl_.filter (pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } private: /** \brief A pointer to the vector of model coefficients. */ ModelCoefficientsConstPtr model_; /** \brief The message filter subscriber for model coefficients. */ message_filters::Subscriber sub_model_; /** \brief Synchronized input, indices, and model coefficients.*/ boost::shared_ptr > > sync_input_indices_model_e_; boost::shared_ptr > > sync_input_indices_model_a_; /** \brief The PCL filter implementation used. */ pcl::ProjectInliers impl_; /** \brief Nodelet initialization routine. */ virtual void onInit (); /** \brief NodeletLazy connection routine. */ void subscribe (); void unsubscribe (); /** \brief PointCloud2 + Indices + Model data callback. */ void input_indices_model_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices, const ModelCoefficientsConstPtr &model); public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h000066400000000000000000000076671420061062000276650ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: radius_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $ * */ #ifndef PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_ #define PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_ // PCL includes #include #include "pcl_ros/filters/filter.h" // Dynamic reconfigure #include "pcl_ros/RadiusOutlierRemovalConfig.h" namespace pcl_ros { /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain * search radius is smaller than a given K. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu */ class RadiusOutlierRemoval : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr > srv_; /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ inline void filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL (*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); pcl::PCLPointCloud2 pcl_output; impl_.filter (pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. * \param nh ROS node handle * \param has_service set to true if the child has a Dynamic Reconfigure service */ virtual inline bool child_init (ros::NodeHandle &nh, bool &has_service); /** \brief Dynamic reconfigure callback * \param config the config object * \param level the dynamic reconfigure level */ void config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level); private: /** \brief The PCL filter implementation used. */ pcl::RadiusOutlierRemoval impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h000066400000000000000000000103751420061062000307100ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: statistical_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $ * */ #ifndef PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_ #define PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_ // PCL includes #include #include "pcl_ros/filters/filter.h" // Dynamic reconfigure #include "pcl_ros/StatisticalOutlierRemovalConfig.h" namespace pcl_ros { /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more * information check: *
    *
  • R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. * Towards 3D Point Cloud Based Object Maps for Household Environments * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. *
* * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu */ class StatisticalOutlierRemoval : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr > srv_; /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ inline void filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*(input), *(pcl_input)); impl_.setInputCloud (pcl_input); impl_.setIndices (indices); pcl::PCLPointCloud2 pcl_output; impl_.filter (pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. * \param nh ROS node handle * \param has_service set to true if the child has a Dynamic Reconfigure service */ bool child_init (ros::NodeHandle &nh, bool &has_service); /** \brief Dynamic reconfigure callback * \param config the config object * \param level the dynamic reconfigure level */ void config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level); private: /** \brief The PCL filter implementation used. */ pcl::StatisticalOutlierRemoval impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/filters/voxel_grid.h000066400000000000000000000064721420061062000252410ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: voxel_grid.h 35876 2011-02-09 01:04:36Z rusu $ * */ #ifndef PCL_ROS_FILTERS_VOXEL_H_ #define PCL_ROS_FILTERS_VOXEL_H_ // PCL includes #include #include "pcl_ros/filters/filter.h" // Dynamic reconfigure #include "pcl_ros/VoxelGridConfig.h" namespace pcl_ros { /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. * \author Radu Bogdan Rusu */ class VoxelGrid : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr > srv_; /** \brief The PCL filter implementation used. */ pcl::VoxelGrid impl_; /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ virtual void filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output); /** \brief Child initialization routine. * \param nh ROS node handle * \param has_service set to true if the child has a Dynamic Reconfigure service */ bool child_init (ros::NodeHandle &nh, bool &has_service); /** \brief Dynamic reconfigure callback * \param config the config object * \param level the dynamic reconfigure level */ void config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level); public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/impl/000077500000000000000000000000001420061062000222065ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/include/pcl_ros/impl/transforms.hpp000066400000000000000000000323121420061062000251160ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * */ #ifndef pcl_ros_IMPL_TRANSFORMS_H_ #define pcl_ros_IMPL_TRANSFORMS_H_ #include #include "pcl_ros/transforms.h" #include using pcl_conversions::fromPCL; using pcl_conversions::toPCL; namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////// template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const tf::Transform &transform) { // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. tf::Quaternion q = transform.getRotation (); Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w) tf::Vector3 v = transform.getOrigin (); Eigen::Vector3f origin (v.x (), v.y (), v.z ()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform //Eigen::Transform3f t; //t = translation * rotation; transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation); } ////////////////////////////////////////////////////////////////////////////////////////////// template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const geometry_msgs::Transform &transform) { Eigen::Quaterniond rotation; tf2::fromMsg(transform.rotation, rotation); Eigen::Vector3d origin; tf2::fromMsg(transform.translation, origin); transformPointCloudWithNormals (cloud_in, cloud_out, origin.cast(), rotation.cast()); } ////////////////////////////////////////////////////////////////////////////////////////////// template void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const tf::Transform &transform) { // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. tf::Quaternion q = transform.getRotation (); Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w) tf::Vector3 v = transform.getOrigin (); Eigen::Vector3f origin (v.x (), v.y (), v.z ()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform //Eigen::Transform3f t; //t = translation * rotation; transformPointCloud (cloud_in, cloud_out, origin, rotation); } ////////////////////////////////////////////////////////////////////////////////////////////// template void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const geometry_msgs::Transform &transform) { Eigen::Quaterniond rotation; tf2::fromMsg(transform.rotation, rotation); Eigen::Vector3d origin; tf2::fromMsg(transform.translation, origin); transformPointCloud (cloud_in, cloud_out, origin.cast(), rotation.cast()); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool transformPointCloudWithNormals (const std::string &target_frame, const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const tf::TransformListener &tf_listener) { if (cloud_in.header.frame_id == target_frame) { cloud_out = cloud_in; return (true); } tf::StampedTransform transform; try { tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform); } catch (const tf::TransformException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloudWithNormals (cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool 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::TransformStamped transform; try { transform = tf_buffer.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp); } catch (tf2::LookupException &e) { ROS_ERROR ("%s", e.what ()); return (false); } catch (tf2::ExtrapolationException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloudWithNormals (cloud_in, cloud_out, transform.transform); cloud_out.header.frame_id = target_frame; return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool transformPointCloud (const std::string &target_frame, const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const tf::TransformListener &tf_listener) { if (cloud_in.header.frame_id == target_frame) { cloud_out = cloud_in; return (true); } tf::StampedTransform transform; try { tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform); } catch (const tf::TransformException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloud (cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool transformPointCloud (const std::string &target_frame, const 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::TransformStamped transform; try { transform = tf_buffer.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp); } catch (tf2::LookupException &e) { ROS_ERROR ("%s", e.what ()); return (false); } catch (tf2::ExtrapolationException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloud (cloud_in, cloud_out, transform.transform); cloud_out.header.frame_id = target_frame; return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool transformPointCloud (const std::string &target_frame, const ros::Time & target_time, const pcl::PointCloud &cloud_in, const std::string &fixed_frame, pcl::PointCloud &cloud_out, const tf::TransformListener &tf_listener) { tf::StampedTransform transform; try { tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform); } catch (const tf::TransformException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloud (cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; cloud_out.header.seq = cloud_in.header.seq; cloud_out.header.stamp = toPCL(target_time); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool transformPointCloud (const std::string &target_frame, const ros::Time & target_time, const pcl::PointCloud &cloud_in, const std::string &fixed_frame, pcl::PointCloud &cloud_out, const tf2_ros::Buffer &tf_buffer) { geometry_msgs::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) { ROS_ERROR ("%s", e.what ()); return (false); } catch (tf2::ExtrapolationException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloud (cloud_in, cloud_out, transform.transform); cloud_out.header.frame_id = target_frame; cloud_out.header.seq = cloud_in.header.seq; cloud_out.header.stamp = toPCL(target_time); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool transformPointCloudWithNormals (const std::string &target_frame, const ros::Time & target_time, const pcl::PointCloud &cloud_in, const std::string &fixed_frame, pcl::PointCloud &cloud_out, const tf::TransformListener &tf_listener) { tf::StampedTransform transform; try { tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform); } catch (const tf::TransformException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloudWithNormals (cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; cloud_out.header.seq = cloud_in.header.seq; cloud_out.header.stamp = toPCL(target_time); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool transformPointCloudWithNormals (const std::string &target_frame, const ros::Time & target_time, const pcl::PointCloud &cloud_in, const std::string &fixed_frame, pcl::PointCloud &cloud_out, const tf2_ros::Buffer &tf_buffer) { geometry_msgs::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) { ROS_ERROR ("%s", e.what ()); return (false); } catch (tf2::ExtrapolationException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloudWithNormals (cloud_in, cloud_out, transform.transform); cloud_out.header.frame_id = target_frame; cloud_out.header.seq = cloud_in.header.seq; cloud_out.header.stamp = toPCL(target_time); return (true); } } // namespace pcl_ros #endif perception_pcl-1.7.4/pcl_ros/include/pcl_ros/io/000077500000000000000000000000001420061062000216545ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/include/pcl_ros/io/bag_io.h000066400000000000000000000107361420061062000232540ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: bag_io.h 35471 2011-01-25 06:50:00Z rusu $ * */ #ifndef PCL_ROS_IO_BAG_IO_H_ #define PCL_ROS_IO_BAG_IO_H_ #include #include #include #include #include namespace pcl_ros { //////////////////////////////////////////////////////////////////////////////////////////// /** \brief BAG PointCloud file format reader. * \author Radu Bogdan Rusu */ class BAGReader: public nodelet::Nodelet { public: typedef sensor_msgs::PointCloud2 PointCloud; typedef PointCloud::Ptr PointCloudPtr; typedef PointCloud::ConstPtr PointCloudConstPtr; /** \brief Empty constructor. */ BAGReader () : publish_rate_ (0), output_ ()/*, cloud_received_ (false)*/ {}; /** \brief Set the publishing rate in seconds. * \param publish_rate the publishing rate in seconds */ inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; } /** \brief Get the publishing rate in seconds. */ inline double getPublishRate () { return (publish_rate_); } /** \brief Get the next point cloud dataset in the BAG file. * \return the next point cloud dataset as read from the file */ inline PointCloudConstPtr getNextCloud () { if (it_ != view_.end ()) { output_ = it_->instantiate (); ++it_; } return (output_); } /** \brief Open a BAG file for reading and select a specified topic * \param file_name the BAG file to open * \param topic_name the topic that we want to read data from */ bool open (const std::string &file_name, const std::string &topic_name); /** \brief Close an open BAG file. */ inline void close () { bag_.close (); } /** \brief Nodelet initialization routine. */ virtual void onInit (); private: /** \brief The publishing interval in seconds. Set to 0 to publish once (default). */ double publish_rate_; /** \brief The BAG object. */ rosbag::Bag bag_; /** \brief The BAG view object. */ rosbag::View view_; /** \brief The BAG view iterator object. */ rosbag::View::iterator it_; /** \brief The name of the topic that contains the PointCloud data. */ std::string topic_name_; /** \brief The name of the BAG file that contains the PointCloud data. */ std::string file_name_; /** \brief The output point cloud dataset containing the points loaded from the file. */ PointCloudPtr output_; /** \brief Signals that a new PointCloud2 message has been read from the file. */ //bool cloud_received_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_ROS_IO_BAG_IO_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/io/concatenate_data.h000066400000000000000000000132071420061062000253050ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: concatenate_data.h 35231 2011-01-14 05:33:20Z rusu $ * */ #ifndef PCL_IO_CONCATENATE_DATA_H_ #define PCL_IO_CONCATENATE_DATA_H_ // ROS includes #include #include #include #include #include #include #include namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single * PointCloud output message. * \author Radu Bogdan Rusu */ class PointCloudConcatenateDataSynchronizer: public nodelet_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 */ 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); }; } #endif //#ifndef PCL_ROS_IO_CONCATENATE_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/io/concatenate_fields.h000066400000000000000000000073451420061062000256500ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: concatenate_fields.h 35052 2011-01-03 21:04:57Z rusu $ * */ #ifndef PCL_IO_CONCATENATE_FIELDS_H_ #define PCL_IO_CONCATENATE_FIELDS_H_ // ROS includes #include #include 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 */ 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_; }; } #endif //#ifndef PCL_IO_CONCATENATE_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/io/pcd_io.h000066400000000000000000000114141420061062000232630ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: pcd_io.h 35054 2011-01-03 21:16:49Z rusu $ * */ #ifndef PCL_ROS_IO_PCD_IO_H_ #define PCL_ROS_IO_PCD_IO_H_ #include #include "pcl_ros/pcl_nodelet.h" namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Point Cloud Data (PCD) file format reader. * \author Radu Bogdan Rusu */ class PCDReader : public PCLNodelet { public: typedef sensor_msgs::PointCloud2 PointCloud2; typedef PointCloud2::Ptr PointCloud2Ptr; typedef PointCloud2::ConstPtr PointCloud2ConstPtr; /** \brief Empty constructor. */ PCDReader () : publish_rate_ (0), tf_frame_ ("/base_link") {}; virtual void onInit (); /** \brief Set the publishing rate in seconds. * \param publish_rate the publishing rate in seconds */ inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; } /** \brief Get the publishing rate in seconds. */ inline double getPublishRate () { return (publish_rate_); } /** \brief Set the TF frame the PointCloud will be published in. * \param tf_frame the TF frame the PointCloud will be published in */ inline void setTFframe (std::string tf_frame) { tf_frame_ = tf_frame; } /** \brief Get the TF frame the PointCloud is published in. */ inline std::string getTFframe () { return (tf_frame_); } protected: /** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */ double publish_rate_; /** \brief The TF frame the data should be published in ("/base_link" by default). */ std::string tf_frame_; /** \brief The name of the file that contains the PointCloud data. */ std::string file_name_; /** \brief The output point cloud dataset containing the points loaded from the file. */ PointCloud2Ptr output_; private: /** \brief The PCL implementation used. */ pcl::PCDReader impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Point Cloud Data (PCD) file format writer. * \author Radu Bogdan Rusu */ class PCDWriter : public PCLNodelet { public: PCDWriter () : file_name_ (""), binary_mode_ (true) {} typedef sensor_msgs::PointCloud2 PointCloud2; typedef PointCloud2::Ptr PointCloud2Ptr; typedef PointCloud2::ConstPtr PointCloud2ConstPtr; virtual void onInit (); void input_callback (const PointCloud2ConstPtr &cloud); /** \brief The input PointCloud subscriber. */ ros::Subscriber sub_input_; protected: /** \brief The name of the file that contains the PointCloud data. */ std::string file_name_; /** \brief Set to true if the output files should be saved in binary mode (true). */ bool binary_mode_; private: /** \brief The PCL implementation used. */ pcl::PCDWriter impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_ROS_IO_PCD_IO_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/pcl_nodelet.h000066400000000000000000000235701420061062000237150ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: pcl_nodelet.h 33238 2010-03-11 00:46:58Z rusu $ * */ /** \author Radu Bogdan Rusu **/ #ifndef PCL_NODELET_H_ #define PCL_NODELET_H_ #include // PCL includes #include #include #include #include #include #include "pcl_ros/point_cloud.h" // ROS Nodelet includes #include #include #include #include #include // Include TF #include using pcl_conversions::fromPCL; namespace pcl_ros { //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */ class PCLNodelet : public nodelet_topic_tools::NodeletLazy { public: typedef sensor_msgs::PointCloud2 PointCloud2; typedef pcl::PointCloud PointCloud; typedef boost::shared_ptr PointCloudPtr; typedef boost::shared_ptr PointCloudConstPtr; typedef pcl_msgs::PointIndices PointIndices; typedef PointIndices::Ptr PointIndicesPtr; typedef PointIndices::ConstPtr PointIndicesConstPtr; typedef pcl_msgs::ModelCoefficients ModelCoefficients; typedef ModelCoefficients::Ptr ModelCoefficientsPtr; typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; typedef pcl::IndicesPtr IndicesPtr; typedef pcl::IndicesConstPtr IndicesConstPtr; /** \brief Empty constructor. */ PCLNodelet () : use_indices_ (false), latched_indices_ (false), max_queue_size_ (3), approximate_sync_ (false) {}; protected: /** \brief Set to true if point indices are used. * * When receiving a point cloud, if use_indices_ is false, the entire * point cloud is processed for the given operation. If use_indices_ is * true, then the ~indices topic is read to get the vector of point * indices specifying the subset of the point cloud that will be used for * the operation. In the case where use_indices_ is true, the ~input and * ~indices topics must be synchronised in time, either exact or within a * specified jitter. See also @ref latched_indices_ and approximate_sync. **/ bool use_indices_; /** \brief Set to true if the indices topic is latched. * * If use_indices_ is true, the ~input and ~indices topics generally must * be synchronised in time. By setting this flag to true, the most recent * value from ~indices can be used instead of requiring a synchronised * message. **/ bool latched_indices_; /** \brief The message filter subscriber for PointCloud2. */ message_filters::Subscriber sub_input_filter_; /** \brief The message filter subscriber for PointIndices. */ message_filters::Subscriber sub_indices_filter_; /** \brief The output PointCloud publisher. */ ros::Publisher pub_output_; /** \brief The maximum queue size (default: 3). */ int max_queue_size_; /** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ bool approximate_sync_; /** \brief TF listener object. */ tf::TransformListener tf_listener_; /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). * \param cloud the point cloud to test * \param topic_name an optional topic name (only used for printing, defaults to "input") */ inline bool isValid (const PointCloud2::ConstPtr &cloud, const std::string &topic_name = "input") { if (cloud->width * cloud->height * cloud->point_step != cloud->data.size ()) { NODELET_WARN ("[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->data.size (), cloud->width, cloud->height, cloud->point_step, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); return (false); } return (true); } /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). * \param cloud the point cloud to test * \param topic_name an optional topic name (only used for printing, defaults to "input") */ inline bool isValid (const PointCloudConstPtr &cloud, const std::string &topic_name = "input") { if (cloud->width * cloud->height != cloud->points.size ()) { NODELET_WARN ("[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->points.size (), cloud->width, cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); return (false); } return (true); } /** \brief Test whether a given PointIndices message is "valid" (i.e., has values). * \param indices the point indices message to test * \param topic_name an optional topic name (only used for printing, defaults to "indices") */ inline bool isValid (const PointIndicesConstPtr &/*indices*/, const std::string &/*topic_name*/ = "indices") { /*if (indices->indices.empty ()) { NODELET_WARN ("[%s] Empty indices (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); return (true); }*/ return (true); } /** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values). * \param model the model coefficients to test * \param topic_name an optional topic name (only used for printing, defaults to "model") */ inline bool isValid (const ModelCoefficientsConstPtr &/*model*/, const std::string &/*topic_name*/ = "model") { /*if (model->values.empty ()) { NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); return (false); }*/ return (true); } /** \brief Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */ virtual void subscribe () {} virtual void unsubscribe () {} /** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */ virtual void onInit () { nodelet_topic_tools::NodeletLazy::onInit(); // Parameters that we care about only at startup pnh_->getParam ("max_queue_size", max_queue_size_); // ---[ Optional parameters pnh_->getParam ("use_indices", use_indices_); pnh_->getParam ("latched_indices", latched_indices_); pnh_->getParam ("approximate_sync", approximate_sync_); NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n" " - approximate_sync : %s\n" " - use_indices : %s\n" " - latched_indices : %s\n" " - max_queue_size : %d", getName ().c_str (), (approximate_sync_) ? "true" : "false", (use_indices_) ? "true" : "false", (latched_indices_) ? "true" : "false", max_queue_size_); } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } #endif //#ifndef PCL_NODELET_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/point_cloud.h000066400000000000000000000305111420061062000237350ustar00rootroot00000000000000#ifndef pcl_ROS_POINT_CLOUD_H_ #define pcl_ROS_POINT_CLOUD_H_ #include #include #include #include #include #include #include #include #include #include namespace pcl { namespace detail { template struct FieldStreamer { FieldStreamer(Stream& stream) : stream_(stream) {} template void operator() () { const char* name = 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 pcl::detail } // namespace pcl namespace ros { 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 ros::message_traits namespace serialization { template struct Serializer > { template inline static void write(Stream& stream, const pcl::PointCloud& m) { stream.next(m.header); // Ease the user's burden on specifying width/height for unorganized datasets uint32_t height = m.height, width = m.width; if (height == 0 && width == 0) { width = m.points.size(); height = 1; } stream.next(height); stream.next(width); // Stream out point field metadata typedef typename pcl::traits::fieldList::type FieldList; uint32_t fields_size = boost::mpl::size::value; stream.next(fields_size); pcl::for_each_type(pcl::detail::FieldStreamer(stream)); // Assume little-endian... uint8_t is_bigendian = false; stream.next(is_bigendian); // Write out point data as binary blob uint32_t point_step = sizeof(T); stream.next(point_step); uint32_t row_step = point_step * width; stream.next(row_step); uint32_t data_size = row_step * height; stream.next(data_size); memcpy(stream.advance(data_size), m.points.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 static pcl::MsgFieldMap mapping; static boost::mutex mutex; if (mapping.empty()) { boost::mutex::scoped_lock lock(mutex); 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 ros::serialization /// @todo Printer specialization in message_operations } // namespace ros // test if testing machinery can be implemented #if defined(__cpp_rvalue_references) && defined(__cpp_constexpr) #define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1 #else #define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0 #endif #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED #include // for std::is_same #include // for std::shared_ptr #include #if PCL_VERSION_COMPARE(>=, 1, 11, 0) #include #elif PCL_VERSION_COMPARE(>=, 1, 10, 0) #include #endif #endif namespace pcl { namespace detail { #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED #if PCL_VERSION_COMPARE(>=, 1, 10, 0) template constexpr static bool pcl_uses_boost = std::is_same, pcl::shared_ptr>::value; #else template constexpr static bool pcl_uses_boost = true; #endif template struct Holder { SharedPointer p; Holder(const SharedPointer &p) : p(p) {} Holder(const Holder &other) : p(other.p) {} Holder(Holder &&other) : p(std::move(other.p)) {} void operator () (...) { p.reset(); } }; template inline std::shared_ptr to_std_ptr(const boost::shared_ptr &p) { typedef Holder> H; if(H *h = boost::get_deleter(p)) { return h->p; } else { return std::shared_ptr(p.get(), Holder>(p)); } } template inline boost::shared_ptr to_boost_ptr(const std::shared_ptr &p) { typedef Holder> H; if(H * h = std::get_deleter(p)) { return h->p; } else { return boost::shared_ptr(p.get(), Holder>(p)); } } #endif } // namespace pcl::detail // add functions to convert to smart pointer used by ROS template inline boost::shared_ptr ros_ptr(const boost::shared_ptr &p) { return p; } #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED template inline boost::shared_ptr ros_ptr(const std::shared_ptr &p) { return detail::to_boost_ptr(p); } // add functions to convert to smart pointer used by PCL, based on PCL's own pointer template >::type> inline std::shared_ptr pcl_ptr(const std::shared_ptr &p) { return p; } template >::type> inline std::shared_ptr pcl_ptr(const boost::shared_ptr &p) { return detail::to_std_ptr(p); } template >::type> inline boost::shared_ptr pcl_ptr(const std::shared_ptr &p) { return detail::to_boost_ptr(p); } template >::type> inline boost::shared_ptr pcl_ptr(const boost::shared_ptr &p) { return p; } #else template inline boost::shared_ptr pcl_ptr(const boost::shared_ptr &p) { return p; } #endif } // namespace pcl #endif perception_pcl-1.7.4/pcl_ros/include/pcl_ros/publisher.h000066400000000000000000000101401420061062000234070ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: publisher.h 33238 2010-03-11 00:46:58Z rusu $ * */ /** \author Patrick Mihelich @b Publisher represents a ROS publisher for the templated PointCloud implementation. **/ #ifndef PCL_ROS_PUBLISHER_H_ #define PCL_ROS_PUBLISHER_H_ #include #include #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_) ? (void*)1 : (void*)0; } protected: ros::Publisher pub_; }; template class Publisher : public BasePublisher { public: Publisher () {} Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) { advertise (nh, topic, queue_size); } ~Publisher () {} inline void publish (const boost::shared_ptr > &point_cloud) const { publish (*point_cloud); } inline void publish (const pcl::PointCloud& point_cloud) const { // Fill point cloud binary data sensor_msgs::PointCloud2 msg; pcl::toROSMsg (point_cloud, msg); pub_.publish (boost::make_shared (msg)); } }; template <> class Publisher : public BasePublisher { public: Publisher () {} Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) { advertise (nh, topic, queue_size); } ~Publisher () {} void publish (const sensor_msgs::PointCloud2Ptr& point_cloud) const { pub_.publish (point_cloud); //pub_.publish (*point_cloud); } void publish (const sensor_msgs::PointCloud2& point_cloud) const { pub_.publish (boost::make_shared (point_cloud)); //pub_.publish (point_cloud); } }; } #endif //#ifndef PCL_ROS_PUBLISHER_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/segmentation/000077500000000000000000000000001420061062000237425ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h000066400000000000000000000105631420061062000275160ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: extract_clusters.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ #define PCL_ROS_EXTRACT_CLUSTERS_H_ #include #include "pcl_ros/pcl_nodelet.h" // Dynamic reconfigure #include #include "pcl_ros/EuclideanClusterExtractionConfig.h" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. * \author Radu Bogdan Rusu */ class EuclideanClusterExtraction : public PCLNodelet { public: /** \brief Empty constructor. */ EuclideanClusterExtraction () : publish_indices_ (false), max_clusters_ (std::numeric_limits::max ()) {}; protected: // ROS nodelet attributes /** \brief Publish indices or convert to PointCloud clusters. Default: false */ bool publish_indices_; /** \brief Maximum number of clusters to publish. */ int max_clusters_; /** \brief Pointer to a dynamic reconfigure service. */ boost::shared_ptr > srv_; /** \brief Nodelet initialization routine. */ void onInit (); /** \brief 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 }; } #endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h000066400000000000000000000125671420061062000320670ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: extract_polygonal_prism_data.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ #define PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ #include "pcl_ros/pcl_nodelet.h" #include #include #include // PCL includes #include // Dynamic reconfigure #include #include "pcl_ros/ExtractPolygonalPrismDataConfig.h" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; /** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with * a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying * inside it. * * An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane). * \author Radu Bogdan Rusu */ class ExtractPolygonalPrismData : public PCLNodelet { typedef pcl::PointCloud PointCloud; typedef 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 }; } #endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h000066400000000000000000000275541420061062000274530ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: sac_segmentation.h 35564 2011-01-27 07:32:12Z rusu $ * */ #ifndef PCL_ROS_SAC_SEGMENTATION_H_ #define PCL_ROS_SAC_SEGMENTATION_H_ #include "pcl_ros/pcl_nodelet.h" #include // PCL includes #include // Dynamic reconfigure #include #include "pcl_ros/SACSegmentationConfig.h" #include "pcl_ros/SACSegmentationFromNormalsConfig.h" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; //////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in * the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. * \author Radu Bogdan Rusu */ class SACSegmentation : public PCLNodelet { typedef pcl::PointCloud PointCloud; typedef 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 }; } #endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/segmentation/segment_differences.h000066400000000000000000000107121420061062000301130ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: segment_differences.h 35361 2011-01-20 04:34:49Z rusu $ * */ #ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ #define PCL_ROS_SEGMENT_DIFFERENCES_H_ #include #include "pcl_ros/pcl_nodelet.h" // Dynamic reconfigure #include #include "pcl_ros/SegmentDifferencesConfig.h" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the * difference between them for a maximum given distance threshold. * \author Radu Bogdan Rusu */ class SegmentDifferences : public PCLNodelet { typedef pcl::PointCloud PointCloud; typedef 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 }; } #endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/surface/000077500000000000000000000000001420061062000226755ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/include/pcl_ros/surface/convex_hull.h000066400000000000000000000067471420061062000254120ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: convex_hull.h 36116 2011-02-22 00:05:23Z rusu $ * */ #ifndef PCL_ROS_CONVEX_HULL_2D_H_ #define PCL_ROS_CONVEX_HULL_2D_H_ #include "pcl_ros/pcl_nodelet.h" // PCL includes #include 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 }; } #endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/surface/moving_least_squares.h000066400000000000000000000134051420061062000273030ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: moving_least_squares.h 36097 2011-02-20 14:18:58Z marton $ * */ #ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ #define PCL_ROS_MOVING_LEAST_SQUARES_H_ #include "pcl_ros/pcl_nodelet.h" // PCL includes #include #include // for KdTree // Dynamic reconfigure #include #include "pcl_ros/MLSConfig.h" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; /** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation. * The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters. * Normals are estimated at each point as well and published on a separate topic. * \author Radu Bogdan Rusu, Zoltan-Csaba Marton */ class MovingLeastSquares : public PCLNodelet { typedef pcl::PointXYZ PointIn; typedef pcl::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 }; } #endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ perception_pcl-1.7.4/pcl_ros/include/pcl_ros/transforms.h000066400000000000000000000312011420061062000236110ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * */ #ifndef pcl_ROS_TRANSFORMS_H_ #define pcl_ROS_TRANSFORMS_H_ #include #include #include #include #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 output point cloud * \param transform a rigid transformation from tf * \note calls the Eigen version */ template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const tf::Transform &transform); /** \brief Transform a point cloud and rotate its normals using an Eigen transform. * \param cloud_in the input point cloud * \param cloud_out the output 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::Transform &transform); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param cloud_in the input point cloud * \param cloud_out the output point cloud * \param tf_listener a TF listener object */ template bool transformPointCloudWithNormals (const std::string &target_frame, const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const tf::TransformListener &tf_listener); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param cloud_in the input point cloud * \param cloud_out the output point cloud * \param tf_buffer a TF2 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 output point cloud * \param tf_listener a TF listener object */ template bool transformPointCloudWithNormals (const std::string &target_frame, const ros::Time & target_time, const pcl::PointCloud &cloud_in, const std::string &fixed_frame, pcl::PointCloud &cloud_out, const tf::TransformListener &tf_listener); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param target_time the target timestamp * \param cloud_in the input point cloud * \param fixed_frame fixed TF frame * \param cloud_out the output point cloud * \param tf_buffer a TF2 buffer object */ template bool transformPointCloudWithNormals (const std::string &target_frame, const ros::Time & target_time, const pcl::PointCloud &cloud_in, const std::string &fixed_frame, pcl::PointCloud &cloud_out, const 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 output point cloud * \param transform a rigid transformation from tf * \note calls the Eigen version */ template void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const tf::Transform &transform); /** \brief Apply a rigid transform defined by a 3D offset and a quaternion * \param cloud_in the input point cloud * \param cloud_out the output 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::Transform &transform); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param cloud_in the input point cloud * \param cloud_out the output point cloud * \param tf_listener a TF listener object */ template bool transformPointCloud (const std::string &target_frame, const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const tf::TransformListener &tf_listener); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param cloud_in the input point cloud * \param cloud_out the output point cloud * \param tf_buffer a TF2 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 output point cloud * \param tf_listener a TF listener object */ template bool transformPointCloud (const std::string &target_frame, const ros::Time & target_time, const pcl::PointCloud &cloud_in, const std::string &fixed_frame, pcl::PointCloud &cloud_out, const tf::TransformListener &tf_listener); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param target_time the target timestamp * \param cloud_in the input point cloud * \param fixed_frame fixed TF frame * \param cloud_out the output point cloud * \param tf_buffer a TF2 buffer object */ template bool transformPointCloud (const std::string &target_frame, const ros::Time & target_time, const pcl::PointCloud &cloud_in, const std::string &fixed_frame, pcl::PointCloud &cloud_out, const tf2_ros::Buffer &tf_buffer); /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. * \param target_frame the target TF frame * \param in the input PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset * \param tf_listener a TF listener object */ bool transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener); /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. * \param target_frame the target TF frame * \param in the input PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset * \param tf_buffer a TF2 buffer object */ bool transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, sensor_msgs::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 tf::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out); /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. * \param target_frame the target TF frame * \param net_transform the TF transformer object * \param in the input PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset */ void transformPointCloud (const std::string &target_frame, const geometry_msgs::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out); /** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix. * \param transform the transformation to use on the points * \param in the input PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset */ void transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out); /** \brief Obtain the transformation matrix from TF into an Eigen form * \param bt the TF transformation * \param out_mat the Eigen transformation */ void transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat); /** \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::Transform& bt, Eigen::Matrix4f &out_mat); } #endif // PCL_ROS_TRANSFORMS_H_ perception_pcl-1.7.4/pcl_ros/package.xml000066400000000000000000000037631420061062000203070ustar00rootroot00000000000000 pcl_ros 1.7.4 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry processing in ROS. Open Perception Julius Kammerl William Woodall Paul Bovbel Kentaro Wada Steve Macenski BSD http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl catkin rosconsole roslib dynamic_reconfigure eigen geometry_msgs message_filters nodelet nodelet_topic_tools libpcl-all-dev pcl_conversions pcl_msgs pluginlib rosbag roscpp sensor_msgs std_msgs tf tf2 tf2_eigen tf2_ros rostest perception_pcl-1.7.4/pcl_ros/plugins/000077500000000000000000000000001420061062000176425ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/plugins/nodelet/000077500000000000000000000000001420061062000212745ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/plugins/nodelet/libpcl_ros_features.xml000066400000000000000000000073331420061062000260520ustar00rootroot00000000000000 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. perception_pcl-1.7.4/pcl_ros/plugins/nodelet/libpcl_ros_filters.xml000066400000000000000000000033571420061062000257060ustar00rootroot00000000000000 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. perception_pcl-1.7.4/pcl_ros/plugins/nodelet/libpcl_ros_io.xml000066400000000000000000000042211420061062000246340ustar00rootroot00000000000000 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. perception_pcl-1.7.4/pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml000066400000000000000000000034511420061062000267260ustar00rootroot00000000000000 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. perception_pcl-1.7.4/pcl_ros/plugins/nodelet/libpcl_ros_surface.xml000066400000000000000000000011061420061062000256540ustar00rootroot00000000000000 MovingLeastSquares is an implementation of the MLS algorithm for data reconstruction through bivariate polynomial fitting. ConvexHull2D represents a 2D ConvexHull implementation. perception_pcl-1.7.4/pcl_ros/samples/000077500000000000000000000000001420061062000176255ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/data/000077500000000000000000000000001420061062000205365ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/data/.gitignore000066400000000000000000000000161420061062000225230ustar00rootroot00000000000000* !.gitignore perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/000077500000000000000000000000001420061062000212665ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/features/000077500000000000000000000000001420061062000231045ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch000066400000000000000000000022061420061062000275170ustar00rootroot00000000000000 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 perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/filters/000077500000000000000000000000001420061062000227365ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/filters/config/000077500000000000000000000000001420061062000242035ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz000066400000000000000000000122311420061062000327320ustar00rootroot00000000000000Panels: - 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 perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/filters/config/voxel_grid.rviz000066400000000000000000000121751420061062000272670ustar00rootroot00000000000000Panels: - 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 perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/filters/sample_statistical_outlier_removal.launch000066400000000000000000000020761420061062000333140ustar00rootroot00000000000000 mean_k: 10 stddev: 1.0 topic: /statistical_outlier_removal/output hz: 20 hzerror: 15 test_duration: 5.0 perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch000066400000000000000000000021051420061062000276330ustar00rootroot00000000000000 filter_field_name: '' leaf_size: $(arg leaf_size) topic: /voxel_grid/output hz: 20 hzerror: 15 test_duration: 5.0 perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/io/000077500000000000000000000000001420061062000216755ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch000066400000000000000000000021321420061062000277050ustar00rootroot00000000000000 output_frame: /base_link input_topics: - /voxel_grid/output - /voxel_grid/output topic: /concatenate_data/output hz: 10 hzerror: 8 test_duration: 5.0 perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/segmentation/000077500000000000000000000000001420061062000237635ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch000066400000000000000000000021341420061062000321160ustar00rootroot00000000000000 cluster_tolerance: 0.03 spatial_locator: 1 # FLANN topic: /extract_clusters/output hz: 3000 hzerror: 2400 test_duration: 5.0 perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/surface/000077500000000000000000000000001420061062000227165ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch000066400000000000000000000017531420061062000300070ustar00rootroot00000000000000 topic: /convex_hull/output hz: 10 hzerror: 8 test_duration: 5.0 perception_pcl-1.7.4/pcl_ros/src/000077500000000000000000000000001420061062000167505ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/pcl_ros/000077500000000000000000000000001420061062000204115ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/pcl_ros/__init__.py000066400000000000000000000000001420061062000225100ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/000077500000000000000000000000001420061062000222275ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/boundary.cpp000066400000000000000000000056101420061062000245600ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/boundary.h" void pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/feature.cpp000066400000000000000000000615601420061062000243760ustar00rootroot00000000000000/* * 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 "pcl_ros/features/feature.h" //////////////////////////////////////////////////////////////////////////////////////////// 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 ((int)(cloud->width * cloud->height) < k_) { NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height)); emptyPublish (cloud); return; } // If indices given... IndicesPtr vindices; if (indices && !indices->header.frame_id.empty ()) vindices.reset (new std::vector (indices->indices)); computePublish (cloud, cloud_surface, vindices); } ////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::FeatureFromNormals::onInit () { // Call the super onInit () PCLNodelet::onInit (); // Call the child init childInit (*pnh_); // Allow each individual class that inherits from us to declare their own Publisher // This is useful for Publisher >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) { NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); return; } if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) { NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); return; } // ---[ Optional parameters pnh_->getParam ("use_surface", use_surface_); // 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 ((int)(cloud->width * cloud->height) < k_) { NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height)); emptyPublish (cloud); return; } // If indices given... IndicesPtr vindices; if (indices && !indices->header.frame_id.empty ()) vindices.reset (new std::vector (indices->indices)); computePublish (cloud, cloud_normals, cloud_surface, vindices); } perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/fpfh.cpp000066400000000000000000000056151420061062000236650ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/fpfh.h" void pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp000066400000000000000000000056551420061062000245440ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/fpfh_omp.h" void pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/moment_invariants.cpp000066400000000000000000000056021420061062000264730ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/moment_invariants.h" void pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/normal_3d.cpp000066400000000000000000000054541420061062000246210ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/normal_3d.h" void pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp000066400000000000000000000055111420061062000254660ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/normal_3d_omp.h" void pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp000066400000000000000000000057241420061062000254500ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: normal_3d_tbb.cpp 35625 2011-01-31 07:56:13Z gbiggs $ * */ #include #include "pcl_ros/features/normal_3d_tbb.h" #if defined HAVE_TBB void pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloud output; output.header = cloud->header; pub_output_.publish (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 perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/pfh.cpp000066400000000000000000000056031420061062000235140ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: pfh.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/pfh.h" void pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp000066400000000000000000000060451420061062000272040ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: principal_curvatures.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/principal_curvatures.h" void pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/shot.cpp000066400000000000000000000055161420061062000237170ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2015, Ryohei Ueda, JSK Lab * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of JSK Lab. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * */ #include #include "pcl_ros/features/shot.h" void pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/shot_omp.cpp000066400000000000000000000055521420061062000245720ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2015, Ryohei Ueda, JSK Lab * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of JSK Lab. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * */ #include #include "pcl_ros/features/shot_omp.h" void pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/features/vfh.cpp000066400000000000000000000056031420061062000235220ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: vfh.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/features/vfh.h" void pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) { PointCloudOut output; output.header = cloud->header; pub_output_.publish (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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/000077500000000000000000000000001420061062000220615ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/crop_box.cpp000066400000000000000000000113051420061062000244000ustar00rootroot00000000000000/* * * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: cropbox.cpp * */ #include #include "pcl_ros/filters/crop_box.h" ////////////////////////////////////////////////////////////////////////////////////////////// bool pcl_ros::CropBox::child_init (ros::NodeHandle &nh, bool &has_service) { // Enable the dynamic reconfigure service has_service = true; srv_ = boost::make_shared > (nh); dynamic_reconfigure::Server::CallbackType f = boost::bind (&CropBox::config_callback, this, _1, _2); srv_->setCallback (f); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t /*level*/) { boost::mutex::scoped_lock lock (mutex_); Eigen::Vector4f min_point,max_point; min_point = impl_.getMin(); max_point = impl_.getMax(); Eigen::Vector4f new_min_point, new_max_point; new_min_point << config.min_x, config.min_y, config.min_z, 0.0; new_max_point << config.max_x, config.max_y, config.max_z, 0.0; // Check the current values for minimum point if (min_point != new_min_point) { NODELET_DEBUG ("[%s::config_callback] Setting the minimum point to: %f %f %f.", getName ().c_str (), new_min_point(0),new_min_point(1),new_min_point(2)); // Set the filter min point if different impl_.setMin(new_min_point); } // Check the current values for the maximum point if (max_point != new_max_point) { NODELET_DEBUG ("[%s::config_callback] Setting the maximum point to: %f %f %f.", getName ().c_str (), new_max_point(0),new_max_point(1),new_max_point(2)); // Set the filter max point if different impl_.setMax(new_max_point); } // Check the current value for keep_organized if (impl_.getKeepOrganized () != config.keep_organized) { NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); // Call the virtual method in the child impl_.setKeepOrganized (config.keep_organized); } // Check the current value for the negative flag if (impl_.getNegative() != config.negative) { NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.negative ? "true" : "false"); // Call the virtual method in the child impl_.setNegative(config.negative); } // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); } if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); } } typedef pcl_ros::CropBox CropBox; PLUGINLIB_EXPORT_CLASS(CropBox,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/extract_indices.cpp000066400000000000000000000056001420061062000257360ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: extract_indices.cpp 35876 2011-02-09 01:04:36Z rusu $ * */ #include #include "pcl_ros/filters/extract_indices.h" ////////////////////////////////////////////////////////////////////////////////////////////// bool pcl_ros::ExtractIndices::child_init (ros::NodeHandle &nh, bool &has_service) { has_service = true; srv_ = boost::make_shared > (nh); dynamic_reconfigure::Server::CallbackType f = boost::bind (&ExtractIndices::config_callback, this, _1, _2); srv_->setCallback (f); use_indices_ = true; return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ExtractIndices::config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t /*level*/) { boost::mutex::scoped_lock lock (mutex_); if (impl_.getNegative () != config.negative) { impl_.setNegative (config.negative); NODELET_DEBUG ("[%s::config_callback] Setting the extraction to: %s.", getName ().c_str (), (config.negative ? "indices" : "everything but the indices")); } } typedef pcl_ros::ExtractIndices ExtractIndices; PLUGINLIB_EXPORT_CLASS(ExtractIndices,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/filter.cpp000066400000000000000000000244611420061062000240610ustar00rootroot00000000000000/* * 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/transforms.h" #include "pcl_ros/filters/filter.h" /*//#include //#include */ /*//typedef pcl::PixelGrid PixelGrid; //typedef pcl::FilterDimension FilterDimension; */ // Include the implementations instead of compiling them separately to speed up compile time //#include "extract_indices.cpp" //#include "passthrough.cpp" //#include "project_inliers.cpp" //#include "radius_outlier_removal.cpp" //#include "statistical_outlier_removal.cpp" //#include "voxel_grid.cpp" /*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet); //PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet); */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices) { PointCloud2 output; // Call the virtual method in the child filter (input, indices, output); PointCloud2::Ptr cloud_tf (new PointCloud2 (output)); // set the output by default // Check whether the user has given a different output TF frame if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_) { NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ()); // Convert the cloud into the different frame PointCloud2 cloud_transformed; if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_)) { NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ()); return; } cloud_tf.reset (new PointCloud2 (cloud_transformed)); } if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_) // no tf_output_frame given, transform the dataset to its original frame { NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ()); // Convert the cloud into the different frame PointCloud2 cloud_transformed; if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_)) { NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ()); return; } cloud_tf.reset (new PointCloud2 (cloud_transformed)); } // Copy timestamp to keep it cloud_tf->header.stamp = input->header.stamp; // Publish a boost shared ptr pub_output_.publish (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 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); if (approximate_sync_) { sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); } else { sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); } } else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ())); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::unsubscribe() { if (use_indices_) { sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); } else sub_input_.shutdown(); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::onInit () { // Call the super onInit () PCLNodelet::onInit (); // Call the child's local init bool has_service = false; if (!child_init (*pnh_, has_service)) { NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ()); return; } pub_output_ = advertise (*pnh_, "output", max_queue_size_); // Enable the dynamic reconfigure service if (!has_service) { srv_ = boost::make_shared > (*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2); srv_->setCallback (f); } NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ()); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t /*level*/) { // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); } if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); } } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices) { // If cloud is given, check if it's valid if (!isValid (cloud)) { NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); return; } // If indices are given, check if they are valid if (indices && !isValid (indices)) { NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); return; } /// DEBUG if (indices) NODELET_DEBUG ("[%s::input_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); else NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); /// // Check whether the user has given a different input TF frame tf_input_orig_frame_ = cloud->header.frame_id; PointCloud2::ConstPtr cloud_tf; if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) { NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); // Save the original frame ID // Convert the cloud into the different frame PointCloud2 cloud_transformed; if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) { NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); return; } cloud_tf = boost::make_shared (cloud_transformed); } else cloud_tf = cloud; // Need setInputCloud () here because we have to extract x/y/z IndicesPtr vindices; if (indices) vindices.reset (new std::vector (indices->indices)); computePublish (cloud_tf, vindices); } perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/passthrough.cpp000066400000000000000000000125661420061062000251460ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: passthrough.cpp 36194 2011-02-23 07:49:21Z rusu $ * */ #include #include "pcl_ros/filters/passthrough.h" ////////////////////////////////////////////////////////////////////////////////////////////// bool pcl_ros::PassThrough::child_init (ros::NodeHandle &nh, bool &has_service) { // Enable the dynamic reconfigure service has_service = true; srv_ = boost::make_shared > (nh); dynamic_reconfigure::Server::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2); srv_->setCallback (f); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t /*level*/) { boost::mutex::scoped_lock lock (mutex_); double filter_min, filter_max; impl_.getFilterLimits (filter_min, filter_max); // Check the current values for filter min-max if (filter_min != config.filter_limit_min) { filter_min = config.filter_limit_min; NODELET_DEBUG ("[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_min); // Set the filter min-max if different impl_.setFilterLimits (filter_min, filter_max); } // Check the current values for filter min-max if (filter_max != config.filter_limit_max) { filter_max = config.filter_limit_max; NODELET_DEBUG ("[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_max); // Set the filter min-max if different impl_.setFilterLimits (filter_min, filter_max); } // Check the current value for the filter field //std::string filter_field = impl_.getFilterFieldName (); if (impl_.getFilterFieldName () != config.filter_field_name) { // Set the filter field if different impl_.setFilterFieldName (config.filter_field_name); NODELET_DEBUG ("[%s::config_callback] Setting the filter field name to: %s.", getName ().c_str (), config.filter_field_name.c_str ()); } // Check the current value for keep_organized if (impl_.getKeepOrganized () != config.keep_organized) { NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); // Call the virtual method in the child impl_.setKeepOrganized (config.keep_organized); } // Check the current value for the negative flag #if PCL_VERSION_COMPARE(<, 1, 10, 0) if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) #else if (impl_.getNegative () != config.filter_limit_negative) #endif { NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); // Call the virtual method in the child #if PCL_VERSION_COMPARE(<, 1, 10, 0) impl_.setFilterLimitsNegative (config.filter_limit_negative); #else impl_.setNegative (config.filter_limit_negative); #endif } // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); } if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); } } typedef pcl_ros::PassThrough PassThrough; PLUGINLIB_EXPORT_CLASS(PassThrough,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/project_inliers.cpp000066400000000000000000000152061420061062000257640ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: project_inliers.cpp 35876 2011-02-09 01:04:36Z rusu $ * */ #include #include "pcl_ros/filters/project_inliers.h" ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ProjectInliers::onInit () { PCLNodelet::onInit (); // ---[ Mandatory parameters // The type of model to use (user given parameter). int model_type; if (!pnh_->getParam ("model_type", model_type)) { NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ()); return; } // ---[ Optional parameters // True if all data will be returned, false if only the projected inliers. Default: false. bool copy_all_data = false; // True if all fields will be returned, false if only XYZ. Default: true. bool copy_all_fields = true; pnh_->getParam ("copy_all_data", copy_all_data); pnh_->getParam ("copy_all_fields", copy_all_fields); pub_output_ = advertise (*pnh_, "output", max_queue_size_); // Subscribe to the input using a filter sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" " - model_type : %d\n" " - copy_all_data : %s\n" " - copy_all_fields : %s", getName ().c_str (), model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); // Set given parameters here impl_.setModelType (model_type); impl_.setCopyAllFields (copy_all_fields); impl_.setCopyAllData (copy_all_data); onInitPostProcess (); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ProjectInliers::subscribe () { /* TODO : implement use_indices_ if (use_indices_) {*/ sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); sub_model_.subscribe (*pnh_, "model", max_queue_size_); if (approximate_sync_) { sync_input_indices_model_a_ = boost::make_shared > > (max_queue_size_); sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); } else { sync_input_indices_model_e_ = boost::make_shared > > (max_queue_size_); sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); } } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ProjectInliers::unsubscribe () { /* TODO : implement use_indices_ if (use_indices_) {*/ sub_input_filter_.unsubscribe (); sub_model_.unsubscribe (); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices, const ModelCoefficientsConstPtr &model) { if (pub_output_.getNumSubscribers () <= 0) return; if (!isValid (model) || !isValid (indices) || !isValid (cloud)) { NODELET_ERROR ("[%s::input_indices_model_callback] Invalid input!", getName ().c_str ()); return; } NODELET_DEBUG ("[%s::input_indices_model_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (), model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ()); tf_input_orig_frame_ = cloud->header.frame_id; IndicesPtr vindices; if (indices) vindices.reset (new std::vector (indices->indices)); model_ = model; computePublish (cloud, vindices); } typedef pcl_ros::ProjectInliers ProjectInliers; PLUGINLIB_EXPORT_CLASS(ProjectInliers,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp000066400000000000000000000063331420061062000273510ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: radius_outlier_removal.cpp 33319 2010-10-15 04:49:28Z rusu $ * */ #include #include "pcl_ros/filters/radius_outlier_removal.h" ////////////////////////////////////////////////////////////////////////////////////////////// bool pcl_ros::RadiusOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) { // Enable the dynamic reconfigure service has_service = true; srv_ = boost::make_shared > (nh); dynamic_reconfigure::Server::CallbackType f = boost::bind (&RadiusOutlierRemoval::config_callback, this, _1, _2); srv_->setCallback (f); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::RadiusOutlierRemoval::config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t /*level*/) { boost::mutex::scoped_lock lock (mutex_); if (impl_.getMinNeighborsInRadius () != config.min_neighbors) { impl_.setMinNeighborsInRadius (config.min_neighbors); NODELET_DEBUG ("[%s::config_callback] Setting the number of neighbors in radius: %d.", getName ().c_str (), config.min_neighbors); } if (impl_.getRadiusSearch () != config.radius_search) { impl_.setRadiusSearch (config.radius_search); NODELET_DEBUG ("[%s::config_callback] Setting the radius to search neighbors: %f.", getName ().c_str (), config.radius_search); } } typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp000066400000000000000000000067371420061062000304160ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: statistical_outlier_removal.cpp 35876 2011-02-09 01:04:36Z rusu $ * */ #include #include "pcl_ros/filters/statistical_outlier_removal.h" ////////////////////////////////////////////////////////////////////////////////////////////// bool pcl_ros::StatisticalOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) { // Enable the dynamic reconfigure service has_service = true; srv_ = boost::make_shared > (nh); dynamic_reconfigure::Server::CallbackType f = boost::bind (&StatisticalOutlierRemoval::config_callback, this, _1, _2); srv_->setCallback (f); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t /*level*/) { boost::mutex::scoped_lock lock (mutex_); if (impl_.getMeanK () != config.mean_k) { impl_.setMeanK (config.mean_k); NODELET_DEBUG ("[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", getName ().c_str (), config.mean_k); } if (impl_.getStddevMulThresh () != config.stddev) { impl_.setStddevMulThresh (config.stddev); NODELET_DEBUG ("[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", getName ().c_str (), config.stddev); } if (impl_.getNegative () != config.negative) { impl_.setNegative (config.negative); NODELET_DEBUG ("[%s::config_callback] Returning only inliers: %s.", getName ().c_str (), config.negative ? "false" : "true"); } } typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval; PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp000066400000000000000000000127671420061062000247440ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ * */ #include #include "pcl_ros/filters/voxel_grid.h" ////////////////////////////////////////////////////////////////////////////////////////////// bool pcl_ros::VoxelGrid::child_init (ros::NodeHandle &nh, bool &has_service) { // Enable the dynamic reconfigure service has_service = true; srv_ = boost::make_shared > (nh); dynamic_reconfigure::Server::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2); srv_->setCallback (f); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); 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); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t /*level*/) { boost::mutex::scoped_lock lock (mutex_); Eigen::Vector3f leaf_size = impl_.getLeafSize (); if (leaf_size[0] != config.leaf_size) { leaf_size.setConstant (config.leaf_size); NODELET_DEBUG ("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]); impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]); } unsigned int minPointsPerVoxel = impl_.getMinimumPointsNumberPerVoxel (); if (minPointsPerVoxel != ((unsigned int) config.min_points_per_voxel)) { minPointsPerVoxel = config.min_points_per_voxel; NODELET_DEBUG ("[config_callback] Setting the minimum points per voxel to: %u.", minPointsPerVoxel); impl_.setMinimumPointsNumberPerVoxel (minPointsPerVoxel); } double filter_min, filter_max; impl_.getFilterLimits (filter_min, filter_max); if (filter_min != config.filter_limit_min) { filter_min = config.filter_limit_min; NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min); } if (filter_max != config.filter_limit_max) { filter_max = config.filter_limit_max; NODELET_DEBUG ("[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max); } impl_.setFilterLimits (filter_min, filter_max); if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) { impl_.setFilterLimitsNegative (config.filter_limit_negative); NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); } if (impl_.getFilterFieldName () != config.filter_field_name) { impl_.setFilterFieldName (config.filter_field_name); NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ()); } // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter if (tf_input_frame_ != config.input_frame) { tf_input_frame_ = config.input_frame; NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ()); } if (tf_output_frame_ != config.output_frame) { tf_output_frame_ = config.output_frame; NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ()); } // ]--- } typedef pcl_ros::VoxelGrid VoxelGrid; PLUGINLIB_EXPORT_CLASS(VoxelGrid,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/io/000077500000000000000000000000001420061062000210205ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/pcl_ros/io/bag_io.cpp000066400000000000000000000100121420061062000227360ustar00rootroot00000000000000/* * 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.h" ////////////////////////////////////////////////////////////////////////////////////////////// bool pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name) { try { bag_.open (file_name, rosbag::bagmode::Read); view_.addQuery (bag_, rosbag::TopicQuery (topic_name)); if (view_.size () == 0) return (false); it_ = view_.begin (); } catch (rosbag::BagException &e) { return (false); } return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::BAGReader::onInit () { boost::shared_ptr pnh_; pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); // ---[ Mandatory parameters if (!pnh_->getParam ("file_name", file_name_)) { NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!"); return; } if (!pnh_->getParam ("topic_name", topic_name_)) { NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!"); return; } // ---[ Optional parameters int max_queue_size = 1; pnh_->getParam ("publish_rate", publish_rate_); pnh_->getParam ("max_queue_size", max_queue_size); ros::Publisher pub_output = pnh_->advertise ("output", max_queue_size); NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n" " - file_name : %s\n" " - topic_name : %s", file_name_.c_str (), topic_name_.c_str ()); if (!open (file_name_, topic_name_)) return; PointCloud output; output_ = boost::make_shared (output); output_->header.stamp = ros::Time::now (); // Continous publishing enabled? while (pnh_->ok ()) { PointCloudConstPtr cloud = getNextCloud (); NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ()); output_->header.stamp = ros::Time::now (); pub_output.publish (output_); ros::Duration (publish_rate_).sleep (); ros::spinOnce (); } } typedef pcl_ros::BAGReader BAGReader; PLUGINLIB_EXPORT_CLASS(BAGReader,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/io/concatenate_data.cpp000066400000000000000000000255141420061062000250100ustar00rootroot00000000000000/* * 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 "pcl_ros/transforms.h" #include "pcl_ros/io/concatenate_data.h" #include ////////////////////////////////////////////////////////////////////////////////////////////// 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 (size_t 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) { if (!pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_)) { NODELET_ERROR ("[%s::combineClouds] Error converting first input dataset from %s to %s.", getName ().c_str (), in1.header.frame_id.c_str (), output_frame_.c_str ()); return; } } else { in1_t = boost::make_shared (in1); } if (output_frame_ != in2.header.frame_id) { if (!pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_)) { NODELET_ERROR ("[%s::combineClouds] Error converting second input dataset from %s to %s.", getName ().c_str (), in2.header.frame_id.c_str (), output_frame_.c_str ()); return; } } 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; if (in8 && in8->width * in8->height > 0) { pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in8, *out1); } } } } } } pub_output_.publish (boost::make_shared (*out1)); } typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer; PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp000066400000000000000000000155741420061062000253520ustar00rootroot00000000000000/* * 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 "pcl_ros/io/concatenate_fields.h" #include //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 ((int)queue_[cloud->header.stamp].size () >= input_messages_) { // Concatenate together and publish std::vector &clouds = queue_[cloud->header.stamp]; PointCloud cloud_out = *clouds[0]; // Resize the output dataset size_t data_size = cloud_out.data.size (); size_t nr_fields = cloud_out.fields.size (); size_t 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 (size_t cp = 0; cp < nr_points; ++cp) { for (size_t i = 0; i < clouds.size (); ++i) { // Copy each individual point memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step); point_offset += clouds[i]->point_step; } } pub_output_.publish (boost::make_shared (cloud_out)); queue_.erase (cloud->header.stamp); } // Clean the queue to avoid overflowing if (maximum_queue_size_ > 0) { while ((int)queue_.size () > maximum_queue_size_) queue_.erase (queue_.begin ()); } } typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer; PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer,nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/io/io.cpp000066400000000000000000000050111420061062000221300ustar00rootroot00000000000000/* * 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); perception_pcl-1.7.4/pcl_ros/src/pcl_ros/io/pcd_io.cpp000066400000000000000000000156501420061062000227700ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: pcd_io.cpp 35812 2011-02-08 00:05:03Z rusu $ * */ #include #include //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PCDReader::onInit () { 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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/segmentation/000077500000000000000000000000001420061062000231065ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp000066400000000000000000000245431420061062000272200ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: extract_clusters.hpp 32052 2010-08-27 02:19:30Z rusu $ * */ #include #include #include "pcl_ros/segmentation/extract_clusters.h" #include 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 ((int)i >= max_clusters_) break; // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. pcl_msgs::PointIndices ros_pi; moveFromPCL(clusters[i], ros_pi); ros_pi.header.stamp += ros::Duration (i * 0.001); pub_output_.publish (ros_pi); } NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ()); } else { for (size_t i = 0; i < clusters.size (); ++i) { if ((int)i >= max_clusters_) break; PointCloud output; copyPointCloud (*cloud, clusters[i].indices, output); //PointCloud output_blob; // Convert from the templated output to the PointCloud blob //pcl::toROSMsg (output, output_blob); // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. 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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp000066400000000000000000000232571420061062000315640ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: extract_polygonal_prism_data.hpp 32996 2010-09-30 23:42:11Z rusu $ * */ #include #include "pcl_ros/transforms.h" #include "pcl_ros/segmentation/extract_polygonal_prism_data.h" #include 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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp000066400000000000000000000725631420061062000271520ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: sac_segmentation.hpp 33195 2010-10-10 14:12:19Z marton $ * */ #include #include "pcl_ros/segmentation/sac_segmentation.h" #include 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)); } // "latched_indices" not set, proceed with regular pairs else { if (approximate_sync_) { sync_input_indices_a_ = boost::make_shared > > (max_queue_size_); sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); } else { sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); } } } else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ())); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SACSegmentation::unsubscribe () { if (use_indices_) { sub_input_filter_.unsubscribe (); sub_indices_filter_.unsubscribe (); } else sub_input_.shutdown (); } ////////////////////////////////////////////////////////////////////////////////////////////// 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 ((int)inliers.indices.size () <= min_inliers_) { inliers.indices.clear (); model.values.clear (); } // Publish pub_indices_.publish (boost::make_shared (inliers)); pub_model_.publish (boost::make_shared (model)); NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (), model.values.size (), pnh_->resolveName ("model").c_str ()); if (inliers.indices.empty ()) NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ()); } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SACSegmentationFromNormals::onInit () { // Call the super onInit () PCLNodelet::onInit (); // Enable the dynamic reconfigure service srv_ = boost::make_shared > (*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2); srv_->setCallback (f); // 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 ((int)inliers.indices.size () <= min_inliers_) { inliers.indices.clear (); model.values.clear (); } // Publish pub_indices_.publish (boost::make_shared (inliers)); pub_model_.publish (boost::make_shared (model)); NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (), model.values.size (), pnh_->resolveName ("model").c_str ()); if (inliers.indices.empty ()) NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ()); } typedef pcl_ros::SACSegmentation SACSegmentation; typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals; PLUGINLIB_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp000066400000000000000000000144771420061062000276260ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: segment_differences.cpp 35361 2011-01-20 04:34:49Z rusu $ * */ #include #include "pcl_ros/segmentation/segment_differences.h" ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp000066400000000000000000000037011420061062000263100ustar00rootroot00000000000000/* * 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" perception_pcl-1.7.4/pcl_ros/src/pcl_ros/surface/000077500000000000000000000000001420061062000220415ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/pcl_ros/surface/convex_hull.cpp000066400000000000000000000203301420061062000250710ustar00rootroot00000000000000/* * 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 "pcl_ros/surface/convex_hull.h" #include ////////////////////////////////////////////////////////////////////////////////////////////// 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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp000066400000000000000000000243021420061062000270000ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: moving_least_squares.cpp 36097 2011-02-20 14:18:58Z marton $ * */ #include #include "pcl_ros/surface/moving_least_squares.h" ////////////////////////////////////////////////////////////////////////////////////////////// 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) perception_pcl-1.7.4/pcl_ros/src/pcl_ros/surface/surface.cpp000066400000000000000000000036161420061062000242030ustar00rootroot00000000000000/* * 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" perception_pcl-1.7.4/pcl_ros/src/test/000077500000000000000000000000001420061062000177275ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/src/test/test_tf_message_filter_pcl.cpp000066400000000000000000000261761420061062000260260ustar00rootroot00000000000000/* * 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 using namespace tf; // using a random point type, as we want to make sure that it does work with // other points than just XYZ typedef pcl::PointCloud PCDType; /// Sets pcl_stamp from stamp, BUT alters stamp /// a little to round it to millisecond. This is because converting back /// and forth from pcd to ros time induces some rounding errors. void setStamp(ros::Time &stamp, 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: Notification(int expected_count) : count_(0) , expected_count_(expected_count) , failure_count_(0) { } void notify(const MessageFilter::MConstPtr& /*message*/) { ++count_; } void failure(const MessageFilter::MConstPtr& /*message*/, FilterFailureReason /*reason*/) { ++failure_count_; } int count_; int expected_count_; int failure_count_; }; TEST(MessageFilter, noTransforms) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); boost::shared_ptr msg(new PCDType); ros::Time stamp = ros::Time::now(); setStamp(stamp, msg->header.stamp); msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); } TEST(MessageFilter, noTransformsSameFrame) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); boost::shared_ptr msg(new PCDType); ros::Time stamp = ros::Time::now(); setStamp(stamp, msg->header.stamp); msg->header.frame_id = "frame1"; filter.add(msg); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, preexistingTransforms) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); boost::shared_ptr msg(new PCDType); ros::Time stamp = ros::Time::now(); setStamp(stamp, msg->header.stamp); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, postTransforms) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); ros::Time stamp = ros::Time::now(); boost::shared_ptr msg(new PCDType); setStamp(stamp, msg->header.stamp); msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, queueSize) { tf::TransformListener tf_client; Notification n(10); MessageFilter filter(tf_client, "frame1", 10); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp = ros::Time::now(); std::uint64_t pcl_stamp; setStamp(stamp, pcl_stamp); for (int i = 0; i < 20; ++i) { boost::shared_ptr msg(new PCDType); msg->header.stamp = pcl_stamp; msg->header.frame_id = "frame2"; filter.add(msg); } EXPECT_EQ(0, n.count_); EXPECT_EQ(10, n.failure_count_); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(10, n.count_); } TEST(MessageFilter, setTargetFrame) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.setTargetFrame("frame1000"); ros::Time stamp = ros::Time::now(); boost::shared_ptr msg(new PCDType); setStamp(stamp, msg->header.stamp); msg->header.frame_id = "frame2"; tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2"); tf_client.setTransform(transform); filter.add(msg); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, multipleTargetFrames) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); std::vector target_frames; target_frames.push_back("frame1"); target_frames.push_back("frame2"); filter.setTargetFrames(target_frames); ros::Time stamp = ros::Time::now(); boost::shared_ptr msg(new PCDType); setStamp(stamp, msg->header.stamp); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3"); tf_client.setTransform(transform); msg->header.frame_id = "frame3"; filter.add(msg); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet) //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0)); transform.child_frame_id_ = "frame2"; tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(1, n.count_); // frame2->frame3 now exists } TEST(MessageFilter, tolerance) { ros::Duration offset(0.2); tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.setTolerance(offset); ros::Time stamp = ros::Time::now(); 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); boost::shared_ptr msg(new PCDType); msg->header.frame_id = "frame2"; msg->header.stamp = pcl_stamp; filter.add(msg); EXPECT_EQ(0, n.count_); //No return due to lack of space for offset //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1)); transform.stamp_ += offset*1.1; tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(1, n.count_); // Now have data for the message published earlier stamp += offset; setStamp(stamp, pcl_stamp); msg->header.stamp = pcl_stamp; filter.add(msg); EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset } TEST(MessageFilter, outTheBackFailure) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp = ros::Time::now(); boost::shared_ptr msg(new PCDType); setStamp(stamp, msg->header.stamp); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); transform.stamp_ = stamp + ros::Duration(10000); tf_client.setTransform(transform); msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.failure_count_); } TEST(MessageFilter, emptyFrameIDFailure) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); boost::shared_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; } perception_pcl-1.7.4/pcl_ros/src/transforms.cpp000066400000000000000000000647111420061062000216630ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * */ #include #include #include #include "pcl_ros/transforms.h" #include "pcl_ros/impl/transforms.hpp" namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener) { if (in.header.frame_id == target_frame) { out = in; return (true); } // Get the TF transform tf::StampedTransform transform; try { tf_listener.waitForTransform (target_frame, in.header.frame_id, in.header.stamp, ros::Duration(1)); tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform); } catch (const tf::TransformException &e) { ROS_ERROR ("%s", e.what ()); return (false); } // Convert the TF transform to Eigen format Eigen::Matrix4f eigen_transform; transformAsMatrix (transform, eigen_transform); transformPointCloud (eigen_transform, in, out); out.header.frame_id = target_frame; return (true); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, sensor_msgs::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::TransformStamped transform; try { transform = tf_buffer.lookupTransform (target_frame, in.header.frame_id, in.header.stamp); } catch (tf2::LookupException &e) { ROS_ERROR ("%s", e.what ()); return (false); } catch (tf2::ExtrapolationException &e) { ROS_ERROR ("%s", e.what ()); return (false); } transformPointCloud (target_frame, transform.transform, in, out); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) { if (in.header.frame_id == target_frame) { out = in; return; } // Get the transformation Eigen::Matrix4f transform; transformAsMatrix (net_transform, transform); transformPointCloud (transform, in, out); out.header.frame_id = target_frame; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void transformPointCloud (const std::string &target_frame, const geometry_msgs::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) { if (in.header.frame_id == target_frame) { out = in; return; } // Get the transformation Eigen::Matrix4f transform; transformAsMatrix (net_transform, transform); transformPointCloud (transform, in, out); out.header.frame_id = target_frame; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) { // Get X-Y-Z indices int x_idx = pcl::getFieldIndex (in, "x"); int y_idx = pcl::getFieldIndex (in, "y"); int z_idx = pcl::getFieldIndex (in, "z"); if (x_idx == -1 || y_idx == -1 || z_idx == -1) { ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); return; } if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) { ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported."); return; } // Check if distance is available int dist_idx = pcl::getFieldIndex (in, "distance"); // Copy the other data if (&in != &out) { out.header = in.header; out.height = in.height; out.width = in.width; out.fields = in.fields; out.is_bigendian = in.is_bigendian; out.point_step = in.point_step; out.row_step = in.row_step; out.is_dense = in.is_dense; out.data.resize (in.data.size ()); // Copy everything as it's faster than copying individual elements memcpy (out.data.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 (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1); Eigen::Vector4f pt_out; bool max_range_point = false; int distance_ptr_offset = (dist_idx < 0 ? -1 : (i*in.point_step + in.fields[dist_idx].offset)); // If dist_idx is negative, it must not be used as an index float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) { if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point { pt_out = pt; } else // max range point { pt[0] = *distance_ptr; // Replace x with the x value saved in distance pt_out = transform * pt; max_range_point = true; //std::cout << pt[0]<<","< "<::quiet_NaN(); } memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float)); memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float)); memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float)); xyz_offset += in.point_step; } // Check if the viewpoint information is present int vp_idx = pcl::getFieldIndex (in, "vp_x"); if (vp_idx != -1) { // Transform the viewpoint info too for (size_t i = 0; i < out.width * out.height; ++i) { float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset]; // Assume vp_x, vp_y, vp_z are consecutive Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1); Eigen::Vector4f vp_out = transform * vp_in; pstep[0] = vp_out[0]; pstep[1] = vp_out[1]; pstep[2] = vp_out[2]; } } } ////////////////////////////////////////////////////////////////////////////////////////////// void transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat) { double mv[12]; bt.getBasis ().getOpenGLSubMatrix (mv); tf::Vector3 origin = bt.getOrigin (); out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1; out_mat (0, 3) = origin.x (); out_mat (1, 3) = origin.y (); out_mat (2, 3) = origin.z (); } ////////////////////////////////////////////////////////////////////////////////////////////// void transformAsMatrix (const geometry_msgs::Transform& bt, Eigen::Matrix4f &out_mat) { out_mat = tf2::transformToEigen(bt).matrix().cast(); } } // namespace pcl_ros ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const 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 ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const geometry_msgs::Transform &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const 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 ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf2_ros::Buffer &); perception_pcl-1.7.4/pcl_ros/tests/000077500000000000000000000000001420061062000173235ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/tests/test_tf_message_filter_pcl.launch000066400000000000000000000001641420061062000260770ustar00rootroot00000000000000 perception_pcl-1.7.4/pcl_ros/tools/000077500000000000000000000000001420061062000173215ustar00rootroot00000000000000perception_pcl-1.7.4/pcl_ros/tools/bag_to_pcd.cpp000066400000000000000000000143071420061062000221130ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: bag_to_pcd.cpp 35812 2011-02-08 00:05:03Z rusu $ * */ /** \author Radu Bogdan Rusu @b bag_to_pcd is a simple node that reads in a BAG file and saves all the PointCloud messages to disk in PCD (Point Cloud Data) format. **/ #include #include #include #include #include "pcl_ros/transforms.h" #include #include typedef sensor_msgs::PointCloud2 PointCloud; typedef PointCloud::Ptr PointCloudPtr; typedef PointCloud::ConstPtr PointCloudConstPtr; /* ---[ */ int main (int argc, char** argv) { ros::init (argc, argv, "bag_to_pcd"); if (argc < 4) { std::cerr << "Syntax is: " << argv[0] << " []" << std::endl; std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /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 (const rosbag::BagException&) { std::cerr << "Error opening file " << argv[1] << std::endl; return (-1); } // check that target topic exists in the bag file: rosbag::View topic_list_view(bag); std::string target_topic; std::map topic_list; for(rosbag::ConnectionInfo const *ci: topic_list_view.getConnections() ) { topic_list[ci->topic] = ci->datatype; if (ci->topic == argv[2]) { if (ci->datatype == std::string("sensor_msgs/PointCloud2")) { target_topic = std::string (argv[2]); view.addQuery (bag, rosbag::TopicQuery (target_topic)); } else { std::cerr << "Provided topic '" << argv[2] << "' is in the bag file, but is not of type sensor_msgs/PointCloud2 (type: " << ci->datatype << ")" << std::endl; } } } if (target_topic.empty()) { std::cerr << "Could not find a sensor_msgs/PointCloud2 type on topic '" << argv[2] << "' in bag file " << argv[1] << std::endl; std::cerr << "Topics found in the bag file:" << std::endl; for (std::map::iterator it=topic_list.begin(); it!=topic_list.end(); ++it) std::cout << " " << it->first << " (" << it->second << ")" << std::endl; return (-1); } 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 " << target_topic << " 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 << " on topic " << view_it->getTopic() << " 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); } /* ]--- */ perception_pcl-1.7.4/pcl_ros/tools/convert_pcd_to_image.cpp000066400000000000000000000056021420061062000242020ustar00rootroot00000000000000/* * 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 /* ---[ */ 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); } /* ]--- */ perception_pcl-1.7.4/pcl_ros/tools/convert_pointcloud_to_image.cpp000066400000000000000000000070071420061062000256150ustar00rootroot00000000000000/* * 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 //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; } perception_pcl-1.7.4/pcl_ros/tools/pcd_to_pointcloud.cpp000066400000000000000000000150721420061062000235420ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id: pcd_to_pointcloud.cpp 33238 2010-03-11 00:46:58Z rusu $ * */ /** \author Radu Bogdan Rusu @b pcd_to_pointcloud is a simple node that loads PCD (Point Cloud Data) files from disk and publishes them as ROS messages on the network. **/ // ROS core #include #include #include #include // helper function to return parsed parameter or default value template T get_param(std::string const& name, T default_value) { T value; ros::param::param(name, value, default_value); return value; } class pcd_to_pointcloud { ros::NodeHandle nh; // the topic to publish at, will be overwritten to give the remapped name std::string cloud_topic; // source file name, will be overwritten to produce actually configured file std::string file_name; // republish interval in seconds double interval; // tf2 frame_id std::string frame_id; // latched topic enabled/disabled bool latch; // pointcloud message and publisher sensor_msgs::PointCloud2 cloud; ros::Publisher pub; // timer to handle republishing ros::Timer timer; void publish() { ROS_DEBUG_STREAM_ONCE("Publishing pointcloud"); ROS_DEBUG_STREAM_ONCE(" * number of points: " << cloud.width * cloud.height); ROS_DEBUG_STREAM_ONCE(" * frame_id: " << cloud.header.frame_id); ROS_DEBUG_STREAM_ONCE(" * topic_name: " << cloud_topic); int num_subscribers = pub.getNumSubscribers(); if (num_subscribers > 0) { ROS_DEBUG("Publishing data to %d subscribers.", num_subscribers); } // update timestamp and publish cloud.header.stamp = ros::Time::now(); pub.publish(cloud); } void timer_callback(ros::TimerEvent const&) { // just re-publish publish(); } public: pcd_to_pointcloud() : cloud_topic("cloud_pcd"), file_name(""), interval(0.0), frame_id("base_link"), latch(false) { // update potentially remapped topic name for later logging cloud_topic = nh.resolveName(cloud_topic); } void parse_ros_params() { file_name = get_param("~file_name", file_name); interval = get_param("~interval", interval); frame_id = get_param("~frame_id", frame_id); latch = get_param("~latch", latch); } void parse_cmdline_args(int argc, char** argv) { if (argc > 1) { file_name = argv[1]; } if (argc > 2) { std::stringstream str(argv[2]); double x; if (str >> x) interval = x; } } bool try_load_pointcloud() { if (file_name.empty()) { ROS_ERROR_STREAM("Can't load pointcloud: no file name provided"); return false; } else if (pcl::io::loadPCDFile(file_name, cloud) < 0) { ROS_ERROR_STREAM("Failed to parse pointcloud from file ('" << file_name << "')"); return false; } // success: set frame_id appropriately cloud.header.frame_id = frame_id; return true; } void init_run() { // init publisher pub = nh.advertise(cloud_topic, 1, latch); // treat publishing once as a special case to interval publishing bool oneshot = interval <= 0; timer = nh.createTimer(ros::Duration(interval), &pcd_to_pointcloud::timer_callback, this, oneshot); } void print_config_info() { ROS_INFO_STREAM("Recognized the following parameters"); ROS_INFO_STREAM(" * file_name: " << file_name); ROS_INFO_STREAM(" * interval: " << interval); ROS_INFO_STREAM(" * frame_id: " << frame_id); ROS_INFO_STREAM(" * topic_name: " << cloud_topic); ROS_INFO_STREAM(" * latch: " << std::boolalpha << latch); } void print_data_info() { ROS_INFO_STREAM("Loaded pointcloud with the following stats"); ROS_INFO_STREAM(" * number of points: " << cloud.width * cloud.height); ROS_INFO_STREAM(" * total size [bytes]: " << cloud.data.size()); ROS_INFO_STREAM(" * channel names: " << pcl::getFieldsList(cloud)); } }; int main (int argc, char** argv) { // init ROS ros::init(argc, argv, "pcd_to_pointcloud"); // set up node pcd_to_pointcloud node; // initializes from ROS parameters node.parse_ros_params(); // also allow config to be provided via command line args // the latter take precedence node.parse_cmdline_args(argc, argv); // print info about effective configuration settings node.print_config_info(); // try to load pointcloud from file if (!node.try_load_pointcloud()) { return -1; } // print info about pointcloud node.print_data_info(); // initialize run node.init_run(); // blocking call to process callbacks etc ros::spin(); } perception_pcl-1.7.4/pcl_ros/tools/pointcloud_to_pcd.cpp000066400000000000000000000135511420061062000235420ustar00rootroot00000000000000/* * 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 // PCL includes #include #include using namespace std; /** \author Radu Bogdan Rusu @b pointcloud_to_pcd is a simple node that retrieves a ROS point cloud message and saves it to disk into a PCD (Point Cloud Data) file format. **/ class PointCloudToPCD { protected: ros::NodeHandle nh_; private: std::string prefix_; std::string filename_; bool binary_; bool compressed_; std::string fixed_frame_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; public: 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; if (filename_ != "") { ss << filename_ << ".pcd"; } else { 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_); priv_nh.getParam ("filename", filename_); if(binary_) { if(compressed_) { ROS_INFO_STREAM ("Saving as binary compressed PCD"); } else { ROS_INFO_STREAM ("Saving as binary uncompressed PCD"); } } else { ROS_INFO_STREAM ("Saving as ASCII PCD"); } if (filename_ != "") { ROS_INFO_STREAM ("Saving to fixed filename: " << filename_); } 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); } /* ]--- */ perception_pcl-1.7.4/perception_pcl/000077500000000000000000000000001420061062000175265ustar00rootroot00000000000000perception_pcl-1.7.4/perception_pcl/CHANGELOG.rst000066400000000000000000000063521420061062000215550ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package perception_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.7.0 (2019-07-03) ------------------ 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 perception_pcl-1.7.4/perception_pcl/CMakeLists.txt000066400000000000000000000001611420061062000222640ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(perception_pcl) find_package(catkin REQUIRED) catkin_metapackage() perception_pcl-1.7.4/perception_pcl/package.xml000066400000000000000000000021601420061062000216420ustar00rootroot00000000000000 perception_pcl 1.7.4 PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred bridge for 3D applications involving n-D Point Clouds and 3D geometry processing in ROS. Open Perception William Woodall Julius Kammerl Paul Bovbel Kentaro Wada Steve Macenski BSD http://ros.org/wiki/perception_pcl https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl catkin pcl_conversions pcl_msgs pcl_ros