From 300febd48abfbcad76fd826aec900fee44802a42 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Fri, 24 Jul 2026 17:11:19 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E4=BA=8C=E7=BB=B4?= =?UTF-8?q?=E7=A0=81=E6=A3=80=E6=B5=8B=E7=9A=84=E4=B8=80=E4=BA=9B=E5=B0=8F?= =?UTF-8?q?bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/qr_detection/CMakeLists.txt | 13 +- src/qr_detection/package.xml | 7 +- src/qr_detection/src/qr_detect.cpp | 180 ++++++++++++++++++++--- src/qr_detection/test/test_qr_detect.cpp | 108 ++++++++++++++ 4 files changed, 289 insertions(+), 19 deletions(-) create mode 100644 src/qr_detection/test/test_qr_detect.cpp diff --git a/src/qr_detection/CMakeLists.txt b/src/qr_detection/CMakeLists.txt index 63aa873..845fb9c 100644 --- a/src/qr_detection/CMakeLists.txt +++ b/src/qr_detection/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(origincar_msg REQUIRED) find_package(OpenCV REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(ZBar REQUIRED zbar) @@ -22,7 +23,7 @@ target_include_directories(qr_detect PUBLIC target_link_libraries(qr_detect ${OpenCV_LIBS} ${ZBar_LIBRARIES}) ament_target_dependencies(qr_detect - rclcpp std_msgs sensor_msgs) + rclcpp std_msgs sensor_msgs origincar_msg) install(TARGETS qr_detect DESTINATION lib/${PROJECT_NAME}) @@ -31,7 +32,17 @@ install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) + ament_add_gtest(test_qr_detect test/test_qr_detect.cpp) + if(TARGET test_qr_detect) + target_include_directories(test_qr_detect PUBLIC + ${OpenCV_INCLUDE_DIRS} ${ZBar_INCLUDE_DIRS}) + target_link_libraries(test_qr_detect + ${OpenCV_LIBS} ${ZBar_LIBRARIES}) + ament_target_dependencies(test_qr_detect + rclcpp std_msgs sensor_msgs origincar_msg) + endif() set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() diff --git a/src/qr_detection/package.xml b/src/qr_detection/package.xml index e6e8971..1f982b0 100644 --- a/src/qr_detection/package.xml +++ b/src/qr_detection/package.xml @@ -10,12 +10,17 @@ ament_cmake rosidl_default_generators rosidl_default_runtime - rosidl_interface_packages rclcpp + origincar_msg + sensor_msgs + std_msgs + ament_cmake_gtest ament_lint_auto ament_lint_common + rosidl_interface_packages + ament_cmake diff --git a/src/qr_detection/src/qr_detect.cpp b/src/qr_detection/src/qr_detect.cpp index 225e13a..6d1ff84 100644 --- a/src/qr_detection/src/qr_detect.cpp +++ b/src/qr_detection/src/qr_detect.cpp @@ -1,21 +1,27 @@ /** - * qr_detect.cpp — RDKx5 二维码识别 (标准 sensor_msgs::Image) + * qr_detect.cpp — RDKx5 二维码识别 (sensor_msgs::Image / CompressedImage) * * 同时支持: - * - USB 相机: 直接订阅 /image_raw (rgb8/bgr8/mono8) + * - USB 相机: 直接订阅 /image_raw (rgb8/bgr8/mono8) 或 JPEG 压缩 /image * - MIPI 相机: hobot_codec 解码后订阅 /image (nv12→bgr8) * - * 结果仅发布到 qr_results。sign4return=0 开 / =5 关。 + * 结果发布到 qr_results。sign4return=0 开 / =5 关。 */ +#include "origincar_msg/srv/speak.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/int32.hpp" #include "std_msgs/msg/string.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" #include "sensor_msgs/msg/image.hpp" #include #include +#include + +using namespace std::chrono_literals; + class QrDetectNode : public rclcpp::Node { @@ -23,29 +29,129 @@ public: explicit QrDetectNode(const rclcpp::NodeOptions &opts = rclcpp::NodeOptions()) : Node("qr_detect", opts), enabled_(true) { - image_topic_ = declare_parameter("image_topic", "/image_raw"); + image_topic_ = declare_parameter("image_topic", "/image"); + image_type_ = declare_parameter("image_type", "auto"); - sub_ = create_subscription( - image_topic_, rclcpp::SensorDataQoS(), - std::bind(&QrDetectNode::cb, this, std::placeholders::_1)); + if (image_type_ == "raw") { + subscribe_raw_image(); + } else if (image_type_ == "compressed") { + subscribe_compressed_image(); + } else if (image_type_ == "auto") { + discovery_timer_ = create_wall_timer( + 500ms, std::bind(&QrDetectNode::auto_select_image_subscription, this)); + auto_select_image_subscription(); + } else { + RCLCPP_WARN( + get_logger(), "Unknown image_type=%s, falling back to auto discovery", image_type_.c_str()); + image_type_ = "auto"; + discovery_timer_ = create_wall_timer( + 500ms, std::bind(&QrDetectNode::auto_select_image_subscription, this)); + auto_select_image_subscription(); + } sign_sub_ = create_subscription( "sign4return", 10, std::bind(&QrDetectNode::sign_cb, this, std::placeholders::_1)); pub_ = create_publisher("qr_results", 10); - pub_tts_ = create_publisher("/vlm_result", 10); + tts_client_ = create_client("/tts/speak"); - RCLCPP_INFO(get_logger(), "QR Detect ready, topic=%s, tts topic=/vlm_result", image_topic_.c_str()); + RCLCPP_INFO( + get_logger(), + "QR Detect ready, topic=%s, image_type=%s", + image_topic_.c_str(), image_type_.c_str()); } private: - void cb(const sensor_msgs::msg::Image::SharedPtr msg) + void subscribe_raw_image() + { + if (image_sub_ || compressed_image_sub_) { + return; + } + + image_sub_ = create_subscription( + image_topic_, rclcpp::SensorDataQoS(), + std::bind(&QrDetectNode::image_cb, this, std::placeholders::_1)); + stop_discovery_timer(); + RCLCPP_INFO(get_logger(), "QR image input selected: sensor_msgs/Image on %s", image_topic_.c_str()); + } + + void subscribe_compressed_image() + { + if (image_sub_ || compressed_image_sub_) { + return; + } + + compressed_image_sub_ = create_subscription( + image_topic_, rclcpp::SensorDataQoS(), + std::bind(&QrDetectNode::compressed_image_cb, this, std::placeholders::_1)); + stop_discovery_timer(); + RCLCPP_INFO( + get_logger(), "QR image input selected: sensor_msgs/CompressedImage on %s", + image_topic_.c_str()); + } + + void stop_discovery_timer() + { + if (discovery_timer_) { + discovery_timer_->cancel(); + } + } + + void auto_select_image_subscription() + { + if (image_sub_ || compressed_image_sub_) { + stop_discovery_timer(); + return; + } + + const auto endpoints = get_publishers_info_by_topic(image_topic_); + for (const auto &endpoint : endpoints) { + if (endpoint.topic_type() == "sensor_msgs/msg/CompressedImage") { + subscribe_compressed_image(); + return; + } + } + + for (const auto &endpoint : endpoints) { + if (endpoint.topic_type() == "sensor_msgs/msg/Image") { + subscribe_raw_image(); + return; + } + } + } + + void image_cb(const sensor_msgs::msg::Image::SharedPtr msg) { if (!enabled_) return; - const std::string &enc = msg->encoding; cv::Mat gray; + if (!raw_image_to_gray(msg, gray)) { + return; + } + + scan_and_publish(gray); + } + + void compressed_image_cb(const sensor_msgs::msg::CompressedImage::SharedPtr msg) + { + if (!enabled_) return; + + cv::Mat gray = cv::imdecode(msg->data, cv::IMREAD_GRAYSCALE); + if (gray.empty()) { + static int warn_count = 0; + if (warn_count++ < 3) { + RCLCPP_WARN(get_logger(), "Failed to decode compressed image: format=%s", msg->format.c_str()); + } + return; + } + + scan_and_publish(gray); + } + + bool raw_image_to_gray(const sensor_msgs::msg::Image::SharedPtr msg, cv::Mat &gray) + { + const std::string &enc = msg->encoding; // 编码自适应 if (enc == "mono8" || enc == "8UC1") { @@ -73,9 +179,14 @@ private: static int warn_count = 0; if (warn_count++ < 3) RCLCPP_WARN(get_logger(), "Unsupported encoding: %s", enc.c_str()); - return; + return false; } + return true; + } + + void scan_and_publish(cv::Mat &gray) + { // ZBar zbar::ImageScanner scanner; scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1); @@ -94,25 +205,60 @@ private: if (!qr_data.empty() && qr_data.find_first_not_of("0123456789") == std::string::npos) { out.data = qr_data + " " + (((qr_data.back() - '0') % 2 == 1) ? "顺时针" : "逆时针"); pub_->publish(out); - pub_tts_->publish(out); // 同时发给TTS播报 + speak_once(out.data); } } } + void speak_once(const std::string &text) + { + if (text == last_spoken_) { + return; + } + last_spoken_ = text; + + if (!tts_client_->service_is_ready()) { + static int warn_count = 0; + if (warn_count++ < 3) { + RCLCPP_WARN(get_logger(), "TTS service /tts/speak not available"); + } + return; + } + + auto request = std::make_shared(); + request->text = text; + tts_client_->async_send_request( + request, + [this](rclcpp::Client::SharedFuture future) { + try { + const auto response = future.get(); + if (!response->success) { + RCLCPP_WARN(get_logger(), "TTS failed: %s", response->message.c_str()); + } + } catch (const std::exception &e) { + RCLCPP_ERROR(get_logger(), "TTS call error: %s", e.what()); + } + }); + } + 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_; - rclcpp::Publisher::SharedPtr pub_tts_; // 发给TTS语音播报 + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Subscription::SharedPtr compressed_image_sub_; + rclcpp::Subscription::SharedPtr sign_sub_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Client::SharedPtr tts_client_; + rclcpp::TimerBase::SharedPtr discovery_timer_; bool enabled_; std::string image_topic_; + std::string image_type_; std::string last_; + std::string last_spoken_; }; int main(int argc, char **argv) diff --git a/src/qr_detection/test/test_qr_detect.cpp b/src/qr_detection/test/test_qr_detect.cpp new file mode 100644 index 0000000..2b4dfb4 --- /dev/null +++ b/src/qr_detection/test/test_qr_detect.cpp @@ -0,0 +1,108 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +#define private public +#define main qr_detect_node_main +#include "../src/qr_detect.cpp" +#undef main +#undef private + +namespace +{ + +bool has_subscription_type( + const std::vector & endpoints, + const std::string & topic_type) +{ + return std::any_of( + endpoints.begin(), endpoints.end(), + [&](const rclcpp::TopicEndpointInfo & endpoint) { + return endpoint.topic_type() == topic_type; + }); +} + +} // namespace + +TEST(QrDetectNode, subscribesToCompressedImageWhenCompressedPublisherExists) +{ + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + auto camera_node = std::make_shared("qr_detect_compressed_camera_test"); + auto camera_pub = camera_node->create_publisher( + "/qr_test_image", rclcpp::SensorDataQoS()); + + rclcpp::NodeOptions options; + options.append_parameter_override("image_topic", "/qr_test_image"); + auto qr_node = std::make_shared(options); + auto graph_node = std::make_shared("qr_detect_graph_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(camera_node); + executor.add_node(qr_node); + executor.add_node(graph_node); + + std::vector endpoints; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3); + while (std::chrono::steady_clock::now() < deadline) { + sensor_msgs::msg::CompressedImage msg; + msg.format = "jpeg"; + msg.data = {0xff, 0xd8, 0xff, 0xd9}; + camera_pub->publish(msg); + executor.spin_some(); + endpoints = graph_node->get_subscriptions_info_by_topic("/qr_test_image"); + if (has_subscription_type(endpoints, "sensor_msgs/msg/CompressedImage")) { + break; + } + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_TRUE(has_subscription_type(endpoints, "sensor_msgs/msg/CompressedImage")); +} + +TEST(QrDetectNode, publishesQrResultsOnly) +{ + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + auto qr_node = std::make_shared(); + auto graph_node = std::make_shared("qr_detect_publication_graph_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(qr_node); + executor.add_node(graph_node); + + std::vector qr_results_publishers; + std::vector vlm_result_publishers; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3); + while (std::chrono::steady_clock::now() < deadline) { + executor.spin_some(); + qr_results_publishers = graph_node->get_publishers_info_by_topic("/qr_results"); + vlm_result_publishers = graph_node->get_publishers_info_by_topic("/vlm_result"); + if (has_subscription_type(qr_results_publishers, "std_msgs/msg/String")) { + break; + } + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_TRUE(has_subscription_type(qr_results_publishers, "std_msgs/msg/String")); + EXPECT_FALSE(has_subscription_type(vlm_result_publishers, "std_msgs/msg/String")); +} + +TEST(QrDetectNode, createsTtsSpeakClient) +{ + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + auto qr_node = std::make_shared(); + ASSERT_NE(qr_node->tts_client_, nullptr); + EXPECT_STREQ(qr_node->tts_client_->get_service_name(), "/tts/speak"); +}