gripper.cpp

Go to the documentation of this file.

#include "gripper.h"
#include <ros/ros.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <fstream>
#include <stdio.h>

#include <tf/tf.h>

#include <geometry_msgs/PointStamped.h>

//------Gripper open/close-----------------------------



#include <math.h>


Gripper::Gripper()
{   //Initialize the client for the Action interface to the gripper controller
        //and tell the action client that we want to spin a thread by default
        gripper_client_right = new GripperClient("r_gripper_controller/gripper_action", true);
        gripper_client_left = new GripperClient("l_gripper_controller/gripper_action", true);

        //wait for the gripper action server to come up
        while(!gripper_client_right->waitForServer(ros::Duration(5.0)))
        {     ROS_INFO("Waiting for the r_gripper_controller/gripper_action action server to come up");
        }
        while(!gripper_client_left->waitForServer(ros::Duration(5.0)))
        {     ROS_INFO("Waiting for the l_gripper_controller/gripper_action action server to come up");
        }
}

Gripper::~Gripper()
{       delete gripper_client_right;
                delete gripper_client_left;
}

//---Open the gripper---
void Gripper::open_gripper(char lr)
        {       pr2_controllers_msgs::Pr2GripperCommandGoal open;
                        open.command.position = 0.08;
                        open.command.max_effort = -1.0;  // Do not limit effort (negative)

                        ROS_INFO("Sending open goal");
                        if(lr == 'l')
                        {       gripper_client_left->sendGoal(open);
                                gripper_client_left->waitForResult();
                                if(gripper_client_left->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                                        ROS_INFO("The left_gripper opened!");
                                else
                                        ROS_INFO("The left_gripper failed to open.");
                        }
                        else
                        {       gripper_client_right->sendGoal(open);
                                gripper_client_right->waitForResult();
                                if(gripper_client_right->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                                        ROS_INFO("The right_gripper opened!");
                                else
                                        ROS_INFO("The right_gripper failed to open.");
                        }
        }

//---Close the gripper---
void Gripper::close_gripper(char lr)
{      pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
           squeeze.command.position = 0.0;
           squeeze.command.max_effort = 50.0;  // Close gently

           ROS_INFO("Sending squeeze goal");
           if (lr == 'l')
           {   gripper_client_left->sendGoal(squeeze);
                   gripper_client_left->waitForResult();
                   if(gripper_client_left->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                           ROS_INFO("The left_gripper closed!");
                   else
                           ROS_INFO("The left_gripper failed to close.");
           }
           else
           {    gripper_client_right->sendGoal(squeeze);
                        gripper_client_right->waitForResult();
                        if(gripper_client_right->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                                ROS_INFO("The right_gripper closed!");
                        else
                                ROS_INFO("The right_gripper failed to close.");
           }
 }