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

added PCL 1.6 stac for Groovy

Browse Source
This commit is contained in:
rusu 2012-09-05 23:29:36 +00:00 committed by Paul Bovbel
parent 693a18feb0
commit adf647f14d
21 changed files with 1467 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
CMakeLists.txt Normal file
View File

@ -0,0 +1,19 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROSPACK_MAKEDIST true)
# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
# directories (or patterns, but directories should suffice) that should
# be excluded from the distro. This is not the place to put things that
# should be ignored everywhere, like "build" directories; that happens in
# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
# ready for inclusion in a distro.
#
# This list is combined with the list in rosbuild/rosbuild.cmake. Note
# that CMake 2.6 may be required to ensure that the two lists are combined
# properly. CMake 2.4 seems to have unpredictable scoping rules for such
# variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
rosbuild_make_distribution(1.6.0)

1
Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake_stack.mk

8
pcl/CMakeLists.txt Normal file
View File

@ -0,0 +1,8 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
find_package(Eigen REQUIRED)
include_directories(${Eigen_INCLUDE_DIRS})
rosbuild_init()
rosbuild_genmsg()

57
pcl/Makefile Normal file
View File

@ -0,0 +1,57 @@
all: installed
SVN_DIR = build/pcl_1.6.0
# Developers, please use this URL:
SVN_URL = http://svn.pointclouds.org/pcl/tags/pcl-1.6.0 # For the very latest version
ifeq ($(strip $(SVN_CMDLINE)),)
SVN_CMDLINE = svn
endif
$(SVN_DIR):
$(SVN_CMDLINE) co $(SVN_REVISION) $(SVN_URL) $(SVN_DIR)
ifneq ($(strip $(SVN_PATCH)),)
cd $(SVN_DIR) && patch -p0 < ../$(SVN_PATCH)
endif
cd $(SVN_DIR) && \
sed -Ei 's/-Wold-style-cast/-Wno-old-style-cast -Wno-unused-parameter -Wno-conversion/g' ./CMakeLists.txt && \
sed -Ei 's/${INCLUDE_INSTALL_ROOT}\/pcl/${INCLUDE_INSTALL_ROOT}\/pcl$(PCL_VERSION)/g' ./cmake/pcl_utils.cmake
SVN_UP: $(SVN_DIR)
cd $(SVN_DIR) && $(SVN_CMDLINE) up $(SVN_REVISION)
download: $(SVN_UP)
installed: $(SVN_DIR) cleaned
mkdir -p msg/build && cd msg/build && cmake ../.. && make && cd -
cd $(SVN_DIR) && mkdir -p build && cd build && \
rm -rf ../common/include/sensor_msgs ../common/include/std_msgs \
../common/include/pcl$(PCL_VERSION)/ModelCoefficients.h ../common/include/pcl$(PCL_VERSION)/PointIndices.h ../common/include/pcl$(PCL_VERSION)/PolygonMesh.h ../common/include/pcl$(PCL_VERSION)/Vertices.h && \
export CPATH="`rospack cflags-only-I sensor_msgs`:`rospack cflags-only-I roscpp_serialization`:`rospack cflags-only-I cpp_common`:`rospack cflags-only-I rostime`:`rospack cflags-only-I roscpp_traits`:`rospack cflags-only-I roscpp`:`rospack cflags-only-I rosconsole`:`rospack cflags-only-I std_msgs`:`rospack cflags-only-I sensor_msgs`:`rospack find pcl$(PCL_VERSION)`/msg_gen/cpp/include:$$CPATH" && \
export LD_LIBRARY_PATH="`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:`rospack libs-only-L roscpp_serialization`:`rospack libs-only-L cpp_common`:`rospack libs-only-L rostime`:`rospack libs-only-L roscpp_traits`:`rospack libs-only-L roscpp`:`rospack libs-only-L rosconsole`:`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:$$LD_LIBRARY_PATH" && \
export LIBRARY_PATH="`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:`rospack libs-only-L roscpp_serialization`:`rospack libs-only-L cpp_common`:`rospack libs-only-L rostime`:`rospack libs-only-L roscpp_traits`:`rospack libs-only-L roscpp`:`rospack libs-only-L rosconsole`:`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:$$LIBRARY_PATH" && \
cmake -DCMAKE_INSTALL_PREFIX=`pwd`/../../.. \
-DCMAKE_BUILD_TYPE=Release \
-DUSE_ROS=ON \
-DBUILD_OPENNI=ON \
-DBUILD_TESTS=OFF \
-DBUILD_apps=OFF \
-DBUILD_examples=OFF \
-DCMAKE_VERBOSE_MAKEFILE=OFF \
.. && \
make ${ROS_PARALLEL_JOBS} install
touch installed
cleaned: Makefile
make clean
touch cleaned
clean:
-rm -rf $(SVN_DIR)/build rospack_nosubdirs patched installed include bin lib64 msg_gen src *~
wiped: Makefile
make wipe
touch wiped
wipe: clean
rm -rf build cleaned msg/build

