pen_gripper.h

Go to the documentation of this file.

/*
 * pen_gripper.h
 *
 *  Created on: 15.12.2010
 *      Author: Richard Schneider
 *
 */
#ifndef PEN_GRIPPER_H_
#include <ros/ros.h>
#include <opencv/cv.h>

// communication
#include <portrait_robot_msgs/alubsc_node_instr.h>
#include <portrait_robot_msgs/alubsc_status_msg.h>

// pointcloud processing
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/passthrough.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/common/vector_average.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/pcl_base.h>
#include "pcl/filters/statistical_outlier_removal.h"
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

// frame transformation
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

// marker visualization
#include <visualization_msgs/Marker.h>

// arm movement
#include <actionlib/client/simple_action_client.h>
#include <ee_cart_imped_action/ee_cart_imped_arm.hh>

// gripper control
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>

// head control
#include <pr2_controllers_msgs/PointHeadAction.h>

#include <iostream>
#include <fstream>
#include <algorithm>
#include <string>
using namespace std;

//#define PEN_GRIPPER_H_

struct pen {
        pcl::PointXYZ middle;
        pcl::PointXYZ head;
        bool isReady;
};

class PenGripper {

private:

        // nodehandle
        ros::NodeHandle *n;

        // pointer to the gripper controller
        EECartImpedArm *arm_r, *arm_l;

        // left and right gripper start position
        pcl::PointXYZ r_start_pos;
        pcl::PointXYZ l_start_pos;

        // pointer to the head client
        actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction>* head;

        //right and left gripperclient pointer
        actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>* GripperRight;
        actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>* GripperLeft;

        // incomming pen cloud
        pcl::PointCloud<pcl::PointXYZ> pen_cloud;

        // pen reprasentation
        pen red_pen;

        // pcl subscriber/ publisher for narrow_stereo_textured
        ros::Subscriber pcl_sub;
        ros::Publisher pcl_pub;

        // visualization stuff for rviz
        ros::Publisher marker_middle_pub;

        // frame transformer
        tf::TransformListener tflistener;

public:

        bool success;

        bool abort;

        portrait_robot_msgs::alubsc_status_msg status;

        ros::ServiceClient status_clt;

        PenGripper(ros::NodeHandle& nh);

        ~PenGripper();

        void narrow_stereo_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);

        void grip_pen();

        void take_start_position();

        void open_gripper(const string gripper);

        void close_gripper(const string gripper);

        void reset();

        bool handle_req(portrait_robot_msgs::alubsc_node_instr::Request &req,
                        portrait_robot_msgs::alubsc_node_instr::Response &res);
};

#endif /* PEN_GRIPPER_H_ */