restructuring perception_pcl in groovy branch
This commit is contained in:
jkammerl
Paul Bovbel
@@ -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_
|
||||
|
||||
Reference in New Issue
Block a user