gripper_model.h
Go to the documentation of this file.
00001 #ifndef _GRIPPER_MODEL_H_
00002 #define _GRIPPER_MODEL_H_
00003 
00004 #include <ros/ros.h>
00005 
00006 #include <tf/tf.h>
00007 #include <tf/transform_listener.h>
00008 #include <tf/transform_broadcaster.h>
00009 #include <visualization_msgs/Marker.h>
00010 
00011 #include <object_manipulator/tools/shape_tools.h>
00012 
00013 #include "helpers.h"
00014 
00015 struct FeatureWeights{
00016     FeatureWeights(){}
00017     FeatureWeights(double cen, double sym, double pnt, double iyz, double col, double norm)
00018     {
00019         setWeights(cen, sym, pnt, iyz, col, norm);
00020 
00021     }
00022 
00023     void setWeights(double cen, double sym, double pnt, double iyz, double col, double norm)
00024     {
00025         centroid = cen;
00026         symmetry = sym;
00027         point = pnt;
00028         Iyz = iyz;
00029         collision = col;
00030         normal = norm;
00031     }
00032 
00033     double  centroid, symmetry, point, Iyz,
00034             collision, normal;
00035 };
00036 
00037 
00044 struct GraspState{
00045 
00046     enum State {
00047         VALID = 0,
00048         NO_OBJECT = 1,
00049         COLLIDE_L = 2,
00050         COLLIDE_R = 3,
00051         COLLIDE_PALM = 4,
00052     };
00053 
00054     GraspState(){ reset(); }
00055 
00056     void reset(){
00057         valid = true;
00058         no_object = collide_l = collide_r = collide_palm = false;
00059     }
00060 
00061     void set(State state){
00062         switch(state)
00063         {
00064         case(VALID):
00065             reset();
00066             break;
00067         case(NO_OBJECT):
00068             reset();
00069             no_object = true;
00070             valid = false;
00071             break;
00072         default:
00073             collide_l       = collide_l || (state == COLLIDE_L);
00074             collide_r       = collide_r || (state == COLLIDE_R);
00075             collide_palm    = collide_palm|| (state == COLLIDE_PALM);
00076             valid = false;
00077             no_object = false;
00078             break;
00079         }
00080 
00081     }
00082 
00083     inline bool inCollision(){
00084         return collide_l || collide_r || collide_palm;
00085     }
00086 
00087     bool valid, no_object, collide_l, collide_r, collide_palm;
00088 };
00089 
00096 using namespace object_manipulator::shapes;
00097 
00098 class GripperModel{
00099 
00100     typedef pcl::PointXYZRGBNormal PointT;
00101 
00102 public:
00103 
00104     // For publishing the gripper model
00105     ros::NodeHandle *nh_ptr_;
00106     ros::Publisher pub_;
00107 
00108     // Constructor
00109     GripperModel(){  reset(true);  }
00110 
00111     // the header
00112     std_msgs::Header header;
00113 
00114     // The geometry that makes up the model.
00115     Box palm_;
00116     Box l_finger_;
00117     Box r_finger_;
00118     Box space_;
00119     Box extended_;
00120     Box bounding_box_;
00121     Box roi_box_;
00122     tf::Transform wrist_frame_;
00123     tf::Transform tool_frame_;
00124 
00125     // State variables
00126     GraspState state_;
00127     double score_;
00128 
00129     // member functions
00130     void transform(const geometry_msgs::Transform &T, const std::string &frame_id = "");
00131     void transform(const tf::Transform &T, const std::string &frame_id = "");
00132 
00133     void toPoseStamped(geometry_msgs::PoseStamped &ps);
00134 
00135     void setFrameId(const std::string &frame_id);
00136 
00137     void setStamp(const ros::Time &stamp);
00138 
00139     void setHeader(const std_msgs::Header &header);
00140 
00141     void reset( bool resetStateAndScore = false);
00142 
00143     void publishModel(ros::Publisher &pub, std::string ns = std::string("gripper_model"),
00144                       int id = 0, ros::Duration lifetime = ros::Duration(5.0),
00145                       double opacity = 1.0, int action = visualization_msgs::Marker::ADD);
00146 
00147     bool collidesWithPlane(tf::Pose yz_plane);
00148 
00159     double evaluateGrasp(const pcl::PointCloud<PointT> &cloud,
00160                          FeatureWeights &weights,
00161                          tf::Vector3 default_centroid,
00162                          bool debug_print = false,
00163                          bool weights_as_output_features = false);
00164 
00171     class compareModels
00172     {
00173     public:
00174         bool operator()(const GripperModel& g1, const GripperModel& g2) {  return g1.score_ < g2.score_;  }
00175     };
00176 
00177 };
00178 
00179 
00180 
00181 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends


pr2_grasp_adjust
Author(s): Adam Leeper
autogenerated on Wed Jan 23 2013 15:27:54