void topicCallback_input(const sensor_msgs::PointCloud2::ConstPtr& msg) { /* protected region user implementation of subscribe callback for input on begin */ pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); // Transformation into PCL type PointCloud2 pcl_conversions::toPCL((*msg), *(pcl_pc)); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud)); //////////////////////// // PassThrough filter // //////////////////////// pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, robot_pose_x+localconfig.inhib_size))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, robot_pose_x-localconfig.inhib_size))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, robot_pose_y+localconfig.inhib_size))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, robot_pose_y-localconfig.inhib_size))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (pcl_cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*pcl_cloud); // Transformation into ROS type pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), output_pcloud2); output_ready = true; /* protected region user implementation of subscribe callback for input end */ }
void My_Filter::pclCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){ pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); sensor_msgs::PointCloud2 pc2; std::vector<int> inliers; // Transformation into PCL type PointCloud2 pcl_conversions::toPCL((*cloud), *(pcl_pc)); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud)); std::list<geometry_msgs::PoseStamped> tmp_list; int robot_counter = 0; for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it) { if(it->pose.orientation.x > 0.001) { // Robot currently not seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE-it->pose.orientation.x*0.002, it->pose.position.x+DETECTION_DISTANCE+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE-it->pose.orientation.x*0.002, it->pose.position.y+DETECTION_DISTANCE+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); } else { // Robot seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE, it->pose.position.x+DETECTION_DISTANCE); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE, it->pose.position.y+DETECTION_DISTANCE); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-DETECTION_DISTANCE))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (pcl_cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*pcl_cloud); } if(final2->size() > 2) { // Something in the box double meanX = 0.0; double meanY = 0.0; for(int i = 0; i < final2->size(); i++) { meanX += final2->at(i).x; meanY += final2->at(i).y; } meanX = meanX / final2->size(); meanY = meanY / final2->size(); //robots.at(robot_counter).pose.position.x = meanX; //robots.at(robot_counter).pose.position.y = meanY; it->pose.position.x = meanX; it->pose.position.y = meanY; char numstr[50]; sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str()); t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)), ros::Time::now(), "/world", numstr); t.stamp_ = ros::Time::now(); broadcaster.sendTransform(t); it->pose.orientation.x = 0.0; tmp_list.push_front(*it); //std::cout << "size : " << final2->size() << std::endl; } else { // Nothing found in the box it->pose.orientation.x += 1.0; if(it->pose.orientation.x > 100.0) { it->pose.orientation.x = 100.0; } //std::cout << "coef : " << it->pose.orientation.x << std::endl; geometry_msgs::PoseStamped tmp = *it; //robots.push_back(*it); //robots.erase(it); //robots.push_back(tmp); tmp_list.push_back(*it); } robot_counter++; } robots = tmp_list; ///////////////////////////////// // Statistical Outlier Removal // ///////////////////////////////// /* pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; sor.setInputCloud (pcl_cloud); sor.setMeanK (200); sor.setStddevMulThresh (1.0); sor.filter (*final); */ ////////////////////////////////// // Euclidian Cluster Extraction // ////////////////////////////////// // Creating the KdTree object for the search method of the extraction /* pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (pcl_cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.05); // 2cm ec.setMinClusterSize (5); ec.setMaxClusterSize (50); ec.setSearchMethod (tree); ec.setInputCloud (pcl_cloud); ec.extract (cluster_indices); std::cout << cluster_indices.size() << std::endl; double x = 0.0; double y = 0.0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { x = 0.0; y = 0.0; for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) { x +=pcl_cloud->points[*pit].x; y +=pcl_cloud->points[*pit].y; } x = x / it->indices.size(); y = y / it->indices.size(); std::cout << "x : " << x << " y : " << y << " size : " << it->indices.size() << std::endl; } */ // Transformation into ROS type pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), pc2); point_cloud_publisher_.publish(pc2); }
void topicCallback_input_cloud(const sensor_msgs::PointCloud2::ConstPtr& msg) { /* protected region user implementation of subscribe callback for input_cloud on begin */ pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); sensor_msgs::PointCloud2 pc2; std::vector<int> inliers; // Transformation into PCL type PointCloud2 pcl_conversions::toPCL((*msg), *(pcl_pc)); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud)); std::list<geometry_msgs::PoseStamped> tmp_list; int robot_counter = 0; for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it) { if(it->pose.orientation.x > 0.001) { // Robot currently not seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance-it->pose.orientation.x*0.002, it->pose.position.x+localconfig.detection_distance+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance-it->pose.orientation.x*0.002, it->pose.position.y+localconfig.detection_distance+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); } else { // Robot seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance, it->pose.position.x+localconfig.detection_distance); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance, it->pose.position.y+localconfig.detection_distance); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-localconfig.detection_distance))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (pcl_cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*pcl_cloud); } if(final2->size() > 2) { // Something in the box double meanX = 0.0; double meanY = 0.0; for(int i = 0; i < final2->size(); i++) { meanX += final2->at(i).x; meanY += final2->at(i).y; } meanX = meanX / final2->size(); meanY = meanY / final2->size(); //robots.at(robot_counter).pose.position.x = meanX; //robots.at(robot_counter).pose.position.y = meanY; it->pose.position.x = (meanX + it->pose.position.x) / 2.0; it->pose.position.y = (meanY + it->pose.position.y) / 2.0; char numstr[50]; sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str()); t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)), ros::Time::now(), "/world", numstr); t.stamp_ = ros::Time::now(); broadcaster.sendTransform(t); it->pose.orientation.x = 0.0; tmp_list.push_front(*it); if(it->header.frame_id == "0") { robot1_output_ready = true; } else { robot2_output_ready = true; } //std::cout << "size : " << final2->size() << std::endl; } else { // Nothing found in the box it->pose.orientation.x += 1.0; if(it->pose.orientation.x > 100.0) { it->pose.orientation.x = 100.0; } //std::cout << "coef : " << it->pose.orientation.x << std::endl; geometry_msgs::PoseStamped tmp = *it; //robots.push_back(*it); //robots.erase(it); //robots.push_back(tmp); tmp_list.push_back(*it); } robot_counter++; } robots = tmp_list; /* protected region user implementation of subscribe callback for input_cloud end */ }
void init(){ ot.setRenderPoints(true); GRandClip r(0,100,Range64f(-500,500)); DataSegment<float,3> ps = obj.selectXYZ(); DataSegment<float,4> cs = obj.selectRGBA32f(); std::vector<FixedColVector<int,4> > nn(1000),nnres(1000); for(int i=0;i<obj.getDim();++i){ ps[i] = Vec3(r,r,r); cs[i] = GeomColor(0,100,255,255); if(i < (int)nn.size()){ nn[i] = FixedColVector<int,4>(r,r,r); } } Time t = Time::now(); for(int i=0;i<obj.getDim();++i){ FixedColVector<float,3> p = ps[i]; ot.insert(FixedColVector<int,4>(p[0],p[1],p[2],1)); } t.showAge("insertion time"); std::vector<FixedColVector<int,4> > q = ot.query(0,-500,-500,500,1000,1000); static PointCloudObject res(q.size(),1,false); ps = res.selectXYZ(); cs = res.selectRGBA32f(); for(size_t i=0;i<q.size();++i){ ps[i] = Vec3(q[i][0],q[i][1],q[i][2],1); cs[i] = GeomColor(255,0,0,255); } PCL_PC pcl_pc(obj.getDim(),1); DataSegment<float,3> pcl_ps = pcl_pc.selectXYZ(); for(int i=0;i<obj.getDim();++i){ pcl_ps[i] = ps[i]; } t = Time::now(); PCL_OT pcl_ot(16); t.showAge("create pcl octree"); static boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > ptr(&pcl_pc.pcl()); t = Time::now(); pcl_ot.setInputCloud(ptr); t.showAge("set input cloud to pcl tree"); // 3 times faster! t = Time::now(); for(size_t i=0;i<nn.size();++i){ nnres[i] = ot.nn(nn[i]); } t.showAge("icl nn search"); t = Time::now(); for(size_t i=0;i<nn.size();++i){ std::vector<int> dstIdx; std::vector<float> dstDist; pcl_ot.nearestKSearch(pcl::PointXYZ(nn[i][0],nn[i][1],nn[i][2]), 1, dstIdx,dstDist); } t.showAge("pcl nn search"); t = Time::now(); for(size_t i=0;i<nn.size();++i){ nnres[i] = ot.nn_approx(nn[i]); } t.showAge("icl nn approx search"); #ifdef TRY_PCL_OCTREE t = Time::now(); for(size_t i=0;i<nn.size();++i){ int idx(0); float dist(0); pcl_ot.approxNearestSearch(pcl::PointXYZ(nn[i][0],nn[i][1],nn[i][2]), idx,dist); } t.showAge("pcl nn approx search"); #endif scene.addObject(&res); //scene.addObject(&obj); scene.addObject(&ot); scene.addCamera(Camera(Vec(0,0,1500,1))); gui << Draw3D().handle("draw3D").minSize(32,24) << Show(); gui["draw3D"].link(scene.getGLCallback(0)); gui["draw3D"].install(scene.getMouseHandler(0)); }
void PlaneFinder::pcCallback(const sensor_msgs::PointCloud2::ConstPtr & pc) { pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2); pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1 (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered3 (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); sensor_msgs::PointCloud2 pc2; double height = -0.5; // Transformation into PCL type PointCloud2 pcl_conversions::toPCL(*(pc), *(pcl_pc)); ////////////////// // Voxel filter // ////////////////// pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud (pcl_pc); sor.setLeafSize (0.01f, 0.01f, 0.01f); sor.filter (*cloud_filtered); // Transformation into ROS type //pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out)); //pcl_conversions::moveFromPCL(*(cloud_filtered), pc2); //debug2_pub.publish(pc2); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(cloud_filtered), *(cloud_filtered1)); if(pcl_ros::transformPointCloud("map", *(cloud_filtered1), *(cloud_filtered2), tf_)) { //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud (cloud_filtered2); pass.setFilterFieldName ("x"); pass.setFilterLimits (-0.003, 0.9); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered2); pass.setInputCloud (cloud_filtered2); pass.setFilterFieldName ("y"); pass.setFilterLimits (-0.5, 0.5); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered2); ///////////////////////// // Planar segmentation // ///////////////////////// pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZRGB> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); //seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); // Create the filtering object pcl::ExtractIndices<pcl::PointXYZRGB> extract; int i = 0, nr_points = (int) cloud_filtered2->points.size (); // While 50% of the original cloud is still there while (cloud_filtered2->points.size () > 0.5 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered2); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } if( (fabs(coefficients->values[0]) < 0.02) && (fabs(coefficients->values[1]) < 0.02) && (fabs(coefficients->values[2]) > 0.9) ) { std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; height = coefficients->values[3]; // Extract the inliers extract.setInputCloud (cloud_filtered2); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_filtered3); // Transformation into ROS type //pcl::toPCLPointCloud2(*(cloud_filtered3), *(cloud_out)); //pcl_conversions::moveFromPCL(*(cloud_out), pc2); //debug_pub.publish(pc2); } // Create the filtering object extract.setNegative (true); extract.filter (*cloud_f); cloud_filtered2.swap (cloud_f); i++; } /* pass.setInputCloud (cloud_filtered2); pass.setFilterFieldName ("z"); pass.setFilterLimits (height, 1.5); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered2); */ // Transformation into ROS type //pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out)); //pcl_conversions::moveFromPCL(*(cloud_out), pc2); //debug_pub.publish(pc2); ///////////////////////////////// // Statistical Outlier Removal // ///////////////////////////////// pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud (cloud_filtered2); sor.setMeanK (50); sor.setStddevMulThresh (1.0); sor.filter (*cloud_filtered2); ////////////////////////////////// // Euclidian Cluster Extraction // ////////////////////////////////// // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>); tree->setInputCloud (cloud_filtered2); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (50); ec.setMaxClusterSize (5000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered2); ec.extract (cluster_indices); int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster1 (new pcl::PointCloud<pcl::PointXYZRGB>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster1->points.push_back (cloud_filtered2->points[*pit]); cloud_cluster1->width = cloud_cluster1->points.size (); cloud_cluster1->height = 1; cloud_cluster1->is_dense = true; cloud_cluster1->header.frame_id = "/map"; if(j == 0) { // Transformation into ROS type pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), pc2); debug_pub.publish(pc2); } else { // Transformation into ROS type pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), pc2); debug2_pub.publish(pc2); } j++; } // Transformation into ROS type //pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out)); //pcl_conversions::moveFromPCL(*(cloud_out), pc2); //debug2_pub.publish(pc2); //debug2_pub.publish(*pc_map); } }
bool service_callback(pcl_perception::PeopleDetectionSrv::Request &req, pcl_perception::PeopleDetectionSrv::Response &res) { //the node cannot be asked to detect people while someone has asked it to detect people if (node_state != IDLE) { ROS_WARN("[segbot_pcl_person_detector] Person detection service may not be called until previous call is processed"); return true; } node_state = DETECTING; ROS_INFO("[general_perception.cpp] Received command %s",req.command.c_str()); //command for detecting people standing up if (req.command == "reocrd_cloud") { PointCloudT::Ptr aggregated_cloud (new PointCloudT); bool collecting_clouds = true; int num_saved = 0; while (collecting_clouds) { //collect to grab cloud if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available { ROS_INFO("Adding next cloud..."); *aggregated_cloud += *cloud; num_saved++; if (num_saved > num_frames_for_detection) { collecting_clouds = false; } //unlock mutex and reset flag new_cloud_available_flag = false; } //ROS_INFO("unlocking mutex..."); cloud_mutex.unlock (); ros::spinOnce(); } if (useVoxelGridFilter) { pcl::PCLPointCloud2::Ptr pcl_pc (new pcl::PCLPointCloud2) ; pcl::toPCLPointCloud2( *aggregated_cloud,*pcl_pc); //filter each step pcl::VoxelGrid< pcl::PCLPointCloud2 > sor; sor.setInputCloud (pcl_pc); sor.setLeafSize (0.005, 0.005, 0.005); sor.filter (*pcl_pc); //covert back pcl::fromPCLPointCloud2(*pcl_pc, *aggregated_cloud); } if (useStatOutlierFilter) { // Create the filtering object pcl::StatisticalOutlierRemoval<PointT> sor; sor.setInputCloud (aggregated_cloud); sor.setMeanK (50); sor.setStddevMulThresh (1.0); sor.filter (*aggregated_cloud); } visualizeCloud(aggregated_cloud); /*current_frame_counter = 0; bool collecting_cloud = true; node_state = DETECTING; while (collecting_cloud){ if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available { ROS_INFO("Detecting on frame %i",current_frame_counter); new_cloud_available_flag = false; // Draw cloud and people bounding boxes in the viewer: //increment counter and see if we need to stop detecting current_frame_counter++; if (current_frame_counter > num_frames_for_detection){ collecting_cloud = false; } } cloud_mutex.unlock (); //spin to collect next point cloud ros::spinOnce(); if (visualize){ visualizeCloud(); } }*/ } node_state = IDLE; return true; }