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 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)