def _execute(self, goal):
print '\n\n\n\n', goal, '\n\n\n\n'
goalPosition = UPPER_LIMIT - goal.command.position / 10.0
# ignore effort for now
# check gripper is ready to accept commands
if not self._stat.is_ready:
print 'Gripper reporting not ready'
self._action_server.set_aborted(
self._generate_result_msg(goalPosition))
return
# verify position is valid
if goalPosition < LOWER_LIMIT or goalPosition > UPPER_LIMIT:
print 'Gripper position out of range'
self._action_server.set_aborted(
self._generate_result_msg(goalPosition))
return
# send gripper command
cmd = GripperCmd()
cmd.position = goalPosition
cmd.speed = 0.1
cmd.force = 100.0
self._cmdPub.publish(cmd)
rospy.sleep(0.05)
# move gripper
preempted = False
while self._stat.is_moving:
if self._action_server.is_preempt_requested():
cmd = GripperCmd()
cmd.stop = True
self._cmdPub.publish(cmd)
preempted = True
break
rospy.sleep(0.05)
self._action_server.publish_feedback(
self._generate_feedback_msg(goalPosition))
# final result
result = self._generate_result_msg(goalPosition)
if preempted:
self._action_server.set_preempted(result)
elif result.reached_goal:
self._action_server.set_succeeded(result)
else:
print goalPosition, '!=', self._stat.position
self._action_server.set_aborted(result)
def _execute(self, goal):
goalPosition = UPPER_LIMIT - goal.command.position / 10.0
# check gripper is ready to accept commands
if not self._stat.is_ready:
rospy.logerr('Gripper reporting not ready')
self._action_server.set_aborted(self._generate_result_msg(goalPosition))
return
# verify position is valid
if goalPosition < LOWER_LIMIT or goalPosition > UPPER_LIMIT:
rospy.logerr('Gripper position out of range')
self._action_server.set_aborted(self._generate_result_msg(goalPosition))
return
# send gripper move command
cmd = GripperCmd()
cmd.position = goalPosition
cmd.speed = self._speed
cmd.force = 100.0
self._cmdPub.publish(cmd)
rospy.sleep(0.5)
# move gripper
preempted = False
while self._stat.is_moving:
if self._action_server.is_preempt_requested():
# send gripper halt command
cmd = GripperCmd()
cmd.stop = True
self._cmdPub.publish(cmd)
preempted = True
break
rospy.sleep(self._loop_delay)
self._action_server.publish_feedback(self._generate_feedback_msg(goalPosition))
# final result
result = self._generate_result_msg(goalPosition)
if preempted:
self._action_server.set_preempted(result)
elif result.reached_goal:
self._action_server.set_succeeded(result)
else:
rospy.logwarn('Gripper did not reach goal position')
self._action_server.set_succeeded(result)
def set_gripper(id, cmd):
gripper_cmd = GripperCmd()
gripper_cmd.force = 50.0
gripper_cmd.speed = 0.03
gripper_cmd.stop = False
gripper_cmd.emergency_release = False
gripper_cmd.emergency_release_dir = 0
if cmd == 'open':
gripper_cmd.position = 0.75
elif cmd == 'close':
gripper_cmd.position = 0.0
if id == 'left':
left_gripper_pub.publish(gripper_cmd)
elif id == "right":
right_gripper_pub.publish(gripper_cmd)