def publisher():
"""
Main loop which requests new commands and publish them on the CModelRobotOutput topic.
"""
rospy.init_node('robotiq_simple_controller')
pub = rospy.Publisher('command', CModelCommand, queue_size=3)
command = CModelCommand()
while not rospy.is_shutdown():
command = genCommand(askForCommand(command), command)
pub.publish(command)
rospy.sleep(0.1)
def __init__(self, topic):
#Initiate output message as an empty list
self.status = CModelStatus()
self.last_command = CModelCommand()
self.pub = rospy.Publisher(topic, std_msgs.msg.String, queue_size=1)
self.rospack = rospkg.RosPack()
self.is_moving = False
self.is_closing = False
self.command_received_time = rospy.get_rostime()
self.status_update_timer = rospy.Timer(rospy.Duration(.1), self.updateStatus)
self.long_move = False # Hacky
def _goto_position(self, pos, vel, force): """ Goto position with desired force and velocity @type pos: float @param pos: Gripper width in meters @type vel: float @param vel: Gripper speed in m/s @type force: float @param force: Gripper force in N """ command = CModelCommand() command.rACT = 1 command.rGTO = 1 command.rPR = int(np.clip((-230)/(self._max_gap - self._min_gap) * (pos - self._min_gap) + 230., 0, 230)) command.rSP = int(np.clip((255)/(self._max_speed - self._min_speed) * (vel - self._min_speed), 0, 255)) command.rFR = int(np.clip((255)/(self._max_force - self._min_force) * (force - self._min_force), 0, 255)) self._cmd_pub.publish(command)
def _activate(self, timeout=5.0):
command = CModelCommand();
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
start_time = rospy.get_time()
while not self._ready():
if rospy.is_shutdown():
self._preempt()
return False
if rospy.get_time() - start_time > timeout:
rospy.logwarn('Failed to activated gripper in ns [%s]' % (self._ns))
return False
self._cmd_pub.publish(command)
rospy.sleep(0.1)
rospy.loginfo('Successfully activated gripper in ns [%s]' % (self._ns))
return True
def genCommand(char, command):
"""Update the command according to the character entered by the user."""
if char == 'o':
command = CModelCommand()
command.rACT = 0
command.rGTO = 1
command.rATR = 1
command.rPR = 4095
command.rSP = 400
command.rFR = 200
if char == 'r':
command = CModelCommand()
command.rACT = 0
command.rGTO = 1
command.rATR = 1
command.rPR = 2000
command.rSP = 400
command.rFR = 200
if char == 'c':
command.rACT = 0
command.rGTO = 1
command.rATR = 1
command.rPR = 0
command.rSP = 400
command.rFR = 200
#If the command entered is a int, assign this value to rPRA
try:
command.rPR = int(char)
if command.rPR > 4095:
command.rPR = 4095
if command.rPR < 0:
command.rPR = 0
except ValueError:
pass
if char == 'f':
command.rSP += 25
if command.rSP > 1000:
command.rSP = 1000
if char == 'l':
command.rSP -= 25
if command.rSP < 0:
command.rSP = 0
if char == 'i':
command.rFR += 25
if command.rFR > 1000:
command.rFR = 1000
if char == 'd':
command.rFR -= 25
if command.rFR < 0:
command.rFR = 0
return command
def genCommand(char, command):
"""Update the command according to the character entered by the user."""
if char == 'a':
command = CModelCommand();
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
if char == 'r':
command = CModelCommand();
command.rACT = 0
if char == 'c':
command.rPR = 255
if char == 'o':
command.rPR = 0
#If the command entered is a int, assign this value to rPRA
try:
command.rPR = int(char)
if command.rPR > 255:
command.rPR = 255
if command.rPR < 0:
command.rPR = 0
except ValueError:
pass
if char == 'f':
command.rSP += 25
if command.rSP > 255:
command.rSP = 255
if char == 'l':
command.rSP -= 25
if command.rSP < 0:
command.rSP = 0
if char == 'i':
command.rFR += 25
if command.rFR > 255:
command.rFR = 255
if char == 'd':
command.rFR -= 25
if command.rFR < 0:
command.rFR = 0
return command
def _stop(self):
command = CModelCommand()
command.rACT = 1
command.rGTO = 0
self._cmd_pub.publish(command)
rospy.logdebug('Stopping gripper in ns [%s]' % (self._ns))