def ros_goal(self, userdata, default_goal):
#import time
#while True:
# time.sleep(.01)
return pm.Pr2GripperCommandGoal(
pm.Pr2GripperCommand(position=self.gripper_size / 100.,
max_effort=self.effort))
def open(self, block, position=0.1, effort=-1):
self.client.send_goal(
pm.Pr2GripperCommandGoal(
pm.Pr2GripperCommand(position=position, max_effort=effort)))
if block:
self.client.wait_for_result()
return self.client.get_state()
def move(self, position, max_effort=-1):
goal = pr2c.Pr2GripperCommandGoal(
pr2c.Pr2GripperCommand(position=position, max_effort=max_effort))
self._ac.send_goal(goal)
rospy.loginfo("Sending goal to gripper and waiting for result...")
self._ac.wait_for_result()
rospy.loginfo("Gripper action returned")
if self._ac.get_state() != am.GoalStatus.SUCCEEDED:
raise ex.ActionFailedError()
def set_gripper(self, grasp, max_effort=None, block=True):
"""
:param grasp: in meters
:param max_effort: max force in Newtons
:param block: if True, waits until grasp completed
"""
assert self.min_grasp <= grasp <= self.max_grasp
max_effort = max_effort if max_effort is not None else self.default_max_effort
self.gripper_command_client.send_goal(pcm.Pr2GripperCommandGoal(pcm.Pr2GripperCommand(position=grasp,max_effort=max_effort)))
#self.gripper_command_pub.publish(pcm.Pr2GripperCommand(position=grasp,max_effort=max_effort))
if block:
timeout = utils.Timeout(10)
timeout.start()
last_grasp = self.current_grasp + 1e-4 # slight offset to trigger loop
while(np.abs(self.current_grasp - max(grasp,0)) > .005 and np.abs(last_grasp - self.current_grasp) > 1e-5 and not timeout.has_timed_out()):
last_grasp = self.current_grasp
rospy.sleep(0.5)
def set_angle(self, a, max_effort=default_max_effort):
self.grip_client.send_goal(
pcm.Pr2GripperCommandGoal(
pcm.Pr2GripperCommand(position=a, max_effort=max_effort)))
self.pr2.start_thread(Thread(target=self.grip_client.wait_for_result))
