将ROS2话题/serit转为V4L2虚拟相机/dev/video4失败求助

问题描述 #

尝试将ROS2话题/serit的图像数据写入V4L2虚拟相机/dev/video4，设备格式为640x480、YU12像素格式，但每次写入时触发错误码22（无效参数）。

错误原因排查 #

• 设备打开模式错误：使用O_RDWR打开输出设备，虚拟相机仅需写入权限，多余的读权限可能导致设备状态异常。
• YU12与I420格式不匹配：OpenCV的COLOR_BGR2YUV_I420输出的是I420格式（Y-V-U平面顺序），而V4L2设备要求的YU12是Y-U-V平面顺序，平面顺序颠倒导致数据格式无效。
• 设备频繁开关：每次回调都打开/关闭设备，容易引发设备状态不一致，且效率低下。
• 图像行对齐不满足V4L2要求：V4L2设备通常要求图像行宽为特定对齐值（如16字节倍数），OpenCV生成的图像行宽可能未对齐，导致写入数据长度与设备预期不符。
• Field参数不匹配：设备当前Field为V4L2_FIELD_INTERLACED（值1），但输入图像是逐行格式，需设置为V4L2_FIELD_NONE。

修复方案及代码 #

关键修复点 #

1. 用O_WRONLY | O_NONBLOCK打开设备，避免不必要的权限和阻塞问题
2. 将OpenCV输出的I420转换为YU12：交换U和V平面的位置
3. 在节点初始化时打开设备，回调时复用文件描述符，避免频繁开关
4. 确保图像行宽符合V4L2的对齐要求，显式设置行宽为640（16的倍数）
5. 显式配置V4L2设备的格式参数，确保与输入图像完全匹配

修复后的完整代码 #

#include <opencv2/opencv.hpp>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <errno.h>
#include <cstring>

using namespace std;
using namespace cv;

class ImageToV4L2Node : public rclcpp::Node {
public:
    ImageToV4L2Node() : Node("image_to_v4l2"), fd_(-1) {
        // 打开V4L2设备
        fd_ = open("/dev/video4", O_WRONLY | O_NONBLOCK);
        if (fd_ == -1) {
            RCLCPP_ERROR(this->get_logger(), "Failed to open V4L2 device: %s", strerror(errno));
            rclcpp::shutdown();
            return;
        }

        // 配置V4L2设备格式为640x480 YU12 逐行
        struct v4l2_format fmt;
        memset(&fmt, 0, sizeof(fmt));
        fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
        fmt.fmt.pix.width = 640;
        fmt.fmt.pix.height = 480;
        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YU12;
        fmt.fmt.pix.field = V4L2_FIELD_NONE;
        fmt.fmt.pix.bytesperline = 640; // 行宽对齐，640是16的倍数
        fmt.fmt.pix.sizeimage = 640 * 480 * 3 / 2; // YU12总字节数

        if (ioctl(fd_, VIDIOC_S_FMT, &fmt) == -1) {
            RCLCPP_ERROR(this->get_logger(), "Failed to set V4L2 format: %s", strerror(errno));
            close(fd_);
            rclcpp::shutdown();
            return;
        }

        // 验证设置后的格式
        if (ioctl(fd_, VIDIOC_G_FMT, &fmt) != -1) {
            RCLCPP_INFO(this->get_logger(), "Configured format - Width: %d, Height: %d, Pixelformat: %c%c%c%c",
                        fmt.fmt.pix.width, fmt.fmt.pix.height,
                        (fmt.fmt.pix.pixelformat >> 0) & 0xFF,
                        (fmt.fmt.pix.pixelformat >> 8) & 0xFF,
                        (fmt.fmt.pix.pixelformat >> 16) & 0xFF,
                        (fmt.fmt.pix.pixelformat >> 24) & 0xFF);
        }

        // 订阅ROS2图像话题
        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/serit", 10, std::bind(&ImageToV4L2Node::listener_callback, this, std::placeholders::_1));

        // 创建显示窗口
        namedWindow("Input Image", WINDOW_KEEPRATIO);
    }

    ~ImageToV4L2Node() {
        if (fd_ != -1) {
            close(fd_);
        }
        destroyWindow("Input Image");
    }

private:
    void listener_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
        try {
            // 将ROS图像转换为OpenCV BGR格式
            cv::Mat cv_image = cv_bridge::toCvCopy(msg, "bgr8")->image;

            // 调整图像大小到640x480，确保行宽对齐
            cv::Mat resized_image;
            cv::resize(cv_image, resized_image, cv::Size(640, 480), 0, 0, cv::INTER_LINEAR);

            // 转换为I420格式
            cv::Mat i420_image;
            cv::cvtColor(resized_image, i420_image, cv::COLOR_BGR2YUV_I420);

            // 将I420转换为YU12：交换U和V平面
            int y_size = 640 * 480;
            int uv_size = y_size / 4;
            cv::Mat yu12_image(i420_image.size(), i420_image.type());
            memcpy(yu12_image.data, i420_image.data, y_size); // Y平面
            memcpy(yu12_image.data + y_size, i420_image.data + y_size + uv_size, uv_size); // U平面（来自I420的V）
            memcpy(yu12_image.data + y_size + uv_size, i420_image.data + y_size, uv_size); // V平面（来自I420的U）

            // 写入V4L2设备
            ssize_t written = write(fd_, yu12_image.data, yu12_image.total() * yu12_image.elemSize());
            if (written == -1) {
                RCLCPP_WARN(this->get_logger(), "Failed to write to V4L2 device: %s", strerror(errno));
            } else if (written != yu12_image.total() * yu12_image.elemSize()) {
                RCLCPP_WARN(this->get_logger(), "Partial write to V4L2 device: %zd/%zu bytes", written, yu12_image.total() * yu12_image.elemSize());
            }

            // 显示图像
            imshow("Input Image", resized_image);
            waitKey(1);
        } catch (cv_bridge::Exception& e) {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
        }
    }

    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
    int fd_;
};

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

额外注意事项 #

• 确保虚拟相机设备（如v4l2loopback）已正确加载，且格式配置为YU12
• 编译时需链接ROS2、OpenCV和V4L2相关库，CMakeLists.txt需包含对应依赖
• 运行程序前需确保用户对/dev/video4有写入权限（可通过chmod o+w /dev/video4临时设置，或添加用户到video组）

内容的提问来源于stack exchange，提问作者Metehan SOYLU