qr_detector.cpp

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <zbar.h>
#include <image_transport/image_transport.hpp>

class QR : public rclcpp::Node {
public:
    QR() : Node("cv_qr_detector") {
        this->declare_parameter<std::string>("camera_topic", "/camera/image_raw");
        this->declare_parameter<std::string>("output_topic", "/camera/qr_detected/image_raw");

        std::string sub_topic = this->get_parameter("camera_topic").as_string();
        std::string pub_topic = this->get_parameter("output_topic").as_string();

        rmw_qos_profile_t custom_qos = rmw_qos_profile_default;

        image_sub_ = image_transport::create_subscription(
            this, sub_topic,
            std::bind(&QR::imageCallback, this, std::placeholders::_1),
            "raw", custom_qos
        );

        image_pub_ = image_transport::create_publisher(this, pub_topic, custom_qos);

        scanner_.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 0);
        scanner_.set_config(zbar::ZBAR_QRCODE, zbar::ZBAR_CFG_ENABLE, 1);
        
        RCLCPP_INFO(this->get_logger(), "QR Detector Node gestartet.");
    }

private:
    void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
        cv_bridge::CvImagePtr cv_ptr;
        try {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        } catch (cv_bridge::Exception& e) {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge Ausnahme: %s", e.what());
            return;
        }

        if (cv_ptr->image.empty()) return;

        cv::Mat gray_image, processed;
        cv::cvtColor(cv_ptr->image, gray_image, cv::COLOR_BGR2GRAY);
        cv::GaussianBlur(gray_image, processed, cv::Size(3, 3), 0);
        cv::adaptiveThreshold(processed, processed, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 11, 2);

        zbar::Image zbar_img(processed.cols, processed.rows, "Y800", processed.data, processed.cols * processed.rows);

        if (scanner_.scan(zbar_img) > 0) {
            for (zbar::Image::SymbolIterator symbol = zbar_img.symbol_begin(); symbol != zbar_img.symbol_end(); ++symbol) {
                if (symbol->get_location_size() >= 4) {
                    int min_x = 10000, max_x = 0, min_y = 10000, max_y = 0;

                    for (int i = 0; i < symbol->get_location_size(); i++) {
                        int x = symbol->get_location_x(i);
                        int y = symbol->get_location_y(i);
                        min_x = std::min(min_x, x); max_x = std::max(max_x, x);
                        min_y = std::min(min_y, y); max_y = std::max(max_y, y);
                    }

                    cv::rectangle(cv_ptr->image, cv::Point(min_x, min_y), cv::Point(max_x, max_y), cv::Scalar(0, 255, 0), 2);

                    std::string qr_data = symbol->get_data();
                    int text_y = std::max(min_y - 10, 0);
                    cv::putText(cv_ptr->image, qr_data, cv::Point(min_x, text_y),
                                cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0), 2);
                }
            }
        }

        image_pub_.publish(cv_ptr->toImageMsg());
    }

    image_transport::Subscriber image_sub_;
    image_transport::Publisher image_pub_;
    zbar::ImageScanner scanner_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<QR>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}