修改了二维码检测的一些小bug

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()

src/qr_detection/package.xml

@@ -10,12 +10,17 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>rosidl_default_generators</build_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
-  <member_of_group>rosidl_interface_packages</member_of_group>
   <depend>rclcpp</depend>
+  <depend>origincar_msg</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

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 <opencv2/opencv.hpp>
 #include <zbar.h>
 
+#include <chrono>
+
+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<sensor_msgs::msg::Image>(
+    if (image_type_ == "raw") {
-        image_topic_, rclcpp::SensorDataQoS(),
+      subscribe_raw_image();
-        std::bind(&QrDetectNode::cb, this, std::placeholders::_1));
+    } 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<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);
-    pub_tts_ = create_publisher<std_msgs::msg::String>("/vlm_result", 10);
+    tts_client_ = create_client<origincar_msg::srv::Speak>("/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<sensor_msgs::msg::Image>(
+        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<sensor_msgs::msg::CompressedImage>(
+        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<origincar_msg::srv::Speak::Request>();
+    request->text = text;
+    tts_client_->async_send_request(
+        request,
+        [this](rclcpp::Client<origincar_msg::srv::Speak>::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<sensor_msgs::msg::Image>::SharedPtr  sub_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr             image_sub_;
-  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr     sign_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr   compressed_image_sub_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr        pub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr                sign_sub_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr        pub_tts_;  // 发给TTS语音播报
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr                  pub_;
+  rclcpp::Client<origincar_msg::srv::Speak>::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)

src/qr_detection/test/test_qr_detect.cpp

@@ -0,0 +1,108 @@
+#include <algorithm>
+#include <chrono>
+#include <string>
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/compressed_image.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <origincar_msg/srv/speak.hpp>
+
+#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<rclcpp::TopicEndpointInfo> & 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<rclcpp::Node>("qr_detect_compressed_camera_test");
+  auto camera_pub = camera_node->create_publisher<sensor_msgs::msg::CompressedImage>(
+    "/qr_test_image", rclcpp::SensorDataQoS());
+
+  rclcpp::NodeOptions options;
+  options.append_parameter_override("image_topic", "/qr_test_image");
+  auto qr_node = std::make_shared<QrDetectNode>(options);
+  auto graph_node = std::make_shared<rclcpp::Node>("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<rclcpp::TopicEndpointInfo> 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<QrDetectNode>();
+  auto graph_node = std::make_shared<rclcpp::Node>("qr_detect_publication_graph_test");
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(qr_node);
+  executor.add_node(graph_node);
+
+  std::vector<rclcpp::TopicEndpointInfo> qr_results_publishers;
+  std::vector<rclcpp::TopicEndpointInfo> 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<QrDetectNode>();
+  ASSERT_NE(qr_node->tts_client_, nullptr);
+  EXPECT_STREQ(qr_node->tts_client_->get_service_name(), "/tts/speak");
+}