def close_gripper(self):
if self._sim:
self._open = False
rospy.loginfo("Close gripper in simulation.")
return True
try:
# close gripper
req = SetIORequest()
# set tool0 false
req.fun = 1
req.pin = 16
req.state = 0.0
self._set_IO_srv(req)
# set tool1 true
req.fun = 1
req.pin = 17
req.state = 1.0
self._set_IO_srv(req)
rospy.loginfo("Close gripper")
self._rate.sleep()
self._open = False
return True
except rospy.ServiceException, e:
rospy.logerr("Failed to close hitbot gripper: {}".format(e))
return False
def open_gripper(self):
if self._sim:
rospy.loginfo("Open gripper in simulation.")
self._open = True
return True
try:
# open gripper
req = SetIORequest()
# set tool0 true
req.fun = 1
req.pin = 16
req.state = 1.0
self._set_IO_srv(req)
# set tool1 false
req.fun = 1
req.pin = 17
req.state = 0.0
self._set_IO_srv(req)
rospy.loginfo("Open gripper.")
self._rate.sleep()
self._open = True
return True
except rospy.ServiceException, e:
rospy.logerr("Failed to open hitbot gripper: {}.".format(e))
return False
def execute_skill(self, goal):
rospy.sleep(1.0) #We don't know
request = SetIORequest()
request.fun = 1
request.state = 1
service = rospy.ServiceProxy("/ur_hardware_interface/set_io", SetIO)
if goal.close:
request.pin = 0
else:
request.pin = 1
answer = service.call(request)
print(answer)
if (answer.success == False):
self.aborted("failed")
rospy.sleep(0.5)
request.state = 0
answer = service.call(request)
if (answer.success == False):
self.aborted("failed")
self.success("success")
rospy.sleep(0.5)
def close_gripper(self):
r = SetIORequest()
r.fun = 1
r.pin = 0
r.state = 1.0
self.io_srv(r)
def open_gripper(self):
r = SetIORequest()
r.fun = 1
r.pin = 0
r.state = 0.0
self.io_srv(r)