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)