add 预处理模块测试节点、图像质量判断功能

vision_detect/configs/launch/default_action_config.json

@@ -28,7 +28,9 @@
     "preprocess_configs": {
         "distortion": false,
         "denoising": false,
-        "enhancement": false
+        "enhancement": false,
+        "quality": false,
+        "quality_threshold": 100.0
     },
     "driver_source_configs": {
         "subscription_name": "/img_msg"

vision_detect/configs/launch/default_service_config.json

@@ -25,6 +25,13 @@
     },
 
     "image_source": "DRIVER",
+    "preprocess_configs": {
+        "distortion": false,
+        "denoising": false,
+        "enhancement": false,
+        "quality": false,
+        "quality_threshold": 100.0
+    },
     "driver_source_configs": {
         "subscription_name": "/img_msg"
     },

vision_detect/configs/launch/source_test_config.json

@@ -0,0 +1,47 @@
+{
+    "node_name": "source_test_node",
+    "output_boxes": false,
+    "output_masks": false,
+    "save_image": false,
+    "image_save_dir": "~/images",
+
+    "calibration": {
+        "left_hand": "calibration/eye_in_left_hand.json",
+        "right_hand": "calibration/eye_in_right_hand.json",
+        "head": "calibration/eye_to_hand.json"
+    },
+
+    "node_mode": "SERVICE",
+    "service_node_configs": {
+        "service_name": "/vision_object_recognition"
+    },
+    "publisher_node_configs": {
+        "publish_time": 0.1,
+        "position": "right",
+        "publisher_name": "/source_test/processed_image"
+    },
+    "action_node_configs": {
+        "action_name": "/vision_object_recognition"
+    },
+
+    "image_source": "DIRECT",
+    "direct_source_configs": {
+        "position": "right",
+        "color_image_topic_name": "/camera/color/image_raw",
+        "depth_image_topic_name": "/camera/depth/image_raw",
+        "camera_info_topic_name": "/camera/color/camera_info"
+    },
+    "preprocess_configs": {},
+
+    "detect_mode": "OBJECT",
+    "object_detector_configs": {},
+    "color_detector_configs": {},
+    "crossboard_detector_configs": {},
+
+    "estimate_mode": "PCA",
+    "pca_estimator_configs": {},
+    "icp_estimator_configs": {},
+    "gsnet_estimator_configs": {},
+
+    "refine_mode": "FIXED"
+}

vision_detect/map/logging/report_logging_define.json

@@ -18,6 +18,8 @@
     "0201": "Receive wrong position, or this position have no camera data",
     "0202": "All input position have no camera data",
 
+    "0210": "The image is too blurry.",
+
     "0300": "Worker thread is not alive",
     "0301": "Can't submit task, task executor is already stop",
     "0302": "Task is aborted",

vision_detect/setup.py

