hbmem usb摄像头的二维码检测

path_follower_demo.py

@@ -32,7 +32,7 @@ class PathFollower(Node):
         # ── PID 参数 ──
         self.declare_parameter("kp", 0.8)
         self.declare_parameter("ki", 0.0)
-        self.declare_parameter("kd", 0.05)
+        self.declare_parameter("kd", 0.1)
         self.declare_parameter("linear_speed", 0.3)     # 前进速度 m/s
         self.declare_parameter("waypoint_tolerance", 0.1)  # 到达判定距离 m
         self.declare_parameter("max_angular", 10.0)       # 最大角速度 rad/s

src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml

@@ -7,7 +7,7 @@
   <BehaviorTree ID="MainTree">
     <RecoveryNode number_of_retries="6" name="NavigateRecovery">
       <PipelineSequence name="NavigateWithReplanning">
-        <RateController hz="1.0">
+        <RateController hz="0.5">
           <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">

src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml

@@ -19,12 +19,12 @@ slam_toolbox:
 
     # === 调试与性能 ===
     debug_logging: false
-    throttle_scans: 1
+    throttle_scans: 2
     transform_publish_period: 0.02
     map_update_interval: 3.0
     resolution: 0.05
     min_laser_range: 0.15
-    max_laser_range: 20.0
+    max_laser_range: 10.0
     minimum_time_interval: 0.5
     transform_timeout: 0.2
     tf_buffer_duration: 30.0

src/qr_detection/CMakeLists.txt

