diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..15d597ea --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +set(ROSPACK_MAKEDIST true) + +# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of +# directories (or patterns, but directories should suffice) that should +# be excluded from the distro. This is not the place to put things that +# should be ignored everywhere, like "build" directories; that happens in +# rosbuild/rosbuild.cmake. Here should be listed packages that aren't +# ready for inclusion in a distro. +# +# This list is combined with the list in rosbuild/rosbuild.cmake. Note +# that CMake 2.6 may be required to ensure that the two lists are combined +# properly. CMake 2.4 seems to have unpredictable scoping rules for such +# variables. +#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) + +rosbuild_make_distribution(1.6.0) diff --git a/Makefile b/Makefile new file mode 100644 index 00000000..a818ccad --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file diff --git a/pcl/CMakeLists.txt b/pcl/CMakeLists.txt new file mode 100644 index 00000000..0853d403 --- /dev/null +++ b/pcl/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +find_package(Eigen REQUIRED) +include_directories(${Eigen_INCLUDE_DIRS}) +rosbuild_init() +rosbuild_genmsg() + diff --git a/pcl/Makefile b/pcl/Makefile new file mode 100644 index 00000000..f42463cf --- /dev/null +++ b/pcl/Makefile @@ -0,0 +1,57 @@ +all: installed + +SVN_DIR = build/pcl_1.6.0 +# Developers, please use this URL: +SVN_URL = http://svn.pointclouds.org/pcl/tags/pcl-1.6.0 # For the very latest version + +ifeq ($(strip $(SVN_CMDLINE)),) +SVN_CMDLINE = svn +endif + +$(SVN_DIR): + $(SVN_CMDLINE) co $(SVN_REVISION) $(SVN_URL) $(SVN_DIR) +ifneq ($(strip $(SVN_PATCH)),) + cd $(SVN_DIR) && patch -p0 < ../$(SVN_PATCH) +endif + cd $(SVN_DIR) && \ + sed -Ei 's/-Wold-style-cast/-Wno-old-style-cast -Wno-unused-parameter -Wno-conversion/g' ./CMakeLists.txt && \ + sed -Ei 's/${INCLUDE_INSTALL_ROOT}\/pcl/${INCLUDE_INSTALL_ROOT}\/pcl$(PCL_VERSION)/g' ./cmake/pcl_utils.cmake + +SVN_UP: $(SVN_DIR) + cd $(SVN_DIR) && $(SVN_CMDLINE) up $(SVN_REVISION) + +download: $(SVN_UP) + +installed: $(SVN_DIR) cleaned + mkdir -p msg/build && cd msg/build && cmake ../.. && make && cd - + cd $(SVN_DIR) && mkdir -p build && cd build && \ + rm -rf ../common/include/sensor_msgs ../common/include/std_msgs \ + ../common/include/pcl$(PCL_VERSION)/ModelCoefficients.h ../common/include/pcl$(PCL_VERSION)/PointIndices.h ../common/include/pcl$(PCL_VERSION)/PolygonMesh.h ../common/include/pcl$(PCL_VERSION)/Vertices.h && \ + export CPATH="`rospack cflags-only-I sensor_msgs`:`rospack cflags-only-I roscpp_serialization`:`rospack cflags-only-I cpp_common`:`rospack cflags-only-I rostime`:`rospack cflags-only-I roscpp_traits`:`rospack cflags-only-I roscpp`:`rospack cflags-only-I rosconsole`:`rospack cflags-only-I std_msgs`:`rospack cflags-only-I sensor_msgs`:`rospack find pcl$(PCL_VERSION)`/msg_gen/cpp/include:$$CPATH" && \ + export LD_LIBRARY_PATH="`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:`rospack libs-only-L roscpp_serialization`:`rospack libs-only-L cpp_common`:`rospack libs-only-L rostime`:`rospack libs-only-L roscpp_traits`:`rospack libs-only-L roscpp`:`rospack libs-only-L rosconsole`:`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:$$LD_LIBRARY_PATH" && \ + export LIBRARY_PATH="`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:`rospack libs-only-L roscpp_serialization`:`rospack libs-only-L cpp_common`:`rospack libs-only-L rostime`:`rospack libs-only-L roscpp_traits`:`rospack libs-only-L roscpp`:`rospack libs-only-L rosconsole`:`rospack libs-only-L std_msgs`:`rospack libs-only-L sensor_msgs`:$$LIBRARY_PATH" && \ + cmake -DCMAKE_INSTALL_PREFIX=`pwd`/../../.. \ + -DCMAKE_BUILD_TYPE=Release \ + -DUSE_ROS=ON \ + -DBUILD_OPENNI=ON \ + -DBUILD_TESTS=OFF \ + -DBUILD_apps=OFF \ + -DBUILD_examples=OFF \ + -DCMAKE_VERBOSE_MAKEFILE=OFF \ + .. && \ + make ${ROS_PARALLEL_JOBS} install + touch installed + +cleaned: Makefile + make clean + touch cleaned + +clean: + -rm -rf $(SVN_DIR)/build rospack_nosubdirs patched installed include bin lib64 msg_gen src *~ + +wiped: Makefile + make wipe + touch wiped + +wipe: clean + rm -rf build cleaned msg/build diff --git a/pcl/cmake/FindEigen.cmake b/pcl/cmake/FindEigen.cmake new file mode 100644 index 00000000..2666481c --- /dev/null +++ b/pcl/cmake/FindEigen.cmake @@ -0,0 +1,81 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN_FOUND - system has eigen lib with correct version +# EIGEN_INCLUDE_DIR - the eigen include directory +# EIGEN_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen_FIND_VERSION) + if(NOT Eigen_FIND_VERSION_MAJOR) + set(Eigen_FIND_VERSION_MAJOR 2) + endif(NOT Eigen_FIND_VERSION_MAJOR) + if(NOT Eigen_FIND_VERSION_MINOR) + set(Eigen_FIND_VERSION_MINOR 91) + endif(NOT Eigen_FIND_VERSION_MINOR) + if(NOT Eigen_FIND_VERSION_PATCH) + set(Eigen_FIND_VERSION_PATCH 0) + endif(NOT Eigen_FIND_VERSION_PATCH) + + set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") +endif(NOT Eigen_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) + if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + set(EIGEN_VERSION_OK FALSE) + else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + set(EIGEN_VERSION_OK TRUE) + endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) + + if(NOT EIGEN_VERSION_OK) + + message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " + "but at least version ${Eigen_FIND_VERSION} is required") + endif(NOT EIGEN_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN_INCLUDE_DIRS) + + # in cache already + _eigen3_check_version() + set(EIGEN_FOUND ${EIGEN_VERSION_OK}) + +else () + + find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) + + mark_as_advanced(EIGEN_INCLUDE_DIR) + SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") + +endif() diff --git a/pcl/manifest.xml b/pcl/manifest.xml new file mode 100644 index 00000000..1f1ea64d --- /dev/null +++ b/pcl/manifest.xml @@ -0,0 +1,33 @@ + + + +

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

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

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

