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