00001 /* 00002 * OverheadGraspPlanner.h 00003 * 00004 * Created on: Aug 10, 2012 00005 * Author: jnicho 00006 */ 00007 00008 #ifndef OVERHEADGRASPPLANNER_H_ 00009 #define OVERHEADGRASPPLANNER_H_ 00010 00011 #include <ros/ros.h> 00012 #include <pcl/point_cloud.h> 00013 #include <pcl/point_types.h> 00014 #include <object_manipulation_tools/grasp_planners/GraspPlannerInterface.h> 00015 #include <XmlRpc.h> 00016 #include <tf/transform_datatypes.h> 00017 #include <tf/transform_broadcaster.h> 00018 #include <tf/transform_listener.h> 00019 00020 00021 // ros parameters bare names 00022 const std::string PARAM_NAME_DEFAULT_PREGRASP_DISTANCE = "/default_pregrasp_distance"; 00023 const std::string PARAM_NAME_PLANE_PROXIMITY_THRESHOLD= "/plane_proximity_threshold"; 00024 const std::string PARAM_NAME_NUM_CANDIDATE_GRASPS = "/num_returned_candidate_grasps"; 00025 const std::string PARAM_NAME_GRASP_IN_WORLD_COORDINATES = "grasp_pose_in_world_coordinates"; 00026 const std::string PARAM_NAME_SEARCH_RADIUS = "/search_radius"; 00027 const std::string PARAM_NAME_APPROACH_VECTOR = "/approach_vector"; 00028 00029 00030 // default ros parameter values 00031 const double PARAM_DEFAULT_PREGRASP_DISTANCE = 0.1f;// meters 00032 const double PARAM_DEFAULT_PLANE_PROXIMITY_THRESHOLD= 0.005; // meters 00033 const double PARAM_DEFAULT_SEARCH_RADIUS = 0.005; //0.5 cm 00034 const int PARAM_DEFAULT_NUM_CANDIDATE_GRASPS = 8; 00035 const bool PARAM_DEFAULT_GRASP_IN_WORLD_COORDINATES = true; // 00036 const tf::Vector3 PARAM_DEFAULT_APPROACH_VECTOR = tf::Vector3(0.0f,0.0f,-1.0f); // in world coordinates 00037 00038 // other defaults 00039 const std::string GRASP_PLANNER_NAME = "OverheadGraspPlanner"; 00040 const int SEARCH_MIN_CLUSTER_SIZE = 50.0f; 00041 const int SEARCH_MAX_CLUSTER_SIZE = 5000; 00042 00043 class OverheadGraspPlanner:public GraspPlannerInterface 00044 { 00045 public: 00046 00047 struct ParameterVals 00048 { 00049 double DefaultPregraspDistance; 00050 double PlaneProximityThreshold; 00051 double SearchRadius; 00052 int NumCandidateGrasps; 00053 bool GraspInWorldCoordinates; 00054 tf::Vector3 ApproachVector; // in world coordinates 00055 }; 00056 00057 OverheadGraspPlanner(); 00058 virtual ~OverheadGraspPlanner(); 00059 00060 bool planGrasp(object_manipulation_msgs::GraspPlanning::Request &req, 00061 object_manipulation_msgs::GraspPlanning::Response &res); 00062 std::string getPlannerName(); 00063 void fetchParameters(bool useNodeNamespace); 00064 00065 // rotates about the approach vector to generate candidate grasp poses 00066 void generateGraspPoses(const geometry_msgs::Pose &pose,int numCandidates, 00067 std::vector<geometry_msgs::Pose> &poses); 00068 00069 protected: 00070 static const std::string _GraspPlannerName; 00071 std::string _WorldFrameId; 00072 ParameterVals _ParamVals; 00073 tf::TransformListener _tfListener; 00074 00075 // computational members 00076 bool _UsingDefaultApproachVector; 00077 }; 00078 00079 #endif /* OVERHEADGRASPPLANNER_H_ */