def _update_gripper_stat(self, dev=0):
stat = GripperStat()
stat.header.stamp = rospy.get_rostime()
stat.header.seq = self._seq[dev]
stat.is_ready = self._gripper.is_ready(dev)
stat.is_reset = self._gripper.is_reset(dev)
stat.is_moving = self._gripper.is_moving(dev)
stat.obj_detected = self._gripper.object_detected(dev)
stat.fault_status = self._gripper.get_fault_status(dev)
stat.position = self._gripper.get_pos(dev)
stat.requested_position = self._gripper.get_req_pos(dev)
stat.current = self._gripper.get_current(dev)
self._seq[dev] += 1
return stat
def __init__(self, _num_grippers):
self._num_grippers = _num_grippers
if (self._num_grippers != 1) and (self._num_grippers != 2):
rospy.logerr(
"Number of grippers not supported (needs to be 1 or 2)")
return
rospy.Subscriber("/movo/right_gripper/stat",
GripperStat,
self._update_right_gripper_stat,
queue_size=10)
self._right_gripper_pub = rospy.Publisher('/movo/right_gripper/cmd',
GripperCmd,
queue_size=10)
if (self._num_grippers == 2):
rospy.Subscriber("/left_gripper/stat",
GripperStat,
self._update_left_gripper_stat,
queue_size=10)
self._left_gripper_pub = rospy.Publisher('/movo/left_gripper/cmd',
GripperCmd,
queue_size=10)
self._gripper_stat = [GripperStat()] * self._num_grippers
self._gripper_cmd = [GripperCmd()] * self._num_grippers
self._run_test()
def _run_driver(self):
last_time = rospy.get_time()
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
dt = rospy.get_time() - last_time
if (0 == self._driver_state):
for i in range(self._num_grippers):
if (dt < 0.5):
self._gripper.deactivate_gripper(i)
else:
self._driver_state = 1
elif (1 == self._driver_state):
grippers_activated = True
for i in range(self._num_grippers):
self._gripper.activate_gripper(i)
grippers_activated &= self._gripper.is_ready(i)
if (grippers_activated):
self._driver_state = 2
elif (2 == self._driver_state):
self._driver_ready = True
for i in range(self._num_grippers):
success = True
success &= self._gripper.process_act_cmd(i)
rospy.sleep(0.01)
success &= self._gripper.process_stat_cmd(i)
if not success:
self._gripper_err[i] += 1
if self._gripper_err[i] > 5:
rospy.logwarn("Failed to contact gripper %d" % i)
else:
self._gripper_err[i] = 0
stat = GripperStat()
js = JointState()
stat = self._update_gripper_stat(i)
js = self._update_gripper_joint_state(i)
if (i == 0):
self._right_gripper_pub.publish(stat)
self._right_gripper_joint_state_pub.publish(js)
else:
self._left_gripper_pub.publish(stat)
self._left_gripper_joint_state_pub.publish(js)
rospy.sleep(0.01)
r.sleep()
self._gripper.shutdown()