+ + + + Maintained by Open Perception + BSD + http://ros.org/wiki/pcl_ros + + + + + + + + + + + + + + + + + + + + diff --git a/pcl_ros/src/pcl_ros/transforms.cpp b/pcl_ros/src/pcl_ros/transforms.cpp new file mode 100644 index 00000000..d35e2366 --- /dev/null +++ b/pcl_ros/src/pcl_ros/transforms.cpp @@ -0,0 +1,280 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#include +#include +#include +#include "pcl_ros/transforms.h" +#include "pcl_ros/impl/transforms.hpp" + +namespace pcl_ros +{ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool +transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, + sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener) +{ + if (in.header.frame_id == target_frame) + { + out = in; + return (true); + } + + // Get the TF transform + tf::StampedTransform transform; + try + { + tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform); + } + catch (tf::LookupException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + catch (tf::ExtrapolationException &e) + { + ROS_ERROR ("%s", e.what ()); + return (false); + } + + // Convert the TF transform to Eigen format + Eigen::Matrix4f eigen_transform; + transformAsMatrix (transform, eigen_transform); + + transformPointCloud (eigen_transform, in, out); + + out.header.frame_id = target_frame; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, + const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) +{ + if (in.header.frame_id == target_frame) + { + out = in; + return; + } + + // Get the transformation + Eigen::Matrix4f transform; + transformAsMatrix (net_transform, transform); + + transformPointCloud (transform, in, out); + + out.header.frame_id = target_frame; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, + sensor_msgs::PointCloud2 &out) +{ + // Get X-Y-Z indices + int x_idx = pcl::getFieldIndex (in, "x"); + int y_idx = pcl::getFieldIndex (in, "y"); + int z_idx = pcl::getFieldIndex (in, "z"); + + if (x_idx == -1 || y_idx == -1 || z_idx == -1) + { + ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); + return; + } + + if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || + in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || + in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) + { + ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported."); + return; + } + + // Check if distance is available + int dist_idx = pcl::getFieldIndex (in, "distance"); + + // Copy the other data + if (&in != &out) + { + out.header = in.header; + out.height = in.height; + out.width = in.width; + out.fields = in.fields; + out.is_bigendian = in.is_bigendian; + out.point_step = in.point_step; + out.row_step = in.row_step; + out.is_dense = in.is_dense; + out.data.resize (in.data.size ()); + // Copy everything as it's faster than copying individual elements + memcpy (&out.data[0], &in.data[0], in.data.size ()); + } + + Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0); + + for (size_t i = 0; i < in.width * in.height; ++i) + { + Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1); + Eigen::Vector4f pt_out; + + bool max_range_point = false; + int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; + float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); + if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) + { + if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point + { + pt_out = pt; + } + else // max range point + { + pt[0] = *distance_ptr; // Replace x with the x value saved in distance + pt_out = transform * pt; + max_range_point = true; + //std::cout << pt[0]<<","< "<::quiet_NaN(); + } + + memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float)); + memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float)); + memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float)); + + + xyz_offset += in.point_step; + } + + // Check if the viewpoint information is present + int vp_idx = pcl::getFieldIndex (in, "vp_x"); + if (vp_idx != -1) + { + // Transform the viewpoint info too + for (size_t i = 0; i < out.width * out.height; ++i) + { + float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset]; + // Assume vp_x, vp_y, vp_z are consecutive + Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1); + Eigen::Vector4f vp_out = transform * vp_in; + + pstep[0] = vp_out[0]; + pstep[1] = vp_out[1]; + pstep[2] = vp_out[2]; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat) +{ + double mv[12]; + bt.getBasis ().getOpenGLSubMatrix (mv); + + tf::Vector3 origin = bt.getOrigin (); + + out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; + out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; + out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; + + out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1; + out_mat (0, 3) = origin.x (); + out_mat (1, 3) = origin.y (); + out_mat (2, 3) = origin.z (); +} + +} // namespace pcl_ros + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); + diff --git a/rosdep.yaml b/rosdep.yaml new file mode 100644 index 00000000..4a9d45d6 --- /dev/null +++ b/rosdep.yaml @@ -0,0 +1,32 @@ +libtbb: + ubuntu: libtbb-dev + debian: libtbb-dev + fedora: tbb + macports: tbb +libvtk: + ubuntu: libvtk5-dev + debian: libvtk5-dev + fedora: vtk-devel + macports: vtk-devel +unzip: + ubuntu: unzip + debian: unzip + fedora: unzip + macports: unzip +hdf5: + ubuntu: libhdf5-serial-dev + debian: libhdf5-serial-dev + fedora: hdf5-devel + macports: hdf5-18 +libqhull: + ubuntu: libqhull-dev + debian: libqhull-dev + fedora: qhull-devel + macports: qhull +cmake: + ubuntu: cmake + debian: cmake + fedora: cmake + macports: cmake +libvtk-qt: + ubuntu: libvtk5-qt4-dev \ No newline at end of file diff --git a/stack.xml b/stack.xml new file mode 100644 index 00000000..9d5c90c1 --- /dev/null +++ b/stack.xml @@ -0,0 +1,17 @@ + + + This contains the Point Cloud Library (PCL), its + 3rd party dependencies, and a ROS interface for PCL nodelets. + + Maintained by Open Perception + BSD + + http://ros.org/wiki/perception_pcl + + + + + + + +