@@ -1,70 +1,35 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(qr_detection)
 
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# Find dependencies
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
-find_package(dnn_node REQUIRED)
-# find_package(hbm_img_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(ai_msgs REQUIRED)
 find_package(OpenCV REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(ZBar REQUIRED zbar)
 
-# rosidl_generate_interfaces(${PROJECT_NAME}
+add_executable(qr_detect src/qr_detect.cpp)
-#   "msg/SampleMessage.msg"
+target_include_directories(qr_detect PUBLIC
-# )
-
-# # Define executables
-# add_executable(talker src/publisher_hbmem.cpp)
-# ...
-
-# add_executable(qr_dete_node src/qr_dete.cpp)
-# ...
-
-# install(DIRECTORY
-#   ${PROJECT_SOURCE_DIR}/launch/
-#   DESTINATION share/${PROJECT_NAME}/launch)
-
-# ---- qr_dete_depth_node (sensor_msgs::Image, no hbmem) ----
-add_executable(qr_dete_depth_node src/qr_dete_depth.cpp)
-ament_target_dependencies(qr_dete_depth_node
-  rclcpp
-  std_msgs
-  sensor_msgs
-  geometry_msgs
-)
-
-target_include_directories(qr_dete_depth_node PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
   ${OpenCV_INCLUDE_DIRS} ${ZBar_INCLUDE_DIRS})
+target_link_libraries(qr_detect
+  ${OpenCV_LIBS} ${ZBar_LIBRARIES})
+ament_target_dependencies(qr_detect
+  rclcpp std_msgs sensor_msgs)
 
-target_link_libraries(qr_dete_depth_node
+install(TARGETS qr_detect
-  ${OpenCV_LIBS} ${ZBar_LIBRARIES}
-)
-
-install(TARGETS qr_dete_depth_node
   DESTINATION lib/${PROJECT_NAME})
 
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)

src/qr_detection/launch/qr_detect.launch.py

@@ -0,0 +1,26 @@
+"""
+qr_detect.launch.py — 二维码识别（USB / MIPI 通用）
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    image_topic = LaunchConfiguration("image_topic", default="/image_raw")
+
+    return LaunchDescription([
+        DeclareLaunchArgument("image_topic",
+            default_value="/image_raw",
+            description="相机图像话题 (bgr8 / rgb8 / nv12 / mono8)"),
+
+        Node(
+            package="qr_detection",
+            executable="qr_detect",
+            name="qr_detect",
+            output="screen",
+            parameters=[{"image_topic": image_topic}],
+        ),
+    ])

src/qr_detection/launch/qr_usb_camera.launch.py

@@ -0,0 +1,65 @@
+"""
+qr_usb_camera.launch.py — 启动 USB 相机 + 二维码识别
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    image_topic    = LaunchConfiguration("image_topic",    default="/image_raw")
+    camera_device  = LaunchConfiguration("camera_device",  default="/dev/video0")
+    result_legal   = LaunchConfiguration("result_legal",   default="true")
+    sign_on_detect = LaunchConfiguration("sign_on_detect", default="true")
+    use_buffer     = LaunchConfiguration("use_buffer",     default="true")
+
+    # USB 相机驱动 (v4l2)
+    v4l2_camera = Node(
+        package="v4l2_camera",
+        executable="v4l2_camera_node",
+        name="usb_camera",
+        parameters=[{
+            "video_device": camera_device,
+            "image_size": [640, 480],
+            "pixel_format": "YUYV",
+        }],
+        remappings=[("image_raw", image_topic)],
+    )
+
+    # QR 识别
+    qr_detector = Node(
+        package="qr_detection",
+        executable="qr_usb_camera",
+        name="qr_usb_camera",
+        output="screen",
+        parameters=[{
+            "image_topic":     image_topic,
+            "result_legal":    result_legal,
+            "sign_on_detect":  sign_on_detect,
+            "use_buffer":      use_buffer,
+            "buffer_timeout":  3.0,
+            "legal_strings":   ["1", "2", "顺时针", "逆时针", "顺", "逆"],
+        }],
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument("image_topic",
+            default_value="/image_raw",
+            description="相机图像话题"),
+        DeclareLaunchArgument("camera_device",
+            default_value="/dev/video0",
+            description="USB 相机设备"),
+        DeclareLaunchArgument("result_legal",
+            default_value="true",
+            description="只发布合法结果"),
+        DeclareLaunchArgument("sign_on_detect",
+            default_value="true",
+            description="检测到后发 sign4return=5"),
+        DeclareLaunchArgument("use_buffer",
+            default_value="true",
+            description="信号消失后保持最近结果"),
+        v4l2_camera,
+        qr_detector,
+    ])

src/qr_detection/msg/QrDetection.msg

@@ -0,0 +1,17 @@
+# QR 检测结果（含位置信息，用于导航逼近）
+std_msgs/Header header
+
+# 解码文本
+string data
+
+# 二维码在图像中的位置（4 个角点，归一化坐标 0~1）
+# points[0]=左上, points[1]=右上, points[2]=右下, points[3]=左下
+float64[4] corner_x
+float64[4] corner_y
+
+# 二维码中心在图像中的归一化坐标 (0~1)
+float64 center_x
+float64 center_y
+
+# 是否检测到二维码
+bool detected

src/qr_detection/src/qr_detect.cpp

@@ -0,0 +1,117 @@
+/**
+ * qr_detect.cpp — RDKx5 二维码识别 (标准 sensor_msgs::Image)
+ *
+ * 同时支持:
+ *   - USB 相机: 直接订阅 /image_raw (rgb8/bgr8/mono8)
+ *   - MIPI 相机: hobot_codec 解码后订阅 /image (nv12→bgr8)
+ *
+ * 结果仅发布到 qr_results。sign4return=0 开 / =5 关。
+ */
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/image.hpp"
+
+#include <opencv2/opencv.hpp>
+#include <zbar.h>
+
+
+class QrDetectNode : public rclcpp::Node
+{
+public:
+  explicit QrDetectNode(const rclcpp::NodeOptions &opts = rclcpp::NodeOptions())
+    : Node("qr_detect", opts), enabled_(true)
+  {
+    image_topic_ = declare_parameter("image_topic", "/image_raw");
+
+    sub_ = create_subscription<sensor_msgs::msg::Image>(
+        image_topic_, rclcpp::SensorDataQoS(),
+        std::bind(&QrDetectNode::cb, this, std::placeholders::_1));
+
+    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
+        "sign4return", 10,
+        std::bind(&QrDetectNode::sign_cb, this, std::placeholders::_1));
+
+    pub_ = create_publisher<std_msgs::msg::String>("qr_results", 10);
+
+    RCLCPP_INFO(get_logger(), "QR Detect ready, topic=%s", image_topic_.c_str());
+  }
+
+private:
+  void cb(const sensor_msgs::msg::Image::SharedPtr msg)
+  {
+    if (!enabled_) return;
+
+    const std::string &enc = msg->encoding;
+    cv::Mat gray;
+
+    // 编码自适应
+    if (enc == "mono8" || enc == "8UC1") {
+      gray = cv::Mat(msg->height, msg->width, CV_8UC1,
+                     const_cast<uint8_t*>(msg->data.data())).clone();
+    } else if (enc == "rgb8") {
+      cv::Mat rgb(msg->height, msg->width, CV_8UC3,
+                  const_cast<uint8_t*>(msg->data.data()));
+      cv::cvtColor(rgb, gray, cv::COLOR_RGB2GRAY);
+    } else if (enc == "bgr8") {
+      cv::Mat bgr(msg->height, msg->width, CV_8UC3,
+                  const_cast<uint8_t*>(msg->data.data()));
+      cv::cvtColor(bgr, gray, cv::COLOR_BGR2GRAY);
+    } else if (enc == "nv12" || enc == "NV12") {
+      cv::Mat nv12(msg->height * 3 / 2, msg->width, CV_8UC1,
+                   const_cast<uint8_t*>(msg->data.data()));
+      cv::Mat bgr;
+      cv::cvtColor(nv12, bgr, cv::COLOR_YUV2BGR_NV12);
+      cv::cvtColor(bgr, gray, cv::COLOR_BGR2GRAY);
+    } else if (enc == "yuv422_yuy2" || enc == "yuyv") {
+      cv::Mat yuv(msg->height, msg->width, CV_8UC2,
+                  const_cast<uint8_t*>(msg->data.data()));
+      cv::cvtColor(yuv, gray, cv::COLOR_YUV2GRAY_YUY2);
+    } else {
+      static int warn_count = 0;
+      if (warn_count++ < 3)
+        RCLCPP_WARN(get_logger(), "Unsupported encoding: %s", enc.c_str());
+      return;
+    }
+
+    // ZBar
+    zbar::ImageScanner scanner;
+    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
+    zbar::Image zimg(gray.cols, gray.rows, "Y800",
+                     gray.data, gray.cols * gray.rows);
+    int n = scanner.scan(zimg);
+
+    auto out = std_msgs::msg::String();
+    if (n > 0) {
+      out.data = zimg.symbol_begin()->get_data();
+      if (out.data != last_) {
+        RCLCPP_INFO(get_logger(), "QR: %s", out.data.c_str());
+        last_ = out.data;
+      }
+    }
+    pub_->publish(out);
+  }
+
+  void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
+  {
+    if (msg->data == 0)      { enabled_ = true;  RCLCPP_INFO(get_logger(), "ON"); }
+    else if (msg->data == 5) { enabled_ = false; RCLCPP_INFO(get_logger(), "OFF"); }
+  }
+
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr  sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr     sign_sub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr        pub_;
+
+  bool        enabled_;
+  std::string image_topic_;
+  std::string last_;
+};
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<QrDetectNode>());
+  rclcpp::shutdown();
+  return 0;
+}

