def _gripper_callback(self, status):
# print('callback! list-len {}, max_release {}'.format(len(self.sem_list), self.max_release))
self._status_mutex.acquire()
self._gripper_width, self._gripper_force = status.width, status.force
self._integrate_gripper_force += status.force
self._force_counter += 1
cmd = Cmd()
cmd.pos = self._desired_gpos
cmd.speed = self.gripper_speed
self.gripper_pub.publish(cmd)
if len(self.sem_list) > 0:
gripper_close = np.isclose(self._gripper_width,
self._desired_gpos,
atol=1e-1)
if gripper_close or self._gripper_force > 0 or self.max_release > 15:
if self.max_release > 15:
self.num_timeouts += 1
for s in self.sem_list:
s.release()
self.sem_list = []
self.max_release += 1 #timeout for when gripper responsive but can't acheive commanded state
else:
self.max_release = 0
self._status_mutex.release()
def set_weiss_gripper(self, des_pos):
"""
:param des_pos:
:return:
"""
cmd = Cmd()
cmd.speed = 100.
cmd.pos = des_pos
# print('command weiss', cmd.pos)
self.weiss_pub.publish(cmd)
def graspmove_nopending(self, width, speed=20):
'''move to the objective position 'width', while stop if the force limit acquired
return before the position reached'''
cmd = Cmd()
cmd.mode = 'Script'
cmd.pos = width
cmd.speed = speed
pub = rospy.Publisher('/wsg_50_driver/goal_position',
Cmd,
queue_size=10)
pub.publish(cmd)
def execute_cb(self, goal):
gripper_command = Cmd()
### Driver simulator do not implement speed regulation
gripper_command.speed = 1
if goal.goal == GraspHandPostureExecutionGoal.GRASP:
if not goal.grasp.grasp_posture.position:
rospy.logerr(
"WSG grasp controller: position vector empty in requested grasp"
)
self.action_server.set_aborted()
return
gripper_command.mode = GraspHandPostureExecutionGoal.GRASP
elif goal.goal == GraspHandPostureExecutionGoal.PRE_GRASP:
if not goal.grasp.pre_grasp_posture.position:
rospy.logerr(
"WSG grasp controller: position vector empty in requested pre_grasp"
)
self.action_server.set_aborted()
return
gripper_command.mode = GraspHandPostureExecutionGoal.PRE_GRASP
gripper_command.pos = goal.grasp.pre_grasp_posture.position[
1] * 2 * 1000 # in mm
elif goal.goal == GraspHandPostureExecutionGoal.RELEASE:
gripper_command.mode = GraspHandPostureExecutionGoal.RELEASE
gripper_command.pos = self.gripper_opened_gap_value
else:
rospy.logerr("WSG grasp controller: unknown goal code (%d)" %
goal.goal)
self.action_server.set_aborted()
return
if gripper_command.mode == GraspHandPostureExecutionGoal.GRASP:
response = self.gripper_grasp_srv(gripper_command.pos,
gripper_command.speed)
else:
response = self.gripper_move_srv(gripper_command.pos,
gripper_command.speed)
if response == 0:
self.action_server.set_succeeded()
else:
rospy.logwarn(
"WSG grasp controller: gripper goal not achieved for pre-grasp or release"
)
self.action_server.set_succeeded()
def execute_cb(self, goal):
gripper_command = Cmd()
### Driver simulator do not implement speed regulation
gripper_command.speed = 1
if goal.goal == GraspHandPostureExecutionGoal.GRASP:
if not goal.grasp.grasp_posture.position:
rospy.logerr("WSG grasp controller: position vector empty in requested grasp")
self.action_server.set_aborted()
return
gripper_command.mode = GraspHandPostureExecutionGoal.GRASP
elif goal.goal == GraspHandPostureExecutionGoal.PRE_GRASP:
if not goal.grasp.pre_grasp_posture.position:
rospy.logerr("WSG grasp controller: position vector empty in requested pre_grasp")
self.action_server.set_aborted()
return
gripper_command.mode = GraspHandPostureExecutionGoal.PRE_GRASP
gripper_command.pos = goal.grasp.pre_grasp_posture.position[1] * 2 * 1000 # in mm
elif goal.goal == GraspHandPostureExecutionGoal.RELEASE:
gripper_command.mode = GraspHandPostureExecutionGoal.RELEASE
gripper_command.pos = self.gripper_opened_gap_value
else:
rospy.logerr("WSG grasp controller: unknown goal code (%d)" % goal.goal)
self.action_server.set_aborted()
return
if gripper_command.mode == GraspHandPostureExecutionGoal.GRASP:
response = self.gripper_grasp_srv(gripper_command.pos, gripper_command.speed)
else:
response = self.gripper_move_srv(gripper_command.pos, gripper_command.speed)
if response == 0:
self.action_server.set_succeeded()
else:
rospy.logwarn("WSG grasp controller: gripper goal not achieved for pre-grasp or release")
self.action_server.set_succeeded()
def set_weiss_griper(self, width):
cmd = Cmd()
cmd.pos = width
cmd.speed = 100.
self.weiss_pub.publish(cmd)
def ctrl_callback(self, status):
print('status', status)
cmd = Cmd()
cmd.pos = 4.
cmd.speed = 100.
self.pub.publish(cmd)