40 #include <pcl/common/transforms.h> 51 pub_cloud_ =
pnh_->advertise<sensor_msgs::PointCloud2>(
"output", 1);
60 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
62 sync_->registerCallback(boost::bind(
68 const sensor_msgs::PointCloud2::ConstPtr& msg,
69 const jsk_recognition_msgs::BoundingBox::ConstPtr& box_msg)
73 pcl::PointCloud<PointT>
output;
74 Eigen::Affine3f offset;
75 transformPointcloudInBoundingBox<PointT>(
79 sensor_msgs::PointCloud2 ros_output;
86 NODELET_ERROR(
"[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
90 NODELET_ERROR(
"[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
void output(int index, double value)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static tf::TransformListener * getInstance()