AntoineH/BaxterInterface
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Detection du centre de l'objet

Browse source
This commit is contained in:
Unknown 2018-06-29 14:33:14 +02:00
parent 77424943e1
commit 656305cd8a
1 changed files with 52 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

54
my_pcl_tutorial/src/plannar_segmentation.cpp
View file

@ -11,7 +11,24 @@
#include <pcl/filters/extract_indices.h>
#include <pcl/common/centroid.h>
#include <geometry_msgs/PointStamped.h>
#include <pcl/pcl_config.h>
// void printinfo()
// {
// std::cout<<"OK"<<std::endl;
// #if PCL_VERSION_COMPARE(<, 1, 7, 2)
// std::cout<<"ARG"<<std::endl;
// #endif
// };
ros::Publisher pub;
ros::Publisher pubc;
using namespace pcl;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
@ -40,7 +57,7 @@ cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
ROS_INFO_STREAM("Coeff : "<<coefficients->values[0]<<" / "<<coefficients->values[1]<<" / "<<coefficients->values[2]<<" / "<<coefficients->values[3]);
//ROS_INFO_STREAM("Coeff : "<<coefficients->values[0]<<" / "<<coefficients->values[1]<<" / "<<coefficients->values[2]<<" / "<<coefficients->values[3]);
//Créations de l'output
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
@ -60,14 +77,45 @@ cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
extract.setNegative(true);
extract.filter(*pcl_output);
//Calcul du centre
// CentroidPoint<pcl::PointXYZ> centroid;
// for (size_t i = 0; i < pcl_output->size (); ++i)
// {
// centroid.add(*pcl_output[i]);
// }
// pcl::PointXYZ c1;
// centroid.get (c1);
Eigen::Vector4f barycentre;
pcl::compute3DCentroid(*pcl_output, barycentre);
geometry_msgs::PointStamped center_msg;
// for (size_t i = 0; i < pcl_output->size (); ++i)
// {
// if( pcl_output->points[i].x)
// center.point.x += pcl_output->points[i]._PointXYZ::data[0];
// center.point.y += pcl_output->points[i]._PointXYZ::data[1];
// center.point.z += pcl_output->points[i]._PointXYZ::data[2];
// ROS_INFO_STREAM("Center clioyf : "<<pcl_output->points[i].x<<" / "<<pcl_output->points[i].y<<" / "<<pcl_output->points[i].z);
// }
// center.point.x = center.point.x/pcl_output->size ();
// center.point.y = center.point.y/pcl_output->size ();
// center.point.z = center.point.z/pcl_output->size ();
center_msg.point.x = barycentre[0];
center_msg.point.y = barycentre[1];
center_msg.point.z = barycentre[2];
center_msg.header.frame_id = input->header.frame_id;
// ROS_INFO_STREAM("Center clioyf : "<<pcl_output->points[0]._PointXYZ::data[0]<<" / "<<pcl_output->points[0]._PointXYZ::data[1]<<" / "<<pcl_output->points[0]._PointXYZ::data[2]);
// ROS_INFO_STREAM("barycentre : "<<barycentre(1,1)<<" / "<<barycentre(2,1)<<" / "<<barycentre(3,1)<<" / "<< barycentre(4,1));
ROS_INFO_STREAM("Center : "<<center_msg.point.x<<" / "<<center_msg.point.y<<" / "<<center_msg.point.z);
//Envoie du nuage de points
pcl::toPCLPointCloud2(*pcl_output,pcl_pc2);
pcl_conversions::fromPCL(pcl_pc2, output);
output.header.frame_id = input->header.frame_id;
// Publish the data.
pub.publish (output);
pubc.publish( center_msg);
}
int main (int argc, char** argv)
@ -81,7 +129,9 @@ int main (int argc, char** argv)
// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> ("/plane_output", 1);
pubc = nh.advertise<geometry_msgs::PointStamped> ("/object_center", 1);
// printinfo();
// Spin
ros::spin ();
}