src/qr_detection/src/qr_hbmem.cpp

@@ -0,0 +1,95 @@
+/**
+ * qr_hbmem.cpp — RDKx5 零拷贝二维码识别
+ *
+ * 通过 hbmem 订阅 NV12 图像（零拷贝），ZBar 识别二维码，
+ * 结果仅发布到 qr_results (std_msgs/String)。
+ * sign4return 控制启停。
+ */
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "hbm_img_msgs/msg/hbm_msg1080_p.hpp"
+
+#include <opencv2/opencv.hpp>
+#include <zbar.h>
+
+
+class QrHbmemNode : public rclcpp::Node
+{
+public:
+  explicit QrHbmemNode(const rclcpp::NodeOptions &opts = rclcpp::NodeOptions())
+    : Node("qr_hbmem", opts), detect_enabled_(true), last_found_("")
+  {
+    // hbmem 零拷贝订阅
+    using HbmMsg = hbm_img_msgs::msg::HbmMsg1080P;
+    hbmem_sub_ = this->create_subscription_hbmem<HbmMsg>(
+      "hbmem_img", 10,
+      std::bind(&QrHbmemNode::callback, this, std::placeholders::_1));
+
+    // sign4return 启停控制
+    sign_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+      "sign4return", 10,
+      std::bind(&QrHbmemNode::sign_cb, this, std::placeholders::_1));
+
+    // 结果发布
+    result_pub_ = this->create_publisher<std_msgs::msg::String>("qr_results", 10);
+
+    RCLCPP_INFO(this->get_logger(), "QR Hbmem ready");
+  }
+
+private:
+  void callback(const hbm_img_msgs::msg::HbmMsg1080P::SharedPtr msg)
+  {
+    if (!detect_enabled_) return;
+
+    // NV12 → BGR → Gray
+    cv::Mat nv12(msg->height * 3 / 2, msg->width, CV_8UC1,
+                 const_cast<uint8_t *>(msg->data.data()));
+    cv::Mat bgr;
+    cv::cvtColor(nv12, bgr, cv::COLOR_YUV2BGR_NV12);
+    cv::Mat gray;
+    cv::cvtColor(bgr, gray, cv::COLOR_BGR2GRAY);
+
+    // ZBar
+    zbar::ImageScanner scanner;
+    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
+    zbar::Image zimg(gray.cols, gray.rows, "Y800",
+                     gray.data, gray.cols * gray.rows);
+    int n = scanner.scan(zimg);
+
+    std::string text;
+    if (n > 0) {
+      text = zimg.symbol_begin()->get_data();
+      if (text != last_found_) {
+        RCLCPP_INFO(this->get_logger(), "QR: %s", text.c_str());
+        last_found_ = text;
+      }
+    }
+
+    auto out = std_msgs::msg::String();
+    out.data = text;
+    result_pub_->publish(out);
+  }
+
+  void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
+  {
+    if (msg->data == 0)      { detect_enabled_ = true;  RCLCPP_INFO(this->get_logger(), "ON");  }
+    else if (msg->data == 5) { detect_enabled_ = false; RCLCPP_INFO(this->get_logger(), "OFF"); }
+  }
+
+  rclcpp::SubscriptionHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr hbmem_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr                      sign_sub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr                        result_pub_;
+
+  bool        detect_enabled_;
+  std::string last_found_;
+};
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<QrHbmemNode>());
+  rclcpp::shutdown();
+  return 0;
+}

