From 630cb182773b75442250981d25fc3311e34088bb Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Mon, 22 Jun 2026 21:54:12 +0800 Subject: [PATCH] =?UTF-8?q?hbmem=20usb=E6=91=84=E5=83=8F=E5=A4=B4=E7=9A=84?= =?UTF-8?q?=E4=BA=8C=E7=BB=B4=E7=A0=81=E6=A3=80=E6=B5=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- path_follower_demo.py | 2 +- .../config/nav_to_pose_ackermann.xml | 2 +- .../config/slam_toolbox_mapping.yaml | 4 +- src/qr_detection/CMakeLists.txt | 63 +--- src/qr_detection/launch/qr_detect.launch.py | 26 ++ .../launch/qr_usb_camera.launch.py | 65 ++++ src/qr_detection/msg/QrDetection.msg | 17 + src/qr_detection/src/qr_detect.cpp | 117 +++++++ src/qr_detection/src/qr_hbmem.cpp | 95 ++++++ src/qr_detection/src/qr_usb_camera.cpp | 307 ++++++++++++++++++ 10 files changed, 645 insertions(+), 53 deletions(-) create mode 100644 src/qr_detection/launch/qr_detect.launch.py create mode 100644 src/qr_detection/launch/qr_usb_camera.launch.py create mode 100644 src/qr_detection/msg/QrDetection.msg create mode 100644 src/qr_detection/src/qr_detect.cpp create mode 100644 src/qr_detection/src/qr_hbmem.cpp create mode 100644 src/qr_detection/src/qr_usb_camera.cpp diff --git a/path_follower_demo.py b/path_follower_demo.py index 03536d0..aba09a6 100755 --- a/path_follower_demo.py +++ b/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 diff --git a/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml b/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml index 8746cd1..f8961fe 100644 --- a/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml +++ b/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml @@ -7,7 +7,7 @@ - + diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml index 44a3d68..5282408 100644 --- a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml +++ b/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 diff --git a/src/qr_detection/CMakeLists.txt b/src/qr_detection/CMakeLists.txt index 6813adf..63aa873 100644 --- a/src/qr_detection/CMakeLists.txt +++ b/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} -# "msg/SampleMessage.msg" -# ) - -# # 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 - $ - $ +add_executable(qr_detect src/qr_detect.cpp) +target_include_directories(qr_detect PUBLIC ${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 - ${OpenCV_LIBS} ${ZBar_LIBRARIES} -) - -install(TARGETS qr_dete_depth_node +install(TARGETS qr_detect 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) diff --git a/src/qr_detection/launch/qr_detect.launch.py b/src/qr_detection/launch/qr_detect.launch.py new file mode 100644 index 0000000..b5fbc3c --- /dev/null +++ b/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}], + ), + ]) diff --git a/src/qr_detection/launch/qr_usb_camera.launch.py b/src/qr_detection/launch/qr_usb_camera.launch.py new file mode 100644 index 0000000..7577018 --- /dev/null +++ b/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, + ]) diff --git a/src/qr_detection/msg/QrDetection.msg b/src/qr_detection/msg/QrDetection.msg new file mode 100644 index 0000000..b89fc4d --- /dev/null +++ b/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 diff --git a/src/qr_detection/src/qr_detect.cpp b/src/qr_detection/src/qr_detect.cpp new file mode 100644 index 0000000..50969c2 --- /dev/null +++ b/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 +#include + + +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( + image_topic_, rclcpp::SensorDataQoS(), + std::bind(&QrDetectNode::cb, this, std::placeholders::_1)); + + sign_sub_ = create_subscription( + "sign4return", 10, + std::bind(&QrDetectNode::sign_cb, this, std::placeholders::_1)); + + pub_ = create_publisher("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(msg->data.data())).clone(); + } else if (enc == "rgb8") { + cv::Mat rgb(msg->height, msg->width, CV_8UC3, + const_cast(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(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(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(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::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sign_sub_; + rclcpp::Publisher::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()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/qr_detection/src/qr_hbmem.cpp b/src/qr_detection/src/qr_hbmem.cpp new file mode 100644 index 0000000..957ffc1 --- /dev/null +++ b/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 +#include + + +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( + "hbmem_img", 10, + std::bind(&QrHbmemNode::callback, this, std::placeholders::_1)); + + // sign4return 启停控制 + sign_sub_ = this->create_subscription( + "sign4return", 10, + std::bind(&QrHbmemNode::sign_cb, this, std::placeholders::_1)); + + // 结果发布 + result_pub_ = this->create_publisher("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(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::SharedPtr hbmem_sub_; + rclcpp::Subscription::SharedPtr sign_sub_; + rclcpp::Publisher::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()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/qr_detection/src/qr_usb_camera.cpp b/src/qr_detection/src/qr_usb_camera.cpp new file mode 100644 index 0000000..a18dc55 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#include +#include + +#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{"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( + image_topic_, rclcpp::QoS(qos_depth_), + std::bind(&QrUsbCameraNode::image_callback, this, std::placeholders::_1)); + + // ── 订阅 sign4return (远程控制开关) ── + sign_sub_ = create_subscription( + "sign4return", 10, + std::bind(&QrUsbCameraNode::sign_callback, this, std::placeholders::_1)); + + // ── 发布者 ── + qr_pub_ = create_publisher("qr_detection", 10); + qr_text_pub_ = create_publisher("qr_text", 10); + sign_pub_ = create_publisher("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(msg->data.data())).clone(); + return true; + } + + if (enc == "rgb8") { + cv::Mat rgb(msg->height, msg->width, CV_8UC3, + const_cast(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(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(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(msg->data.data())); + cv::cvtColor(yuv, gray, cv::COLOR_YUV2GRAY_YUY2); + return true; + } + + // 不支持的类型 + static std::set 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::SharedPtr image_sub_; + rclcpp::Subscription::SharedPtr sign_sub_; + rclcpp::Publisher::SharedPtr qr_pub_; + rclcpp::Publisher::SharedPtr qr_text_pub_; + rclcpp::Publisher::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 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()); + rclcpp::shutdown(); + return 0; +}