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