81
pcl/cmake/FindEigen.cmake Normal file
View File

@ -0,0 +1,81 @@
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN_FOUND - system has eigen lib with correct version
# EIGEN_INCLUDE_DIR - the eigen include directory
# EIGEN_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen_FIND_VERSION)
if(NOT Eigen_FIND_VERSION_MAJOR)
set(Eigen_FIND_VERSION_MAJOR 2)
endif(NOT Eigen_FIND_VERSION_MAJOR)
if(NOT Eigen_FIND_VERSION_MINOR)
set(Eigen_FIND_VERSION_MINOR 91)
endif(NOT Eigen_FIND_VERSION_MINOR)
if(NOT Eigen_FIND_VERSION_PATCH)
set(Eigen_FIND_VERSION_PATCH 0)
endif(NOT Eigen_FIND_VERSION_PATCH)
set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}")
endif(NOT Eigen_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
set(EIGEN_VERSION_OK FALSE)
else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
set(EIGEN_VERSION_OK TRUE)
endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
if(NOT EIGEN_VERSION_OK)
message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, "
"but at least version ${Eigen_FIND_VERSION} is required")
endif(NOT EIGEN_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN_INCLUDE_DIRS)
# in cache already
_eigen3_check_version()
set(EIGEN_FOUND ${EIGEN_VERSION_OK})
else ()
find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK)
mark_as_advanced(EIGEN_INCLUDE_DIR)
SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.")
endif()

33
pcl/manifest.xml Normal file
View File

@ -0,0 +1,33 @@
<package>
<description brief="PCL (Point Cloud Library)">
<p>
<b>PCL</b> - <b>P</b>oint <b>C</b>loud <b>L</b>ibrary: a comprehensive open
source library for <b>n-D Point Clouds</b> and <b>3D geometry processing</b>.
The library contains numerous state-of-the art algorithms for: filtering,
feature estimation, surface reconstruction, registration, model fitting and
segmentation, etc.
</p>
</description>
<author>See http://pcl.ros.org/authors for the complete list of authors.</author>
<license>BSD</license>
<url>http://pointclouds.org</url>
<review status="3rdparty" notes=""/>
<!-- Message type dependencies -->
<depend package="std_msgs" />
<depend package="sensor_msgs" />
<rosdep name="libqhull" />
<rosdep name="libvtk" />
<rosdep name="libvtk-qt" />
<rosdep name="cmake"/>
<rosdep name="boost"/>
<export>
<cpp cflags="-I${prefix}/include/pcl-1.6/" lflags="-Wl,-rpath,${prefix}/lib64 -L${prefix}/lib64 -Wl,-rpath,${prefix}/lib -L${prefix}/lib ${prefix}/lib/libpcl_common.so.1.6.0 ${prefix}/lib/libpcl_filters.so.1.6.0 ${prefix}/lib/libpcl_features.so.1.6.0 ${prefix}/lib/libpcl_io.so.1.6.0 ${prefix}/lib/libpcl_surface.so.1.6.0 ${prefix}/lib/libpcl_registration.so.1.6.0 ${prefix}/lib/libpcl_sample_consensus.so.1.6.0 ${prefix}/lib/libpcl_kdtree.so.1.6.0 ${prefix}/lib/libpcl_segmentation.so.1.6.0 ${prefix}/lib/libpcl_keypoints.so.1.6.0 ${prefix}/lib/libpcl_visualization.so.1.6.0 ${prefix}/lib/libpcl_octree.so.1.6.0 ${prefix}/lib/libpcl_search.so.1.6.0" />
<rosdoc external="http://docs.pointclouds.org/"/>
</export>
</package>

