穹测技能包修改输入方式，添加单次检测的物体id标签

.gitignore

@@ -1,5 +1,5 @@
-.idea/
-.vscode/
+**/.idea/
+**/.vscode/
 **/build/
 **/install/
 **/log/

vision_detect/.idea/.gitignore

@@ -1,8 +0,0 @@
-# 默认忽略的文件
-/shelf/
-/workspace.xml
-# 基于编辑器的 HTTP 客户端请求
-/httpRequests/
-# Datasource local storage ignored files
-/dataSources/
-/dataSources.local.xml

vision_detect/.idea/cv_part.iml

@@ -1,11 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module type="PYTHON_MODULE" version="4">
-  <component name="NewModuleRootManager">
-    <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.10" jdkType="Python SDK" />
-    <orderEntry type="sourceFolder" forTests="false" />
-  </component>
-  <component name="TestRunnerService">
-    <option name="PROJECT_TEST_RUNNER" value="py.test" />
-  </component>
-</module>

vision_detect/.idea/dictionaries/project.xml

@@ -1,7 +0,0 @@
-<component name="ProjectDictionaryState">
-  <dictionary name="project">
-    <words>
-      <w>rclpy</w>
-    </words>
-  </dictionary>
-</component>

vision_detect/.idea/inspectionProfiles/profiles_settings.xml

@@ -1,6 +0,0 @@
-<component name="InspectionProjectProfileManager">
-  <settings>
-    <option name="USE_PROJECT_PROFILE" value="false" />
-    <version value="1.0" />
-  </settings>
-</component>

vision_detect/.idea/misc.xml

@@ -1,7 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="Black">
-    <option name="sdkName" value="Python 3.10" />
-  </component>
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.10" project-jdk-type="Python SDK" />
-</project>

vision_detect/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/cv_part.iml" filepath="$PROJECT_DIR$/.idea/cv_part.iml" />
-    </modules>
-  </component>
-</project>

vision_detect/vision_detect/VisionDetect/node/detect_node.py

@@ -487,7 +487,9 @@ class DetectNode(Node):
 
         # mask_img and box_img is or not output
         if self.output_boxes and not self.output_masks:
-            draw_box(rgb_img, result)
+            home = os.path.expanduser("~")
+            save_path = os.path.join(home, "detect_image.png")
+            draw_box(rgb_img, result, save_path=save_path)
             return self.cv_bridge.cv2_to_imgmsg(rgb_img, "bgr8"), pose_list
         elif self.output_boxes and self.output_masks:
             draw_box(rgb_img, result)

vision_detect/vision_detect/VisionDetect/utils/draw_tools.py

@@ -1,15 +1,23 @@
+import os
 import logging
+from typing import Union
 
 import cv2
 import numpy as np
 import open3d as o3d
 
 
+
 __all__ = [
     "draw_box", "draw_mask", "draw_pointcloud",
 ]
 
-def draw_box(rgb_img: np.ndarray, segmentation_result, put_text: bool = True):
+def draw_box(
+        rgb_img: np.ndarray,
+        segmentation_result,
+        put_text: bool = True,
+        save_path: Union[bool, str] = False
+):
     """
     绘制目标检测边界框
     """
@@ -26,9 +34,23 @@ def draw_box(rgb_img: np.ndarray, segmentation_result, put_text: bool = True):
         cv2.rectangle(rgb_img, p1, p2, (255, 255, 0), 2)
 
         if put_text:
-            cv2.putText(rgb_img, f'{labels[class_ids[i]]}: {confidences[i]*100:.2f}', (p1[0], p1[1] - 10),
+            cv2.putText(rgb_img, f'{labels[class_ids[i]]}: {confidences[i]*100:.2f}',
+                        (p1[0], p1[1] - 10), cv2.FONT_HERSHEY_SIMPLEX, 1,
+                        (255, 255, 0), 2)
+
+            cv2.putText(rgb_img, f"{i}", (p1[0] + 5, p1[1] + 15),
                         cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
 
+    if save_path:
+        if isinstance(save_path, str):
+            dir_path = os.path.dirname(save_path)
+            if dir_path:
+                os.makedirs(dir_path, exist_ok=True)
+        else:
+            home_path = os.path.expanduser("~")
+            save_path = os.path.join(home_path, "detect_color_img.png")
+        cv2.imwrite(save_path, rgb_img)
+
 
 def draw_mask(rgb_img: np.ndarray, segmentation_result):
     """
@@ -57,7 +79,11 @@ def draw_pointcloud(pcd, axis: bool = True):
             [1, 0, 0], [0, 1, 0], [0, 0, 1]
         ]
         # 构造open3d中的LineSet对象，用于主成分显示
-        axis_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(axis_point), lines=o3d.utility.Vector2iVector(axis))
+        axis_set = o3d.geometry.LineSet(
+            points=o3d.utility.Vector3dVector(axis_point),
+            lines=o3d.utility.Vector2iVector(axis)
+        )
+
         axis_set.colors = o3d.utility.Vector3dVector(axis_colors)
 
         pcd.append(axis_set)

vision_detect/vision_detect/flexivaidk_detect_service.py

@@ -322,29 +322,42 @@ class DetectNode(Node):
 
         time3 = time.time()
 
-        rgb_bytes = cv2.imencode('.png', rgb_img)[1]
-        depth_bytes = cv2.imencode('.png', depth_img)[1]
+        # rgb_bytes = cv2.imencode('.png', rgb_img)[1]
+        # depth_bytes = cv2.imencode('.png', depth_img)[1]
 
         for i, (mask, box) in enumerate(zip(masks, boxes)):
             mask = cv2.resize(mask.astype(np.uint8), orig_shape[::-1], interpolation=cv2.INTER_NEAREST)
+            rgb_crop, depth_crop, mask_crop, (x_min, y_min) = crop_mask_bbox(rgb_img, depth_img, mask, box)
+
+            if depth_crop is None:
+                self.get_logger().error("depth_crop is None")
+                continue
+
+            depth_img_crop_mask = np.zeros_like(depth_crop)
+            depth_img_crop_mask[mask_crop > 0] = depth_crop[mask_crop > 0]
+
+            rgb_bytes = cv2.imencode('.png', rgb_crop)[1]
+            depth_bytes = cv2.imencode('.png', depth_img_crop_mask)[1]
             
             x_center, y_center, width, height = box[:4]
 
             p1 = [int((x_center - width / 2)), int((y_center - height / 2))]
             p2 = [int((x_center + width / 2)), int((y_center + height / 2))]
             cv2.rectangle(img, p1, p2, (255, 255, 0), 2)
+            cv2.putText(img, f"{i}", (p1[0], p1[1] - 10), cv2.FONT_HERSHEY_SIMPLEX,
+                        1, (255, 255, 0), 2)
 
-            contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-            mask_contours = contours[0].reshape(1, -1, 2)
-            res = self.aidk_client.set_direct_setting_variables({"one_mask": f"{mask_contours}"})
+            # contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+            # mask_contours = contours[0].reshape(1, -1, 2)
+            # res = self.aidk_client.set_direct_setting_variables({"one_mask": f"{mask_contours}"})
 
             intrinsics = [
                 int(self.camera_size[0]),
                 int(self.camera_size[1]),
-                # self.K[2] - x_min,
-                # self.K[5] - y_min,
-                self.K[2],
-                self.K[5],
+                self.K[2] - x_min,
+                self.K[5] - y_min,
+                # self.K[2],
+                # self.K[5],
                 self.K[0],
                 self.K[4]
             ]