def markers_callback(self, msg):
object_poses = dict()
self.objects = ["kinect", "table", "tripod", "boundary1", "boundary2"]
for object in self.objects:
self.last_time_seen[object] = rospy.Time.now()
if len(msg.objects) == 0:
rospy.logerr("No objects identified")
return
for object in msg.objects:
self.last_time_seen[object.type.key] = rospy.Time.now()
pose = GraspingHelper.getPoseStampedFromPoseWithCovariance(
object.pose)
self.add_object_at_pose(str(object.type.key), pose)
object_poses[str(object.type.key)] = pose
self.objects.append(object.type.key)
marker = MoveHelper.create_pose_marker(pose, object.type.key, 2,
15, (1, 0, 0, 1))
self.markers_publisher.publish(marker)
def pick_and_place(self, objects, object_poses):
for object, pose in object_poses.iteritems():
marker = MoveHelper.create_pose_marker(pose, object, 2, 15,
(0, 1, 0, 1))
self.markers_publisher.publish(marker)
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
