gripper_model.h

Go to the documentation of this file.

#ifndef _GRIPPER_MODEL_H_
#define _GRIPPER_MODEL_H_

#include <ros/ros.h>

#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>

#include <object_manipulator/tools/shape_tools.h>

#include "helpers.h"

struct FeatureWeights{
    FeatureWeights(){}
    FeatureWeights(double cen, double sym, double pnt, double iyz, double col, double norm)
    {
        setWeights(cen, sym, pnt, iyz, col, norm);

    }

    void setWeights(double cen, double sym, double pnt, double iyz, double col, double norm)
    {
        centroid = cen;
        symmetry = sym;
        point = pnt;
        Iyz = iyz;
        collision = col;
        normal = norm;
    }

    double  centroid, symmetry, point, Iyz,
            collision, normal;
};


struct GraspState{

    enum State {
        VALID = 0,
        NO_OBJECT = 1,
        COLLIDE_L = 2,
        COLLIDE_R = 3,
        COLLIDE_PALM = 4,
    };

    GraspState(){ reset(); }

    void reset(){
        valid = true;
        no_object = collide_l = collide_r = collide_palm = false;
    }

    void set(State state){
        switch(state)
        {
        case(VALID):
            reset();
            break;
        case(NO_OBJECT):
            reset();
            no_object = true;
            valid = false;
            break;
        default:
            collide_l       = collide_l || (state == COLLIDE_L);
            collide_r       = collide_r || (state == COLLIDE_R);
            collide_palm    = collide_palm|| (state == COLLIDE_PALM);
            valid = false;
            no_object = false;
            break;
        }

    }

    inline bool inCollision(){
        return collide_l || collide_r || collide_palm;
    }

    bool valid, no_object, collide_l, collide_r, collide_palm;
};

using namespace object_manipulator::shapes;

class GripperModel{

    typedef pcl::PointXYZRGBNormal PointT;

public:

    // For publishing the gripper model
    ros::NodeHandle *nh_ptr_;
    ros::Publisher pub_;

    // Constructor
    GripperModel(){  reset(true);  }

    // the header
    std_msgs::Header header;

    // The geometry that makes up the model.
    Box palm_;
    Box l_finger_;
    Box r_finger_;
    Box space_;
    Box extended_;
    Box bounding_box_;
    Box roi_box_;
    tf::Transform wrist_frame_;
    tf::Transform tool_frame_;

    // State variables
    GraspState state_;
    double score_;

    // member functions
    void transform(const geometry_msgs::Transform &T, const std::string &frame_id = "");
    void transform(const tf::Transform &T, const std::string &frame_id = "");

    void toPoseStamped(geometry_msgs::PoseStamped &ps);

    void setFrameId(const std::string &frame_id);

    void setStamp(const ros::Time &stamp);

    void setHeader(const std_msgs::Header &header);

    void reset( bool resetStateAndScore = false);

    void publishModel(ros::Publisher &pub, std::string ns = std::string("gripper_model"),
                      int id = 0, ros::Duration lifetime = ros::Duration(5.0),
                      double opacity = 1.0, int action = visualization_msgs::Marker::ADD);

    bool collidesWithPlane(tf::Pose yz_plane);

    double evaluateGrasp(const pcl::PointCloud<PointT> &cloud,
                         FeatureWeights &weights,
                         tf::Vector3 default_centroid,
                         bool debug_print = false,
                         bool weights_as_output_features = false);

    class compareModels
    {
    public:
        bool operator()(const GripperModel& g1, const GripperModel& g2) {  return g1.score_ < g2.score_;  }
    };

};



#endif