calc_grasppoints_action_client.cpp
Go to the documentation of this file.
00001 /*
00002  * David Fischinger - Vienna University of Technology
00003  * March 2015
00004  *
00005  * ** Grasp Action Client - for Testing **
00006  *
00007  * This node is serving as a client for the calculation of grasp points
00008  * based on the action server for grasp calculation.
00009  * This nodes subscribes to the topic /haf_grasping/depth_registered/single_cloud/points_in_lcs (lcs: local coordinate system)
00010  * and receives (single) point clouds (in a local coordinate system where z is perpendicular to the floor).
00011  * The node sends the point cloud as an action server goal and receives the grasp result.
00012  *
00013  */
00014 
00015 
00016 //ROS includes
00017 #include <ros/ros.h>
00018 #include <ros/package.h>
00019 #include "sensor_msgs/PointCloud2.h"
00020 #include <geometry_msgs/Point.h>
00021 #include <geometry_msgs/Vector3.h>
00022 //actionlib
00023 #include <actionlib/client/simple_action_client.h>
00024 #include <actionlib/client/terminal_state.h>
00025 //grasp action message
00026 #include <haf_grasping/CalcGraspPointsServerAction.h>
00027 // service messages
00028 #include <haf_grasping/GraspSearchCenter.h>
00029 #include <haf_grasping/GraspSearchRectangleSize.h>
00030 #include <haf_grasping/GraspCalculationTimeMax.h>
00031 #include <haf_grasping/GraspApproachVector.h>
00032 #include <haf_grasping/ShowOnlyBestGrasp.h>
00033 #include <haf_grasping/GraspPreGripperOpeningWidth.h>
00034 // for reading pcd file
00035 #include <iostream>
00036 #include <pcl/io/pcd_io.h>
00037 #include <pcl/point_types.h>
00038 #include "pcl_ros/transforms.h"
00039 #include <pcl_ros/point_cloud.h>
00040 //#include "pcl/io/io.h"
00041 //#include <boost/thread/thread.hpp>
00042 
00043 
00044 
00045 class CCalcGrasppointsClient
00046 {
00047 public:
00048         ros::Subscriber pc_sub;                                                         //subscriber for pointcloud
00049         ros::Subscriber pcd_sub;                                                        //subscriber for path for pcd-file (to read)
00050         ros::ServiceServer srv_set_grasp_center;                        // service to set new grasp center (center of rectangles where grasps are searched for)
00051         ros::ServiceServer srv_set_grasp_search_area_size;      // service to set size of rectangle where grasps are searched
00052         ros::ServiceServer srv_set_grasp_calculation_time_max;  // service to set maximal grasp calculation time (sec) before result is returned
00053         ros::ServiceServer srv_set_approach_vector;                     // service to set approach vector for grasping (only direction hand is approaching, not roll angle)
00054         ros::ServiceServer srv_set_show_only_best_grasp;        // service to set show_only_best_grasp bool variable
00055         ros::ServiceServer srv_set_gripper_width;                       // service to set factor f that is used for scaling point cloud to imitate pre-gripper opening width of 1/f
00056         geometry_msgs::Point graspsearchcenter;                         // center for searching for grasps
00057         geometry_msgs::Vector3 approach_vector;                         // defines the direction from where a grasp should be executed
00058         int grasp_search_size_x;                        // the size (x direction) where grasps are really calculated (in each direction 7cm more are needed for feature calculation!
00059         int grasp_search_size_y;                        // the size (y direction) where grasps are really calculated (in each direction 7cm more are needed for feature calculation!
00060         int max_grasp_search_size_x;    // x-limit for grasp search area size
00061         int max_grasp_search_size_y;    // y-limit for grasp search area size
00062         ros::Duration grasp_calculation_time_max;       //max time used for grasp calculation (sec) before result is returned
00063         bool show_only_best_grasp;
00064         std::string base_frame_default;
00065         int gripper_opening_width;                      //defines pre-grasp gripper opening width
00066 
00067         void get_grasp_cb(const sensor_msgs::PointCloud2ConstPtr& pc_in);
00068         void open_pcd_and_trig_get_grasp_cb(std_msgs::String pcd_path);
00069         bool set_grasp_center(haf_grasping::GraspSearchCenter::Request &req, haf_grasping::GraspSearchCenter::Response &res);
00070         bool set_grasp_search_area_size(haf_grasping::GraspSearchRectangleSize::Request &req, haf_grasping::GraspSearchRectangleSize::Response &res);
00071         bool set_grasp_calculation_time_max(haf_grasping::GraspCalculationTimeMax::Request &req, haf_grasping::GraspCalculationTimeMax::Response &res);
00072         bool set_approach_vector(haf_grasping::GraspApproachVector::Request &req, haf_grasping::GraspApproachVector::Response &res);
00073         bool set_show_only_best_grasp(haf_grasping::ShowOnlyBestGrasp::Request &req, haf_grasping::ShowOnlyBestGrasp::Response &res);
00074         bool set_gripper_width(haf_grasping::GraspPreGripperOpeningWidth::Request &req, haf_grasping::GraspPreGripperOpeningWidth::Response &res);
00075 
00076         CCalcGrasppointsClient(ros::NodeHandle nh_)
00077         {
00078                 //define center of grasp search rectangle (respectively take from launch file)
00079                 this->graspsearchcenter.x = this->graspsearchcenter.y = this->graspsearchcenter.z = 0.0;        //default values
00080                 std::vector<float> grasp_search_center_tmp;
00081                 if ( nh_.getParam("grasp_search_center", grasp_search_center_tmp) ) {
00082                         this->graspsearchcenter.x = grasp_search_center_tmp[0];
00083                         this->graspsearchcenter.y = grasp_search_center_tmp[1];
00084                         this->graspsearchcenter.z = grasp_search_center_tmp[2];
00085                 }
00086                 //define grasp approach direction (respectively take from launch file)
00087                 this->approach_vector.x = this->approach_vector.y = 0.0;
00088                 this->approach_vector.z = 1.0;
00089                 std::vector<float> gripper_approach_vector_tmp;
00090                 if ( nh_.getParam("gripper_approach_vector", gripper_approach_vector_tmp) ) {
00091                         this->approach_vector.x = gripper_approach_vector_tmp[0];
00092                         this->approach_vector.y = gripper_approach_vector_tmp[1];
00093                         this->approach_vector.z = gripper_approach_vector_tmp[2];
00094                 }
00095 
00096                 //define size of grasp search rectangle (respectively take from launch file)
00097                 nh_.param("grasp_search_size_x", this->grasp_search_size_x, 18);        //default value = max. limit 32
00098                 nh_.param("grasp_search_size_y", this->grasp_search_size_y, 30);        //default value = max. limit 44
00099                 this->max_grasp_search_size_x = 18;                             //max. limit 32-14=18
00100                 this->max_grasp_search_size_y = 30;                             //max. limit 44-14=30
00101                 if (this->grasp_search_size_x < 1 or this->grasp_search_size_x > this->max_grasp_search_size_x)
00102                         this->grasp_search_size_x = this->max_grasp_search_size_x;
00103                 if (this->grasp_search_size_y < 1 or this->grasp_search_size_y > this->max_grasp_search_size_y)
00104                         this->grasp_search_size_y = this->max_grasp_search_size_y;
00105 
00106                 // define maximal time before grasp result is returned
00107                 int max_calculation_time;
00108                 nh_.param("max_calculation_time", max_calculation_time, 50);    // in sec, with default value 50
00109                 this->grasp_calculation_time_max = ros::Duration(max_calculation_time);
00110 
00111                 // define if only the best grasp should be visualized (respectively take bool value from launch file)
00112                 nh_.param("show_only_best_grasp", this->show_only_best_grasp, true);
00113 
00114                 // set default cloud frame (if cloud is generated from pcd)
00115 
00116                 nh_.param("base_frame", this->base_frame_default, std::string("base_link"));
00117                 // set gripper opening with factor => 1/gripper opening width is tested
00118                 nh_.param("gripper_width", this->gripper_opening_width, 1);
00119 
00120 
00121                 //subscriber for the point cloud
00122                 std::string input_pc_topic = "/haf_grasping/depth_registered/single_cloud/points_in_lcs";
00123                 nh_.param("input_pc_topic", input_pc_topic, input_pc_topic);
00124                 this->pc_sub = nh_.subscribe(input_pc_topic,1, &CCalcGrasppointsClient::get_grasp_cb, this);
00125                 this->pcd_sub = nh_.subscribe("/haf_grasping/input_pcd_rcs_path",1, &CCalcGrasppointsClient::open_pcd_and_trig_get_grasp_cb, this);
00126                 //services for setting parameters
00127                 this->srv_set_grasp_center = nh_.advertiseService("/haf_grasping/set_grasp_center", &CCalcGrasppointsClient::set_grasp_center,this);
00128                 this->srv_set_grasp_search_area_size = nh_.advertiseService("/haf_grasping/set_grasp_search_area_size", &CCalcGrasppointsClient::set_grasp_search_area_size,this);
00129                 this->srv_set_grasp_calculation_time_max = nh_.advertiseService("/haf_grasping/set_grasp_calculation_time_max", &CCalcGrasppointsClient::set_grasp_calculation_time_max,this);
00130                 this->srv_set_approach_vector = nh_.advertiseService("/haf_grasping/set_approach_vector", &CCalcGrasppointsClient::set_approach_vector, this);
00131                 this->srv_set_show_only_best_grasp = nh_.advertiseService("/haf_grasping/set_show_only_best_grasp", &CCalcGrasppointsClient::set_show_only_best_grasp, this);
00132                 this->srv_set_gripper_width = nh_.advertiseService("/haf_grasping/set_gripper_opening_width", &CCalcGrasppointsClient::set_gripper_width, this);
00133         }
00134 };
00135 
00136 // open pcd file for given path and start get_grasp_cb (that triggers grasp calculation)
00137 void CCalcGrasppointsClient::open_pcd_and_trig_get_grasp_cb(std_msgs::String pcd_path)
00138 {
00139   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00140 
00141   if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcd_path.data.c_str(), *cloud) == -1) //* load the file
00142   {
00143     PCL_ERROR ("Couldn't read input file (pcd) \n");
00144     return;
00145   }
00146   std::cout << "Loaded "
00147             << cloud->width * cloud->height
00148             << " data points from " << pcd_path.data.c_str();
00149 
00150   //transform pcl to ros-msg
00151   sensor_msgs::PointCloud2 pcd_as_ros_msg;// = new sensor_msgs::PointCloud2ConstPtr();
00152   pcl::toROSMsg(*cloud, pcd_as_ros_msg);
00153   pcd_as_ros_msg.header.frame_id = this->base_frame_default /*"base_link"*/;
00154   pcd_as_ros_msg.header.stamp = ros::Time(0);
00155   const sensor_msgs::PointCloud2ConstPtr pcd_as_ros_msg_const_ptr = boost::make_shared<sensor_msgs::PointCloud2>(pcd_as_ros_msg);
00156   CCalcGrasppointsClient::get_grasp_cb(pcd_as_ros_msg_const_ptr);
00157 }
00158 
00159 // define goal (input) for grasp calculation, send it to grasp action server and receive result
00160 void CCalcGrasppointsClient::get_grasp_cb(const sensor_msgs::PointCloud2ConstPtr& pc_in)
00161 {
00162         ROS_INFO("\nFrom calc_grasppoints_action_client: point cloud received");
00163 
00164         // create the action client
00165         // true causes the client to spin its own thread
00166         actionlib::SimpleActionClient<haf_grasping::CalcGraspPointsServerAction> ac("calc_grasppoints_svm_action_server", true);
00167 
00168         ROS_INFO("Waiting for action server to start.");
00169         // wait for the action server to start
00170         ac.waitForServer(); //will wait for infinite time
00171 
00172         ROS_INFO("Action server started, sending goal.");
00173         // send a goal to the action
00174         haf_grasping::CalcGraspPointsServerGoal goal;
00175         goal.graspinput.input_pc = *pc_in;
00176 
00177         goal.graspinput.grasp_area_center = this->graspsearchcenter;
00178 
00179         //set grasp approach vector
00180         goal.graspinput.approach_vector = this->approach_vector;
00181 
00182         // set size of grasp search area
00183         goal.graspinput.grasp_area_length_x = this->grasp_search_size_x+14;
00184         goal.graspinput.grasp_area_length_y = this->grasp_search_size_y+14;
00185 
00186         // set max grasp calculation time
00187         goal.graspinput.max_calculation_time = this->grasp_calculation_time_max;
00188 
00189         // set if only best grasp should be visualized
00190         goal.graspinput.show_only_best_grasp = this->show_only_best_grasp;
00191 
00192         // set pre-grasp gripper opening width (factor for scaling pc)
00193         goal.graspinput.gripper_opening_width = this->gripper_opening_width;
00194 
00195         //send goal
00196         ac.sendGoal(goal);
00197 
00198         //wait for the action to return
00199         bool finished_before_timeout = ac.waitForResult(ros::Duration(50.0));
00200 
00201         if (finished_before_timeout)
00202         {
00203             actionlib::SimpleClientGoalState state = ac.getState();
00204             boost::shared_ptr<const haf_grasping::CalcGraspPointsServerResult_<std::allocator<void> > > result = ac.getResult();
00205             ROS_INFO_STREAM("Result: %s" << (*(result)).graspOutput);
00206             ROS_INFO("Action finished: %s",state.toString().c_str());
00207         }
00208         else
00209             ROS_INFO("Action did not finish before the time out.");
00210 }
00211 
00212 
00213 //set grasp search center
00214 bool CCalcGrasppointsClient::set_grasp_center(haf_grasping::GraspSearchCenter::Request &req,
00215                 haf_grasping::GraspSearchCenter::Response &res)
00216 {
00217         //set grasp search center
00218         this->graspsearchcenter.x = req.graspsearchcenter.x;
00219         this->graspsearchcenter.y = req.graspsearchcenter.y;
00220         this->graspsearchcenter.z = req.graspsearchcenter.z;
00221         ROS_INFO("Set grasp search center to: x=%f, y=%f, z=%f", req.graspsearchcenter.x, req.graspsearchcenter.y, req.graspsearchcenter.z);
00222         res.result = true;
00223         ROS_INFO("sending back response: [%ld]", (long int)res.result);
00224         return res.result;
00225 }
00226 
00227 //set size of rectangle where grasps are searched
00228 bool CCalcGrasppointsClient::set_grasp_search_area_size(haf_grasping::GraspSearchRectangleSize::Request &req,
00229                 haf_grasping::GraspSearchRectangleSize::Response &res)
00230 {
00231         //set grasp search rectangle size
00232         if (req.grasp_search_size_x >= 0 and req.grasp_search_size_x <= this->max_grasp_search_size_x){
00233                 this->grasp_search_size_x = req.grasp_search_size_x;
00234                 ROS_INFO("Set grasp rectangle size to: x=%ld", (long int)req.grasp_search_size_x);
00235         } else {
00236                 ROS_INFO("Could not set grasp rectangle size for x. Allowed values: [0, %ld ]. Received value was: x=%ld", (long int)this->max_grasp_search_size_x,(long int)req.grasp_search_size_x);
00237                 res.result = false;
00238                 return res.result;
00239         }
00240         if (req.grasp_search_size_y >= 0 and req.grasp_search_size_y <= this->max_grasp_search_size_y){
00241                 this->grasp_search_size_y = req.grasp_search_size_y;
00242                 ROS_INFO("Set grasp rectangle size to: y=%ld", (long int)req.grasp_search_size_y);
00243         } else {
00244                 ROS_INFO("Could not set grasp rectangle size for y. Allowed values: [0, %ld ]. Received value was: y=%ld", (long int)this->max_grasp_search_size_y,(long int)req.grasp_search_size_y);
00245                 res.result = false;
00246                 return res.result;
00247         }
00248 
00249         res.result = true;
00250         ROS_INFO("sending back response: [%ld]", (long int)res.result);
00251         return res.result;
00252 }
00253 
00254 
00255 //set maximal grasp calculation time before result has to be returned
00256 bool CCalcGrasppointsClient::set_grasp_calculation_time_max(haf_grasping::GraspCalculationTimeMax::Request &req,
00257                 haf_grasping::GraspCalculationTimeMax::Response &res)
00258 {
00259         //set max grasp calculation time
00260         this->grasp_calculation_time_max = req.max_calculation_time;
00261         ROS_INFO("Set max calculation time (sec) to: x=%d", (int)req.max_calculation_time.toSec());
00262         res.result = true;
00263         ROS_INFO("sending back response: [%d]", (int)res.result);
00264         return res.result;
00265 }
00266 
00267 //set approach vector for approaching the object with gripper
00268 bool CCalcGrasppointsClient::set_approach_vector(haf_grasping::GraspApproachVector::Request &req, haf_grasping::GraspApproachVector::Response &res)
00269 {
00270         //set grasp approach vector
00271         this->approach_vector = req.approach_vector;
00272         ROS_INFO("Set approach vector to: [%f,%f,%f]", this->approach_vector.x,this->approach_vector.y,this->approach_vector.z);
00273         res.result = true;
00274         ROS_INFO("sending back response: [%d]", (int)res.result);
00275         return res.result;
00276 }
00277 
00278 //set show_only_best_grasp (for visualization)
00279 bool CCalcGrasppointsClient::set_show_only_best_grasp(haf_grasping::ShowOnlyBestGrasp::Request &req,
00280                 haf_grasping::ShowOnlyBestGrasp::Response &res)
00281 {
00282         //set show_only_best_grasp
00283         this->show_only_best_grasp = req.show_only_best_grasp;
00284         ROS_INFO("Set show_only_best_grasp to: [%d] ", (int)this->show_only_best_grasp);
00285         res.result = true;
00286         ROS_INFO("sending back response: [%d]", (int)res.result);
00287         return res.result;
00288 }
00289 
00290 //set maximal grasp calculation time before result has to be returned
00291 bool CCalcGrasppointsClient::set_gripper_width(haf_grasping::GraspPreGripperOpeningWidth::Request &req,
00292                 haf_grasping::GraspPreGripperOpeningWidth::Response &res)
00293 {
00294         //set pre-grasp gripper opening width
00295         this->gripper_opening_width = req.gripper_opening_width;
00296         ROS_INFO("Set gripper_opening_width (factor for scaling pc) to: x=%d", (int)req.gripper_opening_width);
00297         res.result = true;
00298         ROS_INFO("sending back response: [%d]", (int)res.result);
00299         return res.result;
00300 }
00301 
00302 
00303 int main (int argc, char **argv)
00304 {
00305   ROS_INFO("ROS NODE calc_grasppoints_client started");
00306   ros::init(argc, argv, "calc_grasppoint_client");
00307   ros::NodeHandle nh_;
00308   CCalcGrasppointsClient grasp_client(nh_);
00309 
00310   ros::spin();
00311   return 0;
00312 }
00313 
00314 


haf_grasping
Author(s): David Fischinger
autogenerated on Wed Jan 11 2017 03:48:49