ROS 2のImage トピックをv4l2loopbackで作成した仮想カメラデバイスに流す

概要

ROS 2のsensor_msgs/Imageトピックをv4l2loopbackで作成した
仮想ビデオデバイスにストリーミングしてみた。

同じことをするROS パッケージとしてimage_to_v4l2loopbackがある。

v4l2loopback のインストールと仮想ビデオデバイスの作成

v4l2loopback のインストール

sudo apt update
sudo apt install v4l2loopback-dkms

仮想ビデオデバイスの作成 (適当にデバイスIDを20とする)

sudo modprobe v4l2loopback video_nr=20

/dev/video20が作成されているか確認

$ v4l2-ctl --list-devices
Dummy video device (0x0000) (platform:v4l2loopback-000):
	/dev/video20

ROS 2ノードの作成と実行

処理の流れ

• 画像トピック (sensor_msgs/Image)をSubscribeする。
• 画像をOpenCVの画像に変換してcv::VideoWriterで作成した仮想ビデオデバイスに書き込む

gstreamerは詳しくないので参考にある記事の設定を引用した。画像のサイズは/image_rawトピックの画像サイズと同じにする。

image_to_v4l2.cpp

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/videoio/videoio.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>

class ImageToV4l2 : public rclcpp::Node {
public:
  ImageToV4l2() : Node("image_to_v4l2") {
    this->declare_parameter<std::string>("video_device", "/dev/video20");
    video_device_ = this->get_parameter("video_device").as_string();

    this->declare_parameter<std::string>("image_topic_name", "/image_raw");
    std::string image_topic_name = this->get_parameter("image_topic_name").as_string();

    std::string gstreamer_pipeline =
        "appsrc ! videoconvert ! videoscale ! video/x-raw,format=I420 ! "
        "v4l2sink device=" +
        video_device_;
    writer_.open(gstreamer_pipeline, 0, 10, cv::Size(640, 480), true);

    if (!writer_.isOpened()) {
      RCLCPP_ERROR(this->get_logger(),
                   "Could not open video writer for device %s",
                   video_device_.c_str());
      rclcpp::shutdown();
    }

    subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
        image_topic_name, 10,
        std::bind(&ImageToV4l2::image_callback, this, std::placeholders::_1));
  }

private:
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
    cv_bridge::CvImagePtr cv_ptr;

    try {
      cv_ptr = cv_bridge::toCvCopy(msg, "rgb8");
    } catch (cv_bridge::Exception &e) {
      RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
      return;
    }

    cv::Mat bgr_image;
    cv::cvtColor(cv_ptr->image, bgr_image, cv::COLOR_RGB2BGR);

    if (!bgr_image.empty()) {
      writer_.write(bgr_image);
      cv::imshow("Received Image", bgr_image);
      cv::waitKey(1);
    }
  }

  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
  cv::VideoWriter writer_;
  std::string video_device_;
};

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

動作確認としてv4l2_cameraを使用して/dev/video0にあるWebカメラの画像を/image_rawトピックで出力する。

ros2 run v4l2_camera v4l2_camera_node

video_deviceに先程作成した仮想ビデオデバイスを指定。

ros2 run image_to_v4l2 image_to_v4l2_node --ros-args -p video_device:="/dev/video20" -p image_topic_name:="/image_raw"

画像の表示

mplayerで見る

mplayer tv:// -tv driver=v4l2:device=/dev/video20

zoom で見る

Video 設定からDummy video device という名前で選択できる。

おわりに

Imageトピックをv4l2loopbackで作成した仮想ビデオデバイスにストリーミングすることで、ROS 2で処理した画像を/dev/video デバイスを使用するアプリケーションで使えるようになった。
ちゃんと使うならimage_to_v4l2loopbackをROS 2移行した方がいいかもしれない。

参考