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;
}
