ROS2 Foxy下C++结合cv_bridge发布摄像头画面非实时问题求助

Ubuntu 20.04 + ROS2-Foxy环境下，通过C++结合OpenCV、cv_bridge实现摄像头画面发布与订阅，已完成基础收发，但无法实时传输——发布节点每2秒才推送一帧画面，延迟明显。

发布者代码 #

#include <chrono>
#include <memory>
#include "cv_bridge/cv_bridge.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "std_msgs/msg/header.hpp"
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <opencv2/core/types.hpp>
#include <opencv2/core/hal/interface.h>
#include <image_transport/image_transport.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace std::chrono_literals;
using namespace cv;

class MinimalPublisher : public rclcpp::Node {

public:

MinimalPublisher() : Node("minimal_publisher"), count_(0) {

    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
    publisher_ = this->create_publisher<sensor_msgs::msg::Image>("topic", qos_profile);
    timer_ = this->create_wall_timer(10ms, std::bind(&MinimalPublisher::timer_callback, this));

}

private:

void timer_callback() {

    cv_bridge::CvImagePtr cv_ptr;
    cv::VideoCapture cap(0);
    cv::Mat img(cv::Size(1280, 720), CV_8UC3);
    cap >> img;
    sensor_msgs::msg::Image::SharedPtr msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", img).toImageMsg();      
    publisher_->publish(*msg);
    RCLCPP_INFO(this->get_logger(), "publishing");
}
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
  size_t count_; 
};

int main(int argc, char *argv[]) {

  printf("Starting...");
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0; 
}

订阅者代码 #

#include <chrono>
#include <memory>

#include "cv_bridge/cv_bridge.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "std_msgs/msg/header.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <stdio.h>

#include <opencv2/core/types.hpp>
#include <opencv2/core/hal/interface.h>
#include <image_transport/image_transport.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node {
  public:
    MinimalSubscriber()
    : Node("minimal_subscriber")
    {
      auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));
      subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
      "topic", qos_profile, std::bind(&MinimalSubscriber::topic_callback, this, _1));
    }

  private :
    void topic_callback(sensor_msgs::msg::Image::SharedPtr msg) const
    {
      RCLCPP_INFO(this->get_logger(), "In callback");
      cv_bridge::CvImagePtr cv_ptr;
      cv_ptr = cv_bridge::toCvCopy(msg,"bgr8");
      cv::imshow("minimal_subscriber", cv_ptr->image);
      cv::waitKey(1);
    }
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
};

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

核心问题分析 #

发布者代码的timer_callback函数中，每次回调都重新创建并初始化cv::VideoCapture对象，反复打开摄像头的流程通常需要1-2秒，这是导致每2秒才推送一帧的直接原因。

修改后的发布者代码 #

#include <chrono>
#include <memory>
#include "cv_bridge/cv_bridge.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "std_msgs/msg/header.hpp"
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <opencv2/core/types.hpp>
#include <opencv2/core/hal/interface.h>
#include <image_transport/image_transport.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace std::chrono_literals;
using namespace cv;

class MinimalPublisher : public rclcpp::Node {

public:
    MinimalPublisher() : Node("minimal_publisher"), count_(0), cap_(0) {
        // 检查摄像头是否成功打开
        if (!cap_.isOpened()) {
            RCLCPP_ERROR(this->get_logger(), "Failed to open camera!");
            rclcpp::shutdown();
            return;
        }
        // 设置摄像头分辨率
        cap_.set(CAP_PROP_FRAME_WIDTH, 1280);
        cap_.set(CAP_PROP_FRAME_HEIGHT, 720);

        // 使用Sensor Data类型QoS，适配实时视频流
        auto qos_profile = rclcpp::SensorDataQoS();
        publisher_ = this->create_publisher<sensor_msgs::msg::Image>("topic", qos_profile);
        
        // 30ms周期对应约30FPS，匹配常见摄像头帧率
        timer_ = this->create_wall_timer(30ms, std::bind(&MinimalPublisher::timer_callback, this));
        RCLCPP_INFO(this->get_logger(), "Camera publisher initialized");
    }

private:
    void timer_callback() {
        cv::Mat img;
        cap_ >> img;
        // 跳过无效帧
        if (img.empty()) {
            RCLCPP_WARN(this->get_logger(), "Empty frame captured!");
            return;
        }
        sensor_msgs::msg::Image::SharedPtr msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", img).toImageMsg();      
        publisher_->publish(*msg);
    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
    size_t count_;
    // 摄像头对象作为类成员，仅初始化一次
    cv::VideoCapture cap_;
};

int main(int argc, char *argv[]) {
    printf("Starting camera publisher...\n");
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MinimalPublisher>());
    rclcpp::shutdown();
    return 0; 
}

订阅者代码优化（可选） #

将订阅者的QoS改为SensorDataQoS，匹配发布者配置，进一步提升实时性：

// 在MinimalSubscriber构造函数中修改QoS
auto qos_profile = rclcpp::SensorDataQoS();
subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
    "topic", qos_profile, std::bind(&MinimalSubscriber::topic_callback, this, _1));

关键修改点说明 #

1. 摄像头对象持久化：将cv::VideoCapture移为类成员，仅在构造函数中初始化一次，避免反复打开设备的开销。
2. QoS适配：使用SensorDataQoS，这是ROS2专为传感器数据设计的低延迟、高可靠传输配置。
3. 有效性检查：添加摄像头打开状态和帧有效性检查，避免无效数据发布。
4. 定时器周期调整：设置为30ms，匹配常见摄像头帧率，避免不必要的高频回调。

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