/* ========================================
* Fill Code: SERVICE
* ========================================*/
bool filterCallback(lesson_perception::FilterCloud::Request& request,
lesson_perception::FilterCloud::Response& response)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (request.pcdfilename.empty())
{
pcl::fromROSMsg(request.input_cloud, *cloud);
ROS_INFO_STREAM("cloud size: " <<cloud->size());
if (cloud->empty())
{
ROS_ERROR("input cloud empty");
response.success = false;
return false;
}
}
else
{
pcl::io::loadPCDFile(request.pcdfilename, *cloud);
}
switch (request.operation)
{
case lesson_perception::FilterCloud::Request::VOXELGRID :
{
filtered_cloud = voxelGrid(cloud, 0.01);
break;
}
default :
{
ROS_ERROR("no point cloud found");
return false;
}
}
/*
* SETUP RESPONSE
*/
pcl::toROSMsg(*filtered_cloud, response.output_cloud);
response.output_cloud.header=request.input_cloud.header;
response.output_cloud.header.frame_id="kinect_link";
response.success = true;
return true;
}
void pcCallback(const sensor_msgs::PointCloud2::Ptr &msg)
{
//sensor_msgs to point cloud
sensor_msgs::PointCloud2::Ptr cloud = msg;
rgbcloudPtr convert_cloud(new rgbcloud);
pcl::fromROSMsg(*cloud, *convert_cloud);
//点群削減
rgbcloudPtr vf_cloud(new rgbcloud);
vf_cloud = voxelGrid(convert_cloud);
rgbcloudPtr bf_cloud(new rgbcloud);
// if(vf_cloud->empty()){}
// else{
// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr flann(new pcl::search::KdTree<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::Normal>::Ptr normals_cloud(new pcl::PointCloud<pcl::Normal>);
// pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> norm_est;
// norm_est.setSearchMethod(flann);
// norm_est.setInputCloud(vf_cloud);
// norm_est.setRadiusSearch(0.02);
// norm_est.compute(*normals_cloud);
//
// pcl::BoundaryEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::Boundary> boundary_est;
// pcl::BoundaryEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::Boundary>::PointCloudOut boundary;
// boundary_est.setRadiusSearch(0.02);
// boundary_est.setInputNormals(normals_cloud);
//
// boundary_est.setSearchMethod(flann);
// boundary_est.setInputCloud(vf_cloud);
// boundary_est.setAngleThreshold(M_PI/6);
// boundary_est.compute(boundary);
//
// for(int i = 0; i < boundary.size(); i++){
// if(boundary.points[i].boundary_point == 0){
// bf_cloud->push_back(vf_cloud->points.at(i));
// }else{
// pcl::PointXYZRGB pt = vf_cloud->points.at(i);
// pt.x = NAN;
// pt.y = NAN;
// pt.z = NAN;
// bf_cloud->push_back(pt);
// }
// }
// }
//平面検出＋平面除去
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
rgbcloudPtr extract_cloud(new rgbcloud);
rgbcloudPtr plane_cloud(new rgbcloud);
extract_cloud = extractindices(vf_cloud, inliers, coefficients, true);
for(int i= 0; i < 7; i++){
planeSegmentation(vf_cloud, inliers, coefficients);
extract_cloud = extractindices(extract_cloud, inliers, coefficients, true);
//plane_cloud = extractindices(vf_cloud, inliers, coefficients, false);
}
//その他の点の除去
// rgbcloudPtr removed_outlier_cloud(new rgbcloud);
// removed_outlier_cloud = removedCloud(extract_cloud, inliers);
//クラスタリング
std::vector<pcl::PointIndices> clusters;
//clusters = regiongrowing(extract_cloud);
clusters = euclideanCluster(extract_cloud);
//std::cout<<"clusters : "<<clusters.size()<<std::endl;
//std::cout<<"Point Size :"<<extract_cloud->size()<<std::endl;
rgbcloudPtr tempcloud(new rgbcloud);
int i = 0;
for(std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it){
for(std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit){
tempcloud->points.push_back(extract_cloud->points[*pit]);
if(tempcloud->points.size() > 300){
pcl::PointXYZRGB pt = extract_cloud->points.at(i);
pt.x = NAN;
pt.y = NAN;
pt.z = NAN;
tempcloud->push_back(pt);
}
tempcloud->width = tempcloud->size();
tempcloud->height = 1;
tempcloud->is_dense = true;
}
}
//重心計算
extract_cloud->header.frame_id = msg->header.frame_id;
Eigen::Vector4f centroid;
//centroid = centroidCompute(tempcloud, clusters);
geometry_msgs::PoseArray objectpoint;
geometry_msgs::Pose objectpose;
objectpoint.header.frame_id = msg->header.frame_id;
objectpoint.header.stamp = ros::Time::now();
for(int i =0; i < clusters.size(); i++){
pcl::compute3DCentroid(*extract_cloud, clusters[i].indices, centroid);
std::cout<<"error no"<<std::endl;
objectpose.position.x = centroid[0];
objectpose.position.y = centroid[1];
objectpose.position.z = centroid[2];
std::cout<<"[x y z] = "<<" ["<<i<<" ] "<<"["<<objectpose.position.x<< " , "<<objectpose.position.y <<" , "<<objectpose.position.z <<"]"<<std::endl;
objectpoint.poses.push_back(objectpose);
point_pub.publish(objectpoint);
}
//ビジュアライズ用
for(size_t i = 0; i < clusters.size(); i++){
mark_cluster(extract_cloud, centroid, i);
}
std::cout<<"tempcloud"<<tempcloud->width<<" : "<<tempcloud->height<<std::endl;
std::cout<<"extract_cloud"<<extract_cloud->width<<" : "<<extract_cloud->height<<std::endl;
#ifdef TEST
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*tempcloud, output);
std::cout<<"stop"<<std::endl;
output.header.frame_id = msg->header.frame_id;
pub.publish(output);
#endif
}
