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