void cloudCallback(const PointCloud::ConstPtr& source_msg) { if (source_msg->empty()) { ROS_ERROR("Can't get source cloud message.\n"); fType_ = t_null; return; } PointCloudWN::Ptr source_n (new PointCloudWN()); PointCloudWN::Ptr cloud_t (new PointCloudWN ()); Utilities::estimateNorCurv(source_msg, source_n); Utilities::rotateCloudXY(source_n, cloud_t, roll_, pitch_, transform_inv_); // std::cout << "trans_inv: "<< transform_inv_ << std::endl; FindPlane FP; FP.getParameters(GH); FP.findPlaneInCloud(cloud_t); PointCloudMono::Ptr plane_max (new PointCloudMono()); PointCloudMono::Ptr ground_max (new PointCloudMono()); pcl::ModelCoefficients::Ptr mcp (new pcl::ModelCoefficients); pcl::ModelCoefficients::Ptr mcg (new pcl::ModelCoefficients); processHullVector(FP.plane_hull, FP.plane_coeff, plane_max, mcp); processHullVector(FP.ground_hull, FP.ground_coeff, ground_max, mcg); if (!plane_max->empty() && (cloudPubPlaneMax_.getNumSubscribers() || posePubPlaneMax_.getNumSubscribers())) { sensor_msgs::PointCloud2 plane_max_msg; pcl::toROSMsg(*plane_max, plane_max_msg); plane_max_msg.header.frame_id = source_msg->header.frame_id; plane_max_msg.header.stamp = pcl_conversions::fromPCL(source_msg->header.stamp); geometry_msgs::PoseStamped pose_plane_max_msg; pose_plane_max_msg.header = plane_max_msg.header; pose_plane_max_msg.pose.orientation.x = mcp->values[0]; pose_plane_max_msg.pose.orientation.y = mcp->values[1]; pose_plane_max_msg.pose.orientation.z = mcp->values[2]; pose_plane_max_msg.pose.orientation.w = mcp->values[3]; cloudPubPlaneMax_.publish(plane_max_msg); posePubPlaneMax_.publish(pose_plane_max_msg); } if (!ground_max->empty() && (cloudPubGround_.getNumSubscribers() || posePubGround_.getNumSubscribers())) { sensor_msgs::PointCloud2 ground_max_msg; pcl::toROSMsg(*ground_max, ground_max_msg); ground_max_msg.header.frame_id = source_msg->header.frame_id; ground_max_msg.header.stamp = pcl_conversions::fromPCL(source_msg->header.stamp); geometry_msgs::PoseStamped pose_ground_msg; pose_ground_msg.header = ground_max_msg.header; pose_ground_msg.pose.orientation.x = mcg->values[0]; pose_ground_msg.pose.orientation.y = mcg->values[1]; pose_ground_msg.pose.orientation.z = mcg->values[2]; pose_ground_msg.pose.orientation.w = mcg->values[3]; cloudPubGround_.publish(ground_max_msg); posePubGround_.publish(pose_ground_msg); } if (!plane_max->empty() && !ground_max->empty()) { fType_ = t_both; } else if (plane_max->empty() && !ground_max->empty()) { fType_ = t_ground; } else if (!plane_max->empty() && ground_max->empty()) { fType_ = t_table; } else { fType_ = t_null; } }
int main(int argc, char *argv[]) { double mcp(double n); printf("--- %.1f\n", mcp(N)); return 0; }