Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import roslib; roslib.load_manifest('life_test')
00035 import rospy
00036 from std_msgs.msg import Float64
00037 from sensor_msgs.msg import JointState
00038
00039 import random
00040 import sys
00041
00042 from time import sleep
00043
00044 class LastMessage():
00045 def __init__(self, topic, msg_type):
00046 self.msg = None
00047 rospy.Subscriber(topic, msg_type, self.callback)
00048
00049 def last(self):
00050 return self.msg
00051
00052 def callback(self, msg):
00053 self.msg = msg
00054
00055 has_warned = False
00056
00057 def main():
00058 controller_name = rospy.myargv()[1]
00059
00060 rospy.init_node('gripper_cmder')
00061 control_topic = '%s/command' % controller_name
00062
00063 eff = 100
00064
00065 grip_name = 'r_gripper_joint' if controller_name.startswith('r') else 'l_gripper_joint'
00066
00067 turn_count = 0
00068 last_state = LastMessage('joint_states', JointState)
00069 try:
00070 pub = rospy.Publisher(control_topic, Float64)
00071 while not last_state.msg and not rospy.is_shutdown(): sleep(0.1)
00072 pub.publish(Float64(eff))
00073 while not rospy.is_shutdown():
00074 sleep(0.01)
00075 jnt_states = last_state.last()
00076 grip_idx = -1
00077 for index, name in enumerate(jnt_states.name):
00078 if name == grip_name:
00079 grip_idx = index
00080 break
00081 if grip_idx < 0:
00082 global has_warned
00083 if not has_warned:
00084 rospy.logwarn("The joint %s was not found in the joints states" % grip_name)
00085 has_warned = True
00086
00087 if abs(jnt_states.velocity[grip_idx]) < 0.0005:
00088 turn_count += 1
00089 else:
00090 turn_count = 0
00091
00092 if turn_count > 25:
00093 eff = -1 * eff
00094 pub.publish(Float64(eff))
00095 turn_count = 0
00096
00097 except KeyboardInterrupt, e:
00098 pass
00099 except:
00100 import traceback
00101 rospy.logerr(traceback.format_exc())
00102 traceback.print_exc()
00103
00104
00105 if __name__ == '__main__':
00106 main()