void cloudCallback(const tf::TransformListener& listener, const PointCloud::ConstPtr& source_msg)
{
		if (modeType_ != m_recognize && modeType_ != m_track)
				{
						return;
				}

    if (source_msg->empty())
        {
            ROS_ERROR("Can't get source cloud message.\n");
            return;
        }

    if (inliers->indices.empty())
        {
            ROS_WARN_THROTTLE(5, "Object to grasp has not been found.\n");
            return;
        }

    PointCloud::Ptr cloud_in (new PointCloud());

    msgToCloud(source_msg, cloud_in);

    PointCloud::Ptr cloud_out (new PointCloud ());
    getCloudByInliers(cloud_in, cloud_out, inliers, false, false);

    MakePlan MP;
    pcl::PointXYZRGB avrPt;
    hasGraspPlan_ = MP.process(cloud_out, pa_, pb_, pc_, pd_, avrPt);

    if (hasGraspPlan_)
        {
            visualization_msgs::Marker marker;
            // Set the frame ID and timestamp.  See the TF tutorials for information on these.
            marker.header.frame_id = "/camera_yaw_frame";
            marker.header.stamp = pcl_conversions::fromPCL(source_msg->header.stamp);

            // Set the namespace and id for this marker.  This serves to create a unique ID
            // Any marker sent with the same namespace and id will overwrite the old one
            marker.ns = "grasp";
            marker.id = 0;

            marker.type = shape;
            marker.action = visualization_msgs::Marker::ADD;

            // transform the vector
            listener.waitForTransform("/camera_yaw_frame", source_msg->header.frame_id, ros::Time::now(), ros::Duration(1.0));

            tf::Stamped<tf::Point> p_in_a;
            tf::Stamped<tf::Point> p_out_a;
            p_in_a.setX(avrPt.x);
            p_in_a.setY(avrPt.y);
            p_in_a.setZ(avrPt.z);
            p_in_a.frame_id_ = source_msg->header.frame_id;

            listener.transformPoint("/camera_yaw_frame", p_in_a, p_out_a);

            tf::Stamped<tf::Point> p_in_b;
            tf::Stamped<tf::Point> p_out_b;

            float la, lb, lc;
            normalize(la, lb, lc, 0.15);

            p_in_b.setX(avrPt.x + la);
            p_in_b.setY(avrPt.y + lb);
            p_in_b.setZ(avrPt.z + lc);
            p_in_b.frame_id_ = source_msg->header.frame_id;

            listener.transformPoint("/camera_yaw_frame", p_in_b, p_out_b);

            // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
            marker.points.resize(2);

            marker.points[0].x = p_out_a.x() + 0.03;
            marker.points[0].y = p_out_a.y();
            marker.points[0].z = p_out_a.z();

            marker.points[1].x = p_out_b.x() + 0.03;
            marker.points[1].y = p_out_b.y();
            marker.points[1].z = p_out_b.z();

            // The point at index 0 is assumed to be the start point, and the point at index 1 is assumed to be the end.

            // scale.x is the shaft diameter, and scale.y is the head diameter. If scale.z is not zero, it specifies the head length.
            // Set the scale of the marker -- 1x1x1 here means 1m on a side
            marker.scale.x = 0.01;
            marker.scale.y = 0.015;
            marker.scale.z = 0.04;

            // Set the color -- be sure to set alpha to something non-zero!
            marker.color.r = 0.0f;
            marker.color.g = 1.0f;
            marker.color.b = 0.0f;
            marker.color.a = 1.0;

            markerPub_.publish(marker);
        }
    else
        {
            ROS_WARN_THROTTLE(5, "Failed to generate grasp plan.\n");
        }
}
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;
        }
}