def get_current_gripper_status(self):
"""
Public function to obtain the current gripper status.
Returns: Instance of `robotiq_2f_gripper_msgs/RobotiqGripperStatus` message. See the message declaration for fields description
"""
status = RobotiqGripperStatus()
status.header.stamp = rospy.get_rostime()
status.header.seq = self._seq
status.is_ready = self._gripper.is_ready()
status.is_reset = self._gripper.is_reset()
status.is_moving = self._gripper.is_moving()
status.obj_detected = self._gripper.object_detected()
status.fault_status = self._gripper.get_fault_status()
status.position = self._gripper.get_pos()
status.requested_position = self._gripper.get_req_pos()
status.current = self._gripper.get_current()
return status
def get_current_gripper_status(self):
"""
Public function to obtain the current gripper status.
Returns: Instance of `robotiq_2f_gripper_msgs/RobotiqGripperStatus` message. See the message declaration for fields description
"""
status = RobotiqGripperStatus()
status.header.stamp = rospy.get_rostime()
status.header.seq = 0
status.is_ready = True
status.is_reset = False
status.is_moving = self._is_moving
status.obj_detected = False
status.fault_status = False
status.position = self.from_radians_to_distance(self._current_joint_pos) #[mm]
status.requested_position = self._current_goal.position #[mm]
status.current = 0.0
return status
def _run_driver(self):
"""
Private function to test the comunication with the gripper and initialize it.
WARNING:
Initialization of the gripper will generate a close motion of the real
gripper.
Raises:
IOError: If Modbus RTU communication with the gripper is not achieved.
"""
last_time = rospy.get_time()
r = rospy.Rate(rospy.get_param('~rate', 100))
while not rospy.is_shutdown() and self._driver_state != 2:
# Check if communication is failing or taking too long
dt = rospy.get_time() - last_time
if (0 == self._driver_state):
if (dt < 0.5):
self._gripper.deactivate_gripper()
else:
self._driver_state = 1
# If driver is not running, activate gripper
elif (1 == self._driver_state):
is_gripper_activated = True
self._gripper.activate_gripper()
is_gripper_activated &= self._gripper.is_ready()
if (is_gripper_activated):
rospy.loginfo("Gripper on port %s Activated" %
self._comport)
self._driver_state = 2
success = True
success &= self._gripper.process_action_cmd()
success &= self._gripper.process_status_cmd()
if not success and not rospy.is_shutdown():
rospy.logerr("Failed to contact gripper %d" %
self._gripper.device_id)
else:
stat = RobotiqGripperStatus()
#js = JointState()
stat = self.get_current_gripper_status()
js = self._update_gripper_joint_state()
if stat.is_ready:
self.is_ready = True
# self._gripper_pub.publish(stat)
# self._gripper_joint_state_pub.publish(js)
r.sleep()