GripperController.py

Go to the documentation of this file.

#! /usr/bin/python

#ROS dependencies
import roslib; roslib.load_manifest('coverage_tools')
import rospy
import actionlib
from actionlib_msgs.msg import *
from pr2_controllers_msgs.msg import *

#python dependencies
import sys

class GripperController:

    def open(self, arm='right'):
        '''
        Opens the gripper by setting up an action client
        Input:
            - arm: the arm ('left' or 'right') of which the gripper shall be opened
                   The default is 'right'. 
        '''
        action = 'r_gripper_controller/gripper_action'
        if arm =='left':
            action = 'l_gripper_controller/gripper_action'
        
        client = actionlib.SimpleActionClient(action, Pr2GripperCommandAction)
        client.wait_for_server()
        client.send_goal(Pr2GripperCommandGoal(Pr2GripperCommand(position = 0.09, max_effort = -1)))
        client.wait_for_result()
        result = client.get_result()
        did = []
        if client.get_state() != GoalStatus.SUCCEEDED:
            did.append("failed")
        else:
            if result.stalled: did.append("stalled")
            if result.reached_goal: did.append("reached goal")
        print ' and '.join(did)
    
    def close(self, arm='right'):
        '''
        Opens the gripper by setting up an action client
        Input:
            - arm: the arm ('left' or 'right') of which the gripper shall be closed
                   The default is 'right'. 
        '''

        action = 'r_gripper_controller/gripper_action'
        if arm =='left':
            action = 'l_gripper_controller/gripper_action'
        
        client = actionlib.SimpleActionClient(action, Pr2GripperCommandAction)
        client.wait_for_server()
        client.send_goal(Pr2GripperCommandGoal(Pr2GripperCommand(position = 0.02, max_effort = 50.0)))
        client.wait_for_result()
        result = client.get_result()
        did = []
        if client.get_state() != GoalStatus.SUCCEEDED:
            did.append("failed")
        else:
            if result.stalled: did.append("stalled")
            if result.reached_goal: did.append("reached goal")
        print ' and '.join(did)

if __name__ == '__main__':
    
    if len(sys.argv) < 2:
        print "Please specify open 'o' or close 'c' command"
        sys.exit(0)
    rospy.init_node('move_the_gripper', anonymous=True)

    gripper = GripperController()
    if sys.argv[1] =='o':
        gripper.open()
    elif sys.argv[1] =='c':
        gripper.close()