def pick_and_place(self, objects, object_poses):
self.is_picking = True
grasps = None
object_name = ""
random.shuffle(objects)
for object in objects:
object_name = object.type.key
rospy.loginfo("Getting grasp for object " + object_name)
graspResponse = self.graspService(object_name)
if graspResponse.success:
rospy.loginfo("grasps were found for object " + object_name)
grasps = graspResponse.grasps
break
if grasps is None:
rospy.logerr(
"Failed to find any grasps for the objects identified")
self.is_picking = False
return
rospy.loginfo("Finding a valid place pose")
place_poses = self.getValidPlacePoses()
rospy.loginfo("Attempting to pick up object " + object_name)
MoveHelper.move_to_neutral("left", True)
pickSuccess = False
try:
pickSuccess = self.pick(object_poses[object_name], object_name)
except Exception as e:
traceback.print_exc()
#if isinstance(e, TypeError):
# pickSuccess = True
#else:
raise e
finally:
self.is_placing = pickSuccess
self.is_picking = False
if not pickSuccess:
rospy.logerr("Object pick up failed")
return
place_result = False
try:
for place_pose in place_poses:
rospy.loginfo("Attempting to place object")
if self.place(object_name, object_poses[object_name],
place_pose):
break
except Exception as e:
traceback.print_exc()
raise e
finally:
self.is_placing = False
def pick_and_place(self, object_id, object_pose, destination_point):
object_name = object_id
print("Attempting to pick up object " + object_name)
self.is_picking = True
rospy.loginfo("Finding a valid place pose")
place_poses = self.getValidPlacePoses(destination_point)
rospy.loginfo("Attempting to pick up object " + object_name)
MoveHelper.move_to_neutral("left", True)
pickSuccess = False
try:
pickSuccess = self.pick(object_pose, object_name)
rospy.loginfo("Pick success: " + str(pickSuccess))
except Exception as e:
traceback.print_exc()
raise e
finally:
self.is_placing = pickSuccess
self.is_picking = False
if not pickSuccess:
rospy.logerr("Object pick up failed")
return
place_result = False
try:
for place_pose in place_poses:
rospy.loginfo("Attempting to place object")
if self.place(object_name, object_pose, place_pose):
break
except Exception as e:
traceback.print_exc()
raise e
finally:
self.is_placing = False
def go(self, args):
#moveit_commander.roscpp_initialize(args)
#left_arm = baxter_interface.limb.Limb("left")
#left_arm.move_to_neutral()
object_name = "spoon"
MoveHelper.move_to_neutral("left", True)
self.scene.remove_world_object(object_name)
rospy.sleep(5.0)
pose = self.addObject(object_name)
place_poses = self.getValidPlacePoses()
pickSuccess = False
try:
pickSuccess = self.pick(pose, object_name)
except Exception as e:
traceback.print_exc()
if isinstance(e, TypeError):
pickSuccess = True
else:
raise e
if not pickSuccess:
rospy.logerr("Object pick up failed")
return
place_result = False
try:
for place_pose in place_poses:
rospy.loginfo("Attempting to place object")
rospy.loginfo(str(place_pose))
if self.place(object_name, place_pose):
break
except Exception as e:
traceback.print_exc()
raise e
def go(self, args):
moveit_commander.roscpp_initialize(args)
MoveHelper.move_to_neutral("left", True)
rospy.sleep(5.0)
rospy.spin()
