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)