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");
+}