apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add 'ros2_ws/src/perception_pcl/' from commit '6642b7398a4488040c44d48a48308411bcde2228'

Browse Source 
git-subtree-dir: ros2_ws/src/perception_pcl
git-subtree-mainline: 0c9504b3439e4fea47710a74d1e2ccde5ad6e19d
git-subtree-split: 6642b7398a4488040c44d48a48308411bcde2228
This commit is contained in:
Apoorva Gupta 2023-03-29 11:40:49 +05:30
commit 0e6937828b
138 changed files with 17571 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ros2_ws/src/perception_pcl/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
*.pyc

27
ros2_ws/src/perception_pcl/.travis.yml Normal file
View File

@ -0,0 +1,27 @@
language: generic
services:
- docker
cache:
directories:
- $HOME/.ccache
env:
global:
- ROS_REPO=ros2
- CCACHE_DIR=$HOME/.ccache
# travis build will time out with no output unless we use verbose output
- VERBOSE_OUTPUT=true
- VERBOSE_TESTS=true
matrix:
- ROS_DISTRO=foxy OS_NAME=ubuntu OS_CODE_NAME=focal
install:
- git clone --branch master --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci
script:
- .industrial_ci/travis.sh
branches:
only:
- /.*-devel$/

7
ros2_ws/src/perception_pcl/README.md Normal file
View File