@@ -67,6 +67,7 @@ setup(
             'get_camera_pose_node = vision_detect.get_camera_pose:main',
             
             'detect_node = vision_detect.detect_node:main',
+            'source_test_node = vision_detect.source_test_node:main',
 
             'service_client_node = vision_detect.service_client:main',
             'test_action_client = vision_detect.action_client_node:main',

vision_detect/vision_detect/source_test_node.py

@@ -0,0 +1,36 @@
+"""
+source_test 节点的创建与启动入口。
+节点类定义位于 vision_core.node_test.source_test。
+"""
+import sys
+
+import rclpy
+from rclpy.executors import MultiThreadedExecutor
+
+from vision_core import SourceTestNode
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    # 支持通过 --config 指定配置文件
+    config_path = None
+    for i, arg in enumerate(sys.argv):
+        if arg == "--config" and i + 1 < len(sys.argv):
+            config_path = sys.argv[i + 1]
+            break
+
+    node = SourceTestNode(config_path=config_path)
+    try:
+        executor = MultiThreadedExecutor()
+        rclpy.spin(node, executor=executor)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

vision_detect/vision_detect/vision_core/__init__.py

@@ -1,3 +1,4 @@
 from .node import NodeManager
+from .node_test import *
 
-__all__ = ["NodeManager"]
+__all__ = ["NodeManager", "SourceTestNode"]

vision_detect/vision_detect/vision_core/core/components/image_providers/source_baseline.py

@@ -64,6 +64,8 @@ class SourceBaseline:
         self.distortion_switch = config.get("distortion", False)
         self.denoising_switch = config.get("denoising", False)
         self.enhancement_switch = config.get("enhancement", False)
+        self.quality_switch = config.get("quality", False)
+        self.quality_threshold = config.get("quality_threshold", 100.0)
 
     def get_images(self, positions: tuple[str, ...]) -> tuple[ImageDataContainer | None, int]:
         time_start = time.time()
@@ -96,6 +98,14 @@ class SourceBaseline:
             color_img_cv = self.cv_bridge.imgmsg_to_cv2(data.image_color, "bgr8")
             depth_img_cv = self.cv_bridge.imgmsg_to_cv2(data.image_depth, '16UC1')
 
+            if self.quality_switch:
+                gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+                laplacian_source = cv2.Laplacian(gray, cv2.CV_64F).var()
+                if laplacian_source < self.quality_threshold:
+                    image_data.add_data(position, Status.IMAGE_QUALITY_LOW)
+                    continue
+
+
             # 畸变矫正
             if self.distortion_switch:
                 camera_size = color_img_cv.shape[:2][::-1]

vision_detect/vision_detect/vision_core/core/enum/logging_code_enum.py

@@ -22,6 +22,8 @@ class Status(IntEnum):
     NO_POSITION_DATA = 201
     NO_ALL_POSITION_DATA = 202
 
+    IMAGE_QUALITY_LOW = 210
+
     WORKER_NOT_ALIVE = 300
     EXECUTOR_ALREADY_STOP = 301
     TASK_ABORTED = 302

vision_detect/vision_detect/vision_core/node_test/__init__.py

@@ -0,0 +1 @@
+from .source_test import *

vision_detect/vision_detect/vision_core/node_test/source_test.py

@@ -0,0 +1,131 @@
+"""
+仅使用 SingleTopicSource 的测试节点。
+通过 topic 订阅相机图像，处理后通过 topic 发布。
+"""
+import time
+import os
+
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+
+from ..core import SourceManager
+from ..core.enum import SourceType, Status
+from ..logging_map import LOGGING_MAP
+
+
+__all__ = ["SourceTestNode"]
+
+
+def _check(code: int, node: Node) -> bool:
+    """检查获取图像是否成功"""
+    if code != Status.SUCCESS:
+        node.get_logger().error(LOGGING_MAP.get(f"{code:04d}", f"Unknown code: {code}"))
+        return False
+    return True
+
+
+class SourceTestNode(Node):
+    """
+    仅使用 SingleTopicSource 的测试节点。
+    - 使用 SingleTopicSource (DIRECT 模式) 订阅相机 color/depth 图像
+    - 定时获取处理后的图像并通过 topic 发布
+    """
+
+    def __init__(self, config_path: str | None = None):
+        super().__init__("source_test_node")
+
+        # 加载配置
+        if config_path is None:
+            try:
+                from ament_index_python.packages import get_package_share_directory
+                pkg_share = get_package_share_directory("vision_detect")
+                config_path = os.path.join(pkg_share, "configs", "launch", "source_test_config.json")
+            except Exception:
+                self.get_logger().warn("无法获取 package share 目录，使用默认配置")
+
+        self._load_config(config_path)
+        self._cv_bridge = CvBridge()
+
+        # 仅创建 SourceManager（使用 SingleTopicSource）
+        self._source_manager = SourceManager(
+            self._source_mode,
+            self._source_config,
+            self
+        )
+        self.get_logger().info("SingleTopicSource 已就绪")
+
+        # 创建定时器和发布者
+        self._timer = self.create_timer(
+            self._publish_time,
+            self._timer_callback
+        )
+        self._publisher = self.create_publisher(
+            Image,
+            self._output_topic,
+            10
+        )
+        self.get_logger().info(f"将发布处理后的图像到: {self._output_topic}")
+
+    def _load_config(self, config_path: str | None):
+        """加载配置，仅提取 source 和 publisher 相关参数"""
+        # 默认配置（使用 SingleTopicSource）
+        self._source_mode = SourceType.DIRECT
+        self._source_config = {
+            "position": "right",
+            "color_image_topic_name": "/camera/color/image_raw",
+            "depth_image_topic_name": "/camera/depth/image_raw",
+            "camera_info_topic_name": "/camera/color/camera_info",
+        }
+        self._position = "right"
+        self._publish_time = 0.1
+        self._output_topic = "/source_test/processed_image"
+        if config_path and os.path.exists(config_path):
+            import json
+            with open(config_path, "r", encoding="utf-8") as f:
+                config = json.load(f)
+
+            # 本节点固定使用 SingleTopicSource (DIRECT)，从 direct_source_configs 读取
+            direct_cfg = config.get("direct_source_configs", {})
+            if direct_cfg:
+                self._source_config.update(direct_cfg)
+                self._position = self._source_config.get("position", "right")
+            preprocess = config.get("preprocess_configs") or {}
+            self._source_config.update(preprocess)
+
+            # publisher 配置
+            pub_cfg = config.get("publisher_node_configs", {})
+            self._publish_time = pub_cfg.get("publish_time", 0.1)
+            self._output_topic = pub_cfg.get("publisher_name", self._output_topic)
+
+    def _timer_callback(self):
+        """定时回调：获取图像并发布处理后的彩色图"""
+        self.get_logger().debug("source_test: 获取图像...")
+        time_start = time.time()
+
+        # 从 SingleTopicSource 获取图像（已包含畸变矫正、去噪、增强等处理）
+        image_container, code = self._source_manager.get_images(
+            positions=[self._position]
+        )
+
+        if not _check(code, self):
+            return
+
+        image_container.check_data_status(self.get_logger())
+
+        # 获取处理后的彩色图像并发布
+        for pos, data in image_container:
+            if data.status != Status.SUCCESS:
+                continue
+            if data.color_image is None:
+                continue
+
+            # 转换为 ROS Image 并发布
+            msg = self._cv_bridge.cv2_to_imgmsg(data.color_image, encoding="bgr8")
+            msg.header.stamp = self.get_clock().now().to_msg()
+            msg.header.frame_id = "camera_detect"
+            self._publisher.publish(msg)
+
+            elapsed = (time.time() - time_start) * 1000
+            self.get_logger().info(f"已发布处理后的图像 ({pos}), 耗时: {elapsed:.1f} ms")
+            break