class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the Navigator callback feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
# connect callback fns to signals
self._lights = None
if lights:
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format(arm))
try:
self._gripper = Gripper(arm)
if not (self._gripper.is_calibrated() or
self._gripper.calibrate() == True):
rospy.logerr("({0}_gripper) calibration failed.".format(
self._gripper.name))
raise
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(arm.capitalize())
rospy.logwarn(msg)
def _open_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper open triggered")
self._gripper.open()
if self._lights:
self._set_lights('red', False)
self._set_lights('green', True)
def _close_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper close triggered")
self._gripper.close()
if self._lights:
self._set_lights('green', False)
self._set_lights('red', True)
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
if self._lights:
self._set_lights('red', False)
self._set_lights('green', False)
self._set_lights('blue', value)
def _set_lights(self, color, value):
self._lights.set_light_state('head_{0}_light'.format(color), on=bool(value))
self._lights.set_light_state('{0}_hand_{1}_light'.format(self._arm, color),
on=bool(value))
class GripperConnect(object):
def __init__(self, arm):
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
self._gripper_position = []
self._max_gripper_position = 0.01
self._toogle = -1
self._recorder = []
self._data_number = 1
self._path = '/home/cloud/code/learn-dmp/data_collector/'
try:
self._gripper = Gripper(arm)
if not (self._gripper.is_calibrated()
or self._gripper.calibrate() == True):
rospy.logerr("({0}_gripper) calibration failed.".format(
self._gripper.name))
raise
self._cuff.register_callback(self._record_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot").format(
arm.capitalize())
rospy.logwarn(msg)
def _open_action(self, value):
self._position = self._gripper.get_position()
if value and self._gripper.is_ready():
if self._position > self._max_gripper_position:
rospy.logdebug("gripper open triggered")
self._gripper.close()
print 'gripper is closed'
print self._position
else:
rospy.logdebug("gripper close triggered")
self._gripper.open()
print 'gripper is opened'
print self._position
def _record_action(self, value):
if value and self._gripper.is_ready():
self._toogle = (-1) * self._toogle
if self._toogle == 1:
print 'Opening a new file named demo_' + str(self._data_number)
self._recorder = subprocess.Popen([
'rosrun', 'intera_examples', 'joint_recorder.py', '-f',
self._path + 'demo' + str(self._data_number) + '.txt'
],
stdout=subprocess.PIPE)
if self._toogle == -1:
print self._toogle
print 'Close the file'
self._data_number = self._data_number + 1
self._recorder.kill()