def detect_tabletop_objects(self): DETECT_ERROR = 0. cbbf = ClusterBoundingBoxFinder(tf_listener=self.gman.cm.tf_listener) object_detector = rospy.ServiceProxy("/object_detection", TabletopSegmentation) try: detects = object_detector() object_detector.close() except ServiceException as se: rospy.logerr("Tabletop segmentation crashed") return [] if detects.result != 4: rospy.logerr("Detection failed (err %d)" % (detects.result)) return [] table_z = detects.table.pose.pose.position.z objects = [] for cluster in detects.clusters: (object_points, object_bounding_box_dims, object_bounding_box, object_to_base_link_frame, object_to_cluster_frame) = cbbf.find_object_frame_and_bounding_box(cluster) # rospy.loginfo("bounding box:", object_bounding_box) (object_pos, object_quat) = cf.mat_to_pos_and_quat(object_to_cluster_frame) angs = euler_from_quaternion(object_quat) # position is half of height object_pos[2] = table_z + object_bounding_box[1][2] / 2. + DETECT_ERROR objects += [[object_pos, angs[2]]] return objects
#!/usr/bin/env python from object_manipulator.cluster_bounding_box_finder import ClusterBoundingBoxFinder from tabletop_object_detector.srv import TabletopDetection from tabletop_object_detector.msg import TabletopDetectionResult cbbf = ClusterBoundingBoxFinder() object_detector = rospy.ServiceProxy("/object_detection", TabletopDetection) detects = object_detector(True, False).detection object_detector.close() if detects.result != 4: err("Detection failed (err %d)" % (detects.result)) table_z = detects.table.pose.pose.position.z objects = [] for cluster in detects.clusters: (object_points, object_bounding_box_dims, object_bounding_box, object_to_base_link_frame, object_to_cluster_frame) = cbbf.find_object_frame_and_bounding_box(cluster) # log("bounding box:", object_bounding_box) (object_pos, object_quat) = cf.mat_to_pos_and_quat(object_to_cluster_frame) angs = euler_from_quaternion(object_quat) log("angs:", angs) # position is half of height object_pos[2] = table_z + object_bounding_box[1][2] / 2. + DETECT_ERROR log("pos:", object_pos) log("table_z:", table_z) objects += [[object_pos, angs[2]]]