//////////////////////////////////////////////////////////////////////////////// // 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); } }
CloudPublisher() : tf_frame("/base_link"), private_nh("~") { cloud_topic = "cloud"; pub.advertise(nh, cloud_topic.c_str(), 1); private_nh.param("frame_id", tf_frame, std::string("/base_link")); ROS_INFO_STREAM("Publishing data on topic \"" << nh.resolveName(cloud_topic) << "\" with frame_id \"" << tf_frame << "\""); }
SegmentDifferencesNode (ros::NodeHandle &n) : nh_(n) { // Maximum number of outgoing messages to be queued for delivery to subscribers = 1 nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/cloud_pcd")); nh_.param("output_cloud_topic", output_cloud_topic_, std::string("difference")); nh_.param("output_filtered_cloud_topic", output_filtered_cloud_topic_, std::string("difference_filtered")); nh_.param("distance_threshold", distance_threshold_, 0.0005); ROS_INFO ("Distance threshold set to %lf.", distance_threshold_); pub_diff_.advertise (nh_, output_cloud_topic_.c_str (), 1); ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ()); pub_filtered_.advertise (nh_, output_filtered_cloud_topic_.c_str (), 1); ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_filtered_cloud_topic_).c_str ()); sub_.subscribe (nh_, input_cloud_topic_, 1, boost::bind (&SegmentDifferencesNode::cloud_cb, this, _1)); ROS_INFO ("Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ()); //set PCL classes seg_.setDistanceThreshold (distance_threshold_); rate_ = 1; counter_ = 0; }
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); }