src/qr_detection/src/qr_usb_camera.cpp

@@ -0,0 +1,307 @@
+/**
+ * qr_usb_camera.cpp — USB 相机二维码识别节点
+ *
+ * 订阅普通 USB 相机的 sensor_msgs::Image → 识别二维码并发布结果
+ * 相比旧版 qr_dete.cpp 的改进:
+ *   1. 图像话题可配置 (参数化)
+ *   2. 编码自适应 (bgr8 / rgb8 / mono8 / yuv422)
+ *   3. 输出二维码位置信息 (用于导航逼近)
+ *   4. 结果合法性过滤 (白名单可配)
+ *   5. 缓冲区管理 (信号消失后清空)
+ *   6. 减少日志刷屏
+ */
+
+#include <memory>
+#include <iostream>
+#include <unordered_set>
+#include <vector>
+#include <string>
+#include <cmath>
+
+#include <opencv2/opencv.hpp>
+#include <zbar.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "qr_detection/msg/qr_detection.hpp"
+
+
+class QrUsbCameraNode : public rclcpp::Node
+{
+public:
+  QrUsbCameraNode()
+  : Node("qr_usb_camera"),
+    detect_enabled_(true),
+    qr_found_(false),
+    qr_lost_count_(0),
+    prev_qr_text_("")
+  {
+    // ── 参数声明 ──
+    image_topic_     = declare_parameter("image_topic", "/image_raw");
+    result_legal_    = declare_parameter("result_legal", false);
+    legal_strings_   = declare_parameter("legal_strings",
+                        std::vector<std::string>{"1","2","顺时针","逆时针","顺","逆"});
+    use_buffer_      = declare_parameter("use_buffer", true);
+    buffer_timeout_  = declare_parameter("buffer_timeout", 3.0);   // 信号消失 N 秒后清缓存
+    sign_on_detect_  = declare_parameter("sign_on_detect", true);  // 检测到后发 sign4return
+    sign_value_      = declare_parameter("sign_value", 5);
+    qos_depth_       = declare_parameter("qos_depth", 10);
+
+    // ── 订阅 image ──
+    image_sub_ = create_subscription<sensor_msgs::msg::Image>(
+        image_topic_, rclcpp::QoS(qos_depth_),
+        std::bind(&QrUsbCameraNode::image_callback, this, std::placeholders::_1));
+
+    // ── 订阅 sign4return (远程控制开关) ──
+    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
+        "sign4return", 10,
+        std::bind(&QrUsbCameraNode::sign_callback, this, std::placeholders::_1));
+
+    // ── 发布者 ──
+    qr_pub_      = create_publisher<qr_detection::msg::QrDetection>("qr_detection", 10);
+    qr_text_pub_ = create_publisher<std_msgs::msg::String>("qr_text", 10);
+    sign_pub_    = create_publisher<std_msgs::msg::Int32>("sign4return", 10);
+
+    // ── 定时器: 检查缓冲区超时 ──
+    buffer_timer_ = create_wall_timer(
+        std::chrono::milliseconds(500),
+        std::bind(&QrUsbCameraNode::buffer_check, this));
+
+    RCLCPP_INFO(get_logger(),
+        "QR USB Camera node ready\n"
+        "  image_topic: %s\n"
+        "  result_legal: %s\n"
+        "  use_buffer: %s (timeout=%.1fs)\n"
+        "  sign_on_detect: %s (value=%ld)",
+        image_topic_.c_str(),
+        result_legal_ ? "true" : "false",
+        use_buffer_ ? "true" : "false", buffer_timeout_,
+        sign_on_detect_ ? "true" : "false", sign_value_);
+  }
+
+private:
+  // ── 图像回调 ──
+  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
+  {
+    if (!detect_enabled_) return;
+
+    // 延时
+    auto now = get_clock()->now();
+    auto msg_time = rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec);
+    auto delay_ms = (now - msg_time).nanoseconds() / 1e6;
+
+    // 解码 → OpenCV
+    cv::Mat gray;
+    if (!image_to_gray(msg, gray)) return;
+
+    // ZBar 扫描
+    zbar::ImageScanner scanner;
+    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
+    zbar::Image zbar_img(gray.cols, gray.rows, "Y800",
+                         gray.data, gray.cols * gray.rows);
+    scanner.scan(zbar_img);
+
+    // 构造消息
+    auto result = qr_detection::msg::QrDetection();
+    result.header = msg->header;
+    result.detected = false;
+    result.data = "";
+
+    bool found_now = false;
+    std::string all_texts;
+
+    for (auto sym = zbar_img.symbol_begin();
+         sym != zbar_img.symbol_end(); ++sym)
+    {
+      std::string text = sym->get_data();
+      all_texts += (all_texts.empty() ? "" : "|") + text;
+
+      // 合法性过滤（只取第一个合法的）
+      if (!found_now && (!result_legal_ || is_legal(text))) {
+        result.data     = text;
+        result.detected = true;
+        found_now       = true;
+
+        // 提取位置
+        int n = sym->get_location_size();
+        if (n == 4) {
+          for (int i = 0; i < 4; ++i) {
+            result.corner_x[i] = sym->get_location_x(i) / (double)gray.cols;
+            result.corner_y[i] = sym->get_location_y(i) / (double)gray.rows;
+          }
+          // 中心
+          result.center_x = (result.corner_x[0] + result.corner_x[2]) * 0.5;
+          result.center_y = (result.corner_y[0] + result.corner_y[2]) * 0.5;
+        }
+      }
+    }
+
+    // ── 状态转换处理 ──
+    if (found_now) {
+      qr_lost_count_ = 0;
+      if (!qr_found_) {
+        // 刚发现 → 发 sign4return
+        qr_found_ = true;
+        buffer_text_ = result.data;
+        RCLCPP_INFO(get_logger(), "QR DETECTED: \"%s\" delay=%.0fms",
+                    result.data.c_str(), delay_ms);
+
+        if (sign_on_detect_) {
+          auto sign = std_msgs::msg::Int32();
+          sign.data = sign_value_;
+          sign_pub_->publish(sign);
+        }
+      }
+      // 持续检测: 只在新文本出现时打印
+      if (result.data != prev_qr_text_) {
+        RCLCPP_INFO(get_logger(), "QR changed: \"%s\" → \"%s\"",
+                    prev_qr_text_.c_str(), result.data.c_str());
+        prev_qr_text_ = result.data;
+        buffer_text_  = result.data;
+      }
+    } else {
+      qr_lost_count_++;
+      if (qr_found_ && qr_lost_count_ > 4) {  // ~2s 无信号
+        qr_found_ = false;
+        RCLCPP_INFO(get_logger(), "QR LOST (was: \"%s\")",
+                    prev_qr_text_.c_str());
+      }
+    }
+
+    // ── 发布 ──
+    qr_pub_->publish(result);  // 带位置的结构化消息
+
+    // 文本消息 (带缓冲)
+    auto text_msg = std_msgs::msg::String();
+    text_msg.data = use_buffer_ ? buffer_text_
+                                : (found_now ? result.data : "");
+    qr_text_pub_->publish(text_msg);
+  }
+
+  // ── 缓冲区超时检查 ──
+  void buffer_check()
+  {
+    if (!qr_found_ && !buffer_text_.empty()) {
+      buffer_time_acc_ += 0.5;
+      if (buffer_time_acc_ >= buffer_timeout_) {
+        buffer_text_.clear();
+        buffer_time_acc_ = 0.0;
+        prev_qr_text_.clear();
+        RCLCPP_INFO(get_logger(), "Buffer cleared (QR lost for %.1fs)",
+                    buffer_timeout_);
+      }
+    } else {
+      buffer_time_acc_ = 0.0;
+    }
+  }
+
+  // ── sign4return 回调 ──
+  void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
+  {
+    if (msg->data == 0) {
+      detect_enabled_ = true;
+      RCLCPP_INFO(get_logger(), "QR detection ENABLED");
+    } else if (msg->data == 5) {
+      detect_enabled_ = false;
+      RCLCPP_INFO(get_logger(), "QR detection DISABLED");
+    }
+  }
+
+  // ── Image → Grayscale (编码自适应) ──
+  static bool image_to_gray(const sensor_msgs::msg::Image::SharedPtr &msg,
+                            cv::Mat &gray)
+  {
+    const std::string &enc = msg->encoding;
+
+    if (enc == "mono8" || enc == "8UC1") {
+      gray = cv::Mat(msg->height, msg->width, CV_8UC1,
+                     const_cast<uint8_t*>(msg->data.data())).clone();
+      return true;
+    }
+
+    if (enc == "rgb8") {
+      cv::Mat rgb(msg->height, msg->width, CV_8UC3,
+                  const_cast<uint8_t*>(msg->data.data()));
+      cv::cvtColor(rgb, gray, cv::COLOR_RGB2GRAY);
+      return true;
+    }
+
+    if (enc == "bgr8") {
+      cv::Mat bgr(msg->height, msg->width, CV_8UC3,
+                  const_cast<uint8_t*>(msg->data.data()));
+      cv::cvtColor(bgr, gray, cv::COLOR_BGR2GRAY);
+      return true;
+    }
+
+    if (enc == "bgra8") {
+      cv::Mat bgra(msg->height, msg->width, CV_8UC4,
+                   const_cast<uint8_t*>(msg->data.data()));
+      cv::cvtColor(bgra, gray, cv::COLOR_BGRA2GRAY);
+      return true;
+    }
+
+    if (enc == "yuv422_yuy2" || enc == "yuyv") {
+      cv::Mat yuv(msg->height, msg->width, CV_8UC2,
+                  const_cast<uint8_t*>(msg->data.data()));
+      cv::cvtColor(yuv, gray, cv::COLOR_YUV2GRAY_YUY2);
+      return true;
+    }
+
+    // 不支持的类型
+    static std::set<std::string> warned;
+    if (warned.find(enc) == warned.end()) {
+      warned.insert(enc);
+      RCLCPP_ERROR(rclcpp::get_logger("qr_usb_camera"),
+          "Unsupported encoding: %s (h=%d, w=%d, step=%d)",
+          enc.c_str(), msg->height, msg->width, msg->step);
+    }
+    return false;
+  }
+
+  // ── 合法性检查 ──
+  bool is_legal(const std::string &text)
+  {
+    const auto &whitelist = get_parameter("legal_strings").as_string_array();
+    for (const auto &s : whitelist) {
+      if (text.find(s) != std::string::npos) return true;
+    }
+    return false;
+  }
+
+  // ── 成员变量 ──
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr     sign_sub_;
+  rclcpp::Publisher<qr_detection::msg::QrDetection>::SharedPtr  qr_pub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr           qr_text_pub_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr            sign_pub_;
+  rclcpp::TimerBase::SharedPtr buffer_timer_;
+
+  // 参数
+  std::string image_topic_;
+  bool        result_legal_;
+  bool        use_buffer_;
+  double      buffer_timeout_;
+  bool        sign_on_detect_;
+  int64_t     sign_value_;
+  int         qos_depth_;
+  std::vector<std::string> legal_strings_;
+
+  // 状态
+  bool        detect_enabled_;
+  bool        qr_found_;
+  int         qr_lost_count_;
+  std::string prev_qr_text_;
+  std::string buffer_text_;
+  double      buffer_time_acc_ = 0.0;
+};
+
+// ──────────────────────────────
+int main(int argc, char *argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<QrUsbCameraNode>());
+  rclcpp::shutdown();
+  return 0;
+}