lc_gripper_cmder.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('life_test')
import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
import time
import sys

from pr2_controller_manager import pr2_controller_manager_interface

class LastMessage():
    def __init__(self, topic, msg_type):
        self.msg = None
        rospy.Subscriber(topic, msg_type, self.callback)

    def last(self):
        return self.msg

    def callback(self, msg):
        self.msg = msg

start_pos = 0
end_pos = 0

fwd = -0.008
bck = 0.008
stop = 0
eff_list = []
pos_list = []

controller_name = None

def stop_controller():    
    print "Please wait, stopping controller..."
    global controller_name
    if controller_name is not None:
        pr2_controller_manager_interface.stop_controller(controller_name)
    print "Done."


def calibrate():
    calibration = False    
    global controller_name 
    controller_name = rospy.myargv()[1]
    control_topic = '%s/command' % controller_name
    pub = rospy.Publisher(control_topic, Float64) 
    rospy.init_node('calibration')
    rospy.on_shutdown(stop_controller) 

    pr2_controller_manager_interface.start_controller(controller_name)    

    while calibration == False:
       
        last_state = LastMessage('joint_states', JointState)
        msg = last_state.last() 

        rospy.sleep(0.4)
        pub.publish(Float64(-.0025)) 
        rospy.sleep(8.5)
        pub.publish(Float64(stop))
        
        msg = last_state.last()
        end_pos = msg.position[0]
        print "end position =", end_pos

        start_pos = end_pos + (0.010914138798)
        print "start position =", start_pos
        
        calibration = True
        sleep = False
        fwd_cnt = 0
        cycle_cnt = 0
        crnt_pos = end_pos
        target = start_pos
        seize_cnt = 0
        new_pos = 0

        while not rospy.is_shutdown():

            if crnt_pos < target and target == start_pos:
                pub.publish(Float64(bck))
                msg = last_state.last()
                crnt_pos = msg.position[0]
        
                rospy.sleep(0.02)
                target = start_pos
                fwd_cnt = 0
                seize_cnt += 1
                sleep = True
               
            if crnt_pos > (target) and sleep == True:
                pub.publish(Float64(0.0)) 
                time.sleep(3.0)
                sleep = False
            if crnt_pos > target:
                pub.publish(Float64(fwd))
                msg = last_state.last()
                crnt_pos = msg.position[0]
                new_pos = crnt_pos

                rospy.sleep(0.02)
                target = end_pos  
                fwd_cnt += 1
                sleep = False
                seize_cnt = 0
            if fwd_cnt > 110:
                target = start_pos
                cycle_cnt += 1
                print "grip count =", cycle_cnt        
                sleep = False
            if crnt_pos <= target:
                target = start_pos
                sleep = False
            if seize_cnt > 200:
                pr2_controller_manager_interface.stop_controller(controller_name)
                rospy.sleep(0.5)
                sys.exit()

              
if __name__ == '__main__':
    calibrate()