3
pcl/msg/ModelCoefficients.msg Normal file
View File

@ -0,0 +1,3 @@
Header header
float32[] values

3
pcl/msg/PointIndices.msg Normal file
View File

@ -0,0 +1,3 @@
Header header
int32[] indices

6
pcl/msg/PolygonMesh.msg Normal file
View File

@ -0,0 +1,6 @@
# Separate header for the polygonal surface
Header header
# Vertices of the mesh as a point cloud
sensor_msgs/PointCloud2 cloud
# List of polygons
Vertices[] polygons

2
pcl/msg/Vertices.msg Normal file
View File

@ -0,0 +1,2 @@
# List of point indices
uint32[] vertices

21
pcl_ros/CMakeLists.txt Normal file
View File

@ -0,0 +1,21 @@
cmake_minimum_required (VERSION 2.4.6)
include ($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init ()
rosbuild_add_boost_directories ()
add_definitions (-DEIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET)
rosbuild_check_for_sse ()
set (EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
include_directories (${CMAKE_CURRENT_BINARY_DIR})
include_directories (src)
# Uses Eigen
list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
find_package(Eigen REQUIRED)
include_directories(${Eigen_INCLUDE_DIRS})
include_directories(${EIGEN_INCLUDE_DIRS})
# ---[ Point Cloud Library - Transforms
rosbuild_add_library (pcl_ros_tf src/pcl_ros/transforms.cpp)
rosbuild_add_compile_flags (pcl_ros_tf ${SSE_FLAGS})

1
pcl_ros/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

81
pcl_ros/cmake/FindEigen.cmake Normal file
View File

@ -0,0 +1,81 @@
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN_FOUND - system has eigen lib with correct version
# EIGEN_INCLUDE_DIR - the eigen include directory
# EIGEN_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen_FIND_VERSION)
if(NOT Eigen_FIND_VERSION_MAJOR)
set(Eigen_FIND_VERSION_MAJOR 2)
endif(NOT Eigen_FIND_VERSION_MAJOR)
if(NOT Eigen_FIND_VERSION_MINOR)
set(Eigen_FIND_VERSION_MINOR 91)
endif(NOT Eigen_FIND_VERSION_MINOR)
if(NOT Eigen_FIND_VERSION_PATCH)
set(Eigen_FIND_VERSION_PATCH 0)
endif(NOT Eigen_FIND_VERSION_PATCH)
set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}")
endif(NOT Eigen_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
set(EIGEN_VERSION_OK FALSE)
else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
set(EIGEN_VERSION_OK TRUE)
endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
if(NOT EIGEN_VERSION_OK)
message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, "
"but at least version ${Eigen_FIND_VERSION} is required")
endif(NOT EIGEN_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN_INCLUDE_DIRS)
# in cache already
_eigen3_check_version()
set(EIGEN_FOUND ${EIGEN_VERSION_OK})
else ()
find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK)
mark_as_advanced(EIGEN_INCLUDE_DIR)
SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.")
endif()

216
pcl_ros/include/pcl_ros/impl/transforms.hpp Normal file
View File

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

256
pcl_ros/include/pcl_ros/point_cloud.h Normal file
View File

