def genCommand(char, command):
"""Update the command according to the character entered by the user."""
if char == 'a':
command = outputMsg()
command.r_act = 1
command.r_gto = 1
command.r_sp = 255
command.r_fr = 150
if char == 'r':
command = outputMsg()
command.r_act = 0
if char == 'c':
command.r_pr = 255
if char == 'o':
command.r_pr = 0
#If the command entered is a int, assign this value to r_prA
try:
if int(char) > 255:
command.r_pr = 255
elif int(char) < 0:
command.r_pr = 0
else:
command.r_pr = int(char)
except ValueError:
pass
if char == 'f':
if (command.r_sp + 25) > 255:
command.r_sp = 255
else:
command.r_sp += 25
if char == 'l':
if (command.r_sp - 25) < 0:
command.r_sp = 0
else:
command.r_sp -= 25
if char == 'i':
if (command.r_fr + 25) > 255:
command.r_fr = 255
else:
command.r_fr += 25
if char == 'd':
if (command.r_fr - 25) < 0:
command.r_fr = 0
else:
command.r_fr -= 25
return command
def set_gripper_position(position): ##0=open, 255=close
command = outputMsg()
command.rPR = position
#gripper_publisher.publish(command)
print "Gripper Position:"
print position
return command
def gripper_pos(self, pos):
cmd = outputMsg()
cmd.rACT = 0
cmd.rGTO = 0
cmd.rATR = 0
cmd.rPR = int(pos)
cmd.rSP = int(self.speed)
cmd.rFR = int(self.force)
self.gripper_cmd_pub.publish(cmd)
def stop(self, block=False, timeout=-1):
cmd = outputMsg()
cmd.r_act = 1
cmd.r_gto = 0
self.cmd_pub.publish(cmd)
time.sleep(0.1)
if block:
return self.wait_until_stopped(timeout)
return True
def stop(self, block=False, timeout=-1):
cmd = outputMsg()
cmd.rACT = 1
cmd.rGTO = 0
self.cmd_pub.publish(cmd)
rospy.sleep(0.1)
if block:
return self.wait_until_stopped(timeout)
return True
def gripper_open(self):
cmd = outputMsg()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rATR = 0
cmd.rPR = 0
cmd.rSP = int(self.speed)
cmd.rFR = int(self.force)
self.gripper_cmd_pub.publish(cmd)
rospy.sleep(0.5)
def gripper_reset(self):
cmd = outputMsg()
cmd.rACT = 0
cmd.rGTO = 1
cmd.rATR = 0
cmd.rPR = 0
cmd.rSP = 255
cmd.rFR = 150
self.gripper_cmd_pub.publish(cmd)
rospy.sleep(0.5)
def gripper_pos(self, pos):
#print('===================get in pos====================')
cmd = outputMsg()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rATR = 0
cmd.rPR = int(pos)
cmd.rSP = int(self.speed)
cmd.rFR = int(self.force)
self.gripper_cmd_pub.publish(cmd)
def activate_gripper():
command = outputMsg()
command.rACT = 1
command.rGTO = 1
command.rSP = 50
command.rFR = 150 ##force need to be adjusted later
#gripper_publisher.publish(command)
rospy.sleep(2)
print "Gripper Activated"
return command
def gripper_close(self):
cmd = outputMsg()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rATR = 0
cmd.rPR = 255
cmd.rSP = int(self.speed)
cmd.rFR = int(self.force)
self.__gripper_pub.publish(self.gripper_topic, cmd)
rospy.sleep(0.3)
def gripper_setting(self, speed = -1, force = -1):
self.speed = self.speed if speed == -1 else speed
self.force = self.force if force == -1 else force
cmd = outputMsg()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rATR = 0
cmd.rPR = self.curr_pos
cmd.rSP = int(self.speed)
cmd.rFR = int(self.force)
self.gripper_cmd_pub.publish(cmd)
def goto(self, pos, vel, force, block=False, timeout=-1):
cmd = outputMsg()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rPR = int(np.clip((13. - 230.) / 0.087 * pos + 230., 0, 255))
cmd.rSP = int(np.clip(255. / (0.1 - 0.013) * (vel - 0.013), 0, 255))
cmd.rFR = int(np.clip(255. / (100. - 30.) * (force - 30.), 0, 255))
self.cmd_pub.publish(cmd)
rospy.sleep(0.1)
if block:
if not self.wait_until_moving(timeout):
return False
return self.wait_until_stopped(timeout)
return True
def activate(self, timeout=-1):
cmd = outputMsg()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rPR = 0
cmd.rSP = 255
cmd.rFR = 150
self.cmd_pub.publish(cmd)
r = rospy.Rate(30)
start_time = rospy.get_time()
while not rospy.is_shutdown():
if timeout >= 0. and rospy.get_time() - start_time > timeout:
return False
if self.is_ready():
return True
r.sleep()
return False
def activate(self, timeout=-1):
cmd = outputMsg()
cmd.r_act = 1
cmd.r_gto = 1
cmd.r_pr = 0
cmd.r_sp = 255
cmd.r_fr = 150
self.cmd_pub.publish(cmd)
r = self.create_rate(30)
start_time = time.time()
while rclpy.ok():
if timeout >= 0. and time.time() - start_time > timeout:
return False
if self.is_ready():
return True
r.sleep()
return False
def publisher(args=None):
"""Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic."""
rclpy.init(args=args)
node = Node('Robotiq2FGripperSimpleController')
pub = node.create_publisher(outputMsg, 'Robotiq2FGripperRobotOutput', 1)
command = outputMsg()
try:
while rclpy.ok():
command = genCommand(askForCommand(command), command)
pub.publish(command)
sleep(0.1)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
def reset(self):
cmd = outputMsg()
cmd.rACT = 0
self.cmd_pub.publish(cmd)
def auto_release(self):
cmd = outputMsg()
cmd.rACT = 1
cmd.rATR = 1
self.cmd_pub.publish(cmd)
def reset_gripper():
command = outputMsg()
command.rACT = 0
#gripper_publisher.publish(command)
print "Gripper Reset"
def auto_release(self):
cmd = outputMsg()
cmd.r_act = 1
cmd.r_atr = 1
self.cmd_pub.publish(cmd)
