From be02e551b638cd1485c16f3799aa73badb99a96c Mon Sep 17 00:00:00 2001 From: jproberge Date: Wed, 29 Jul 2020 13:50:55 -0400 Subject: [PATCH] Adding a small patch to fix potential synchronization issues --- .../baseRobotiqVacuumGrippers.py | 52 +++++++++++++------ 1 file changed, 35 insertions(+), 17 deletions(-) diff --git a/robotiq_vacuum_grippers_control/src/robotiq_vacuum_grippers_control/baseRobotiqVacuumGrippers.py b/robotiq_vacuum_grippers_control/src/robotiq_vacuum_grippers_control/baseRobotiqVacuumGrippers.py index 0e96792e..cc186ec9 100644 --- a/robotiq_vacuum_grippers_control/src/robotiq_vacuum_grippers_control/baseRobotiqVacuumGrippers.py +++ b/robotiq_vacuum_grippers_control/src/robotiq_vacuum_grippers_control/baseRobotiqVacuumGrippers.py @@ -41,6 +41,7 @@ from robotiq_vacuum_grippers_control.msg import _RobotiqVacuumGrippers_robot_input as inputMsg from robotiq_vacuum_grippers_control.msg import _RobotiqVacuumGrippers_robot_output as outputMsg +import time class robotiqbaseRobotiqVacuumGrippers: """Base class (communication protocol agnostic) for sending commands and receiving the status of the Robotic Vacuum grippers""" @@ -105,22 +106,39 @@ def sendCommand(self): def getStatus(self): """Request the status from the gripper and return it in the RobotiqVacuumGrippers_robot_input msg type.""" - - #Acquire status from the Gripper - status = self.client.getStatus(6); - - #Message to output - message = inputMsg.RobotiqVacuumGrippers_robot_input() - - #Assign the values to their respective variables - message.gACT = (status[0] >> 0) & 0x01; - message.gMOD = (status[0] >> 1) & 0x03; - message.gGTO = (status[0] >> 3) & 0x01; - message.gSTA = (status[0] >> 4) & 0x03; - message.gOBJ = (status[0] >> 6) & 0x03; - message.gFLT = status[2] - message.gPR = status[3] - message.gPO = status[4] - + try: + #Acquire status from the Gripper + status = self.client.getStatus(6); + + #Message to output + message = inputMsg.RobotiqVacuumGrippers_robot_input() + + #Assign the values to their respective variables + message.gACT = (status[0] >> 0) & 0x01; + message.gMOD = (status[0] >> 1) & 0x03; + message.gGTO = (status[0] >> 3) & 0x01; + message.gSTA = (status[0] >> 4) & 0x03; + message.gOBJ = (status[0] >> 6) & 0x03; + message.gFLT = status[2] + message.gPR = status[3] + message.gPO = status[4] + + except: + time.sleep(1) #Small delay for in case of synchronization issues + #Acquire status from the Gripper + status = self.client.getStatus(6); + + #Message to output + message = inputMsg.RobotiqVacuumGrippers_robot_input() + + #Assign the values to their respective variables + message.gACT = (status[0] >> 0) & 0x01; + message.gMOD = (status[0] >> 1) & 0x03; + message.gGTO = (status[0] >> 3) & 0x01; + message.gSTA = (status[0] >> 4) & 0x03; + message.gOBJ = (status[0] >> 6) & 0x03; + message.gFLT = status[2] + message.gPR = status[3] + message.gPO = status[4] return message