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 powerup_gripper(self):
if self._sim:
rospy.loginfo("Set the gripper voltage in simulation.")
return True
try:
# open gripper
req = SetIORequest()
# set tool0 true
req.fun = 4
req.state = 24
self._set_IO_srv(req)
rospy.loginfo("Set the gripper voltage.")
self._rate.sleep()
return True
except rospy.ServiceException, e:
rospy.logerr("Failed to set gripper voltage: {}.".format(e))
return False
def ros_setup(self):
"""Creates and inits ROS components"""
GripperController.ros_setup(self)
self.set_joints_request = SetJointsRequest()
self.set_joints_request.joints_names = [self.master_joint]
self.open_positions = [self.master_joint_open_position]
self.close_positions = [self.master_joint_close_position]
self.update_joints_client = rospy.ServiceProxy(
self.update_joints_service_name, SetJoints)
self.set_io_request = SetIORequest()
self.set_io_request.fun = 1
self.set_io_request.state = 1
self.ur_io_client = rospy.ServiceProxy(self.ur_io_service_name, SetIO)
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)
