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 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 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 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)