def actionCb(self, goal):
""" Take an input command of width to open gripper. """
rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
command = goal.command.position
# publish msgs
cmsg = GripperCmd();
pos = command;
if pos>0.08:
cmsg.position = 0.08 - pos/10;
else:
cmsg.position = 0.08 - pos;
#cmsg.position = 0.8 - command
cmsg.speed = 0.06
cmsg.force = 100.0
cmsg.emergency_release = bool(0)
#cmsg.emergency_release_dir = 0
#cmsg.stop = 0
self.r_pub.publish(cmsg)
rospy.sleep(5.0)
self.server.set_succeeded()
rospy.loginfo('Gripper Controller: Done.')
def set_gripper(id, cmd):
gripper_cmd = GripperCmd()
gripper_cmd.force = 50.0
gripper_cmd.speed = 0.03
gripper_cmd.stop = False
gripper_cmd.emergency_release = False
gripper_cmd.emergency_release_dir = 0
if cmd == 'open':
gripper_cmd.position = 0.75
elif cmd == 'close':
gripper_cmd.position = 0.0
if id == 'left':
left_gripper_pub.publish(gripper_cmd)
elif id == "right":
right_gripper_pub.publish(gripper_cmd)