@ -0,0 +1,256 @@
#ifndef pcl_ROS_POINT_CLOUD_H_
#define pcl_ROS_POINT_CLOUD_H_
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_traits.h>
#include <pcl/for_each_type.h>
#include <pcl/ros/conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <boost/mpl/size.hpp>
#include <boost/ref.hpp>
namespace pcl
{
namespace detail
{
template<typename Stream, typename PointT>
struct FieldStreamer
{
FieldStreamer(Stream& stream) : stream_(stream) {}
template<typename U> void operator() ()
{
const char* name = traits::name<PointT, U>::value;
uint32_t name_length = strlen(name);
stream_.next(name_length);
if (name_length > 0)
memcpy(stream_.advance(name_length), name, name_length);
uint32_t offset = traits::offset<PointT, U>::value;
stream_.next(offset);
uint8_t datatype = traits::datatype<PointT, U>::value;
stream_.next(datatype);
uint32_t count = traits::datatype<PointT, U>::size;
stream_.next(count);
}
Stream& stream_;
};
template<typename PointT>
struct FieldsLength
{
FieldsLength() : length(0) {}
template<typename U> void operator() ()
{
uint32_t name_length = strlen(traits::name<PointT, U>::value);
length += name_length + 13;
}
uint32_t length;
};
} // namespace pcl::detail
} // namespace pcl
namespace ros
{
// In ROS 1.3.1+, we can specialize the functor used to create PointCloud<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(); }
};
template<typename T> struct HasHeader<pcl::PointCloud<T> > : TrueType {};
} // namespace ros::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[0], 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)
{
stream.next(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[0]);
// If the data layouts match, can copy a whole row in one memcpy
if (mapping.size() == 1 &&
mapping[0].serialized_offset == 0 &&
mapping[0].struct_offset == 0 &&
point_step == sizeof(T))
{
uint32_t m_row_step = sizeof(T) * m.width;
// And if the row steps match, can copy whole point cloud in one memcpy
if (m_row_step == row_step)
{
memcpy (m_data, stream.advance(data_size), data_size);
}
else
{
for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step)
memcpy (m_data, stream.advance(row_step), m_row_step);
}
}
else
{
// If not, do a lot of memcpys to copy over the fields
for (uint32_t row = 0; row < m.height; ++row) {
const uint8_t* stream_data = stream.advance(row_step);
for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) {
memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
}
m_data += sizeof(T);
}
}
}
uint8_t is_dense;
stream.next(is_dense);
m.is_dense = is_dense;
}
inline static uint32_t serializedLength(const pcl::PointCloud<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 ros::serialization
/// @todo Printer specialization in message_operations
} // namespace ros
#endif

146
pcl_ros/include/pcl_ros/publisher.h Normal file
View File

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

167
pcl_ros/include/pcl_ros/transforms.h Normal file
View File

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

37
pcl_ros/manifest.xml Normal file
View File

@ -0,0 +1,37 @@
<package>
<description brief="PCL - ROS (Unstable)">
<p>
PCL (Point Cloud Library) ROS interface package. PCL-ROS is the preferred
bridge for 3D applications involving n-D Point Clouds and 3D geometry
processing in ROS. The package contains powerful nodelet interfaces for PCL
algorithms, accepts dynamic reconfiguration of parameters, and supports
multiple threading natively for large scale PPG (Perception Processing
Graphs) construction and usage.
</p>
</description>
<author>Maintained by Open Perception</author>
<license>BSD</license>
<url>http://ros.org/wiki/pcl_ros</url>
<review status="doc reviewed" notes=""/>
<!-- ROS dependencies -->
<depend package="roscpp" />
<depend package="rosbag" />
<depend package="geometry_msgs" />
<depend package="message_filters" />
<depend package="tf" />
<!-- Eigen -->
<depend package="common_rosdeps" />
<rosdep name="eigen" />
<!-- PCL -->
<depend package="pcl" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lpcl_ros_tf" />
</export>
</package>

280
pcl_ros/src/pcl_ros/transforms.cpp Normal file
View File

