Where is the interface of transfering the object position?
Hi all, I'm confusing in failed to find the interface in pickandplaceactionserver.cpp(in turtlebotarm package) of inputting the object position that is generated by function "pickandplacesub" in "blockdetectionactionserver.cpp".But in "blockdetectionactionserver.cpp" I found a subscribe function "pickandplacesub_ = nh.subscribe(goal->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this)". I don't what the use of "goal" for in it. Pls offer me a hand, thanks a lot!
Here is the code of blockdetectionaction_server.cpp:
include
include
include
include
include
include
include
include
include
include
include
include
include
include
include
include
include
include
include
include
namespace turtlebotblockmanipulation {
class BlockDetectionServer { private:
ros::NodeHandle nh; actionlib::SimpleActionServer<turtlebotblockmanipulation::BlockDetectionAction> as; std::string actionname; turtlebotblockmanipulation::BlockDetectionFeedback feedback; turtlebotblockmanipulation::BlockDetectionResult result; turtlebotblockmanipulation::BlockDetectionGoalConstPtr goal; ros::Subscriber sub; ros::Publisher pub_;
tf::TransformListener tflistener;
// Parameters from goal std::string armlink; double blocksize; double table_height;
ros::Publisher blockpub;
// Parameters from node
public: BlockDetectionServer(const std::string name) : nh("~"), as(name, false), actionname(name) { // Load parameters from the server.
// Register the goal and feeback callbacks.
as_.registerGoalCallback(boost::bind(&BlockDetectionServer::goalCB, this));
as_.registerPreemptCallback(boost::bind(&BlockDetectionServer::preemptCB, this));
as_.start();
// Subscribe to point cloud
sub_ = nh_.subscribe("/camera/depth_registered/points", 1, &BlockDetectionServer::cloudCb, this);
pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("block_output", 1);
block_pub_ = nh_.advertise< geometry_msgs::PoseArray >("/turtlebot_blocks", 1, true);
}
void goalCB() { ROSINFO("[block detection] Received goal!"); // accept the new goal result.blocks.poses.clear();
goal_ = as_.acceptNewGoal();
block_size = goal_->block_size;
table_height = goal_->table_height;
arm_link = goal_->frame;
result_.blocks.header.frame_id = arm_link;
}
void preemptCB() { ROSINFO("%s: Preempted", actionname.cstr()); // set the action state to preempted as_.setPreempted(); }
void cloudCb ( const sensormsgs::PointCloud2ConstPtr& msg ) { // Only do this if we're actually actively working on a goal. if (!as.isActive()) return;
result_.blocks.header.stamp = msg->header.stamp;
// convert to PCL
pcl::PointCloud<pcl::PointXYZRGB> cloud;
pcl::fromROSMsg (*msg, cloud);
// transform to whatever frame we're working in, probably the arm frame.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZRGB>);
tf_listener_.waitForTransform(std::string(arm_link), cloud.header.frame_id, cloud.header.stamp, ros::Duration(1.0));
if (!pcl_ros::transformPointCloud (std::string(arm_link), cloud, *cloud_transformed, tf_listener_))
{
ROS_ERROR ("Error converting to desired frame");
return;
}
// Create the segmentation object for the planar model and set all the parameters
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (200);
seg.setDistanceThreshold (0.005);
// Limit to things we think are roughly at the table height.
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud(cloud_transformed);
pass.setFilterFieldName("z");
pass.setFilterLimits(table_height - 0.05, table_height + block_size + 0.05);
pass.filter(*cloud_filtered);
if( cloud_filtered->points.size() == 0 ){
ROS_ERROR("0 points left");
return;
}else
ROS_INFO("Filtered, %d points left", (int) cloud_filtered->points.size());
int nr_points = cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
return;
}
std::cout << "Inliers: " << (inliers->indices.size ()) << std::endl;
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered);
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.005);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud( cloud_filtered);
ec.extract (cluster_indices);
pub_.publish(cloud_filtered);
// for each cluster, see if it is a block
for (size_t c = 0; c < cluster_indices.size (); ++c)
{
// find the outer dimensions of the cluster
float xmin = 0; float xmax = 0; float ymin = 0; float ymax = 0;
float zmin = 0; float zmax = 0;
for (size_t i = 0; i < cluster_indices[c].indices.size(); i++)
{
int j = cluster_indices[c].indices[i];
float x = cloud_filtered->points[j].x;
float y = cloud_filtered->points[j].y;
float z = cloud_filtered->points[j].z;
if (i == 0)
{
xmin = xmax = x;
ymin = ymax = y;
zmin = zmax = z;
}
else
{
xmin = std::min(xmin, x);
xmax = std::max(xmax, x);
ymin = std::min(ymin, y);
ymax = std::max(ymax, y);
zmin = std::min(zmin, z);
zmax = std::max(zmax, z);
}
}
// Check if these dimensions make sense for the block size specified
float xside = xmax-xmin;
float yside = ymax-ymin;
float zside = zmax-zmin;
const float tol = 0.01; // 1 cm error tolerance
// In order to be part of the block, xside and yside must be between
// blocksize and blocksize*sqrt(2)
// z must be equal to or smaller than blocksize
if (xside > block_size-tol && xside < block_size*sqrt(2)+tol &&
yside > block_size-tol && yside < block_size*sqrt(2)+tol &&
zside > tol && zside < block_size+tol)
{
// If so, then figure out the position and the orientation of the block
float angle = atan(block_size/((xside+yside)/2));
if (yside < block_size)
angle = 0.0;
ROS_INFO_STREAM("xside: " << xside << " yside: " << yside << " zside " << zside << " angle: " << angle);
// Then add it to our set
ROS_INFO("Adding a new block!");
addBlock( xmin+(xside)/2.0, ymin+(yside)/2.0, zmax - block_size/2.0, angle);
}
}
if (result_.blocks.poses.size() > 0)
{
as_.setSucceeded(result_);
block_pub_.publish(result_.blocks);
ROS_INFO("[block detection] Set as succeeded!");
}
else
ROS_INFO("[block detection] Couldn't find any blocks this iteration!");
// as_.setAborted(result_);
}
void addBlock(float x, float y, float z, float angle) { geometrymsgs::Pose blockpose; blockpose.position.x = x; blockpose.position.y = y; block_pose.position.z = z;
Eigen::Quaternionf quat(Eigen::AngleAxis<float>(angle, Eigen::Vector3f(0,0,1)));
block_pose.orientation.x = quat.x();
block_pose.orientation.y = quat.y();
block_pose.orientation.z = quat.z();
block_pose.orientation.w = quat.w();
result_.blocks.poses.push_back(block_pose);
}
};
};
int main(int argc, char** argv) { ros::init(argc, argv, "blockdetectionaction_server");
turtlebotblockmanipulation::BlockDetectionServer server("block_detection"); ros::spin();
return 0; }
Here is the code of pickandplaceactionserver.cpp:
include
include
include
include
include
include
include
namespace turtlebotblockmanipulation {
class PickAndPlaceServer { private:
ros::NodeHandle nh; actionlib::SimpleActionServer<turtlebotblockmanipulation::PickAndPlaceAction> as; std::string actionname;
turtlebotblockmanipulation::PickAndPlaceFeedback feedback; turtlebotblockmanipulation::PickAndPlaceResult result; turtlebotblockmanipulation::PickAndPlaceGoalConstPtr goal_;
actionlib::SimpleActionClient
ros::Subscriber pickandplacesub;
// Parameters from goal std::string armlink; double gripperopen; double gripperclosed; double zup; public: PickAndPlaceServer(const std::string name) : nh("~"), as(name, false), actionname(name), client("/movearm", true) {
//register the goal and feeback callbacks
as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));
as_.start();
}
void goalCB() { ROSINFO("[pick and place] Received goal!"); goal = as.acceptNewGoal(); armlink = goal->frame; gripperopen = goal->gripperopen; gripperclosed = goal->gripperclosed; zup = goal->zup;
if (goal_->topic.length() < 1)
pickAndPlace(goal_->pickup_pose, goal_->place_pose);
else
pick_and_place_sub_ = nh_.subscribe(goal_->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this);
}
void sendGoalFromTopic(const geometrymsgs::PoseArrayConstPtr& msg) { ROSINFO("[pick and place] Got goal from topic! %s", goal->topic.cstr()); pickAndPlace(msg->poses[0], msg->poses[1]); pickandplacesub.shutdown(); }
void preemptCB() { ROSINFO("%s: Preempted", actionname.cstr()); // set the action state to preempted as_.setPreempted(); }
void pickAndPlace(const geometrymsgs::Pose& startpose, const geometrymsgs::Pose& endpose) { ROS_INFO("[pick and place] Picking. Also placing.");
simple_arm_server::MoveArmGoal goal;
simple_arm_server::ArmAction action;
simple_arm_server::ArmAction grip;
/* open gripper */
grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
grip.move_time.sec = 1.0;
grip.command = gripper_open;
goal.motions.push_back(grip);
/* arm straight up */
tf::Quaternion temp;
temp.setRPY(0,1.57,0);
action.goal.orientation.x = temp.getX();
action.goal.orientation.y = temp.getY();
action.goal.orientation.z = temp.getZ();
action.goal.orientation.w = temp.getW();
/* hover over */
action.goal.position.x = start_pose.position.x;
action.goal.position.y = start_pose.position.y;
action.goal.position.z = z_up;
action.move_time.sec = 0.25;
goal.motions.push_back(action);
/* go down */
action.goal.position.z = start_pose.position.z;
action.move_time.sec = 1.5;
goal.motions.push_back(action);
/* close gripper */
grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
grip.command = gripper_closed;
grip.move_time.sec = 1.0;
goal.motions.push_back(grip);
/* go up */
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
/* hover over */
action.goal.position.x = end_pose.position.x;
action.goal.position.y = end_pose.position.y;
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
/* go down */
action.goal.position.z = end_pose.position.z;
action.move_time.sec = 1.5;
goal.motions.push_back(action);
/* open gripper */
grip.command = gripper_open;
goal.motions.push_back(grip);
/* go up */
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
goal.header.frame_id = arm_link;
client_.sendGoal(goal);
ROS_INFO("[pick and place] Sent goal. Waiting.");
client_.waitForResult(ros::Duration(30.0));
ROS_INFO("[pick and place] Received result.");
if (client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
as_.setSucceeded(result_);
else
{
ROS_INFO("Actual state: %s", client_.getState().toString().c_str());
as_.setAborted(result_);
}
} };
};
int main(int argc, char** argv) { ros::init(argc, argv, "pickandplaceactionserver");
turtlebotblockmanipulation::PickAndPlaceServer server("pickandplace"); ros::spin();
return 0; }
Asked by Yantian_Zha on 2014-04-17 03:38:09 UTC
Comments