예제 #1
0
 ////////////////////////////////////////////////////////////////////////////////
 // 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);
  }