@ -0,0 +1,280 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
#include <sensor_msgs/PointCloud2.h>
#include <pcl/common/io.h>
#include <pcl/point_types.h>
#include "pcl_ros/transforms.h"
#include "pcl_ros/impl/transforms.hpp"
namespace pcl_ros
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in,
sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener)
{
if (in.header.frame_id == target_frame)
{
out = in;
return (true);
}
// Get the TF transform
tf::StampedTransform transform;
try
{
tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform);
}
catch (tf::LookupException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
}
catch (tf::ExtrapolationException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
}
// Convert the TF transform to Eigen format
Eigen::Matrix4f eigen_transform;
transformAsMatrix (transform, eigen_transform);
transformPointCloud (eigen_transform, in, out);
out.header.frame_id = target_frame;
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform,
const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
{
if (in.header.frame_id == target_frame)
{
out = in;
return;
}
// Get the transformation
Eigen::Matrix4f transform;
transformAsMatrix (net_transform, transform);
transformPointCloud (transform, in, out);
out.header.frame_id = target_frame;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in,
sensor_msgs::PointCloud2 &out)
{
// Get X-Y-Z indices
int x_idx = pcl::getFieldIndex (in, "x");
int y_idx = pcl::getFieldIndex (in, "y");
int z_idx = pcl::getFieldIndex (in, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
{
ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
return;
}
if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
{
ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.");
return;
}
// Check if distance is available
int dist_idx = pcl::getFieldIndex (in, "distance");
// Copy the other data
if (&in != &out)
{
out.header = in.header;
out.height = in.height;
out.width = in.width;
out.fields = in.fields;
out.is_bigendian = in.is_bigendian;
out.point_step = in.point_step;
out.row_step = in.row_step;
out.is_dense = in.is_dense;
out.data.resize (in.data.size ());
// Copy everything as it's faster than copying individual elements
memcpy (&out.data[0], &in.data[0], in.data.size ());
}
Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
for (size_t i = 0; i < in.width * in.height; ++i)
{
Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1);
Eigen::Vector4f pt_out;
bool max_range_point = false;
int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
{
if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point
{
pt_out = pt;
}
else // max range point
{
pt[0] = *distance_ptr; // Replace x with the x value saved in distance
pt_out = transform * pt;
max_range_point = true;
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
}
}
else
{
pt_out = transform * pt;
}
if (max_range_point)
{
// Save x value in distance again
*(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
pt_out[0] = std::numeric_limits<float>::quiet_NaN();
}
memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float));
memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float));
memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float));
xyz_offset += in.point_step;
}
// Check if the viewpoint information is present
int vp_idx = pcl::getFieldIndex (in, "vp_x");
if (vp_idx != -1)
{
// Transform the viewpoint info too
for (size_t i = 0; i < out.width * out.height; ++i)
{
float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
// Assume vp_x, vp_y, vp_z are consecutive
Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1);
Eigen::Vector4f vp_out = transform * vp_in;
pstep[0] = vp_out[0];
pstep[1] = vp_out[1];
pstep[2] = vp_out[2];
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
{
double mv[12];
bt.getBasis ().getOpenGLSubMatrix (mv);
tf::Vector3 origin = bt.getOrigin ();
out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];
out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
out_mat (0, 3) = origin.x ();
out_mat (1, 3) = origin.y ();
out_mat (2, 3) = origin.z ();
}
} // namespace pcl_ros
//////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
//////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
//////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
//////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloud<pcl::PointXYZ> (const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZI> (const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::InterestPoint> (const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointWithRange> (const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::Transform &);
//////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);
//////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZ> &, const std::string &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZI> &, const std::string &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBA> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGB> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::InterestPoint> &, const std::string &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithRange> &, const std::string &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithViewpoint> &, const std::string &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);

32
rosdep.yaml Normal file
View File

@ -0,0 +1,32 @@
libtbb:
ubuntu: libtbb-dev
debian: libtbb-dev
fedora: tbb
macports: tbb
libvtk:
ubuntu: libvtk5-dev
debian: libvtk5-dev
fedora: vtk-devel
macports: vtk-devel
unzip:
ubuntu: unzip
debian: unzip
fedora: unzip
macports: unzip
hdf5:
ubuntu: libhdf5-serial-dev
debian: libhdf5-serial-dev
fedora: hdf5-devel
macports: hdf5-18
libqhull:
ubuntu: libqhull-dev
debian: libqhull-dev
fedora: qhull-devel
macports: qhull
cmake:
ubuntu: cmake
debian: cmake
fedora: cmake
macports: cmake
libvtk-qt:
ubuntu: libvtk5-qt4-dev

17
stack.xml Normal file
View File

@ -0,0 +1,17 @@
<stack>
<description brief="Point Cloud Library">
This contains the Point Cloud Library (PCL), its
3rd party dependencies, and a ROS interface for PCL nodelets.
</description>
<author>Maintained by Open Perception</author>
<license>BSD</license>
<review status="Doc reviewed" notes=""/>
<url>http://ros.org/wiki/perception_pcl</url>
<depend stack="nodelet_core" /> <!-- nodelet_topic_tools, nodelet -->
<depend stack="common_msgs" /> <!-- sensor_msgs, geometry_msgs -->
<depend stack="driver_common" /> <!-- dynamic_reconfigure -->
<depend stack="geometry" /> <!-- tf -->
<depend stack="ros" />
<depend stack="ros_comm" /> <!-- std_msgs, rosbag, roscpp, message_filters -->
<depend stack="common_rosdeps" /> <!-- eigen -->
</stack>