cyy/yiliao2026
1
0
Fork 0
forked from zbw/yiliao2026
Code Pull Requests Activity

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

Browse Source
This commit is contained in:
Orange
2026-07-24 17:11:19 +08:00
parent 929d8e2e5c
commit 300febd48a
4 changed files with 289 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/qr_detection/CMakeLists.txt
View File

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

7
src/qr_detection/package.xml
View File

@@ -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>

176
src/qr_detection/src/qr_detect.cpp
View File

@@ -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>(
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<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<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_image_sub_;
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sign_sub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_tts_; // 发给TTS语音播报
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)

108
src/qr_detection/test/test_qr_detect.cpp Normal file
View File

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