def activateValve(self, movegroup):
print "\n\n\n\n\n\Valve: "
# move valve, select the correct pipe
# Activate suction for correct move group
# the valve is connected to the left gripper outlet
if movegroup.data == 'left_arm_cartesian' or movegroup.data == 'left_arm' or movegroup.data == 'right_arm' or movegroup.data == 'right_arm_cartesian':
print "straight go"
msg = EndEffectorCommand()
msg.command = 'stop'
msg.id = 65537
msg.sender = 'user'
msg.args = "{\"grip_attempt_seconds\": 1000}"
self.valvePublisher.publish(msg)
rospy.loginfo('Left')
if movegroup.data == 'left_arm_90_cartesian' or movegroup.data == 'left_arm_90' or movegroup.data == 'right_arm_90_cartesian' or movegroup.data == 'right_arm_90':
# NOTE must trigger line low before high to guarantee success
msg = EndEffectorCommand()
msg.command = 'stop'
msg.id = 65537
msg.sender = 'user'
self.valvePublisher.publish(msg)
print "90 go"
msg = EndEffectorCommand()
msg.command = 'go'
msg.id = 65537
msg.sender = 'user'
msg.args = "{\"grip_attempt_seconds\": 1000}"
self.valvePublisher.publish(msg)
def command(self,
cmd,
block=False,
test=lambda: True,
timeout=0.0,
args=None):
"""
Raw command call to directly control gripper.
@type cmd: str
@param cmd: string of known gripper commands
@type block: bool
@param block: command is blocking or non-blocking [False]
@type test: func
@param test: test function for command validation
@type timeout: float
@param timeout: timeout in seconds for command evaluation
@type args: dict({str:float})
@param args: dictionary of parameter:value
"""
ee_cmd = EndEffectorCommand()
ee_cmd.id = self.hardware_id()
ee_cmd.command = cmd
ee_cmd.sender = self._cmd_sender % (cmd, )
ee_cmd.sequence = self._inc_cmd_sequence()
ee_cmd.args = ''
if args != None:
ee_cmd.args = JSONEncoder().encode(args)
seq_test = lambda: (self._state.command_sender == ee_cmd.sender and
(self._state.command_sequence == ee_cmd.sequence or
self._state.command_sequence == 0))
self._cmd_pub.publish(ee_cmd)
if block:
finish_time = rospy.get_time() + timeout
cmd_seq = baxter_dataflow.wait_for(
test=seq_test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd))
if not cmd_seq:
seq_msg = (("Timed out on gripper command acknowledgement for"
" %s:%s") % (self.name, ee_cmd.command))
rospy.logdebug(seq_msg)
time_remain = max(0.5, finish_time - rospy.get_time())
return baxter_dataflow.wait_for(
test=test,
timeout=time_remain,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd))
else:
return True
def command(self, cmd, block=False, test=lambda: True,
timeout=0.0, args=None):
"""
Raw command call to directly control gripper.
@type cmd: str
@param cmd: string of known gripper commands
@type block: bool
@param block: command is blocking or non-blocking [False]
@type test: func
@param test: test function for command validation
@type timeout: float
@param timeout: timeout in seconds for command evaluation
@type args: dict({str:float})
@param args: dictionary of parameter:value
"""
ee_cmd = EndEffectorCommand()
ee_cmd.id = self.hardware_id()
ee_cmd.command = cmd
ee_cmd.sender = self._cmd_sender % (cmd,)
ee_cmd.sequence = self._inc_cmd_sequence()
ee_cmd.args = ''
if args != None:
ee_cmd.args = JSONEncoder().encode(args)
seq_test = lambda: (self._state.command_sender == ee_cmd.sender and
(self._state.command_sequence == ee_cmd.sequence
or self._state.command_sequence == 0))
self._cmd_pub.publish(ee_cmd)
if block:
finish_time = rospy.get_time() + timeout
cmd_seq = baxter_dataflow.wait_for(
test=seq_test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd)
)
if not cmd_seq:
seq_msg = (("Timed out on gripper command acknowledgement for"
" %s:%s") % (self.name, ee_cmd.command))
rospy.logdebug(seq_msg)
time_remain = max(0.5, finish_time - rospy.get_time())
return baxter_dataflow.wait_for(
test=test,
timeout=time_remain,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd)
)
else:
return True
def deactivatePump(self):
print "pump stop"
msg = EndEffectorCommand()
msg.command = 'stop'
msg.id = 65537
msg.sender = 'user'
msg.args = "{\"grip_attempt_seconds\": 1000}"
self.pumpPublisher.publish(msg)
def activatePump(self):
# NOTE Need to trigger line low before high
self.deactivatePump()
msg = EndEffectorCommand()
msg.command = 'go'
msg.id = 65537
msg.sender = 'user'
msg.args = "{\"grip_attempt_seconds\": 1000}"
self.pumpPublisher.publish(msg)
def setObjectWeight(self, kg=0.0):
"""
NOT used yet as the gripper does not now when it gripped something
"""
cmd = EndEffectorCommand()
cmd.id = self.id
cmd.command = "configure"
cmd.args = ''.join([
'{ "object": { "name": "' + self.side + '_gripper",',
'"inertial": { "mass": { "value": ' + str(kg) +
'}, "origin":{ "xyz": [0.0, 0.0, 0.0] },',
' "inertia": { "ixx": 0.0, "ixy": 0.0, "ixz": 0.0, "iyy": 0.0, "iyz": 0.0, "izz": 0.0 } } } }'
])
self.object_weight = kg
self.weight_pub.publish(cmd)
def setWeight(self, kg=0.8):
cmd = EndEffectorCommand()
cmd.id = self.id
cmd.command = "configure"
cmd.args = ''.join([
'{"urdf":{ "name": "' + self.side +
'_gripper","link":[{ "name": "' + self.side + '_hand"',
'}',
',{ "name": "' + self.side +
'_gripper_base","inertial":{ "mass":{ "value": ' + str(kg) + '}',
',"origin":{"xyz":[0.0, 0.0, 0.08]} }',
# ',"visual":{"origin":{"xyz":[0.0, 0.0, 0.0098]},"geometry":{"mesh":{"filename":"package://baxter_tasker/data/universalGripper.DAE","scale":[1,1,1]} } }',
'}]}}'
])
# ',"collision":{"origin":{"xyz":[0.0, 0.0, 0.0098],"rpy":[0.0, 0.0, 0.0]},"geometry":{"cylinder":{"radius": 0.5,"length":0.6} } }',
# print json.dumps(args)
self.gripper_weight = kg
self.weight_pub.publish(cmd)