def main():
# find objects on the table
rospy.loginfo("Waiting for find_cluster_bounding_box2 service")
find_box = rospy.ServiceProxy("/find_cluster_bounding_box", FindClusterBoundingBox)
detector = TablewareDetection(table_thickness=0.1)
det = detector.detect_objects(point_head_at=None)
table = det.table
table_dims = (table.x_max - table.x_min, table.y_max - table.y_min)
rospy.loginfo("Table dims: %s", table_dims)
objs = []
for goal in det.pickup_goals:
pointcloud = goal.target.cluster
box = find_box(pointcloud)
rospy.loginfo("----- Found label %s", goal.label)
dims = box.box_dims
pos = (box.pose.pose.position.x, box.pose.pose.position.y)
obj = ( (pos[0] - dims.x/2 - table.x_min, pos[1] - dims.y/2 - table.y_min), (dims.x, dims.y))
rospy.loginfo("Obj tuple: %s", obj)
objs.append(obj)
obj_dim = [(0.1, 0.10)]
spots = free_spots(table_dims, obj_dim, 0.01, objs)
rospy.loginfo("Spots: %s", spots)
