restructuring perception_pcl in groovy branch
This commit is contained in:
jkammerl
Paul Bovbel
@@ -0,0 +1,37 @@
|
||||
cmake_minimum_required(VERSION 2.8)
|
||||
|
||||
project(perception_pcl)
|
||||
|
||||
# Deal with catkin
|
||||
find_package(catkin REQUIRED roscpp sensor_msgs tf)
|
||||
find_package(Boost COMPONENTS system filesystem thread REQUIRED)
|
||||
find_package(Eigen)
|
||||
find_package(PCL)
|
||||
|
||||
# deal with ROS
|
||||
include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
|
||||
include_directories(include)
|
||||
|
||||
link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS})
|
||||
|
||||
catkin_stack()
|
||||
|
||||
catkin_project(${PROJECT_NAME}
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES pcl_ros_tf
|
||||
DEPENDS roscpp common_msgs sensor_msgs tf
|
||||
)
|
||||
|
||||
# ---[ Point Cloud Library - Transforms
|
||||
add_library (pcl_ros_tf SHARED src/transforms.cpp)
|
||||
target_link_libraries(pcl_ros_tf ${PCL_LIBS} ${Boost_LIBS} ${catkin_LIBS})
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
|
||||
)
|
||||
|
||||
install(TARGETS pcl_ros_tf
|
||||
RUNTIME DESTINATION ${CATKIN_PROJECT_BIN_DESTINATION}
|
||||
ARCHIVE DESTINATION ${CATKIN_PROJECT_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PROJECT_LIB_DESTINATION}
|
||||
)
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
|
||||
@@ -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 &);
|
||||
|
||||
Reference in New Issue
Block a user