@ -0,0 +1,7 @@
# perception_pcl
[![Build Status](https://travis-ci.org/ros-perception/perception_pcl.svg?branch=melodic-devel)](https://travis-ci.org/ros-perception/perception_pcl)
PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred
bridge for 3D applications involving n-D Point Clouds and 3D geometry
processing in ROS.

68
ros2_ws/src/perception_pcl/pcl_conversions/CHANGELOG.rst Normal file
View File

@ -0,0 +1,68 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package pcl_conversions
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.6.2 (2018-05-20)
------------------
1.6.1 (2018-05-08)
------------------
* Add 1.6.0 section to CHANGELOG.rst
* Use foreach + string regex to implement list(filter on old cmake
* Downgrade the required cmake version for backward compatibility
* update package.xml links to point to new repository
* CMake 3.6.3 is sufficient
* Fix a bug building on artful.
* Fixup pcl_conversions test
* Contributors: Chris Lalancette, Kentaro Wada, Mikael Arguedas, Paul Bovbel
1.6.0 (2018-04-30)
------------------
* Fix build and update maintainers
* Contributors: Paul Bovbel, Kentaro Wada
0.2.1 (2015-06-08)
------------------
* Added a test for rounding errors in stamp conversion
for some values the test fails.
* add pcl::PointCloud to Image msg converter for extracting the rgb component of a cloud
* Contributors: Brice Rebsamen, Lucid One, Michael Ferguson, Paul Bovbel
0.2.0 (2014-04-10)
------------------
* Added conversions for stamp types
* update maintainer info, add eigen dependency
* fix Eigen dependency
* Make pcl_conversions run_depend on libpcl-all-dev
* Contributors: Brice Rebsamen, Paul Bovbel, Scott K Logan, William Woodall
0.1.5 (2013-08-27)
------------------
* Use new pcl rosdep keys (libpcl-all and libpcl-all-dev)
0.1.4 (2013-07-13)
------------------
* Fixup dependencies and CMakeLists.txt:
* Added a versioned dependency on pcl, fixes `#1 <https://github.com/ros-perception/pcl_conversions/issues/1>`_
* Added a dependency on pcl_msgs, fixes `#2 <https://github.com/ros-perception/pcl_conversions/issues/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

55
ros2_ws/src/perception_pcl/pcl_conversions/CMakeLists.txt Normal file
View File

@ -0,0 +1,55 @@
cmake_minimum_required(VERSION 3.5)
project(pcl_conversions)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(Boost REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(message_filters REQUIRED)
find_package(PCL REQUIRED QUIET COMPONENTS common io)
find_package(pcl_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
set(dependencies
message_filters
pcl_msgs
rclcpp
sensor_msgs
std_msgs
)
include_directories(
include
${PCL_COMMON_INCLUDE_DIRS}
)
install(DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
# Add gtest based cpp test target
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp)
ament_target_dependencies(${PROJECT_NAME}-test
${dependencies}
)
target_link_libraries(${PROJECT_NAME}-test ${Boost_LIBRARIES} ${PCL_LIBRARIES})
endif()
# Export old-style CMake variables
ament_export_include_directories("include/${PROJECT_NAME}")
ament_export_dependencies(${dependencies} PCL)
ament_package()

22
ros2_ws/src/perception_pcl/pcl_conversions/README.rst Normal file
View File

@ -0,0 +1,22 @@
pcl_conversions
===============
This package provides conversions from PCL data types and ROS message types.
Code & tickets
--------------
.. Build status: |Build Status|
.. .. |Build Status| image:: https://secure.travis-ci.org/ros-perception/pcl_conversions.png
:target: http://travis-ci.org/ros-perception/pcl_conversions
+-----------------+------------------------------------------------------------+
| pcl_conversions | http://ros.org/wiki/pcl_conversions |
+-----------------+------------------------------------------------------------+
| Issues | http://github.com/ros-perception/perception_pcl/issues |
+-----------------+------------------------------------------------------------+
.. | Documentation | http://ros-perception.github.com/pcl_conversions/doc |
.. +-----------------+------------------------------------------------------------+

942
ros2_ws/src/perception_pcl/pcl_conversions/include/pcl_conversions/pcl_conversions.h Normal file
View File

@ -0,0 +1,942 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, Open Source Robotics Foundation, Inc.
* Copyright (c) 2010-2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation, Inc. nor
* the names of its contributors may be used to endorse or promote
* products derived from this software without specific prior
* written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef PCL_CONVERSIONS_H__
#define PCL_CONVERSIONS_H__
#include <cstddef>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <message_filters/message_event.h>
#include <message_filters/message_traits.h>
#include <pcl/conversions.h>
#include <pcl/PCLHeader.h>
#include <std_msgs/msg/header.hpp>
#include <pcl/PCLImage.h>
#include <sensor_msgs/msg/image.hpp>
#include <pcl/PCLPointField.h>
#include <sensor_msgs/msg/point_field.hpp>
#include <pcl/PCLPointCloud2.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/PointIndices.h>
#include <pcl_msgs/msg/point_indices.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl_msgs/msg/model_coefficients.hpp>
#include <pcl/Vertices.h>
#include <pcl_msgs/msg/vertices.hpp>
#include <pcl/PolygonMesh.h>
#include <pcl_msgs/msg/polygon_mesh.hpp>
#include <pcl/io/pcd_io.h>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
namespace pcl_conversions {
/** PCLHeader <=> Header **/
inline
void fromPCL(const std::uint64_t &pcl_stamp, rclcpp::Time &stamp)
{
stamp = rclcpp::Time(
static_cast<rcl_time_point_value_t>(pcl_stamp * 1000ull)); // Convert from us to ns
}
inline
void toPCL(const rclcpp::Time &stamp, std::uint64_t &pcl_stamp)
{
pcl_stamp = static_cast<std::uint64_t>(stamp.nanoseconds()) / 1000ull; // Convert from ns to us
}
inline
rclcpp::Time fromPCL(const std::uint64_t &pcl_stamp)
{
rclcpp::Time stamp;
fromPCL(pcl_stamp, stamp);
return stamp;
}
inline
std::uint64_t toPCL(const rclcpp::Time &stamp)
{
std::uint64_t pcl_stamp;
toPCL(stamp, pcl_stamp);
return pcl_stamp;
}
/** PCLHeader <=> Header **/
inline
void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::msg::Header &header)
{
header.stamp = fromPCL(pcl_header.stamp);
header.frame_id = pcl_header.frame_id;
}
inline
void toPCL(const std_msgs::msg::Header &header, pcl::PCLHeader &pcl_header)
{
toPCL(header.stamp, pcl_header.stamp);
// TODO(clalancette): Seq doesn't exist in the ROS2 header
// anymore. wjwwood suggests that we might be able to get this
// information from the middleware in the future, but for now we
// just set it to 0.
pcl_header.seq = 0;
pcl_header.frame_id = header.frame_id;
}
inline
std_msgs::msg::Header fromPCL(const pcl::PCLHeader &pcl_header)
{
std_msgs::msg::Header header;
fromPCL(pcl_header, header);
return header;
}
inline
pcl::PCLHeader toPCL(const std_msgs::msg::Header &header)
{
pcl::PCLHeader pcl_header;
toPCL(header, pcl_header);
return pcl_header;
}
/** PCLImage <=> Image **/
inline
void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image)
{
fromPCL(pcl_image.header, image.header);
image.height = pcl_image.height;
image.width = pcl_image.width;
image.encoding = pcl_image.encoding;
image.is_bigendian = pcl_image.is_bigendian;
image.step = pcl_image.step;
}
inline
void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image)
{
copyPCLImageMetaData(pcl_image, image);
image.data = pcl_image.data;
}
inline
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image)
{
copyPCLImageMetaData(pcl_image, image);
image.data.swap(pcl_image.data);
}
inline
void copyImageMetaData(const sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image)
{
toPCL(image.header, pcl_image.header);
pcl_image.height = image.height;
pcl_image.width = image.width;
pcl_image.encoding = image.encoding;
pcl_image.is_bigendian = image.is_bigendian;
pcl_image.step = image.step;
}
inline
void toPCL(const sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image)
{
copyImageMetaData(image, pcl_image);
pcl_image.data = image.data;
}
inline
void moveToPCL(sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image)
{
copyImageMetaData(image, pcl_image);
pcl_image.data.swap(image.data);
}
/** PCLPointField <=> PointField **/
inline
void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::msg::PointField &pf)
{
pf.name = pcl_pf.name;
pf.offset = pcl_pf.offset;
pf.datatype = pcl_pf.datatype;
pf.count = pcl_pf.count;
}
inline
void fromPCL(const std::vector<pcl::PCLPointField> &pcl_pfs, std::vector<sensor_msgs::msg::PointField> &pfs)
{
pfs.resize(pcl_pfs.size());
std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();
size_t i = 0;
for(; it != pcl_pfs.end(); ++it, ++i) {
fromPCL(*(it), pfs[i]);
}
}
inline
void toPCL(const sensor_msgs::msg::PointField &pf, pcl::PCLPointField &pcl_pf)
{
pcl_pf.name = pf.name;
pcl_pf.offset = pf.offset;
pcl_pf.datatype = pf.datatype;
pcl_pf.count = pf.count;
}
inline
void toPCL(const std::vector<sensor_msgs::msg::PointField> &pfs, std::vector<pcl::PCLPointField> &pcl_pfs)
{
pcl_pfs.resize(pfs.size());
std::vector<sensor_msgs::msg::PointField>::const_iterator it = pfs.begin();
size_t i = 0;
for(; it != pfs.end(); ++it, ++i) {
toPCL(*(it), pcl_pfs[i]);
}
}
/** PCLPointCloud2 <=> PointCloud2 **/
inline
void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2)
{
fromPCL(pcl_pc2.header, pc2.header);
pc2.height = pcl_pc2.height;
pc2.width = pcl_pc2.width;
fromPCL(pcl_pc2.fields, pc2.fields);
pc2.is_bigendian = pcl_pc2.is_bigendian;
pc2.point_step = pcl_pc2.point_step;
pc2.row_step = pcl_pc2.row_step;
pc2.is_dense = pcl_pc2.is_dense;
}
inline
void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2)
{
copyPCLPointCloud2MetaData(pcl_pc2, pc2);
pc2.data = pcl_pc2.data;
}
inline
void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2)
{
copyPCLPointCloud2MetaData(pcl_pc2, pc2);
pc2.data.swap(pcl_pc2.data);
}
inline
void copyPointCloud2MetaData(const sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
{
toPCL(pc2.header, pcl_pc2.header);
pcl_pc2.height = pc2.height;
pcl_pc2.width = pc2.width;
toPCL(pc2.fields, pcl_pc2.fields);
pcl_pc2.is_bigendian = pc2.is_bigendian;
pcl_pc2.point_step = pc2.point_step;
pcl_pc2.row_step = pc2.row_step;
pcl_pc2.is_dense = pc2.is_dense;
}
inline
void toPCL(const sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
{
copyPointCloud2MetaData(pc2, pcl_pc2);
pcl_pc2.data = pc2.data;
}
inline
void moveToPCL(sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
{
copyPointCloud2MetaData(pc2, pcl_pc2);
pcl_pc2.data.swap(pc2.data);
}
/** pcl::PointIndices <=> pcl_msgs::PointIndices **/
inline
void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::msg::PointIndices &pi)
{
fromPCL(pcl_pi.header, pi.header);
pi.indices = pcl_pi.indices;
}
inline
void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::msg::PointIndices &pi)
{
fromPCL(pcl_pi.header, pi.header);
pi.indices.swap(pcl_pi.indices);
}
inline
void toPCL(const pcl_msgs::msg::PointIndices &pi, pcl::PointIndices &pcl_pi)
{
toPCL(pi.header, pcl_pi.header);
pcl_pi.indices = pi.indices;
}
inline
void moveToPCL(pcl_msgs::msg::PointIndices &pi, pcl::PointIndices &pcl_pi)
{
toPCL(pi.header, pcl_pi.header);
pcl_pi.indices.swap(pi.indices);
}
/** pcl::ModelCoefficients <=> pcl_msgs::ModelCoefficients **/
inline
void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::msg::ModelCoefficients &mc)
{
fromPCL(pcl_mc.header, mc.header);
mc.values = pcl_mc.values;
}
inline
void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::msg::ModelCoefficients &mc)
{
fromPCL(pcl_mc.header, mc.header);
mc.values.swap(pcl_mc.values);
}
inline
void toPCL(const pcl_msgs::msg::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
{
toPCL(mc.header, pcl_mc.header);
pcl_mc.values = mc.values;
}
inline
void moveToPCL(pcl_msgs::msg::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
{
toPCL(mc.header, pcl_mc.header);
pcl_mc.values.swap(mc.values);
}
/** pcl::Vertices <=> pcl_msgs::Vertices **/
namespace internal
{
template <class T>
inline void move(std::vector<T> &a, std::vector<T> &b)
{
b.swap(a);
}
template <class T1, class T2>
inline void move(std::vector<T1> &a, std::vector<T2> &b)
{
b.assign(a.cbegin(), a.cend());
}
}
inline
void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert)
{
vert.vertices.assign(pcl_vert.vertices.cbegin(), pcl_vert.vertices.cend());
}
inline
void fromPCL(const std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::msg::Vertices> &verts)
{
verts.resize(pcl_verts.size());
std::vector<pcl::Vertices>::const_iterator it = pcl_verts.begin();
std::vector<pcl_msgs::msg::Vertices>::iterator jt = verts.begin();
for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
fromPCL(*(it), *(jt));
}
}
inline
void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert)
{
internal::move(pcl_vert.vertices, vert.vertices);
}
inline
void fromPCL(std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::msg::Vertices> &verts)
{
verts.resize(pcl_verts.size());
std::vector<pcl::Vertices>::iterator it = pcl_verts.begin();
std::vector<pcl_msgs::msg::Vertices>::iterator jt = verts.begin();
for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
moveFromPCL(*(it), *(jt));
}
}
inline
void toPCL(const pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert)
{
pcl_vert.vertices.assign(vert.vertices.cbegin(), vert.vertices.cend());
}
inline
void toPCL(const std::vector<pcl_msgs::msg::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
{
pcl_verts.resize(verts.size());
std::vector<pcl_msgs::msg::Vertices>::const_iterator it = verts.begin();
std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
toPCL(*(it), *(jt));
}
}
inline
void moveToPCL(pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert)
{
internal::move(vert.vertices, pcl_vert.vertices);
}
inline
void moveToPCL(std::vector<pcl_msgs::msg::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
{
pcl_verts.resize(verts.size());
std::vector<pcl_msgs::msg::Vertices>::iterator it = verts.begin();
std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
moveToPCL(*(it), *(jt));
}
}
/** pcl::PolygonMesh <=> pcl_msgs::PolygonMesh **/
inline
void fromPCL(const pcl::PolygonMesh &pcl_mesh, pcl_msgs::msg::PolygonMesh &mesh)
{
fromPCL(pcl_mesh.header, mesh.header);
fromPCL(pcl_mesh.cloud, mesh.cloud);
fromPCL(pcl_mesh.polygons, mesh.polygons);
}
inline
void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::msg::PolygonMesh &mesh)
{
fromPCL(pcl_mesh.header, mesh.header);
moveFromPCL(pcl_mesh.cloud, mesh.cloud);
}
inline
void toPCL(const pcl_msgs::msg::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
{
toPCL(mesh.header, pcl_mesh.header);
toPCL(mesh.cloud, pcl_mesh.cloud);
toPCL(mesh.polygons, pcl_mesh.polygons);
}
inline
void moveToPCL(pcl_msgs::msg::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
{
toPCL(mesh.header, pcl_mesh.header);
moveToPCL(mesh.cloud, pcl_mesh.cloud);
moveToPCL(mesh.polygons, pcl_mesh.polygons);
}
} // namespace pcl_conversions
namespace pcl {
/** Overload pcl::getFieldIndex **/
inline int getFieldIndex(const sensor_msgs::msg::PointCloud2 &cloud, const std::string &field_name)
{
// Get the index we need
for (size_t d = 0; d < cloud.fields.size(); ++d) {
if (cloud.fields[d].name == field_name) {
return (static_cast<int>(d));
}
}
return (-1);
}
/** Overload pcl::getFieldsList **/
inline std::string getFieldsList(const sensor_msgs::msg::PointCloud2 &cloud)
{
std::string result;
for (size_t i = 0; i < cloud.fields.size () - 1; ++i) {
result += cloud.fields[i].name + " ";
}
result += cloud.fields[cloud.fields.size () - 1].name;
return (result);
}
/** Provide pcl::toROSMsg **/
inline
void toROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::Image &image)
{
pcl::PCLPointCloud2 pcl_cloud;
pcl_conversions::toPCL(cloud, pcl_cloud);
pcl::PCLImage pcl_image;
pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
pcl_conversions::moveFromPCL(pcl_image, image);
}
inline
void moveToROSMsg(sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::Image &image)
{
pcl::PCLPointCloud2 pcl_cloud;
pcl_conversions::moveToPCL(cloud, pcl_cloud);
pcl::PCLImage pcl_image;
pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
pcl_conversions::moveFromPCL(pcl_image, image);
}
template<typename T> void
toROSMsg (const pcl::PointCloud<T> &cloud, sensor_msgs::msg::Image& msg)
{
// Ease the user's burden on specifying width/height for unorganized datasets
if (cloud.width == 0 && cloud.height == 0)
{
throw std::runtime_error("Needs to be a dense like cloud!!");
}
else
{
if (cloud.points.size () != cloud.width * cloud.height)
throw std::runtime_error("The width and height do not match the cloud size!");
msg.height = cloud.height;
msg.width = cloud.width;
}
// sensor_msgs::image_encodings::BGR8;
msg.encoding = "bgr8";
msg.step = msg.width * sizeof (std::uint8_t) * 3;
msg.data.resize (msg.step * msg.height);
for (size_t y = 0; y < cloud.height; y++)
{
for (size_t x = 0; x < cloud.width; x++)
{
std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(std::uint8_t));
}
}
}
/** Provide to/fromROSMsg for sensor_msgs::msg::PointCloud2 <=> pcl::PointCloud<T> **/
template<typename T>
void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::msg::PointCloud2 &cloud)
{
pcl::PCLPointCloud2 pcl_pc2;
pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
pcl_conversions::moveFromPCL(pcl_pc2, cloud);
}
template<typename T>
void fromROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
{
pcl::PCLPointCloud2 pcl_pc2;
#if PCL_VERSION_COMPARE(>=, 1, 13, 1)
pcl_conversions::copyPointCloud2MetaData(cloud, pcl_pc2); // Like pcl_conversions::toPCL, but does not copy the binary data
pcl::MsgFieldMap field_map;
pcl::createMapping<T> (pcl_pc2.fields, field_map);
pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud, field_map, &cloud.data[0]);
#else
pcl_conversions::toPCL(cloud, pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
#endif
}
template<typename T>
void moveFromROSMsg(sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
{
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::moveToPCL(cloud, pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
}
/** Overload pcl::createMapping **/
template<typename PointT>
void createMapping(const std::vector<sensor_msgs::msg::PointField>& msg_fields, MsgFieldMap& field_map)
{
std::vector<pcl::PCLPointField> pcl_msg_fields;
pcl_conversions::toPCL(msg_fields, pcl_msg_fields);
return createMapping<PointT>(pcl_msg_fields, field_map);
}
namespace io {
/** Overload pcl::io::savePCDFile **/
inline int
savePCDFile(const std::string &file_name, const sensor_msgs::msg::PointCloud2 &cloud,
const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
const bool binary_mode = false)
{
pcl::PCLPointCloud2 pcl_cloud;
pcl_conversions::toPCL(cloud, pcl_cloud);
return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
}
inline int
destructiveSavePCDFile(const std::string &file_name, sensor_msgs::msg::PointCloud2 &cloud,
const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
const bool binary_mode = false)
{
pcl::PCLPointCloud2 pcl_cloud;
pcl_conversions::moveToPCL(cloud, pcl_cloud);
return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
}
/** Overload pcl::io::loadPCDFile **/
inline int loadPCDFile(const std::string &file_name, sensor_msgs::msg::PointCloud2 &cloud)
{
pcl::PCLPointCloud2 pcl_cloud;
int ret = pcl::io::loadPCDFile(file_name, pcl_cloud);
pcl_conversions::moveFromPCL(pcl_cloud, cloud);
return ret;
}
} // namespace io
/** Overload asdf **/
inline
bool concatenatePointCloud (const sensor_msgs::msg::PointCloud2 &cloud1,
const sensor_msgs::msg::PointCloud2 &cloud2,
sensor_msgs::msg::PointCloud2 &cloud_out)
{
//if one input cloud has no points, but the other input does, just return the cloud with points
if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0)
{
cloud_out = cloud2;
return (true);
}
else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
{
cloud_out = cloud1;
return (true);
}
bool strip = false;
for (size_t i = 0; i < cloud1.fields.size (); ++i)
if (cloud1.fields[i].name == "_")
strip = true;
for (size_t i = 0; i < cloud2.fields.size (); ++i)
if (cloud2.fields[i].name == "_")
strip = true;
if (!strip && cloud1.fields.size () != cloud2.fields.size ())
{
PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
return (false);
}
// Copy cloud1 into cloud_out
cloud_out = cloud1;
size_t nrpts = cloud_out.data.size ();
// Height = 1 => no more organized
cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
cloud_out.height = 1;
if (!cloud1.is_dense || !cloud2.is_dense)
cloud_out.is_dense = false;
else
cloud_out.is_dense = true;
// We need to strip the extra padding fields
if (strip)
{
// Get the field sizes for the second cloud
std::vector<sensor_msgs::msg::PointField> fields2;
std::vector<size_t> fields2_sizes;
for (size_t j = 0; j < cloud2.fields.size (); ++j)
{
if (cloud2.fields[j].name == "_")
continue;
fields2_sizes.push_back(
cloud2.fields[j].count *
static_cast<size_t>(pcl::getFieldSize(cloud2.fields[j].datatype)));
fields2.push_back(cloud2.fields[j]);
}
cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);
// Copy the second cloud
for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
{
size_t i = 0;
for (size_t j = 0; j < fields2.size (); ++j)
{
if (cloud1.fields[i].name == "_")
{
++i;
continue;
}
// We're fine with the special RGB vs RGBA use case
if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") ||
(cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") ||
(cloud1.fields[i].name == fields2[j].name))
{
memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]),
reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
fields2_sizes[j]);
++i; // increment the field size i
}
}
}
}
else
{
for (size_t i = 0; i < cloud1.fields.size (); ++i)
{
// We're fine with the special RGB vs RGBA use case
if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") ||
(cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb"))
continue;
// Otherwise we need to make sure the names are the same
if (cloud1.fields[i].name != cloud2.fields[i].name)
{
PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());
return (false);
}
}
cloud_out.data.resize (nrpts + cloud2.data.size ());
memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
}
return (true);
}
} // namespace pcl
/* TODO when ROS2 type masquerading is implemented */
/**
namespace ros
{
template<>
struct DefaultMessageCreator<pcl::PCLPointCloud2>
{
std::shared_ptr<pcl::PCLPointCloud2> operator() ()
{
std::shared_ptr<pcl::PCLPointCloud2> msg(new pcl::PCLPointCloud2());
return msg;
}
};
namespace message_traits
{
template<>
struct MD5Sum<pcl::PCLPointCloud2>
{
static const char* value() { return MD5Sum<sensor_msgs::msg::PointCloud2>::value(); }
static const char* value(const pcl::PCLPointCloud2&) { return value(); }
static const uint64_t static_value1 = MD5Sum<sensor_msgs::msg::PointCloud2>::static_value1;
static const uint64_t static_value2 = MD5Sum<sensor_msgs::msg::PointCloud2>::static_value2;
// If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
static_assert(static_value1 == 0x1158d486dd51d683ULL);
static_assert(static_value2 == 0xce2f1be655c3c181ULL);
};
template<>
struct DataType<pcl::PCLPointCloud2>
{
static const char* value() { return DataType<sensor_msgs::msg::PointCloud2>::value(); }
static const char* value(const pcl::PCLPointCloud2&) { return value(); }
};
template<>
struct Definition<pcl::PCLPointCloud2>
{
static const char* value() { return Definition<sensor_msgs::msg::PointCloud2>::value(); }
static const char* value(const pcl::PCLPointCloud2&) { return value(); }
};
template<> struct HasHeader<pcl::PCLPointCloud2> : std::true_type {};
} // namespace message_filters::message_traits
namespace serialization
{
**/
/*
* Provide a custom serialization for pcl::PCLPointCloud2
*/
/**
template<>
struct Serializer<pcl::PCLPointCloud2>
{
template<typename Stream>
inline static void write(Stream& stream, const pcl::PCLPointCloud2& m)
{
std_msgs::msg::Header header;
pcl_conversions::fromPCL(m.header, header);
stream.next(header);
stream.next(m.height);
stream.next(m.width);
std::vector<sensor_msgs::msg::PointField> 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<typename Stream>
inline static void read(Stream& stream, pcl::PCLPointCloud2& m)
{
std_msgs::msg::Header header;
stream.next(header);
pcl_conversions::toPCL(header, m.header);
stream.next(m.height);
stream.next(m.width);
std::vector<sensor_msgs::msg::PointField> pfs;
stream.next(pfs);
pcl_conversions::toPCL(pfs, m.fields);
stream.next(m.is_bigendian);
stream.next(m.point_step);
stream.next(m.row_step);
stream.next(m.data);
stream.next(m.is_dense);
}
inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m)
{
uint32_t length = 0;
std_msgs::msg::Header header;
pcl_conversions::fromPCL(m.header, header);
length += serializationLength(header);
length += 4; // height
length += 4; // width
std::vector<sensor_msgs::msg::PointField> 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<pcl::PCLPointField>
{
template<typename Stream>
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<typename Stream>
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<pcl::PCLHeader>
{
template<typename Stream>
inline static void write(Stream& stream, const pcl::PCLHeader& m)
{
std_msgs::msg::Header header;
pcl_conversions::fromPCL(m, header);
stream.next(header);
}
template<typename Stream>
inline static void read(Stream& stream, pcl::PCLHeader& m)
{
std_msgs::msg::Header header;
stream.next(header);
pcl_conversions::toPCL(header, m);
}
inline static uint32_t serializedLength(const pcl::PCLHeader& m)
{
uint32_t length = 0;
std_msgs::msg::Header header;
pcl_conversions::fromPCL(m, header);
length += serializationLength(header);
return length;
}
};
} // namespace ros::serialization
} // namespace ros
**/
#endif /* PCL_CONVERSIONS_H__ */

41
ros2_ws/src/perception_pcl/pcl_conversions/package.xml Normal file
View File

@ -0,0 +1,41 @@
<?xml version="1.0"?>
<package format="2">
<name>pcl_conversions</name>
<version>2.4.1</version>
<description>Provides conversions from PCL data types and ROS message types</description>
<author email="william@osrfoundation.org">William Woodall</author>
<author email="paul@bovbel.com">Paul Bovbel</author>
<author email="bill@neautomation.com">Bill Morris</author>
<author email="ankl@kth.se">Andreas Klintberg</author>
<maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
<maintainer email="www.kentaro.wada@gmail.com">Kentaro Wada</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>BSD</license>
<url>http://wiki.ros.org/pcl_conversions</url>
<url type="repository">https://github.com/ros-perception/perception_pcl</url>
<url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>libpcl-all-dev</build_depend>
<depend>eigen</depend>
<depend>message_filters</depend>
<depend>pcl_msgs</depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<exec_depend>libpcl-common</exec_depend>
<exec_depend>libpcl-io</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

158
ros2_ws/src/perception_pcl/pcl_conversions/test/test_pcl_conversions.cpp Normal file
View File

@ -0,0 +1,158 @@
#include <string>
#include "gtest/gtest.h"
#include "pcl_conversions/pcl_conversions.h"
namespace {
class PCLConversionTests : public ::testing::Test {
protected:
virtual void SetUp() {
pcl_image.header.stamp = 3141592653;
pcl_image.header.frame_id = "pcl";
pcl_image.height = 1;
pcl_image.width = 2;
pcl_image.step = 1;
pcl_image.is_bigendian = true;
pcl_image.encoding = "bgr8";
pcl_image.data.resize(2);
pcl_image.data[0] = 0x42;
pcl_image.data[1] = 0x43;
pcl_pc2.header.stamp = 3141592653;
pcl_pc2.header.frame_id = "pcl";
pcl_pc2.height = 1;
pcl_pc2.width = 2;
pcl_pc2.point_step = 1;
pcl_pc2.row_step = 1;
pcl_pc2.is_bigendian = true;
pcl_pc2.is_dense = true;
pcl_pc2.fields.resize(2);
pcl_pc2.fields[0].name = "XYZ";
pcl_pc2.fields[0].datatype = pcl::PCLPointField::INT8;
pcl_pc2.fields[0].count = 3;
pcl_pc2.fields[0].offset = 0;
pcl_pc2.fields[1].name = "RGB";
pcl_pc2.fields[1].datatype = pcl::PCLPointField::INT8;
pcl_pc2.fields[1].count = 3;
pcl_pc2.fields[1].offset = 8 * 3;
pcl_pc2.data.resize(2);
pcl_pc2.data[0] = 0x42;
pcl_pc2.data[1] = 0x43;
}
pcl::PCLImage pcl_image;
sensor_msgs::msg::Image image;
pcl::PCLPointCloud2 pcl_pc2;
sensor_msgs::msg::PointCloud2 pc2;
};
template<class T>
void test_image(T &image) {
EXPECT_EQ(std::string("pcl"), image.header.frame_id);
EXPECT_EQ(1U, image.height);
EXPECT_EQ(2U, image.width);
EXPECT_EQ(1U, image.step);
EXPECT_TRUE(image.is_bigendian);
EXPECT_EQ(std::string("bgr8"), image.encoding);
EXPECT_EQ(2U, image.data.size());
EXPECT_EQ(0x42, image.data[0]);
EXPECT_EQ(0x43, image.data[1]);
}
TEST_F(PCLConversionTests, imageConversion) {
pcl_conversions::fromPCL(pcl_image, image);
test_image(image);
pcl::PCLImage pcl_image2;
pcl_conversions::toPCL(image, pcl_image2);
test_image(pcl_image2);
EXPECT_EQ(pcl_image.header.stamp, pcl_image2.header.stamp);
}
template<class T>
void test_pc(T &pc) {
EXPECT_EQ(std::string("pcl"), pc.header.frame_id);
EXPECT_EQ(1U, pc.height);
EXPECT_EQ(2U, pc.width);
EXPECT_EQ(1U, pc.point_step);
EXPECT_EQ(1U, pc.row_step);
EXPECT_TRUE(pc.is_bigendian);
EXPECT_TRUE(pc.is_dense);
EXPECT_EQ("XYZ", pc.fields[0].name);
EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[0].datatype);
EXPECT_EQ(3U, pc.fields[0].count);
EXPECT_EQ(0U, pc.fields[0].offset);
EXPECT_EQ("RGB", pc.fields[1].name);
EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[1].datatype);
EXPECT_EQ(3U, pc.fields[1].count);
EXPECT_EQ(8U * 3U, pc.fields[1].offset);
EXPECT_EQ(2U, pc.data.size());
EXPECT_EQ(0x42, pc.data[0]);
EXPECT_EQ(0x43, pc.data[1]);
}
TEST_F(PCLConversionTests, pointcloud2Conversion) {
pcl_conversions::fromPCL(pcl_pc2, pc2);
test_pc(pc2);
pcl::PCLPointCloud2 pcl_pc2_2;
pcl_conversions::toPCL(pc2, pcl_pc2_2);
test_pc(pcl_pc2_2);
EXPECT_EQ(pcl_pc2.header.stamp, pcl_pc2_2.header.stamp);
}
} // namespace
struct StampTestData
{
const rclcpp::Time stamp_;
rclcpp::Time stamp2_;
explicit StampTestData(const rclcpp::Time &stamp)
: stamp_(stamp)
{
std::uint64_t pcl_stamp;
pcl_conversions::toPCL(stamp_, pcl_stamp);
pcl_conversions::fromPCL(pcl_stamp, stamp2_);
}
};
TEST(PCLConversionStamp, Stamps)
{
{
const StampTestData d(rclcpp::Time(1, 1000));
EXPECT_TRUE(d.stamp_==d.stamp2_);
}
{
const StampTestData d(rclcpp::Time(1, 999999000));
EXPECT_TRUE(d.stamp_==d.stamp2_);
}
{
const StampTestData d(rclcpp::Time(1, 999000000));
EXPECT_TRUE(d.stamp_==d.stamp2_);
}
{
const StampTestData d(rclcpp::Time(1423680574, 746000000));
EXPECT_TRUE(d.stamp_==d.stamp2_);
}
{
const StampTestData d(rclcpp::Time(1423680629, 901000000));
EXPECT_TRUE(d.stamp_==d.stamp2_);
}
}
int main(int argc, char **argv) {
try {
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
} catch (std::exception &e) {
std::cerr << "Unhandled Exception: " << e.what() << std::endl;
}
return 1;
}

328
ros2_ws/src/perception_pcl/pcl_ros/CHANGELOG.rst Normal file
View File

@ -0,0 +1,328 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package pcl_ros
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.6.2 (2018-05-20)
------------------
* Fix exported includes in Ubuntu Artful
* Increase limits on CropBox filter parameters
* Contributors: James Ward, Jiri Horner
1.6.1 (2018-05-08)
------------------
* Add 1.6.0 section to CHANGELOG.rst
* Fix the use of Eigen3 in cmake
* Contributors: Kentaro Wada
1.6.0 (2018-04-30)
------------------
* Fix build and update maintainers
* Add message_filters to find_package
* Remove unnecessary dependency on genmsg
* Contributors: Paul Bovbel, Kentaro Wada
1.5.4 (2018-03-31)
------------------
* update to use non deprecated pluginlib macro
* Fix config path of sample_voxel_grid.launch
* remove hack now that upstream pcl has been rebuilt
* Looser hzerror in test for extract_clusters to make it pass on Travis
* Add sample & test for surface/convex_hull
* Add sample & test for segmentation/extract_clusters.cpp
* Add sample & test for io/concatenate_data.cpp
* Add sample & test for features/normal_3d.cpp
* Organize samples of pcl_ros/features
* Add test arg to avoid duplicated testing
* LazyNodelet for features/*
* LazyNodelet for filters/ProjectInliers
* Refactor io/PCDReader and io/PCDWriter as child of PCLNodelet
* LazyNodelet for io/PointCloudConcatenateFieldsSynchronizer
* LazyNodelet for io/PointCloudConcatenateDataSynchronizer
* LazyNodelet for segmentation/SegmentDifferences
* LazyNodelet for segmentation/SACSegmentationFromNormals
* LazyNodelet for segmentation/SACSegmentation
* LazyNodelet for segmentation/ExtractPolygonalPrismData
* LazyNodelet for segmentation/EuclideanClusterExtraction
* LazyNodelet for surface/MovingLeastSquares
* LazyNodelet for surface/ConvexHull2D
* Add missing COMPONENTS of PCL
* Inherit NodeletLazy for pipeline with less cpu load
* Set leaf_size 0.02
* Install samples
* Add sample and test for pcl/StatisticalOutlierRemoval
Conflicts:
pcl_ros/CMakeLists.txt
* Add sample and test for pcl/VoxelGrid
Conflicts:
pcl_ros/CMakeLists.txt
* no need to remove duplicates
* spourious line change
* remove now unnecessary build_depend on qtbase5
* exclude PCL IO libraries exporting Qt flag
* find only PCL components used instead of all PCL
* Remove dependency on vtk/libproj-dev (`#145 <https://github.com/ros-perception/perception_pcl/issues/145>`_)
* Remove dependency on vtk/libproj-dev
These dependencies were introduced in `#124 <https://github.com/ros-perception/perception_pcl/issues/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 <https://github.com/ros-perception/perception_pcl/issues/124>`_ and `ros-infrastructure/reprepro-updater#32 <https://github.com/ros-infrastructure/reprepro-updater/issues/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 <https://github.com/ros-perception/perception_pcl/issues/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 <https://github.com/ros-perception/perception_pcl/issues/92>`_
* commented out test_tf_message_filter_pcl
Until `ros/geometry#80 <https://github.com/ros/geometry/issues/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<T> 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 <https://github.com/ros-perception/perception_pcl/issues/87>`_ for Indigo
* Fixes `#85 <https://github.com/ros-perception/perception_pcl/issues/85>`_ for Indigo
* Fixes `#77 <https://github.com/ros-perception/perception_pcl/issues/77>`_ and `#80 <https://github.com/ros-perception/perception_pcl/issues/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 <https://github.com/ros-perception/perception_pcl/issues/60>`_
* Contributors: Paul Bovbel
1.2.0 (2014-04-09)
------------------
* Updated maintainership
* Fix TF2 support for bag_to_pcd `#46 <https://github.com/ros-perception/perception_pcl/issues/46>`_
* Use cmake_modules to find eigen on indigo `#45 <https://github.com/ros-perception/perception_pcl/issues/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 <https://github.com/ros-perception/perception_pcl/issues/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 <https://github.com/ros-perception/perception_pcl/issues/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 <https://github.com/ros-perception/perception_pcl/issues/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 <https://github.com/ros-perception/perception_pcl/issues/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 <buildtool_depend>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

242
ros2_ws/src/perception_pcl/pcl_ros/CMakeLists.txt Normal file
View File

@ -0,0 +1,242 @@
cmake_minimum_required(VERSION 3.5)
project(pcl_ros)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -fPIC)
endif()
## Find system dependencies
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED QUIET COMPONENTS common features filters io segmentation surface)
## Find ROS package dependencies
find_package(ament_cmake REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
set(dependencies
pcl_conversions
rclcpp
rclcpp_components
sensor_msgs
geometry_msgs
tf2
tf2_geometry_msgs
tf2_ros
EIGEN3
PCL
)
## Declare the pcl_ros_tf library
add_library(pcl_ros_tf src/transforms.cpp)
target_include_directories(pcl_ros_tf PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
)
ament_target_dependencies(pcl_ros_tf
${dependencies}
)
### Nodelets
#
### Declare the pcl_ros_io library
#add_library(pcl_ros_io
# src/pcl_ros/io/bag_io.cpp
# src/pcl_ros/io/concatenate_data.cpp
# src/pcl_ros/io/concatenate_fields.cpp
# src/pcl_ros/io/io.cpp
# src/pcl_ros/io/pcd_io.cpp
#)
#target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#class_loader_hide_library_symbols(pcl_ros_io)
#
### Declare the pcl_ros_features library
#add_library(pcl_ros_features
# src/pcl_ros/features/feature.cpp
# # Compilation is much faster if we include all the following CPP files in feature.cpp
# src/pcl_ros/features/boundary.cpp
# src/pcl_ros/features/fpfh.cpp
# src/pcl_ros/features/fpfh_omp.cpp
# src/pcl_ros/features/shot.cpp
# src/pcl_ros/features/shot_omp.cpp
# src/pcl_ros/features/moment_invariants.cpp
# src/pcl_ros/features/normal_3d.cpp
# src/pcl_ros/features/normal_3d_omp.cpp
# src/pcl_ros/features/pfh.cpp
# src/pcl_ros/features/principal_curvatures.cpp
# src/pcl_ros/features/vfh.cpp
#)
#target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg)
#class_loader_hide_library_symbols(pcl_ros_features)
#
#
### Declare the pcl_ros_filters library
add_library(pcl_ros_filters SHARED
src/pcl_ros/filters/extract_indices.cpp
src/pcl_ros/filters/filter.cpp
src/pcl_ros/filters/passthrough.cpp
src/pcl_ros/filters/project_inliers.cpp
src/pcl_ros/filters/radius_outlier_removal.cpp
src/pcl_ros/filters/statistical_outlier_removal.cpp
src/pcl_ros/filters/voxel_grid.cpp
src/pcl_ros/filters/crop_box.cpp
)
target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES})
ament_target_dependencies(pcl_ros_filters ${dependencies})
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::ExtractIndices"
EXECUTABLE filter_extract_indices_node
)
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::PassThrough"
EXECUTABLE filter_passthrough_node
)
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::ProjectInliers"
EXECUTABLE filter_project_inliers_node
)
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::RadiusOutlierRemoval"
EXECUTABLE filter_radius_outlier_removal_node
)
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::StatisticalOutlierRemoval"
EXECUTABLE filter_statistical_outlier_removal_node
)
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::CropBox"
EXECUTABLE filter_crop_box_node
)
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::VoxelGrid"
EXECUTABLE filter_voxel_grid_node
)
class_loader_hide_library_symbols(pcl_ros_filters)
#
### Declare the pcl_ros_segmentation library
#add_library (pcl_ros_segmentation
# src/pcl_ros/segmentation/extract_clusters.cpp
# src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
# src/pcl_ros/segmentation/sac_segmentation.cpp
# src/pcl_ros/segmentation/segment_differences.cpp
# src/pcl_ros/segmentation/segmentation.cpp
#)
#target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg)
#class_loader_hide_library_symbols(pcl_ros_segmentation)
#
### Declare the pcl_ros_surface library
#add_library (pcl_ros_surface
# src/pcl_ros/surface/surface.cpp
# # Compilation is much faster if we include all the following CPP files in surface.cpp
# src/pcl_ros/surface/convex_hull.cpp
# src/pcl_ros/surface/moving_least_squares.cpp
#)
#target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg)
#class_loader_hide_library_symbols(pcl_ros_surface)
#
### Tools
#
add_library(pcd_to_pointcloud_lib SHARED tools/pcd_to_pointcloud.cpp)
target_link_libraries(pcd_to_pointcloud_lib
${PCL_LIBRARIES})
target_include_directories(pcd_to_pointcloud_lib PUBLIC
${PCL_INCLUDE_DIRS})
ament_target_dependencies(pcd_to_pointcloud_lib
rclcpp
rclcpp_components
sensor_msgs
pcl_conversions)
rclcpp_components_register_node(pcd_to_pointcloud_lib
PLUGIN "pcl_ros::PCDPublisher"
EXECUTABLE pcd_to_pointcloud)
#
#add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp)
#target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#
#add_executable(bag_to_pcd tools/bag_to_pcd.cpp)
#target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#
#add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp)
#target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#
#add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp)
#target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#
### Downloads
#
#catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd
# DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data
# MD5 546b5b4822fb1de21b0cf83d41ad6683
#)
#add_custom_target(download ALL DEPENDS download_extra_data)
#############
## Testing ##
#############
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
add_subdirectory(tests/filters)
#add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp)
#target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
#add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false)
#add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false)
#add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false)
#add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false)
#add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false)
endif()
install(
DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(
TARGETS
pcl_ros_tf
pcd_to_pointcloud_lib
# pcl_ros_io
# pcl_ros_features
pcl_ros_filters
# pcl_ros_surface
# pcl_ros_segmentation
# pointcloud_to_pcd
# bag_to_pcd
# convert_pcd_to_image
# convert_pointcloud_to_image
EXPORT export_pcl_ros
RUNTIME DESTINATION bin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
)
install(DIRECTORY plugins samples
DESTINATION share/${PROJECT_NAME})
# Export old-style CMake variables
ament_export_include_directories("include/${PROJECT_NAME}")
ament_export_libraries(pcl_ros_tf)
# Export modern CMake targets
ament_export_targets(export_pcl_ros HAS_LIBRARY_TARGET)
ament_export_dependencies(${dependencies})
ament_package()

25
ros2_ws/src/perception_pcl/pcl_ros/cfg/CropBox.cfg Executable file
View File

@ -0,0 +1,25 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
import os
import sys
sys.path.insert(0, os.path.dirname(__file__))
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator ()
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("min_x", double_t, 0, "X coordinate of the minimum point of the box.", -1, -1000, 1000)
gen.add ("max_x", double_t, 0, "X coordinate of the maximum point of the box.", 1, -1000, 1000)
gen.add ("min_y", double_t, 0, "Y coordinate of the minimum point of the box.", -1, -1000, 1000)
gen.add ("max_y", double_t, 0, "Y coordinate of the maximum point of the box.", 1, -1000, 1000)
gen.add ("min_z", double_t, 0, "Z coordinate of the minimum point of the box.", -1, -1000, 1000)
gen.add ("max_z", double_t, 0, "Z coordinate of the maximum point of the box.", 1, -1000, 1000)
gen.add ("keep_organized", bool_t, 0, "Set whether the filtered points should be kept and set to NaN, or removed from the PointCloud, thus potentially breaking its organized structure.", False)
gen.add ("negative", bool_t, 0, "If True the box will be empty Else the remaining points will be the ones in the box", False)
gen.add ("input_frame", str_t, 0, "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.", "")
gen.add ("output_frame", str_t, 0, "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different.", "")
exit (gen.generate (PACKAGE, "pcl_ros", "CropBox"))

17
ros2_ws/src/perception_pcl/pcl_ros/cfg/EuclideanClusterExtraction.cfg Executable file
View File

@ -0,0 +1,17 @@
#! /usr/bin/env python
PACKAGE='pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# enabling/disabling the unit limits
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("cluster_tolerance", double_t, 0, "The spatial tolerance as a measure in the L2 Euclidean space", 0.05, 0.0, 2.0)
gen.add ("cluster_min_size", int_t, 0, "The minimum number of points that a cluster must contain in order to be accepted", 1, 0, 1000)
gen.add ("cluster_max_size", int_t, 0, "The maximum number of points that a cluster must contain in order to be accepted", 2147483647, 0, 2147483647)
gen.add ("max_clusters", int_t, 0, "The maximum number of clusters to extract.", 2147483647, 1, 2147483647)
exit (gen.generate (PACKAGE, "pcl_ros", "EuclideanClusterExtraction"))

12
ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractIndices.cfg Executable file
View File

@ -0,0 +1,12 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *;
gen = ParameterGenerator ()
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("negative", bool_t, 0, "Extract indices or the negative (all-indices)", False)
exit (gen.generate (PACKAGE, "pcl_ros", "ExtractIndices"))

13
ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractPolygonalPrismData.cfg Executable file
View File

@ -0,0 +1,13 @@
#! /usr/bin/env python
PACKAGE='pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("height_min", double_t, 0, "The minimum allowed distance to the plane model value a point will be considered from", 0.0, -10.0, 10.0)
gen.add ("height_max", double_t, 0, "The maximum allowed distance to the plane model value a point will be considered from", 0.5, -10.0, 10.0)
exit (gen.generate (PACKAGE, "pcl_ros", "ExtractPolygonalPrismData"))

17
ros2_ws/src/perception_pcl/pcl_ros/cfg/Feature.cfg Executable file
View File

@ -0,0 +1,17 @@
#! /usr/bin/env python
PACKAGE='pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
#enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator")
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000)
gen.add ("radius_search", double_t, 0, "Sphere radius for nearest neighbor search", 0.0, 0.0, 0.5)
#gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum)
#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN")
exit (gen.generate (PACKAGE, "pcl_ros", "Feature"))

17
ros2_ws/src/perception_pcl/pcl_ros/cfg/Filter.cfg Executable file
View File

@ -0,0 +1,17 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
import os
import sys
sys.path.insert(0, os.path.dirname(__file__))
from common import add_common_parameters
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator ()
add_common_parameters (gen)
exit (gen.generate (PACKAGE, "pcl_ros", "Filter"))

20
ros2_ws/src/perception_pcl/pcl_ros/cfg/MLS.cfg Executable file
View File

@ -0,0 +1,20 @@
#! /usr/bin/env python
PACKAGE='pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator")
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum)
gen.add ("search_radius", double_t, 0, "Sphere radius for nearest neighbor search", 0.02, 0.0, 0.5)
#gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000)
gen.add ("use_polynomial_fit", bool_t, 0, "Whether to use polynomial fit", True)
gen.add ("polynomial_order", int_t, 0, "Set the spatial locator", 2, 0, 5)
gen.add ("gaussian_parameter", double_t, 0, "How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit)", 0.02, 0.0, 0.5)
#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN")
exit (gen.generate (PACKAGE, "pcl_ros", "MLS"))

15
ros2_ws/src/perception_pcl/pcl_ros/cfg/RadiusOutlierRemoval.cfg Executable file
View File

@ -0,0 +1,15 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *;
gen = ParameterGenerator ()
# enabling/disabling the unit limits
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("radius_search", double_t, 0, "Radius of the sphere that will determine which points are neighbors.", 0.1, 0.0, 10.0)
gen.add ("min_neighbors", int_t, 0, "The number of neighbors that need to be present in order to be classified as an inlier.", 5, 0, 1000)
exit (gen.generate (PACKAGE, "pcl_ros", "RadiusOutlierRemoval"))

13
ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation.cfg Executable file
View File

@ -0,0 +1,13 @@
#! /usr/bin/env python
PACKAGE='pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *
import SACSegmentation_common as common
gen = ParameterGenerator ()
common.add_common_parameters (gen)
exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentation"))

18
ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentationFromNormals.cfg Executable file
View File

@ -0,0 +1,18 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
PACKAGE='pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *
import roslib.packages
import SACSegmentation_common as common
gen = ParameterGenerator()
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("normal_distance_weight", double_t, 0, "The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point normals and the plane normal.", 0.1, 0, 1.0)
common.add_common_parameters (gen)
exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentationFromNormals"))

47
ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation_common.py Normal file
View File

@ -0,0 +1,47 @@
#! /usr/bin/env python
from dynamic_reconfigure.parameter_generator_catkin import bool_t
from dynamic_reconfigure.parameter_generator_catkin import double_t
from dynamic_reconfigure.parameter_generator_catkin import int_t
from dynamic_reconfigure.parameter_generator_catkin import str_t
# set up parameters that we care about
PACKAGE = 'pcl_ros'
def add_common_parameters(gen):
# add(self, name, paramtype, level, description, default = None, min = None,
# max = None, edit_method = '')
gen.add('max_iterations', int_t, 0,
'The maximum number of iterations the algorithm will run for',
50, 0, 100000)
gen.add('probability', double_t, 0,
'The desired probability of choosing at least one sample free from outliers',
0.99, 0.5, 0.99)
gen.add('distance_threshold', double_t, 0,
'The distance to model threshold',
0.02, 0, 1.0)
gen.add('optimize_coefficients', bool_t, 0,
'Model coefficient refinement',
True)
gen.add('radius_min', double_t, 0,
'The minimum allowed model radius (where applicable)',
0.0, 0, 1.0)
gen.add('radius_max', double_t, 0,
'The maximum allowed model radius (where applicable)',
0.05, 0, 1.0)
gen.add('eps_angle', double_t, 0,
('The maximum allowed difference between the model normal '
'and the given axis in radians.'),
0.17, 0.0, 1.5707)
gen.add('min_inliers', int_t, 0,
'The minimum number of inliers a model must have in order to be considered valid.',
0, 0, 100000)
gen.add('input_frame', str_t, 0,
('The input TF frame the data should be transformed into, '
'if input.header.frame_id is different.'),
'')
gen.add('output_frame', str_t, 0,
('The output TF frame the data should be transformed into, '
'if input.header.frame_id is different.'),
'')

16
ros2_ws/src/perception_pcl/pcl_ros/cfg/SegmentDifferences.cfg Executable file
View File

@ -0,0 +1,16 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE='pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# enabling/disabling the unit limits
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("distance_threshold", double_t, 0, "The distance tolerance as a measure in the L2 Euclidean space between corresponding points.", 0.0, 0.0, 2.0)
exit (gen.generate (PACKAGE, "pcl_ros", "SegmentDifferences"))

17
ros2_ws/src/perception_pcl/pcl_ros/cfg/StatisticalOutlierRemoval.cfg Executable file
View File

@ -0,0 +1,17 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
from dynamic_reconfigure.parameter_generator_catkin import *;
gen = ParameterGenerator ()
# enabling/disabling the unit limits
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("mean_k", int_t, 0, "The number of points (k) to use for mean distance estimation", 2, 2, 100)
gen.add ("stddev", double_t, 0, "The standard deviation multiplier threshold. All points outside the mean +- sigma * std_mul will be considered outliers.", 0.0, 0.0, 5.0)
gen.add ("negative", bool_t, 0, "Set whether the inliers should be returned (true) or the outliers (false)", False)
exit (gen.generate (PACKAGE, "pcl_ros", "StatisticalOutlierRemoval"))

18
ros2_ws/src/perception_pcl/pcl_ros/cfg/VoxelGrid.cfg Executable file
View File

@ -0,0 +1,18 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
import os
import sys
sys.path.insert(0, os.path.dirname(__file__))
from common import add_common_parameters
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator ()
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("leaf_size", double_t, 0, "The size of a leaf (on x,y,z) used for downsampling.", 0.01, 0, 1.0)
add_common_parameters (gen)
exit (gen.generate (PACKAGE, "pcl_ros", "VoxelGrid"))

36
ros2_ws/src/perception_pcl/pcl_ros/cfg/common.py Normal file
View File

@ -0,0 +1,36 @@
#! /usr/bin/env python
from dynamic_reconfigure.parameter_generator_catkin import bool_t
from dynamic_reconfigure.parameter_generator_catkin import double_t
from dynamic_reconfigure.parameter_generator_catkin import str_t
# set up parameters that we care about
PACKAGE = 'pcl_ros'
def add_common_parameters(gen):
# def add (self, name, paramtype, level, description, default = None, min = None,
# max = None, edit_method = ''):
gen.add('filter_field_name', str_t, 0, 'The field name used for filtering', 'z')
gen.add('filter_limit_min', double_t, 0,
'The minimum allowed field value a point will be considered from',
0.0, -100000.0, 100000.0)
gen.add('filter_limit_max', double_t, 0,
'The maximum allowed field value a point will be considered from',
1.0, -100000.0, 100000.0)
gen.add('filter_limit_negative', bool_t, 0,
('Set to true if we want to return the data outside '
'[filter_limit_min; filter_limit_max].'),
False)
gen.add('keep_organized', bool_t, 0,
('Set whether the filtered points should be kept and set to NaN, '
'or removed from the PointCloud, thus potentially breaking its organized structure.'),
False)
gen.add('input_frame', str_t, 0,
('The input TF frame the data should be transformed into before processing, '
'if input.header.frame_id is different.'),
'')
gen.add('output_frame', str_t, 0,
('The output TF frame the data should be transformed into after processing, '
'if input.header.frame_id is different.'),
'')

87
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/boundary.hpp Normal file
View File

@ -0,0 +1,87 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: boundary.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__BOUNDARY_HPP_
#define PCL_ROS__FEATURES__BOUNDARY_HPP_
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true
#include <pcl/features/boundary.h>
#include "pcl_ros/features/feature.hpp"
namespace pcl_ros
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle
* criterion. The code makes use of the estimated surface normals at each point in the input dataset.
*
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu
*/
class BoundaryEstimation : public FeatureFromNormals
{
private:
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_;
typedef pcl::PointCloud<pcl::Boundary> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__BOUNDARY_HPP_

267
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/feature.hpp Normal file
View File

@ -0,0 +1,267 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: feature.h 35422 2011-01-24 20:04:44Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__FEATURE_HPP_
#define PCL_ROS__FEATURES__FEATURE_HPP_
// PCL includes
#include <pcl/features/feature.h>
#include <pcl_msgs/PointIndices.h>
#include <message_filters/pass_through.h>
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
// PCL conversions
#include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/FeatureConfig.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
///////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Feature represents the base feature class. Some generic 3D operations that
* are applicable to all features are defined here as static methods.
* \author Radu Bogdan Rusu
*/
class Feature : public PCLNodelet
{
public:
typedef pcl::KdTree<pcl::PointXYZ> KdTree;
typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
typedef boost::shared_ptr<const PointCloudIn> 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<PointCloudIn> 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<dynamic_reconfigure::Server<FeatureConfig>> 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<PointIndices> nf_pi_;
message_filters::PassThrough<PointCloudIn> 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<PointIndices>(indices));
}
private:
/** \brief Synchronized input, surface, and point indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointCloudIn, PointIndices>>> sync_input_surface_indices_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudIn, PointIndices>>> 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<pcl::Normal> PointCloudN;
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
typedef boost::shared_ptr<const PointCloudN> 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<PointCloudN> sub_normals_filter_;
/** \brief Synchronized input, normals, surface, and point indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_e_;
/** \brief Internal method. */
void computePublish(
const PointCloudInConstPtr &,
const PointCloudInConstPtr &,
const IndicesPtr &) {} // This should never be called
/** \brief Nodelet initialization routine. */
virtual void onInit();
/** \brief NodeletLazy connection routine. */
virtual void subscribe();
virtual void unsubscribe();
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
* \param cloud the pointer to the input point cloud
* \param cloud_normals the pointer to the input point cloud normals
* \param cloud_surface the pointer to the surface point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_normals_surface_indices_callback(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & cloud_normals,
const PointCloudInConstPtr & cloud_surface,
const PointIndicesConstPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__FEATURE_HPP_

99
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh.hpp Normal file
View File

@ -0,0 +1,99 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: fpfh.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__FPFH_HPP_
#define PCL_ROS__FEATURES__FPFH_HPP_
#include <pcl/features/fpfh.h>
#include "pcl_ros/features/pfh.hpp"
namespace pcl_ros
{
/** \brief @b FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point cloud
* dataset containing points and normals.
*
* @note If you use this code in any academic work, please cite:
*
* <ul>
* <li> 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.
* </li>
* <li> 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.
* </li>
* </ul>
*
* @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<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__FPFH_HPP_

96
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp Normal file
View File

@ -0,0 +1,96 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: fpfh_omp.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__FPFH_OMP_HPP_
#define PCL_ROS__FEATURES__FPFH_OMP_HPP_
#include <pcl/features/fpfh_omp.h>
#include "pcl_ros/features/fpfh.hpp"
namespace pcl_ros
{
/** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud
* dataset containing points and normals, in parallel, using the OpenMP standard.
*
* @note If you use this code in any academic work, please cite:
*
* <ul>
* <li> 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.
* </li>
* <li> 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.
* </li>
* </ul>
* \author Radu Bogdan Rusu
*/
class FPFHEstimationOMP : public FeatureFromNormals
{
private:
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__FPFH_OMP_HPP_

82
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/moment_invariants.hpp Normal file
View File

@ -0,0 +1,82 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: moment_invariants.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_
#define PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_
#include <pcl/features/moment_invariants.h>
#include "pcl_ros/features/feature.hpp"
namespace pcl_ros
{
/** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
*
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu
*/
class MomentInvariantsEstimation : public Feature
{
private:
pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_;
typedef pcl::PointCloud<pcl::MomentInvariants> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_

86
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d.hpp Normal file
View File

@ -0,0 +1,86 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: normal_3d.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__NORMAL_3D_HPP_
#define PCL_ROS__FEATURES__NORMAL_3D_HPP_
#include <pcl/features/normal_3d.h>
#include "pcl_ros/features/feature.hpp"
namespace pcl_ros
{
/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
* curvatures.
*
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations.
* \author Radu Bogdan Rusu
*/
class NormalEstimation : public Feature
{
private:
/** \brief PCL implementation object. */
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_;
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type.
* \param cloud the input point cloud to copy the header from.
*/
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__NORMAL_3D_HPP_

80
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp Normal file
View File

@ -0,0 +1,80 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: normal_3d_omp.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_
#define PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_
#include <pcl/features/normal_3d_omp.h>
#include "pcl_ros/features/normal_3d.hpp"
namespace pcl_ros
{
/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
* curvatures, in parallel, using the OpenMP standard.
* \author Radu Bogdan Rusu
*/
class NormalEstimationOMP : public Feature
{
private:
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_;
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_

85
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp Normal file
View File

@ -0,0 +1,85 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: normal_3d_tbb.h 35661 2011-02-01 06:04:14Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_
#define PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_
// #include "pcl_ros/pcl_ros_config.hpp"
// #if defined(HAVE_TBB)
#include <pcl/features/normal_3d_tbb.h>
#include "pcl_ros/features/normal_3d.hpp"
namespace pcl_ros
{
/** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and
* curvatures, in parallel, using Intel's Threading Building Blocks library.
* \author Radu Bogdan Rusu
*/
class NormalEstimationTBB : public Feature
{
private:
pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_;
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloud>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
// #endif // HAVE_TBB
#endif // PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_

99
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/pfh.hpp Normal file
View File

@ -0,0 +1,99 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: pfh.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__PFH_HPP_
#define PCL_ROS__FEATURES__PFH_HPP_
#include <pcl/features/pfh.h>
#include "pcl_ros/features/feature.hpp"
namespace pcl_ros
{
/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
* containing points and normals.
*
* @note If you use this code in any academic work, please cite:
*
* <ul>
* <li> 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.
* </li>
* <li> 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.
* </li>
* </ul>
*
* @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<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_;
typedef pcl::PointCloud<pcl::PFHSignature125> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__PFH_HPP_

85
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp Normal file
View File

@ -0,0 +1,85 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: principal_curvatures.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_
#define PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true
#include <pcl/features/principal_curvatures.h>
#include "pcl_ros/features/feature.hpp"
namespace pcl_ros
{
/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
* principal surface curvatures for a given point cloud dataset containing points and normals.
*
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu, Jared Glover
*/
class PrincipalCurvaturesEstimation : public FeatureFromNormals
{
private:
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_;
typedef pcl::PointCloud<pcl::PrincipalCurvatures> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_

79
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot.hpp Normal file
View File

@ -0,0 +1,79 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, Ryohei Ueda, JSK Lab
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of JSK Lab. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
#ifndef PCL_ROS__FEATURES__SHOT_HPP_
#define PCL_ROS__FEATURES__SHOT_HPP_
#include <pcl/features/shot.h>
#include "pcl_ros/features/pfh.hpp"
namespace pcl_ros
{
/** \brief @b SHOTEstimation estimates SHOT descriptor.
*
*/
class SHOTEstimation : public FeatureFromNormals
{
private:
pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__SHOT_HPP_

78
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot_omp.hpp Normal file
View File

@ -0,0 +1,78 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, Ryohei Ueda, JSK Lab
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of JSK Lab. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
#ifndef PCL_ROS__FEATURES__SHOT_OMP_HPP_
#define PCL_ROS__FEATURES__SHOT_OMP_HPP_
#include <pcl/features/shot_omp.h>
#include "pcl_ros/features/shot.hpp"
namespace pcl_ros
{
/** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP.
*/
class SHOTEstimationOMP : public FeatureFromNormals
{
private:
pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__SHOT_OMP_HPP_

84
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/vfh.hpp Normal file
View File

@ -0,0 +1,84 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: vfh.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__FEATURES__VFH_HPP_
#define PCL_ROS__FEATURES__VFH_HPP_
#include <pcl/features/vfh.h>
#include "pcl_ros/features/fpfh.hpp"
namespace pcl_ros
{
/** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> 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<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_;
typedef pcl::PointCloud<pcl::VFHSignature308> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FEATURES__VFH_HPP_

87
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/crop_box.hpp Normal file
View File

@ -0,0 +1,87 @@
/*
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: cropbox.cpp
*
*/
#ifndef PCL_ROS__FILTERS__CROP_BOX_HPP_
#define PCL_ROS__FILTERS__CROP_BOX_HPP_
// PCL includes
#include <pcl/filters/crop_box.h>
#include <vector>
#include "pcl_ros/filters/filter.hpp"
namespace pcl_ros
{
/** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box.
*
* \author Radu Bogdan Rusu
* \author Justin Rosen
* \author Marti Morta Garriga
*/
class CropBox : public Filter
{
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override;
/** \brief Parameter callback
* \param params parameter values to set
*/
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
private:
/** \brief The PCL filter implementation used. */
pcl::CropBox<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit CropBox(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros
#endif // PCL_ROS__FILTERS__CROP_BOX_HPP_

84
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/extract_indices.hpp Normal file
View File

@ -0,0 +1,84 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: extract_indices.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
#define PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
// PCL includes
#include <pcl/filters/extract_indices.h>
#include <vector>
#include "pcl_ros/filters/filter.hpp"
namespace pcl_ros
{
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ExtractIndices : public Filter
{
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override;
/** \brief Parameter callback
* \param params parameter values to set
*/
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
private:
/** \brief The PCL filter implementation used. */
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit ExtractIndices(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros
#endif // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_

163
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/filter.hpp Normal file
View File

@ -0,0 +1,163 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: filter.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS__FILTERS__FILTER_HPP_
#define PCL_ROS__FILTERS__FILTER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "pcl_ros/pcl_node.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are
* applicable to all filters are defined here as static methods.
* \author Radu Bogdan Rusu
*/
class Filter : public PCLNode<sensor_msgs::msg::PointCloud2>
{
public:
typedef sensor_msgs::msg::PointCloud2 PointCloud2;
typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
/** \brief Filter constructor
* \param node_name node name
* \param options node options
*/
Filter(std::string node_name, const rclcpp::NodeOptions & options);
protected:
/** \brief declare and subscribe to param callback for input_frame and output_frame params */
void
use_frame_params();
/** \brief Add common parameters */
std::vector<std::string> add_common_params();
/** \brief The input PointCloud subscriber. */
rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;
message_filters::Subscriber<PointCloud2> sub_input_filter_;
/** \brief The desired user filter field name. */
std::string filter_field_name_;
/** \brief The minimum allowed filter value a point will be considered from. */
double filter_limit_min_;
/** \brief The maximum allowed filter value a point will be considered from. */
double filter_limit_max_;
/** \brief Set to true if we want to return the data outside
* (\a filter_limit_min_;\a filter_limit_max_). Default: false.
*/
bool filter_limit_negative_;
/** \brief The input TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_input_frame_;
/** \brief The original data input TF frame. */
std::string tf_input_orig_frame_;
/** \brief The output TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_output_frame_;
/** \brief Internal mutex. */
std::mutex mutex_;
/** \brief Virtual abstract filter method. To be implemented by every child.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
* \param output the resultant filtered PointCloud2
*/
virtual void
filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) = 0;
/** \brief Lazy transport subscribe routine. */
virtual void
subscribe();
/** \brief Lazy transport unsubscribe routine. */
virtual void
unsubscribe();
/** \brief Call the child filter () method, optionally transform the result, and publish it.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
*/
void
computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices);
private:
/** \brief Pointer to parameters callback handle. */
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
/** \brief Synchronized input, and indices.*/
std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
PointIndices>>> sync_input_indices_e_;
std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
PointIndices>>> sync_input_indices_a_;
/** \brief Parameter callback
* \param params parameter values to set
*/
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
/** \brief PointCloud2 + Indices data callback. */
void
input_indices_callback(
const PointCloud2::ConstSharedPtr & cloud,
const PointIndices::ConstSharedPtr & indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FILTERS__FILTER_HPP_

84
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/passthrough.hpp Normal file
View File

@ -0,0 +1,84 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: passthrough.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS__FILTERS__PASSTHROUGH_HPP_
#define PCL_ROS__FILTERS__PASSTHROUGH_HPP_
// PCL includes
#include <pcl/filters/passthrough.h>
#include <vector>
#include "pcl_ros/filters/filter.hpp"
namespace pcl_ros
{
/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
* constraints.
* \author Radu Bogdan Rusu
*/
class PassThrough : public Filter
{
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override;
/** \brief Parameter callback
* \param params parameter values to set
*/
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
private:
/** \brief The PCL filter implementation used. */
pcl::PassThrough<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit PassThrough(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros
#endif // PCL_ROS__FILTERS__PASSTHROUGH_HPP_

103
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/project_inliers.hpp Normal file
View File

@ -0,0 +1,103 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: project_inliers.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
#define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
// PCL includes
#include <pcl/filters/project_inliers.h>
#include <message_filters/subscriber.h>
#include <memory>
#include "pcl_ros/filters/filter.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
* separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ProjectInliers : public Filter
{
public:
explicit ProjectInliers(const rclcpp::NodeOptions & options);
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override;
private:
/** \brief A pointer to the vector of model coefficients. */
ModelCoefficientsConstPtr model_;
/** \brief The message filter subscriber for model coefficients. */
message_filters::Subscriber<ModelCoefficients> sub_model_;
/** \brief Synchronized input, indices, and model coefficients.*/
std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
PointIndices, ModelCoefficients>>> sync_input_indices_model_e_;
std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
PointIndices, ModelCoefficients>>> sync_input_indices_model_a_;
/** \brief The PCL filter implementation used. */
pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
void subscribe() override;
void unsubscribe() override;
/** \brief PointCloud2 + Indices + Model data callback. */
void
input_indices_model_callback(
const PointCloud2::ConstSharedPtr & cloud,
const PointIndicesConstPtr & indices,
const ModelCoefficientsConstPtr & model);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_

85
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp Normal file
View File

@ -0,0 +1,85 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: radius_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_
#define PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_
// PCL includes
#include <pcl/filters/radius_outlier_removal.h>
#include <vector>
#include "pcl_ros/filters/filter.hpp"
namespace pcl_ros
{
/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
* search radius is smaller than a given K.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class RadiusOutlierRemoval : public Filter
{
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override;
/** \brief Parameter callback
* \param params parameter values to set
*/
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
private:
/** \brief The PCL filter implementation used. */
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit RadiusOutlierRemoval(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros
#endif // PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_

91
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp Normal file
View File

@ -0,0 +1,91 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: statistical_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_
#define PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_
// PCL includes
#include <pcl/filters/statistical_outlier_removal.h>
#include <vector>
#include "pcl_ros/filters/filter.hpp"
namespace pcl_ros
{
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
* information check:
* <ul>
* <li> 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.
* </ul>
*
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class StatisticalOutlierRemoval : public Filter
{
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override;
/** \brief Parameter callback
* \param params parameter values to set
*/
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
private:
/** \brief The PCL filter implementation used. */
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit StatisticalOutlierRemoval(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros
#endif // PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_

83
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp Normal file
View File

@ -0,0 +1,83 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: voxel_grid.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS__FILTERS__VOXEL_GRID_HPP_
#define PCL_ROS__FILTERS__VOXEL_GRID_HPP_
// PCL includes
#include <pcl/filters/voxel_grid.h>
#include <vector>
#include "pcl_ros/filters/filter.hpp"
namespace pcl_ros
{
/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
* \author Radu Bogdan Rusu
*/
class VoxelGrid : public Filter
{
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override;
/** \brief Parameter callback
* \param params parameter values to set
*/
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
private:
/** \brief The PCL filter implementation used. */
pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit VoxelGrid(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros
#endif // PCL_ROS__FILTERS__VOXEL_GRID_HPP_

266
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/impl/transforms.hpp Normal file
View File

@ -0,0 +1,266 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
#ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_
#define PCL_ROS__IMPL__TRANSFORMS_HPP_
#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/convert.h>
#include <tf2/exceptions.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <string>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/time.hpp>
#include "pcl_ros/transforms.hpp"
using pcl_conversions::fromPCL;
using pcl_conversions::toPCL;
namespace pcl_ros
{
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
void
transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const tf2::Transform & transform)
{
// Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
// of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
// this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
// Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
// the conversion of the point cloud anyway. Idem for the origin.
tf2::Quaternion q = transform.getRotation();
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
tf2::Vector3 v = transform.getOrigin();
Eigen::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v);
// Assemble an Eigen Transform
// Eigen::Transform3f t;
// t = translation * rotation;
transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
void
transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const geometry_msgs::msg::TransformStamped & transform)
{
tf2::Transform tf;
tf2::convert(transform.transform, tf);
transformPointCloudWithNormals(cloud_in, cloud_out, tf);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
void
transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const tf2::Transform & transform)
{
// Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
// of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
// this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
// Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
// the conversion of the point cloud anyway. Idem for the origin.
tf2::Quaternion q = transform.getRotation();
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
tf2::Vector3 v = transform.getOrigin();
Eigen::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v);
// Assemble an Eigen Transform
// Eigen::Transform3f t;
// t = translation * rotation;
transformPointCloud(cloud_in, cloud_out, origin, rotation);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
void
transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const geometry_msgs::msg::TransformStamped & transform)
{
tf2::Transform tf;
tf2::convert(transform.transform, tf);
transformPointCloud(cloud_in, cloud_out, tf);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
bool
transformPointCloudWithNormals(
const std::string & target_frame,
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf2_ros::Buffer & tf_buffer)
{
if (cloud_in.header.frame_id == target_frame) {
cloud_out = cloud_in;
return true;
}
geometry_msgs::msg::TransformStamped transform;
try {
transform =
tf_buffer.lookupTransform(
target_frame, cloud_in.header.frame_id,
fromPCL(cloud_in.header.stamp));
} catch (tf2::LookupException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false;
} catch (tf2::ExtrapolationException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false;
}
transformPointCloudWithNormals(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame;
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
bool
transformPointCloud(
const std::string & target_frame,
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf2_ros::Buffer & tf_buffer)
{
if (cloud_in.header.frame_id == target_frame) {
cloud_out = cloud_in;
return true;
}
geometry_msgs::msg::TransformStamped transform;
try {
transform =
tf_buffer.lookupTransform(
target_frame, cloud_in.header.frame_id,
fromPCL(cloud_in.header.stamp));
} catch (tf2::LookupException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false;
} catch (tf2::ExtrapolationException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false;
}
transformPointCloud(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame;
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
bool
transformPointCloud(
const std::string & target_frame,
const rclcpp::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf2_ros::Buffer & tf_buffer)
{
geometry_msgs::msg::TransformStamped transform;
try {
transform = tf_buffer.lookupTransform(
target_frame, target_time, cloud_in.header.frame_id,
fromPCL(cloud_in.header.stamp), fixed_frame);
} catch (tf2::LookupException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false;
} catch (tf2::ExtrapolationException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false;
}
transformPointCloud(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame;
std_msgs::msg::Header header;
header.stamp = target_time;
cloud_out.header = toPCL(header);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
bool
transformPointCloudWithNormals(
const std::string & target_frame,
const rclcpp::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf2_ros::Buffer & tf_buffer)
{
geometry_msgs::msg::TransformStamped transform;
try {
transform = tf_buffer.lookupTransform(
target_frame, target_time, cloud_in.header.frame_id,
fromPCL(cloud_in.header.stamp), fixed_frame);
} catch (tf2::LookupException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false;
} catch (tf2::ExtrapolationException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false;
}
transformPointCloudWithNormals(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame;
std_msgs::msg::Header header;
header.stamp = target_time;
cloud_out.header = toPCL(header);
return true;
}
} // namespace pcl_ros
#endif // PCL_ROS__IMPL__TRANSFORMS_HPP_

131
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/bag_io.hpp Normal file
View File

@ -0,0 +1,131 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: bag_io.h 35471 2011-01-25 06:50:00Z rusu $
*
*/
#ifndef PCL_ROS__IO__BAG_IO_HPP_
#define PCL_ROS__IO__BAG_IO_HPP_
#include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <string>
#include <pcl_ros/pcl_nodelet.hpp>
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<sensor_msgs::PointCloud2>();
++it_;
}
return output_;
}
/** \brief Open a BAG file for reading and select a specified topic
* \param file_name the BAG file to open
* \param topic_name the topic that we want to read data from
*/
bool open(const std::string & file_name, const std::string & topic_name);
/** \brief Close an open BAG file. */
inline void
close()
{
bag_.close();
}
/** \brief Nodelet initialization routine. */
virtual void onInit();
private:
/** \brief The publishing interval in seconds. Set to 0 to publish once (default). */
double publish_rate_;
/** \brief The BAG object. */
rosbag::Bag bag_;
/** \brief The BAG view object. */
rosbag::View view_;
/** \brief The BAG view iterator object. */
rosbag::View::iterator it_;
/** \brief The name of the topic that contains the PointCloud data. */
std::string topic_name_;
/** \brief The name of the BAG file that contains the PointCloud data. */
std::string file_name_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloudPtr output_;
/** \brief Signals that a new PointCloud2 message has been read from the file. */
// bool cloud_received_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__IO__BAG_IO_HPP_

146
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_data.hpp Normal file
View File

@ -0,0 +1,146 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: concatenate_data.h 35231 2011-01-14 05:33:20Z rusu $
*
*/
#ifndef PCL_ROS__IO__CONCATENATE_DATA_HPP_
#define PCL_ROS__IO__CONCATENATE_DATA_HPP_
// ROS includes
#include <tf/transform_listener.h>
#include <nodelet_topic_tools/nodelet_lazy.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/pass_through.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <string>
#include <vector>
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
* checks their timestamps, and concatenates their fields together into a single
* PointCloud output message.
* \author Radu Bogdan Rusu
*/
class PointCloudConcatenateDataSynchronizer : public nodelet_topic_tools::NodeletLazy
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
/** \brief Empty constructor. */
PointCloudConcatenateDataSynchronizer()
: maximum_queue_size_(3) {}
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
explicit PointCloudConcatenateDataSynchronizer(int queue_size)
: maximum_queue_size_(queue_size), approximate_sync_(false) {}
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateDataSynchronizer() {}
void onInit();
void subscribe();
void unsubscribe();
private:
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
/** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_;
/** \brief True if we use an approximate time synchronizer
* versus an exact one (false by default).
*/
bool approximate_sync_;
/** \brief A vector of message filters. */
std::vector<boost::shared_ptr<message_filters::Subscriber<PointCloud2>>> 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<PointCloud2> nf_;
/** \brief Synchronizer.
* \note This will most likely be rewritten soon using the DynamicTimeSynchronizer.
*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2,
PointCloud2>>> ts_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2,
PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2>>> 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<PointCloud2>(cloud));
}
/** \brief Input callback for 8 synchronized topics. */
void input(
const PointCloud2::ConstPtr & in1, const PointCloud2::ConstPtr & in2,
const PointCloud2::ConstPtr & in3, const PointCloud2::ConstPtr & in4,
const PointCloud2::ConstPtr & in5, const PointCloud2::ConstPtr & in6,
const PointCloud2::ConstPtr & in7, const PointCloud2::ConstPtr & in8);
void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out);
};
} // namespace pcl_ros
#endif // PCL_ROS__IO__CONCATENATE_DATA_HPP_

106
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp Normal file
View File

@ -0,0 +1,106 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: concatenate_fields.h 35052 2011-01-03 21:04:57Z rusu $
*
*/
#ifndef PCL_ROS__IO__CONCATENATE_FIELDS_HPP_
#define PCL_ROS__IO__CONCATENATE_FIELDS_HPP_
// ROS includes
#include <nodelet_topic_tools/nodelet_lazy.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/PointCloud2.h>
#include <map>
#include <vector>
namespace pcl_ros
{
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
* input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
* a single PointCloud output message.
* \author Radu Bogdan Rusu
*/
class PointCloudConcatenateFieldsSynchronizer : public nodelet_topic_tools::NodeletLazy
{
public:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
/** \brief Empty constructor. */
PointCloudConcatenateFieldsSynchronizer()
: maximum_queue_size_(3), maximum_seconds_(0) {}
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
explicit PointCloudConcatenateFieldsSynchronizer(int queue_size)
: maximum_queue_size_(queue_size), maximum_seconds_(0) {}
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateFieldsSynchronizer() {}
void onInit();
void subscribe();
void unsubscribe();
void input_callback(const PointCloudConstPtr & cloud);
private:
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
/** \brief The number of input messages that we expect on the input topic. */
int input_messages_;
/** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_;
/** \brief The maximum number of seconds to wait until we drop the synchronization. */
double maximum_seconds_;
/** \brief A queue for messages. */
std::map<ros::Time, std::vector<PointCloudConstPtr>> queue_;
};
} // namespace pcl_ros
#endif // PCL_ROS__IO__CONCATENATE_FIELDS_HPP_

137
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/pcd_io.hpp Normal file
View File

@ -0,0 +1,137 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: pcd_io.h 35054 2011-01-03 21:16:49Z rusu $
*
*/
#ifndef PCL_ROS__IO__PCD_IO_HPP_
#define PCL_ROS__IO__PCD_IO_HPP_
#include <pcl/io/pcd_io.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format reader.
* \author Radu Bogdan Rusu
*/
class PCDReader : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
/** \brief Empty constructor. */
PCDReader()
: publish_rate_(0), tf_frame_("/base_link") {}
virtual void onInit();
/** \brief Set the publishing rate in seconds.
* \param publish_rate the publishing rate in seconds
*/
inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;}
/** \brief Get the publishing rate in seconds. */
inline double getPublishRate() {return publish_rate_;}
/** \brief Set the TF frame the PointCloud will be published in.
* \param tf_frame the TF frame the PointCloud will be published in
*/
inline void setTFframe(std::string tf_frame) {tf_frame_ = tf_frame;}
/** \brief Get the TF frame the PointCloud is published in. */
inline std::string getTFframe() {return tf_frame_;}
protected:
/** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
double publish_rate_;
/** \brief The TF frame the data should be published in ("/base_link" by default). */
std::string tf_frame_;
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloud2Ptr output_;
private:
/** \brief The PCL implementation used. */
pcl::PCDReader impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format writer.
* \author Radu Bogdan Rusu
*/
class PCDWriter : public PCLNodelet
{
public:
PCDWriter()
: file_name_(""), binary_mode_(true) {}
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
virtual void onInit();
void input_callback(const PointCloud2ConstPtr & cloud);
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
protected:
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \brief Set to true if the output files should be saved in binary mode (true). */
bool binary_mode_;
private:
/** \brief The PCL implementation used. */
pcl::PCDWriter impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__IO__PCD_IO_HPP_

296
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/pcl_node.hpp Normal file
View File

@ -0,0 +1,296 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: pcl_node.h 33238 2010-03-11 00:46:58Z rusu $
*
*/
/**
\author Radu Bogdan Rusu
**/
#ifndef PCL_ROS__PCL_NODE_HPP_
#define PCL_ROS__PCL_NODE_HPP_
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
// #include <nodelet_topic_tools/nodelet_lazy.h> // TODO(daisukes): lazy subscription
#include <memory>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl_msgs/msg/point_indices.hpp>
#include <pcl_msgs/msg/model_coefficients.hpp>
// #include "pcl_ros/point_cloud.hpp"
using pcl_conversions::fromPCL;
namespace pcl_ros
{
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b PCLNode represents the base PCL Node class. All PCL node should inherit from
* this class. */
template<typename T, typename PublisherT = rclcpp::Publisher<T>>
class PCLNode : public rclcpp::Node
{
public:
typedef sensor_msgs::msg::PointCloud2 PointCloud2;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
typedef pcl_msgs::msg::PointIndices PointIndices;
typedef PointIndices::SharedPtr PointIndicesPtr;
typedef PointIndices::ConstSharedPtr PointIndicesConstPtr;
typedef pcl_msgs::msg::ModelCoefficients ModelCoefficients;
typedef ModelCoefficients::SharedPtr ModelCoefficientsPtr;
typedef ModelCoefficients::ConstSharedPtr ModelCoefficientsConstPtr;
typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
/** \brief Empty constructor. */
PCLNode(std::string node_name, const rclcpp::NodeOptions & options)
: rclcpp::Node(node_name, options),
use_indices_(false), transient_local_indices_(false),
max_queue_size_(3), approximate_sync_(false),
tf_buffer_(this->get_clock()),
tf_listener_(tf_buffer_)
{
{
rcl_interfaces::msg::ParameterDescriptor desc;
desc.name = "max_queue_size";
desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
desc.description = "QoS History depth";
desc.read_only = true;
max_queue_size_ = declare_parameter(desc.name, max_queue_size_, desc);
}
{
rcl_interfaces::msg::ParameterDescriptor desc;
desc.name = "use_indices";
desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
desc.description = "Only process a subset of the point cloud from an indices topic";
desc.read_only = true;
use_indices_ = declare_parameter(desc.name, use_indices_, desc);
}
{
rcl_interfaces::msg::ParameterDescriptor desc;
desc.name = "transient_local_indices";
desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
desc.description = "Does indices topic use transient local documentation";
desc.read_only = true;
transient_local_indices_ = declare_parameter(desc.name, transient_local_indices_, desc);
}
{
rcl_interfaces::msg::ParameterDescriptor desc;
desc.name = "approximate_sync";
desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
desc.description =
"Match indices and point cloud messages if time stamps are approximatly the same.";
desc.read_only = true;
approximate_sync_ = declare_parameter(desc.name, approximate_sync_, desc);
}
RCLCPP_DEBUG(
this->get_logger(), "PCL Node successfully created with the following parameters:\n"
" - approximate_sync : %s\n"
" - use_indices : %s\n"
" - transient_local_indices_ : %s\n"
" - max_queue_size : %d",
(approximate_sync_) ? "true" : "false",
(use_indices_) ? "true" : "false",
(transient_local_indices_) ? "true" : "false",
max_queue_size_);
}
protected:
/** \brief Set to true if point indices are used.
*
* When receiving a point cloud, if use_indices_ is false, the entire
* point cloud is processed for the given operation. If use_indices_ is
* true, then the ~indices topic is read to get the vector of point
* indices specifying the subset of the point cloud that will be used for
* the operation. In the case where use_indices_ is true, the ~input and
* ~indices topics must be synchronised in time, either exact or within a
* specified jitter. See also @ref transient_local_indices_ and approximate_sync.
**/
bool use_indices_;
/** \brief Set to true if the indices topic has transient_local durability.
*
* If use_indices_ is true, the ~input and ~indices topics generally must
* be synchronised in time. By setting this flag to true, the most recent
* value from ~indices can be used instead of requiring a synchronised
* message.
**/
bool transient_local_indices_;
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud2> sub_input_filter_;
/** \brief The message filter subscriber for PointIndices. */
message_filters::Subscriber<PointIndices> sub_indices_filter_;
/** \brief The output PointCloud publisher. Allow each individual class that inherits from this
* to declare the Publisher with their type.
*/
std::shared_ptr<PublisherT> pub_output_;
/** \brief The maximum queue size (default: 3). */
int max_queue_size_;
/** \brief True if we use an approximate time synchronizer
* versus an exact one (false by default).
**/
bool approximate_sync_;
/** \brief TF listener object. */
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
* \param cloud the point cloud to test
* \param topic_name an optional topic name (only used for printing, defaults to "input")
*/
inline bool
isValid(const PointCloud2::ConstSharedPtr & cloud, const std::string & topic_name = "input")
{
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
RCLCPP_WARN(
this->get_logger(),
"Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) "
"with stamp %d.%09d, and frame %s on topic %s received!",
cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
cloud->header.stamp.sec, cloud->header.stamp.nanosec,
cloud->header.frame_id.c_str(), topic_name.c_str());
return false;
}
return true;
}
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
* \param cloud the point cloud to test
* \param topic_name an optional topic name (only used for printing, defaults to "input")
*/
inline bool
isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input")
{
if (cloud->width * cloud->height != cloud->points.size()) {
RCLCPP_WARN(
this->get_logger(),
"Invalid PointCloud (points = %zu, width = %d, height = %d) "
"with stamp %d.%09d, and frame %s on topic %s received!",
cloud->points.size(), cloud->width, cloud->height,
fromPCL(cloud->header).stamp.sec, fromPCL(cloud->header).stamp.nanosec,
cloud->header.frame_id.c_str(), topic_name.c_str());
return false;
}
return true;
}
/** \brief Test whether a given PointIndices message is "valid" (i.e., has values).
* \param indices the point indices message to test
* \param topic_name an optional topic name (only used for printing, defaults to "indices")
*/
inline bool
isValid(
const PointIndicesConstPtr & /*indices*/,
const std::string & /*topic_name*/ = "indices")
{
/*if (indices->indices.empty ())
{
RCLCPP_WARN(
this->get_logger(), "Empty indices (values = %zu) with stamp %d.%09d, "
"and frame %s on topic %s received!",
indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec,
indices->header.frame_id.c_str(), topic_name.c_str());
//return (true); // looks like it should be false
return false;
}*/
return true;
}
/** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values).
* \param model the model coefficients to test
* \param topic_name an optional topic name (only used for printing, defaults to "model")
*/
inline bool
isValid(
const ModelCoefficientsConstPtr & /*model*/,
const std::string & /*topic_name*/ = "model")
{
/*if (model->values.empty ())
{
RCLCPP_WARN(
this->get_logger(), "Empty model (values = %zu) with stamp %d.%09d, "
"and frame %s on topic %s received!",
model->values.size(), model->header.stamp.sec, model->header.stamp.nanosec,
model->header.frame_id.c_str(), topic_name.c_str());
return (false);
}*/
return true;
}
/** \brief Lazy transport subscribe/unsubscribe routine.
* It is optional for backward compatibility.
**/
virtual void subscribe() {}
virtual void unsubscribe() {}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__PCL_NODE_HPP_

467
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/point_cloud.hpp Normal file
View File

@ -0,0 +1,467 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef PCL_ROS__POINT_CLOUD_HPP__
#define PCL_ROS__POINT_CLOUD_HPP__
// test if testing machinery can be implemented
#if defined(__cpp_rvalue_references) && defined(__cpp_constexpr)
#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1
#else
#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0
#endif
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/pcl_config.h> // for PCL_VERSION_COMPARE
#if PCL_VERSION_COMPARE(>=, 1, 11, 0)
#include <pcl/type_traits.h>
#else
#include <pcl/point_traits.h>
#endif // PCL_VERSION_COMPARE(>=, 1, 11, 0)
#include <pcl/for_each_type.h>
#include <pcl/conversions.h>
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#if PCL_VERSION_COMPARE(>=, 1, 11, 0)
#include <pcl/memory.h>
#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
#include <pcl/make_shared.h>
#endif
#endif
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <string>
#include <utility>
#include <vector>
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#include <type_traits>
#include <memory>
#endif
#include <boost/foreach.hpp> // for BOOST_FOREACH
#include <boost/mpl/size.hpp>
#include <boost/ref.hpp>
namespace pcl
{
namespace detail
{
template<typename Stream, typename PointT>
struct FieldStreamer
{
explicit FieldStreamer(Stream & stream)
: stream_(stream) {}
template<typename U>
void operator()()
{
const char * name = pcl::traits::name<PointT, U>::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<PointT, U>::value;
stream_.next(offset);
std::uint8_t datatype = pcl::traits::datatype<PointT, U>::value;
stream_.next(datatype);
std::uint32_t count = pcl::traits::datatype<PointT, U>::size;
stream_.next(count);
}
Stream & stream_;
};
template<typename PointT>
struct FieldsLength
{
FieldsLength()
: length(0) {}
template<typename U>
void operator()()
{
std::uint32_t name_length = strlen(pcl::traits::name<PointT, U>::value);
length += name_length + 13;
}
std::uint32_t length;
};
} // namespace detail
} // namespace pcl
namespace ros
{
// In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects
// on the subscriber side. This allows us to generate the mapping between message
// data and object fields only once and reuse it.
#if ROS_VERSION_MINIMUM(1, 3, 1)
template<typename T>
struct DefaultMessageCreator<pcl::PointCloud<T>>
{
boost::shared_ptr<pcl::MsgFieldMap> mapping_;
DefaultMessageCreator()
: mapping_(boost::make_shared<pcl::MsgFieldMap>() )
{
}
boost::shared_ptr<pcl::PointCloud<T>> operator()()
{
boost::shared_ptr<pcl::PointCloud<T>> msg(new pcl::PointCloud<T>());
pcl::detail::getMapping(*msg) = mapping_;
return msg;
}
};
#endif
namespace message_traits
{
template<typename T>
struct MD5Sum<pcl::PointCloud<T>>
{
static const char * value() {return MD5Sum<sensor_msgs::PointCloud2>::value();}
static const char * value(const pcl::PointCloud<T> &) {return value();}
static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::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<typename T>
struct DataType<pcl::PointCloud<T>>
{
static const char * value() {return DataType<sensor_msgs::PointCloud2>::value();}
static const char * value(const pcl::PointCloud<T> &) {return value();}
};
template<typename T>
struct Definition<pcl::PointCloud<T>>
{
static const char * value() {return Definition<sensor_msgs::PointCloud2>::value();}
static const char * value(const pcl::PointCloud<T> &) {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<typename T>
struct HasHeader<pcl::PointCloud<T>>: FalseType {};
template<typename T>
struct TimeStamp<pcl::PointCloud<T>>
{
// 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<T> & 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<T> & 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<T> & m)
{
return pcl_conversions::fromPCL(m.header).stamp;
}
private:
static boost::shared_ptr<std_msgs::Header> header_;
static boost::shared_ptr<std_msgs::Header> header_const_;
};
template<typename T>
struct FrameId<pcl::PointCloud<T>>
{
static std::string * pointer(pcl::PointCloud<T> & m) {return &m.header.frame_id;}
static std::string const * pointer(const pcl::PointCloud<T> & m) {return &m.header.frame_id;}
static std::string value(const pcl::PointCloud<T> & m) {return m.header.frame_id;}
};
} // namespace message_traits
namespace serialization
{
template<typename T>
struct Serializer<pcl::PointCloud<T>>
{
template<typename Stream>
inline static void write(Stream & stream, const pcl::PointCloud<T> & 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<T>::type FieldList;
uint32_t fields_size = boost::mpl::size<FieldList>::value;
stream.next(fields_size);
pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(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<typename Stream>
inline static void read(Stream & stream, pcl::PointCloud<T> & 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<sensor_msgs::PointField> fields;
stream.next(fields);
// Construct field mapping if deserializing for the first time
boost::shared_ptr<pcl::MsgFieldMap> & mapping_ptr = pcl::detail::getMapping(m);
if (!mapping_ptr) {
// This normally should get allocated by DefaultMessageCreator, but just in case
mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
}
pcl::MsgFieldMap & mapping = *mapping_ptr;
if (mapping.empty()) {
pcl::createMapping<T>(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<uint8_t *>(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<T> & m)
{
uint32_t length = 0;
length += serializationLength(m.header);
length += 8; // height/width
pcl::detail::FieldsLength<T> fl;
typedef typename pcl::traits::fieldList<T>::type FieldList;
pcl::for_each_type<FieldList>(boost::ref(fl));
length += 4; // size of 'fields'
length += fl.length;
length += 1; // is_bigendian
length += 4; // point_step
length += 4; // row_step
length += 4; // size of 'data'
length += m.points.size() * sizeof(T); // data
length += 1; // is_dense
return length;
}
};
} // namespace serialization
/// @todo Printer specialization in message_operations
} // namespace ros
namespace pcl
{
namespace detail
{
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
template<class T>
constexpr static bool pcl_uses_boost = std::is_same<boost::shared_ptr<T>,
pcl::shared_ptr<T>>::value;
#else
template<class T>
constexpr static bool pcl_uses_boost = true;
#endif
template<class SharedPointer>
struct Holder
{
SharedPointer p;
explicit Holder(const SharedPointer & p)
: p(p) {}
Holder(const Holder & other)
: p(other.p) {}
Holder(Holder && other)
: p(std::move(other.p)) {}
void operator()(...) {p.reset();}
};
template<class T>
inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> & p)
{
typedef Holder<std::shared_ptr<T>> H;
if (H * h = boost::get_deleter<H>(p)) {
return h->p;
} else {
return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
}
}
template<class T>
inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> & p)
{
typedef Holder<boost::shared_ptr<T>> H;
if (H * h = std::get_deleter<H>(p)) {
return h->p;
} else {
return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
}
}
#endif
} // namespace detail
// add functions to convert to smart pointer used by ROS
template<class T>
inline boost::shared_ptr<T> ros_ptr(const boost::shared_ptr<T> & p)
{
return p;
}
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
template<class T>
inline boost::shared_ptr<T> ros_ptr(const std::shared_ptr<T> & p)
{
return detail::to_boost_ptr(p);
}
// add functions to convert to smart pointer used by PCL, based on PCL's own pointer
template<class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
inline std::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> & p)
{
return p;
}
template<class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
inline std::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
{
return detail::to_std_ptr(p);
}
template<class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
inline boost::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> & p)
{
return detail::to_boost_ptr(p);
}
template<class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
{
return p;
}
#else
template<class T>
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
{
return p;
}
#endif
} // namespace pcl
#endif // PCL_ROS__POINT_CLOUD_HPP__

151
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/publisher.hpp Normal file
View File

@ -0,0 +1,151 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: publisher.h 33238 2010-03-11 00:46:58Z rusu $
*
*/
/**
\author Patrick Mihelich
@b Publisher represents a ROS publisher for the templated PointCloud implementation.
**/
#ifndef PCL_ROS__PUBLISHER_HPP_
#define PCL_ROS__PUBLISHER_HPP_
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>
namespace pcl_ros
{
class BasePublisher
{
public:
void
advertise(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
{
pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
}
std::string
getTopic()
{
return pub_.getTopic();
}
uint32_t
getNumSubscribers() const
{
return pub_.getNumSubscribers();
}
void
shutdown()
{
pub_.shutdown();
}
operator void *() const
{
return (pub_) ? reinterpret_cast<void *>(1) : reinterpret_cast<void *>(0);
}
protected:
ros::Publisher pub_;
};
template<typename PointT>
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<const pcl::PointCloud<PointT>> & point_cloud) const
{
publish(*point_cloud);
}
inline void
publish(const pcl::PointCloud<PointT> & point_cloud) const
{
// Fill point cloud binary data
sensor_msgs::PointCloud2::Ptr msg_ptr(new sensor_msgs::PointCloud2);
pcl::toROSMsg(point_cloud, *msg_ptr);
pub_.publish(msg_ptr);
}
};
template<>
class Publisher<sensor_msgs::PointCloud2>: 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<const sensor_msgs::PointCloud2>(point_cloud));
// pub_.publish (point_cloud);
}
};
} // namespace pcl_ros
#endif // PCL_ROS__PUBLISHER_HPP_

113
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp Normal file
View File

@ -0,0 +1,113 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: extract_clusters.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_
#define PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_
#include <dynamic_reconfigure/server.h>
#include <pcl/segmentation/extract_clusters.h>
#include <limits>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/EuclideanClusterExtractionConfig.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
* \author Radu Bogdan Rusu
*/
class EuclideanClusterExtraction : public PCLNodelet
{
public:
/** \brief Empty constructor. */
EuclideanClusterExtraction()
: publish_indices_(false), max_clusters_(std::numeric_limits<int>::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<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>> 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<pcl::PointXYZ> impl_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>> sync_input_indices_a_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_

130
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp Normal file
View File

@ -0,0 +1,130 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: extract_polygonal_prism_data.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_
#define PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/pass_through.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp"
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with
* a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying
* inside it.
*
* An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane).
* \author Radu Bogdan Rusu
*/
class ExtractPolygonalPrismData : public PCLNodelet
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
protected:
/** \brief The output PointIndices publisher. */
ros::Publisher pub_output_;
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_hull_filter_;
/** \brief Synchronized input, planar hull, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud,
PointIndices>>> sync_input_hull_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud, PointIndices>>> sync_input_hull_indices_a_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>> srv_;
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointIndices> 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<PointIndices>(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<pcl::PointXYZ> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_

301
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp Normal file
View File

@ -0,0 +1,301 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: sac_segmentation.h 35564 2011-01-27 07:32:12Z rusu $
*
*/
#ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
#define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
#include <message_filters/pass_through.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <dynamic_reconfigure/server.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/SACSegmentationConfig.hpp"
#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus
* methods and models, in the sense that it just creates a Nodelet wrapper for generic-purpose
* SAC-based segmentation.
* \author Radu Bogdan Rusu
*/
class SACSegmentation : public PCLNodelet
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> 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<dynamic_reconfigure::Server<SACSegmentationConfig>> 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<pcl_msgs::PointIndices> 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<pcl::PointXYZ> impl_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>> 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<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
typedef boost::shared_ptr<const PointCloudN> 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<PointCloudN> sub_normals_filter_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_axis_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>> 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<PointIndices>(indices));
}
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointIndices> 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<pcl::PointXYZ, pcl::Normal> impl_;
/** \brief Synchronized input, normals, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloudN, PointIndices>>> sync_input_normals_indices_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN,
PointIndices>>> sync_input_normals_indices_e_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_

111
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp Normal file
View File

@ -0,0 +1,111 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: segment_differences.h 35361 2011-01-20 04:34:49Z rusu $
*
*/
#ifndef PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_
#define PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_
#include <pcl/segmentation/segment_differences.h>
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/SegmentDifferencesConfig.hpp"
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
* difference between them for a maximum given distance threshold.
* \author Radu Bogdan Rusu
*/
class SegmentDifferences : public PCLNodelet
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
public:
/** \brief Empty constructor. */
SegmentDifferences() {}
protected:
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_target_filter_;
/** \brief Synchronized input, and planar hull.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloud>>> sync_input_target_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud>>> sync_input_target_a_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig>> 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<pcl::PointXYZ> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_

95
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/convex_hull.hpp Normal file
View File

@ -0,0 +1,95 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: convex_hull.h 36116 2011-02-22 00:05:23Z rusu $
*
*/
#ifndef PCL_ROS__SURFACE__CONVEX_HULL_HPP_
#define PCL_ROS__SURFACE__CONVEX_HULL_HPP_
#include <pcl/surface/convex_hull.h>
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
* \author Radu Bogdan Rusu
*/
class ConvexHull2D : public PCLNodelet
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> 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<pcl::PointXYZ> 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<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>> sync_input_indices_a_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__SURFACE__CONVEX_HULL_HPP_

152
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp Normal file
View File

@ -0,0 +1,152 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: moving_least_squares.h 36097 2011-02-20 14:18:58Z marton $
*
*/
#ifndef PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_
#define PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_
#include <pcl/surface/mls.h>
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/MLSConfig.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
* The type of the output is the same as the input, it only smooths the XYZ coordinates according
* to the parameters.
* 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<PointIn> PointCloudIn;
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
typedef pcl::PointCloud<NormalOut> NormalCloudOut;
typedef pcl::KdTree<PointIn> KdTree;
typedef pcl::KdTree<PointIn>::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<PointCloudIn> 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<dynamic_reconfigure::Server<MLSConfig>> 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<PointIn, NormalOut> 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<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointIndices>>> sync_input_indices_a_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
#endif // PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_

233
ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/transforms.hpp Normal file
View File

@ -0,0 +1,233 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
#ifndef PCL_ROS__TRANSFORMS_HPP_
#define PCL_ROS__TRANSFORMS_HPP_
#include <pcl/point_cloud.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Core>
#include <string>
#include <rclcpp/time.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
namespace pcl_ros
{
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param transform a rigid transformation from tf
* \note calls the Eigen version
*/
template<typename PointT>
void
transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf2::Transform & transform);
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param transform a rigid transformation from tf
* \note calls the Eigen version
*/
template<typename PointT>
void
transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const geometry_msgs::msg::TransformStamped & transform);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
* \param target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param tf_buffer a TF buffer object
*/
template<typename PointT>
bool
transformPointCloudWithNormals(
const std::string & target_frame,
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf2_ros::Buffer & tf_buffer);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
* \param target_frame the target TF frame the point cloud should be transformed to
* \param target_time the target timestamp
* \param cloud_in the input point cloud
* \param fixed_frame fixed TF frame
* \param cloud_out the input point cloud
* \param tf_buffer a TF buffer object
*/
template<typename PointT>
bool
transformPointCloudWithNormals(
const std::string & target_frame,
const rclcpp::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf2_ros::Buffer & tf_buffer);
/** \brief Apply a rigid transform defined by a 3D offset and a quaternion
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param transform a rigid transformation from tf
* \note calls the Eigen version
*/
template<typename PointT>
void
transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf2::Transform & transform);
/** \brief Apply a rigid transform defined by a 3D offset and a quaternion
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param transform a rigid transformation from tf
* \note calls the Eigen version
*/
template<typename PointT>
void
transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const geometry_msgs::msg::TransformStamped & transform);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
* \param target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param tf_buffer a TF buffer object
*/
template<typename PointT>
bool
transformPointCloud(
const std::string & target_frame,
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf2_ros::Buffer & tf_buffer);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
* \param target_frame the target TF frame the point cloud should be transformed to
* \param target_time the target timestamp
* \param cloud_in the input point cloud
* \param fixed_frame fixed TF frame
* \param cloud_out the input point cloud
* \param tf_buffer a TF buffer object
*/
template<typename PointT>
bool
transformPointCloud(
const std::string & target_frame,
const rclcpp::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf2_ros::Buffer & tf_buffer);
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
* \param target_frame the target TF frame
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
* \param tf_buffer a TF buffer object
*/
bool
transformPointCloud(
const std::string & target_frame,
const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::msg::PointCloud2 & out,
const tf2_ros::Buffer & tf_buffer);
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
* \param target_frame the target TF frame
* \param net_transform the TF transformer object
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
*/
void
transformPointCloud(
const std::string & target_frame,
const tf2::Transform & net_transform,
const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::msg::PointCloud2 & out);
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
* \param target_frame the target TF frame
* \param net_transform the TF transformer object
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
*/
void
transformPointCloud(
const std::string & target_frame,
const geometry_msgs::msg::TransformStamped & net_transform,
const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::msg::PointCloud2 & out);
/** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix.
* \param transform the transformation to use on the points
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
*/
void
transformPointCloud(
const Eigen::Matrix4f & transform,
const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::msg::PointCloud2 & out);
/** \brief Obtain the transformation matrix from TF into an Eigen form
* \param bt the TF transformation
* \param out_mat the Eigen transformation
*/
void
transformAsMatrix(const tf2::Transform & bt, Eigen::Matrix4f & out_mat);
/** \brief Obtain the transformation matrix from TF into an Eigen form
* \param bt the TF transformation
* \param out_mat the Eigen transformation
*/
void
transformAsMatrix(const geometry_msgs::msg::TransformStamped & bt, Eigen::Matrix4f & out_mat);
} // namespace pcl_ros
#endif // PCL_ROS__TRANSFORMS_HPP_

73
ros2_ws/src/perception_pcl/pcl_ros/package.xml Normal file
View File

@ -0,0 +1,73 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pcl_ros</name>
<version>2.4.1</version>
<description>
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.
</description>
<maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<maintainer email="www.kentaro.wada@gmail.com">Kentaro Wada</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/perception_pcl</url>
<url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
<url type="repository">https://github.com/ros-perception/perception_pcl</url>
<author>Open Perception</author>
<author email="julius@kammerl.de">Julius Kammerl</author>
<author email="william@osrfoundation.org">William Woodall</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>libpcl-all-dev</build_depend>
<depend>eigen</depend>
<depend>pcl_conversions</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<exec_depend>libpcl-common</exec_depend>
<exec_depend>libpcl-features</exec_depend>
<exec_depend>libpcl-filters</exec_depend>
<exec_depend>libpcl-io</exec_depend>
<exec_depend>libpcl-segmentation</exec_depend>
<exec_depend>libpcl-surface</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<test_depend>launch</test_depend>
<test_depend>launch_ros</test_depend>
<test_depend>launch_testing</test_depend>
<test_depend>launch_testing_ros</test_depend>
<test_depend>sensor_msgs</test_depend>
<!--
<export>
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_features.xml"/>
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_filters.xml"/>
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_io.xml"/>
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_segmentation.xml"/>
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_surface.xml"/>
</export>
-->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

85
ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_features.xml Normal file
View File

@ -0,0 +1,85 @@
<!-- PCL Features library component -->
<library path="lib/libpcl_ros_features">
<class name="pcl/BoundaryEstimation" type="BoundaryEstimation" base_class_type="nodelet::Nodelet">
<description>
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.
</description>
</class>
<class name="pcl/FPFHEstimation" type="FPFHEstimation" base_class_type="nodelet::Nodelet">
<description>
FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset
containing points and normals.
</description>
</class>
<class name="pcl/FPFHEstimationOMP" type="FPFHEstimationOMP" base_class_type="nodelet::Nodelet">
<description>
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.
</description>
</class>
<class name="pcl/SHOTEstimation" type="SHOTEstimation" base_class_type="nodelet::Nodelet">
<description>
SHOTEstimation estimates SHOT descriptor for a given point cloud dataset
containing points and normals.
</description>
</class>
<class name="pcl/SHOTEstimationOMP" type="SHOTEstimationOMP" base_class_type="nodelet::Nodelet">
<description>
SHOTEstimationOMP estimates SHOT descriptor for a given point cloud dataset
containing points and normals, in parallel, using the OpenMP standard.
</description>
</class>
<class name="pcl/MomentInvariantsEstimation" type="MomentInvariantsEstimation" base_class_type="nodelet::Nodelet">
<description>
MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
</description>
</class>
<class name="pcl/NormalEstimationOMP" type="NormalEstimationOMP" base_class_type="nodelet::Nodelet">
<description>
NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and curvatures,
in parallel, using the OpenMP standard.
</description>
</class>
<class name="pcl/NormalEstimationTBB" type="NormalEstimationTBB" base_class_type="nodelet::Nodelet">
<description>
NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and curvatures, in
parallel, using Intel's Threading Building Blocks library.
</description>
</class>
<class name="pcl/NormalEstimation" type="NormalEstimation" base_class_type="nodelet::Nodelet">
<description>
NormalEstimation estimates local surface properties at each 3D point, such as surface normals and curvatures.
</description>
</class>
<class name="pcl/PFHEstimation" type="PFHEstimation" base_class_type="nodelet::Nodelet">
<description>
PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset containing
points and normals.
</description>
</class>
<class name="pcl/PrincipalCurvaturesEstimation" type="PrincipalCurvaturesEstimation" base_class_type="nodelet::Nodelet">
<description>
PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of principal surface
curvatures for a given point cloud dataset containing points and normals.
</description>
</class>
<class name="pcl/VFHEstimation" type="VFHEstimation" base_class_type="nodelet::Nodelet">
<description>
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) global descriptor for a given point cloud cluster dataset
containing points and normals.
</description>
</class>
</library>

43
ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_filters.xml Normal file
View File

@ -0,0 +1,43 @@
<!-- PCL Filters library component -->
<library path="lib/libpcl_ros_filters">
<class name="pcl/PassThrough" type="PassThrough" base_class_type="nodelet::Nodelet">
<description>
PassThrough is a filter that uses the basic Filter class mechanisms for passing data around.
</description>
</class>
<class name="pcl/VoxelGrid" type="VoxelGrid" base_class_type="nodelet::Nodelet">
<description>
VoxelGrid assembles a local 3D grid over a given PointCloud, and uses that to downsample the data.
</description>
</class>
<class name="pcl/ProjectInliers" type="ProjectInliers" base_class_type="nodelet::Nodelet">
<description>
ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a separate PointCloud.
</description>
</class>
<class name="pcl/ExtractIndices" type="ExtractIndices" base_class_type="nodelet::Nodelet">
<description>
ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
</description>
</class>
<class name="pcl/StatisticalOutlierRemoval" type="StatisticalOutlierRemoval" base_class_type="nodelet::Nodelet">
<description>
StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data.
</description>
</class>
<class name="pcl/RadiusOutlierRemoval" type="RadiusOutlierRemoval" base_class_type="nodelet::Nodelet">
<description>
RadiusOutlierRemoval uses point neighborhood statistics to filter outlier data.
</description>
</class>
<class name="pcl/CropBox" type="CropBox" base_class_type="nodelet::Nodelet">
<description>
CropBox is a filter that allows the user to filter all the data inside of a given box.
</description>
</class>
</library>

49
ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_io.xml Normal file
View File

@ -0,0 +1,49 @@
<!-- PCL IO library component -->
<library path="lib/libpcl_ros_io">
<class name="pcl/NodeletMUX" type="NodeletMUX" base_class_type="nodelet::Nodelet">
<description>
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.
</description>
</class>
<class name="pcl/NodeletDEMUX" type="NodeletDEMUX" base_class_type="nodelet::Nodelet">
<description>
NodeletDEMUX represent a demux nodelet for PointCloud topics: it
publishes 1 input topic to N output topics.
</description>
</class>
<class name="pcl/PCDReader" type="PCDReader" base_class_type="nodelet::Nodelet">
<description>
PCDReader reads in a PCD (Point Cloud Data) v.5 file from disk and converts it to a PointCloud message.
</description>
</class>
<class name="pcl/BAGReader" type="BAGReader" base_class_type="nodelet::Nodelet">
<description>
BAGReader reads in sensor_msgs/PointCloud2 messages from BAG files.
</description>
</class>
<class name="pcl/PCDWriter" type="PCDWriter" base_class_type="nodelet::Nodelet">
<description>
PCDWriter writes a PointCloud message to disk in a PCD (Point Cloud Data) v.5 file format.
</description>
</class>
<class name="pcl/PointCloudConcatenateFieldsSynchronizer" type="PointCloudConcatenateFieldsSynchronizer" base_class_type="nodelet::Nodelet">
<description>
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.
</description>
</class>
<class name="pcl/PointCloudConcatenateDataSynchronizer" type="PointCloudConcatenateDataSynchronizer" base_class_type="nodelet::Nodelet">
<description>
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.
</description>
</class>
</library>

37
ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml Normal file
View File

@ -0,0 +1,37 @@
<!-- PCL Segmentation library component -->
<library path="lib/libpcl_ros_segmentation">
<class name="pcl/ExtractPolygonalPrismData" type="ExtractPolygonalPrismData" base_class_type="nodelet::Nodelet">
<description>
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.
</description>
</class>
<class name="pcl/EuclideanClusterExtraction" type="EuclideanClusterExtraction" base_class_type="nodelet::Nodelet">
<description>
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
</description>
</class>
<class name="pcl/SACSegmentationFromNormals" type="SACSegmentationFromNormals" base_class_type="nodelet::Nodelet">
<description>
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.
</description>
</class>
<class name="pcl/SACSegmentation" type="SACSegmentation" base_class_type="nodelet::Nodelet">
<description>
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.
</description>
</class>
<class name="pcl/SegmentDifferences" type="SegmentDifferences" base_class_type="nodelet::Nodelet">
<description>
SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
difference between them for a maximum given distance threshold.
</description>
</class>
</library>

13
ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_surface.xml Normal file
View File

@ -0,0 +1,13 @@
<!-- PCL Surface reconstruction library component -->
<library path="lib/libpcl_ros_surface">
<class name="pcl/MovingLeastSquares" type="MovingLeastSquares" base_class_type="nodelet::Nodelet">
<description>
MovingLeastSquares is an implementation of the MLS algorithm for data reconstruction through bivariate polynomial fitting.
</description>
</class>
<class name="pcl/ConvexHull2D" type="ConvexHull2D" base_class_type="nodelet::Nodelet">
<description>
ConvexHull2D represents a 2D ConvexHull implementation.
</description>
</class>
</library>

2
ros2_ws/src/perception_pcl/pcl_ros/samples/data/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
*
!.gitignore

44
ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch Normal file
View File

@ -0,0 +1,44 @@
<launch>
<arg name="gui" default="true" />
<!-- use voxel_grid for small cpu load in filtering -->
<include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
<arg name="gui" value="false" />
<arg name="test" value="false" />
<arg name="leaf_size" value="0.02" />
</include>
<node name="normal_estimation"
pkg="nodelet" type="nodelet"
args="standalone pcl/NormalEstimation">
<remap from="~input" to="voxel_grid/output" />
<rosparam>
radius_search: 0
k_search: 10
# 0, => ANN, 1 => FLANN, 2 => Organized
spatial_locator: 1
</rosparam>
</node>
<test test-name="test_normal_estimation"
name="test_normal_estimation"
pkg="rostest" type="hztest">
<rosparam>
topic: /normal_estimation/output
hz: 10
hzerror: 8
test_duration: 5.0
</rosparam>
</test>
<!-- TODO(wkentaro): Add sample visualization
<group if="$(arg gui)">
<node name="rviz"
pkg="rviz" type="rviz"
args="-d $(find pcl_ros)/samples/pcl_ros/filters/config/normal_estimation.rviz">
</node>
</group>
-->
</launch>

169
ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz Normal file
View File

@ -0,0 +1,169 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Filtered1
Splitter Ratio: 0.5
Tree Height: 565
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Raw
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: false
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed 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: <Fixed 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

169
ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/config/voxel_grid.rviz Normal file
View File

@ -0,0 +1,169 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Filtered1
Splitter Ratio: 0.5
Tree Height: 565
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Raw
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: false
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed 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: <Fixed 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

40
ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_statistical_outlier_removal.launch Normal file
View File

@ -0,0 +1,40 @@
<launch>
<arg name="gui" default="true" />
<!-- use voxel_grid for small cpu load in filtering -->
<include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
<arg name="gui" value="false" />
<arg name="test" value="false" />
<arg name="leaf_size" value="0.02" />
</include>
<node name="statistical_outlier_removal"
pkg="nodelet" type="nodelet"
args="standalone pcl/StatisticalOutlierRemoval">
<remap from="~input" to="voxel_grid/output" />
<rosparam>
mean_k: 10
stddev: 1.0
</rosparam>
</node>
<test test-name="test_statistical_outlier_removal"
name="test_statistical_outlier_removal"
pkg="rostest" type="hztest">
<rosparam>
topic: /statistical_outlier_removal/output
hz: 20
hzerror: 15
test_duration: 5.0
</rosparam>
</test>
<group if="$(arg gui)">
<node name="rviz"
pkg="rviz" type="rviz"
args="-d $(find pcl_ros)/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz">
</node>
</group>
</launch>

44
ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch Normal file
View File

@ -0,0 +1,44 @@
<launch>
<arg name="gui" default="true" />
<arg name="test" default="true" />
<arg name="leaf_size" default="0.05" />
<node name="pcd_to_pointcloud"
pkg="pcl_ros" type="pcd_to_pointcloud"
args="$(find pcl_ros)/samples/data/table_scene_lms400.pcd 0.033">
<remap from="cloud_pcd" to="points" />
</node>
<node name="voxel_grid"
pkg="nodelet" type="nodelet"
args="standalone pcl/VoxelGrid">
<remap from="~input" to="points" />
<rosparam subst_value="true">
filter_field_name: ''
leaf_size: $(arg leaf_size)
</rosparam>
</node>
<group if="$(arg test)">
<test test-name="test_voxel_grid"
name="test_voxel_grid"
pkg="rostest" type="hztest">
<rosparam>
topic: /voxel_grid/output
hz: 20
hzerror: 15
test_duration: 5.0
</rosparam>
</test>
</group>
<group if="$(arg gui)">
<node name="rviz"
pkg="rviz" type="rviz"
args="-d $(find pcl_ros)/samples/pcl_ros/filters/config/voxel_grid.rviz">
</node>
</group>
</launch>

43
ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch Normal file
View File

@ -0,0 +1,43 @@
<launch>
<arg name="gui" default="true" />
<!-- use voxel_grid for small cpu load in filtering -->
<include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
<arg name="gui" value="false" />
<arg name="test" value="false" />
<arg name="leaf_size" value="0.02" />
</include>
<node name="concatenate_data"
pkg="nodelet" type="nodelet"
args="standalone pcl/PointCloudConcatenateDataSynchronizer">
<rosparam>
output_frame: /base_link
input_topics:
- /voxel_grid/output
- /voxel_grid/output
</rosparam>
</node>
<test test-name="test_concatenate_data"
name="test_concatenate_data"
pkg="rostest" type="hztest">
<rosparam>
topic: /concatenate_data/output
hz: 10
hzerror: 8
test_duration: 5.0
</rosparam>
</test>
<!-- TODO(wkentaro): Add sample visualization
<group if="$(arg gui)">
<node name="rviz"
pkg="rviz" type="rviz"
args="-d $(find pcl_ros)/samples/pcl_ros/io/config/concatenate_data.rviz">
</node>
</group>
-->
</launch>

42
ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch Normal file
View File

@ -0,0 +1,42 @@
<launch>
<arg name="gui" default="true" />
<!-- use voxel_grid for small cpu load in filtering -->
<include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
<arg name="gui" value="false" />
<arg name="test" value="false" />
<arg name="leaf_size" value="0.02" />
</include>
<node name="extract_clusters"
pkg="nodelet" type="nodelet"
args="standalone pcl/EuclideanClusterExtraction">
<remap from="~input" to="voxel_grid/output" />
<rosparam>
cluster_tolerance: 0.03
spatial_locator: 1 # FLANN
</rosparam>
</node>
<test test-name="test_extract_clusters"
name="test_extract_clusters"
pkg="rostest" type="hztest">
<rosparam>
topic: /extract_clusters/output
hz: 3000
hzerror: 2400
test_duration: 5.0
</rosparam>
</test>
<!-- TODO(wkentaro): Add sample visualization
<group if="$(arg gui)">
<node name="rviz"
pkg="rviz" type="rviz"
args="-d $(find pcl_ros)/samples/pcl_ros/segmentation/config/extract_clusters.rviz">
</node>
</group>
-->
</launch>

40
ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch Normal file
View File

@ -0,0 +1,40 @@
<launch>
<arg name="gui" default="true" />
<!-- use voxel_grid for small cpu load in filtering -->
<include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
<arg name="gui" value="false" />
<arg name="test" value="false" />
<arg name="leaf_size" value="0.02" />
</include>
<node name="convex_hull"
pkg="nodelet" type="nodelet"
args="standalone pcl/ConvexHull2D">
<remap from="~input" to="voxel_grid/output" />
<rosparam>
</rosparam>
</node>
<test test-name="test_convex_hull"
name="test_convex_hull"
pkg="rostest" type="hztest">
<rosparam>
topic: /convex_hull/output
hz: 10
hzerror: 8
test_duration: 5.0
</rosparam>
</test>
<!-- TODO(wkentaro): Add sample visualization
<group if="$(arg gui)">
<node name="rviz"
pkg="rviz" type="rviz"
args="-d $(find pcl_ros)/samples/pcl_ros/surface/config/convex_hull.rviz">
</node>
</group>
-->
</launch>

0
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/__init__.py Normal file
View File

75
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/boundary.cpp Normal file
View File

@ -0,0 +1,75 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/boundary.hpp"
void
pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::BoundaryEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet)

641
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/feature.cpp Normal file
View File

@ -0,0 +1,641 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
*
*/
// #include <pluginlib/class_list_macros.h>
// 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/common/io.h>
#include <message_filters/null_types.h>
#include <vector>
#include "pcl_ros/features/feature.hpp"
////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Feature::onInit()
{
// Call the super onInit ()
PCLNodelet::onInit();
// Call the child init
childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("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<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
dynamic_reconfigure::Server<FeatureConfig>::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<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
} else {
sync_input_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices>>>(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<PointCloudIn>(
"input", max_queue_size_,
bind(
&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(),
PointIndicesConstPtr()));
}
}
////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Feature::unsubscribe()
{
if (use_indices_ || use_surface_) {
sub_input_filter_.unsubscribe();
if (use_indices_) {
sub_indices_filter_.unsubscribe();
if (use_surface_) {
sub_surface_filter_.unsubscribe();
}
} else {
sub_surface_filter_.unsubscribe();
}
} else {
sub_input_.shutdown();
}
}
////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
{
if (k_ != config.k_search) {
k_ = config.k_search;
NODELET_DEBUG(
"[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
}
if (search_radius_ != config.radius_search) {
search_radius_ = config.radius_search;
NODELET_DEBUG(
"[config_callback] Setting the nearest neighbors search radius for each point: %f.",
search_radius_);
}
}
////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Feature::input_surface_indices_callback(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers() <= 0) {
return;
}
// If cloud is given, check if it's valid
if (!isValid(cloud)) {
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
emptyPublish(cloud);
return;
}
// If surface is given, check if it's valid
if (cloud_surface && !isValid(cloud_surface, "surface")) {
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
emptyPublish(cloud);
return;
}
// If indices are given, check if they are valid
if (indices && !isValid(indices)) {
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
emptyPublish(cloud);
return;
}
/// DEBUG
if (cloud_surface) {
if (indices) {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, "
"and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, "
"and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(), fromPCL(
cloud_surface->header).stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(), fromPCL(
cloud_surface->header).stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
}
} else if (indices) {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame "
"%s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on "
"topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on "
"topic %s received.", cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
}
///
if (static_cast<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<int>(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<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("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<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
dynamic_reconfigure::Server<FeatureConfig>::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<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloudIn, PointCloudN,
PointCloudIn, PointIndices>>>(max_queue_size_);
} else {
sync_input_normals_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
}
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
if (use_indices_ || use_surface_) {
if (use_indices_) {
// If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
// and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput(
sub_input_filter_,
sub_normals_filter_,
sub_surface_filter_,
sub_indices_filter_);
} else {
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_,
sub_normals_filter_,
sub_surface_filter_,
sub_indices_filter_);
}
} else { // Use only indices
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) {
// surface not enabled, connect the input-indices duo and register
sync_input_normals_surface_indices_a_->connectInput(
sub_input_filter_,
sub_normals_filter_, nf_pc_,
sub_indices_filter_);
} else {
// surface not enabled, connect the input-indices duo and register
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_,
sub_normals_filter_, nf_pc_,
sub_indices_filter_);
}
}
} else { // Use only surface
// indices not enabled, connect the input-surface duo and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput(
sub_input_filter_, sub_normals_filter_,
sub_surface_filter_, nf_pi_);
} else {
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_, sub_normals_filter_,
sub_surface_filter_, nf_pi_);
}
}
} else {
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput(
sub_input_filter_, sub_normals_filter_,
nf_pc_, nf_pi_);
} else {
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_, sub_normals_filter_,
nf_pc_, nf_pi_);
}
}
// Register callbacks
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->registerCallback(
bind(
&FeatureFromNormals::
input_normals_surface_indices_callback, this, _1, _2, _3, _4));
} else {
sync_input_normals_surface_indices_e_->registerCallback(
bind(
&FeatureFromNormals::
input_normals_surface_indices_callback, this, _1, _2, _3, _4));
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::FeatureFromNormals::unsubscribe()
{
sub_input_filter_.unsubscribe();
sub_normals_filter_.unsubscribe();
if (use_indices_ || use_surface_) {
if (use_indices_) {
sub_indices_filter_.unsubscribe();
if (use_surface_) {
sub_surface_filter_.unsubscribe();
}
} else {
sub_surface_filter_.unsubscribe();
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals,
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers() <= 0) {
return;
}
// If cloud+normals is given, check if it's valid
if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals"))
NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
emptyPublish(cloud);
return;
}
// If surface is given, check if it's valid
if (cloud_surface && !isValid(cloud_surface, "surface")) {
NODELET_ERROR(
"[%s::input_normals_surface_indices_callback] Invalid input surface!",
getName().c_str());
emptyPublish(cloud);
return;
}
// If indices are given, check if they are valid
if (indices && !isValid(indices)) {
NODELET_ERROR(
"[%s::input_normals_surface_indices_callback] Invalid input indices!",
getName().c_str());
emptyPublish(cloud);
return;
}
/// DEBUG
if (cloud_surface) {
if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(), fromPCL(
cloud_surface->header).stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(), fromPCL(
cloud_normals->header).stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(), fromPCL(
cloud_surface->header).stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(), fromPCL(
cloud_normals->header).stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
}
} else if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(), fromPCL(
cloud_normals->header).stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(), fromPCL(
cloud_normals->header).stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
}
///
if (static_cast<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<int>(indices->indices));
}
computePublish(cloud, cloud_normals, cloud_surface, vindices);
}

76
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh.cpp Normal file
View File

@ -0,0 +1,76 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/fpfh.hpp"
void
pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::FPFHEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::FPFHEstimation FPFHEstimation;
PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet)

76
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp Normal file
View File

@ -0,0 +1,76 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/fpfh_omp.hpp"
void
pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::FPFHEstimationOMP::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet)

74
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/moment_invariants.cpp Normal file
View File

@ -0,0 +1,74 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/moment_invariants.hpp"
void
pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::MomentInvariantsEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet)

74
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d.cpp Normal file
View File

@ -0,0 +1,74 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/normal_3d.hpp"
void
pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::NormalEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::NormalEstimation NormalEstimation;
PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet)

74
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp Normal file
View File

@ -0,0 +1,74 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/normal_3d_omp.hpp"
void
pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::NormalEstimationOMP::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet)

81
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp Normal file
View File

@ -0,0 +1,81 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: normal_3d_tbb.cpp 35625 2011-01-31 07:56:13Z gbiggs $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/normal_3d_tbb.hpp"
#if defined HAVE_TBB
void
pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloud output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::NormalEstimationTBB::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator
initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod(tree_);
// Set the inputs
impl_.setInputCloud(cloud);
impl_.setIndices(indices);
impl_.setSearchSurface(surface);
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet)
#endif // HAVE_TBB

76
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/pfh.cpp Normal file
View File

@ -0,0 +1,76 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: pfh.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/pfh.hpp"
void
pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::PFHEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::PFHEstimation PFHEstimation;
PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet)

76
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp Normal file
View File

@ -0,0 +1,76 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: principal_curvatures.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/principal_curvatures.hpp"
void
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::PrincipalCurvaturesEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet)

75
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/shot.cpp Normal file
View File

@ -0,0 +1,75 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, Ryohei Ueda, JSK Lab
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of JSK Lab. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/shot.hpp"
void
pcl_ros::SHOTEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::SHOTEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::SHOTEstimation SHOTEstimation;
PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet)

75
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/shot_omp.cpp Normal file
View File

@ -0,0 +1,75 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, Ryohei Ueda, JSK Lab
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of JSK Lab. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/shot_omp.hpp"
void
pcl_ros::SHOTEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::SHOTEstimationOMP::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP;
PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet)

76
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/vfh.cpp Normal file
View File

@ -0,0 +1,76 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: vfh.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/vfh.hpp"
void
pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::VFHEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::VFHEstimation VFHEstimation;
PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet)

253
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/crop_box.cpp Normal file
View File

@ -0,0 +1,253 @@
/*
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: cropbox.cpp
*
*/
#include "pcl_ros/filters/crop_box.hpp"
pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options)
: Filter("CropBoxNode", options)
{
// This both declares and initializes the input and output frames
use_frame_params();
rcl_interfaces::msg::ParameterDescriptor min_x_desc;
min_x_desc.name = "min_x";
min_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
min_x_desc.description =
"Minimum x value below which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
min_x_desc.floating_point_range.push_back(float_range);
}
declare_parameter(min_x_desc.name, rclcpp::ParameterValue(-1.0), min_x_desc);
rcl_interfaces::msg::ParameterDescriptor max_x_desc;
max_x_desc.name = "max_x";
max_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
max_x_desc.description =
"Maximum x value above which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
max_x_desc.floating_point_range.push_back(float_range);
}
declare_parameter(max_x_desc.name, rclcpp::ParameterValue(1.0), max_x_desc);
rcl_interfaces::msg::ParameterDescriptor min_y_desc;
min_y_desc.name = "min_y";
min_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
min_y_desc.description =
"Minimum y value below which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
min_y_desc.floating_point_range.push_back(float_range);
}
declare_parameter(min_y_desc.name, rclcpp::ParameterValue(-1.0), min_y_desc);
rcl_interfaces::msg::ParameterDescriptor max_y_desc;
max_y_desc.name = "max_y";
max_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
max_y_desc.description =
"Maximum y value above which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
max_y_desc.floating_point_range.push_back(float_range);
}
declare_parameter(max_y_desc.name, rclcpp::ParameterValue(1.0), max_y_desc);
rcl_interfaces::msg::ParameterDescriptor min_z_desc;
min_z_desc.name = "min_z";
min_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
min_z_desc.description =
"Minimum z value below which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
min_z_desc.floating_point_range.push_back(float_range);
}
declare_parameter(min_z_desc.name, rclcpp::ParameterValue(-1.0), min_z_desc);
rcl_interfaces::msg::ParameterDescriptor max_z_desc;
max_z_desc.name = "max_z";
max_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
max_z_desc.description =
"Maximum z value above which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
max_z_desc.floating_point_range.push_back(float_range);
}
declare_parameter(max_z_desc.name, rclcpp::ParameterValue(1.0), max_z_desc);
rcl_interfaces::msg::ParameterDescriptor keep_organized_desc;
keep_organized_desc.name = "keep_organized";
keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
keep_organized_desc.description =
"Set whether the filtered points should be kept and set to NaN, "
"or removed from the PointCloud, thus potentially breaking its organized structure.";
declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc);
rcl_interfaces::msg::ParameterDescriptor negative_desc;
negative_desc.name = "negative";
negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
negative_desc.description =
"Set whether the inliers should be returned (true) or the outliers (false).";
declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc);
const std::vector<std::string> param_names {
min_x_desc.name,
max_x_desc.name,
min_y_desc.name,
max_y_desc.name,
min_z_desc.name,
max_z_desc.name,
keep_organized_desc.name,
negative_desc.name,
};
callback_handle_ =
add_on_set_parameters_callback(
std::bind(
&CropBox::config_callback, this,
std::placeholders::_1));
config_callback(get_parameters(param_names));
// TODO(daisukes): lazy subscription after rclcpp#2060
subscribe();
}
void
pcl_ros::CropBox::filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{
std::lock_guard<std::mutex> lock(mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
//////////////////////////////////////////////////////////////////////////////////////////////
rcl_interfaces::msg::SetParametersResult
pcl_ros::CropBox::config_callback(const std::vector<rclcpp::Parameter> & params)
{
std::lock_guard<std::mutex> lock(mutex_);
Eigen::Vector4f min_point, max_point;
min_point = impl_.getMin();
max_point = impl_.getMax();
for (const rclcpp::Parameter & param : params) {
if (param.get_name() == "min_x") {
min_point(0) = param.as_double();
}
if (param.get_name() == "max_x") {
max_point(0) = param.as_double();
}
if (param.get_name() == "min_y") {
min_point(1) = param.as_double();
}
if (param.get_name() == "max_y") {
max_point(1) = param.as_double();
}
if (param.get_name() == "min_z") {
min_point(2) = param.as_double();
}
if (param.get_name() == "max_z") {
max_point(2) = param.as_double();
}
if (param.get_name() == "negative") {
// Check the current value for the negative flag
if (impl_.getNegative() != param.as_bool()) {
RCLCPP_DEBUG(
get_logger(), "Setting the filter negative flag to: %s.",
param.as_bool() ? "true" : "false");
// Call the virtual method in the child
impl_.setNegative(param.as_bool());
}
}
if (param.get_name() == "keep_organized") {
// Check the current value for keep_organized
if (impl_.getKeepOrganized() != param.as_bool()) {
RCLCPP_DEBUG(
get_logger(), "Setting the filter keep_organized value to: %s.",
param.as_bool() ? "true" : "false");
// Call the virtual method in the child
impl_.setKeepOrganized(param.as_bool());
}
}
}
// Check the current values for minimum point
if (min_point != impl_.getMin()) {
RCLCPP_DEBUG(
get_logger(), "Setting the minimum point to: %f %f %f.",
min_point(0), min_point(1), min_point(2));
impl_.setMin(min_point);
}
// Check the current values for the maximum point
if (max_point != impl_.getMax()) {
RCLCPP_DEBUG(
get_logger(), "Setting the maximum point to: %f %f %f.",
max_point(0), max_point(1), max_point(2));
impl_.setMax(max_point);
}
// Range constraints are enforced by rclcpp::Parameter.
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
return result;
}
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::CropBox)

102
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/extract_indices.cpp Normal file
View File

@ -0,0 +1,102 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: extract_indices.cpp 35876 2011-02-09 01:04:36Z rusu $
*
*/
#include "pcl_ros/filters/extract_indices.hpp"
pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options)
: Filter("ExtractIndicesNode", options)
{
rcl_interfaces::msg::ParameterDescriptor neg_desc;
neg_desc.name = "negative";
neg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
neg_desc.description = "Extract indices or the negative (all-indices)";
declare_parameter(neg_desc.name, rclcpp::ParameterValue(false), neg_desc);
// Validate initial values using same callback
callback_handle_ =
add_on_set_parameters_callback(
std::bind(&ExtractIndices::config_callback, this, std::placeholders::_1));
std::vector<std::string> param_names{neg_desc.name};
auto result = config_callback(get_parameters(param_names));
if (!result.successful) {
throw std::runtime_error(result.reason);
}
// TODO(daisukes): lazy subscription after rclcpp#2060
subscribe();
}
void
pcl_ros::ExtractIndices::filter(
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{
std::lock_guard<std::mutex> lock(mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
//////////////////////////////////////////////////////////////////////////////////////////////
rcl_interfaces::msg::SetParametersResult
pcl_ros::ExtractIndices::config_callback(const std::vector<rclcpp::Parameter> & params)
{
std::lock_guard<std::mutex> lock(mutex_);
for (const rclcpp::Parameter & param : params) {
if (param.get_name() == "negative") {
// Check the current value for the negative flag
if (impl_.getNegative() != param.as_bool()) {
RCLCPP_DEBUG(
get_logger(), "Setting the filter negative flag to: %s.",
param.as_bool() ? "true" : "false");
// Call the virtual method in the child
impl_.setNegative(param.as_bool());
}
}
}
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
return result;
}
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ExtractIndices)

78
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/boundary.cpp Normal file
View File

@ -0,0 +1,78 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/boundary.hpp"
void
pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
void
pcl_ros::BoundaryEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator
initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod(tree_);
// Set the inputs
impl_.setInputCloud(cloud);
impl_.setIndices(indices);
impl_.setSearchSurface(surface);
impl_.setInputNormals(normals);
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet);

592
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/feature.cpp Normal file
View File

@ -0,0 +1,592 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
*
*/
// #include <pluginlib/class_list_macros.h>
// 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/common/io.h>
#include <message_filters/null_types.h>
#include <vector>
#include "pcl_ros/features/feature.hpp"
////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Feature::onInit()
{
// Call the super onInit ()
PCLNodelet::onInit();
// Call the child init
childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("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<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
&Feature::config_callback, this, _1, _2);
srv_->setCallback(f);
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
if (use_indices_ || use_surface_) {
// Create the objects here
if (approximate_sync_) {
sync_input_surface_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
} else {
sync_input_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<
PointCloudIn, PointCloudIn, PointIndices>>>(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<PointCloudIn>(
"input", max_queue_size_,
bind(
&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(),
PointIndicesConstPtr()));
}
NODELET_DEBUG(
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - use_surface : %s\n"
" - k_search : %d\n"
" - radius_search : %f\n"
" - spatial_locator: %d",
getName().c_str(),
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
}
////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
{
if (k_ != config.k_search) {
k_ = config.k_search;
NODELET_DEBUG(
"[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
}
if (search_radius_ != config.radius_search) {
search_radius_ = config.radius_search;
NODELET_DEBUG(
"[config_callback] Setting the nearest neighbors search radius for each point: %f.",
search_radius_);
}
}
////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Feature::input_surface_indices_callback(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers() <= 0) {
return;
}
// If cloud is given, check if it's valid
if (!isValid(cloud)) {
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
emptyPublish(cloud);
return;
}
// If surface is given, check if it's valid
if (cloud_surface && !isValid(cloud_surface, "surface")) {
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
emptyPublish(cloud);
return;
}
// If indices are given, check if they are valid
if (indices && !isValid(indices)) {
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
emptyPublish(cloud);
return;
}
/// DEBUG
if (cloud_surface) {
if (indices) {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, "
"and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, "
"and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, "
"and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(),
cloud_surface->header.stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(),
cloud_surface->header.stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
}
} else if (indices) {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and "
"frame %s on topic %s received.", cloud->width * cloud->height,
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
}
///
if (static_cast<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<int>(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<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str());
return;
}
if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
NODELET_ERROR(
"[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return;
}
// ---[ Optional parameters
pnh_->getParam("use_surface", use_surface_);
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
// Enable the dynamic reconfigure service
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
&FeatureFromNormals::config_callback, this, _1, _2);
srv_->setCallback(f);
// Create the objects here
if (approximate_sync_) {
sync_input_normals_surface_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
} else {
sync_input_normals_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<
PointCloudIn, PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
}
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
if (use_indices_ || use_surface_) {
if (use_indices_) {
// If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
// and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput(
sub_input_filter_,
sub_normals_filter_,
sub_surface_filter_,
sub_indices_filter_);
} else {
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_,
sub_normals_filter_,
sub_surface_filter_,
sub_indices_filter_);
}
} else { // Use only indices
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) {
// surface not enabled, connect the input-indices duo and register
sync_input_normals_surface_indices_a_->connectInput(
sub_input_filter_,
sub_normals_filter_, nf_pc_,
sub_indices_filter_);
} else {
// surface not enabled, connect the input-indices duo and register
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_,
sub_normals_filter_, nf_pc_,
sub_indices_filter_);
}
}
} else { // Use only surface
// indices not enabled, connect the input-surface duo and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput(
sub_input_filter_, sub_normals_filter_,
sub_surface_filter_, nf_pi_);
} else {
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_, sub_normals_filter_,
sub_surface_filter_, nf_pi_);
}
}
} else {
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput(
sub_input_filter_, sub_normals_filter_,
nf_pc_, nf_pi_);
} else {
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_, sub_normals_filter_,
nf_pc_, nf_pi_);
}
}
// Register callbacks
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->registerCallback(
bind(
&FeatureFromNormals::
input_normals_surface_indices_callback, this, _1, _2, _3, _4));
} else {
sync_input_normals_surface_indices_e_->registerCallback(
bind(
&FeatureFromNormals::
input_normals_surface_indices_callback, this, _1, _2, _3, _4));
}
NODELET_DEBUG(
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - use_surface : %s\n"
" - k_search : %d\n"
" - radius_search : %f\n"
" - spatial_locator: %d",
getName().c_str(),
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals,
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers() <= 0) {
return;
}
// If cloud+normals is given, check if it's valid
if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals"))
NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
emptyPublish(cloud);
return;
}
// If surface is given, check if it's valid
if (cloud_surface && !isValid(cloud_surface, "surface")) {
NODELET_ERROR(
"[%s::input_normals_surface_indices_callback] Invalid input surface!",
getName().c_str());
emptyPublish(cloud);
return;
}
// If indices are given, check if they are valid
if (indices && !isValid(indices)) {
NODELET_ERROR(
"[%s::input_normals_surface_indices_callback] Invalid input indices!",
getName().c_str());
emptyPublish(cloud);
return;
}
/// DEBUG
if (cloud_surface) {
if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(),
cloud_surface->header.stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(),
cloud_normals->header.stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(),
cloud_surface->header.stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(),
cloud_normals->header.stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
}
} else if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(),
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(),
pnh_->resolveName("normals").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(),
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(),
pnh_->resolveName("normals").c_str());
}
///
if (static_cast<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<int>(indices->indices));
}
computePublish(cloud, cloud_normals, cloud_surface, vindices);
}

79
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp Normal file
View File

@ -0,0 +1,79 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/fpfh.hpp"
void
pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
void
pcl_ros::FPFHEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator
initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod(tree_);
// Set the inputs
impl_.setInputCloud(cloud);
impl_.setIndices(indices);
impl_.setSearchSurface(surface);
impl_.setInputNormals(normals);
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
typedef pcl_ros::FPFHEstimation FPFHEstimation;
PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet);

79
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp Normal file
View File

@ -0,0 +1,79 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/fpfh_omp.hpp"
void
pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
void
pcl_ros::FPFHEstimationOMP::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator
initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod(tree_);
// Set the inputs
impl_.setInputCloud(cloud);
impl_.setIndices(indices);
impl_.setSearchSurface(surface);
impl_.setInputNormals(normals);
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet);

77
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp Normal file
View File

@ -0,0 +1,77 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/moment_invariants.hpp"
void
pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
void
pcl_ros::MomentInvariantsEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator
initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod(tree_);
// Set the inputs
impl_.setInputCloud(cloud);
impl_.setIndices(indices);
impl_.setSearchSurface(surface);
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet);

77
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp Normal file
View File

@ -0,0 +1,77 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/normal_3d.hpp"
void
pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
void
pcl_ros::NormalEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator
initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod(tree_);
// Set the inputs
impl_.setInputCloud(cloud);
impl_.setIndices(indices);
impl_.setSearchSurface(surface);
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
typedef pcl_ros::NormalEstimation NormalEstimation;
PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet);

77
ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp Normal file
View File

@ -0,0 +1,77 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/normal_3d_omp.hpp"
void
pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
void
pcl_ros::NormalEstimationOMP::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator
initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod(tree_);
// Set the inputs
impl_.setInputCloud(cloud);
impl_.setIndices(indices);
impl_.setSearchSurface(surface);
// Estimate the feature
PointCloudOut output;
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish(output.makeShared());
}
typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet);

Some files were not shown because too many files have changed in this diff Show More