////////////////////////////////////////////////////////////////////////////////
// cloud_cb (!)
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
{
pcl::PointCloud<pcl::PointXYZ> cloud_in;
pcl::PointCloud<pcl::PointXYZ> output;
pcl::PointCloud<pcl::PointXYZ> output_filtered;
pcl::fromROSMsg(*pc, cloud_in);
if (counter_ == 0)
{
ROS_INFO("Setting input cloud with %ld points", cloud_in.points.size());
seg_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
counter_++;
}
else
{
ROS_INFO("Setting target cloud with %ld points", cloud_in.points.size());
seg_.setTargetCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
seg_.segment (output);
counter_ = 0;
ROS_INFO("Publishing difference cloud with %ld points", output.points.size());
pub_diff_.publish (output);
outrem_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(output));
outrem_.setRadiusSearch (0.02);
outrem_.setMinNeighborsInRadius (15);
outrem_.filter (output_filtered);
pub_filtered_.publish (output_filtered);
}
}
bool spin ()
{
int nr_points = cloud.width * cloud.height;
std::string fields_list = pcl::getFieldsList(cloud);
ros::Rate r(ros::Duration(1,0)); //1s tact
while(nh.ok ())
{
ROS_DEBUG_STREAM_ONCE("Publishing data with " << nr_points
<< " points " << fields_list
<< " on topic \"" << nh.resolveName(cloud_topic)
<< "\" in frame \"" << cloud.header.frame_id << "\"");
cloud.header.stamp = ros::Time::now();
pub.publish(cloud);
r.sleep();
}
return (true);
}