1

vision_detect/vision_detect/detect_bottle_service.py

@@ -288,6 +288,22 @@ def crop_mask_bbox(depth_img, mask, box):
     return depth_crop, mask_crop, (max(0, x_min), max(0, y_min))
 
 
+def calculate_grav_width(mask, K, depth):
+    """计算重心宽度"""
+    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    if contours:
+        box = cv2.boxPoints(cv2.minAreaRect(contours[0]))
+        if np.linalg.norm(box[1] - box[0]) < np.linalg.norm(box[1] - box[2]):
+            point_diff = box[1] - box[0]
+        else:
+            point_diff = box[1] - box[2]
+
+        grab_width = depth * np.sqrt(point_diff[0]**2 / K[0]**2 + point_diff[1]**2 / K[4]**2)
+        return grab_width
+    else:
+        return 0.0
+
+
 class DetectNode(Node):
     def __init__(self, name, mode):
         super().__init__(name)
@@ -505,6 +521,9 @@ class DetectNode(Node):
 
             x, y, z, rw, rx, ry, rz = self.calculate_function(mask_crop, depth_crop, intrinsics, self.source_model)
 
+            grab_width = calculate_grav_width(mask, self.K, z)
+            z = z + grab_width * 0.45
+
             if (x, y, z) == (None, None, None):
                 self.get_logger().error("have wrong pose")
                 continue