增加建议的夹爪宽度以及物体中心深度估计

vision_detect/vision_detect/VisionDetect/node/detect_node.py

@@ -2,7 +2,6 @@ import os
 import time
 import threading
 import json
-from collections import defaultdict
 from ament_index_python.packages import get_package_share_directory
 
 import cv2
@@ -26,7 +25,7 @@ from interfaces.srv import VisionObjectRecognition
 
 
 from ..utils import calculate_pose_pca, calculate_pose_icp, get_map, distortion_correction, crop_imgs_box_xywh, \
-    draw_box, draw_mask, crop_imgs_mask
+    draw_box, draw_mask, crop_imgs_mask, calculate_grav_width
 
 share_dir = get_package_share_directory('vision_detect')
 
@@ -427,6 +426,9 @@ class DetectNode(Node):
                 self.configs
             )
 
+            grab_width = calculate_grav_width(mask, self.K, z)
+            z = z + grab_width * 0.45
+
             if (x, y, z) != (0.0, 0.0, 0.0):
                 pose = Pose()
                 pose.position = Point(x=x, y=y, z=z)
@@ -436,7 +438,7 @@ class DetectNode(Node):
                         "class_id": int(class_ids[i]),
                         "class_name": labels[class_ids[i]],
                         "pose": pose,
-                        "grap_width": getattr(result, "double_value")
+                        "grap_width": grab_width
                     }
                 )
 
@@ -517,7 +519,7 @@ class DetectNode(Node):
                 }
             )
 
-        return self.cv_bridge.cv2_to_imgmsg(rgb_img, "bgr8"), pose_dict
+        return self.cv_bridge.cv2_to_imgmsg(rgb_img, "bgr8"), pose_list
 
     def _seg_crossboard(self, rgb_img, depth_img):
         pose_list = []

vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py

@@ -2,6 +2,7 @@ import math
 import json
 import logging
 
+import cv2
 import numpy as np
 import open3d as o3d
 import transforms3d as tfs
@@ -152,3 +153,21 @@ def calculate_pose_icp(
     rw, rx, ry, rz = tfs.quaternions.mat2quat(rmat[0:3, 0:3])
 
     return x, y, z, rw, rx, ry, rz
+
+
+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
+
+