39 Command-line interface for receiving and interpreting the status of a 3F gripper gripper. 41 This serves as an example for receiving messages from the 'Robotiq3FGripperRobotInput' topic using the 'Robotiq3FGripper_robot_input' msg type and interpreting the corresponding status of the 3F gripper gripper. 44 from __future__
import print_function
48 roslib.load_manifest(
'robotiq_3f_gripper_control')
50 from robotiq_3f_gripper_articulated_msgs.msg
import Robotiq3FGripperRobotInput
54 """Print the status string generated by the statusInterpreter function.""" 60 """Initialize the node and subscribe to the Robotiq3FGripperRobotInput topic.""" 62 rospy.init_node(
'Robotiq3FGripperStatusListener')
63 rospy.Subscriber(
"Robotiq3FGripperRobotInput", Robotiq3FGripperRobotInput, printStatus)
68 """Generate a string according to the current value of the status variables.""" 70 output =
'\n-----\n3F gripper status interpreter\n-----\n' 73 output +=
'gACT = ' + str(status.gACT) +
': ' 74 if (status.gACT == 0):
75 output +=
'Gripper reset\n' 76 if (status.gACT == 1):
77 output +=
'Gripper activation\n' 80 output +=
'gMOD = ' + str(status.gMOD) +
': ' 81 if (status.gMOD == 0):
82 output +=
'Basic Mode\n' 83 if (status.gMOD == 1):
84 output +=
'Pinch Mode\n' 85 if (status.gMOD == 2):
86 output +=
'Wide Mode\n' 87 if (status.gMOD == 3):
88 output +=
'Scissor Mode\n' 91 output +=
'gGTO = ' + str(status.gGTO) +
': ' 92 if (status.gGTO == 0):
93 output +=
'Stopped (or performing activation/grasping mode change/automatic release)\n' 94 if (status.gGTO == 1):
95 output +=
'Go to Position Request\n' 98 output +=
'gIMC = ' + str(status.gIMC) +
': ' 99 if (status.gIMC == 0):
100 output +=
'Gripper is in reset (or automatic release) state. see Fault Status if Gripper is activated\n' 101 if (status.gIMC == 1):
102 output +=
'Activation in progress\n' 103 if (status.gIMC == 2):
104 output +=
'Mode change in progress\n' 105 if (status.gIMC == 3):
106 output +=
'Activation and mode change are completed\n' 109 output +=
'gSTA = ' + str(status.gSTA) +
': ' 110 if (status.gSTA == 0):
111 output +=
'Gripper is in motion towards requested position (only meaningful if gGTO = 1)\n' 112 if (status.gSTA == 1):
113 output +=
'Gripper is stopped. One or two fingers stopped before requested position\n' 114 if (status.gSTA == 2):
115 output +=
'Gripper is stopped. All fingers stopped before requested position\n' 116 if (status.gSTA == 3):
117 output +=
'Gripper is stopped. All fingers reached requested position\n' 120 output +=
'gDTA = ' + str(status.gDTA) +
': ' 121 if (status.gDTA == 0):
122 output +=
'Finger A is in motion (only meaningful if gGTO = 1)\n' 123 if (status.gDTA == 1):
124 output +=
'Finger A has stopped due to a contact while opening\n' 125 if (status.gDTA == 2):
126 output +=
'Finger A has stopped due to a contact while closing\n' 127 if (status.gDTA == 3):
128 output +=
'Finger A is at requested position\n' 131 output +=
'gDTB = ' + str(status.gDTB) +
': ' 132 if (status.gDTB == 0):
133 output +=
'Finger B is in motion (only meaningful if gGTO = 1)\n' 134 if (status.gDTB == 1):
135 output +=
'Finger B has stopped due to a contact while opening\n' 136 if (status.gDTB == 2):
137 output +=
'Finger B has stopped due to a contact while closing\n' 138 if (status.gDTB == 3):
139 output +=
'Finger B is at requested position\n' 142 output +=
'gDTC = ' + str(status.gDTC) +
': ' 143 if (status.gDTC == 0):
144 output +=
'Finger C is in motion (only meaningful if gGTO = 1)\n' 145 if (status.gDTC == 1):
146 output +=
'Finger C has stopped due to a contact while opening\n' 147 if (status.gDTC == 2):
148 output +=
'Finger C has stopped due to a contact while closing\n' 149 if (status.gDTC == 3):
150 output +=
'Finger C is at requested position\n' 153 output +=
'gDTS = ' + str(status.gDTS) +
': ' 154 if (status.gDTS == 0):
155 output +=
'Scissor is in motion (only meaningful if gGTO = 1)\n' 156 if (status.gDTS == 1):
157 output +=
'Scissor has stopped due to a contact while opening\n' 158 if (status.gDTS == 2):
159 output +=
'Scissor has stopped due to a contact while closing\n' 160 if (status.gDTS == 3):
161 output +=
'Scissor is at requested position\n' 164 output +=
'gFLT = ' + str(status.gFLT) +
': ' 165 if (status.gFLT == 0x00):
166 output +=
'No Fault\n' 167 if (status.gFLT == 0x05):
168 output +=
'Priority Fault: Action delayed, activation (reactivation) must be completed prior to action\n' 169 if (status.gFLT == 0x06):
170 output +=
'Priority Fault: Action delayed, mode change must be completed prior to action\n' 171 if (status.gFLT == 0x07):
172 output +=
'Priority Fault: The activation bit must be set prior to action\n' 173 if (status.gFLT == 0x09):
174 output +=
'Minor Fault: The communication chip is not ready\n' 175 if (status.gFLT == 0x0A):
176 output +=
'Minor Fault: Changing mode fault, interferences detected on Scissor (for less than 20 sec)\n' 177 if (status.gFLT == 0x0B):
178 output +=
'Minor Fault: Automatic release in progress\n' 179 if (status.gFLT == 0x0D):
180 output +=
'Major Fault: Activation fault, verify that no interference or other error occured\n' 181 if (status.gFLT == 0x0E):
182 output +=
'Major Fault: Changing mode fault, interferences detected on Scissor (for more than 20 sec)\n' 183 if (status.gFLT == 0x0F):
184 output +=
'Major Fault: Automatic release completed. Reset and activation is required\n' 187 output +=
'gPRA = ' + str(status.gPRA) +
': ' 188 output +=
'Echo of the requested position for the Gripper (or finger A in individual mode): ' + str(
189 status.gPRA) +
'/255\n' 192 output +=
'gPOA = ' + str(status.gPOA) +
': ' 193 output +=
'Position of Finger A: ' + str(status.gPOA) +
'/255\n' 196 output +=
'gCUA = ' + str(status.gCUA) +
': ' 197 output +=
'Current of Finger A: ' + str(status.gCUA * 10) +
' mA\n' 200 output +=
'gPRB = ' + str(status.gPRB) +
': ' 201 output +=
'Echo of the requested position for finger B: ' + str(status.gPRB) +
'/255\n' 204 output +=
'gPOB = ' + str(status.gPOB) +
': ' 205 output +=
'Position of Finger B: ' + str(status.gPOB) +
'/255\n' 208 output +=
'gCUB = ' + str(status.gCUB) +
': ' 209 output +=
'Current of Finger B: ' + str(status.gCUB * 10) +
' mA\n' 212 output +=
'gPRC = ' + str(status.gPRC) +
': ' 213 output +=
'Echo of the requested position for finger C: ' + str(status.gPRC) +
'/255\n' 216 output +=
'gPOC = ' + str(status.gPOC) +
': ' 217 output +=
'Position of Finger C: ' + str(status.gPOC) +
'/255\n' 220 output +=
'gCUC = ' + str(status.gCUC) +
': ' 221 output +=
'Current of Finger C: ' + str(status.gCUC * 10) +
' mA\n' 224 output +=
'gPRS = ' + str(status.gPRS) +
': ' 225 output +=
'Echo of the requested position for the scissor axis: ' + str(status.gPRS) +
'/255\n' 228 output +=
'gPOS = ' + str(status.gPOS) +
': ' 229 output +=
'Position of the scissor axis: ' + str(status.gPOS) +
'/255\n' 232 output +=
'gCUS = ' + str(status.gCUS) +
': ' 233 output +=
'Current of the scissor axis: ' + str(status.gCUS * 10) +
' mA\n' 238 if __name__ ==
'__main__':
def Robotiq3FGripperStatusListener()
def statusInterpreter(status)