えいあーるれいの技術日記

ROS2やM5 Stack、Ubuntuについて書いています

ROS2-Foxyで動画をPublishするシンプルなプログラムの作成

ROS2

　https://ar-ray.hatenablog.com/entry/2021/03/26/180000の実装時の話です。

　ROS2の動画をパブリッシュする公式パッケージってどこなんですかーー

ということで、ROS-Melodicの時によく使っていたmovie_publisherをROS2仕様にしました。自分で使うようなので、特にパラメータを渡すということはしていません。いずれ実装するとは思いますが…

　プログラムはGitHubにupしています。

https://github.com/Ar-Ray-code/movie_publisher/blob/foxy/src/movie_publisher.cpp

実装

パブリッシュしている部分を抜粋して簡単な解説をします。

movie_pub(const std::string &name_space, rclcpp::NodeOptions options):rclcpp::Node(name_space, options)
{
// 1で解説
    name = "/home/ubuntu/Desktop/video.mp4";
    cv::Mat image;
    cv::VideoCapture camera(name.c_str());

    if (!camera.isOpened()) {
        std::cout << name << " failed to open camera." << std::endl;
        exit(1);
    }
// 2で解説    
    rclcpp::Rate looprate (camera.get(cv::CAP_PROP_FPS));

    image_pub = this->create_publisher<sensor_msgs::msg::Image>("image_raw",1);
// 3で解説
    while(rclcpp::ok())
    {
        camera >> image;
                
        header.stamp = system_clock.now();
        header.frame_id = "/map";

        msg = cv_bridge::CvImage(header, "bgr8", image).toImageMsg();
        image_pub->publish(*msg);
        looprate.sleep();
    }
}

1． 動画ファイルのオープン

　動画ファイルの名前を指定してVideoCaptureクラスのcameraを作成する。cameraが開けない場合はエラーとなります。

2．パブリッシャ用の宣言

rclcpp::Rateは、While処理中のROSが周期的にトピックを公開し続けるために必要です。レートは動画のフレームレートを参考に取得します。

create_publisherはトピックの公開を行うために必要です。

3．ループ処理

　whileからは動画を1フレームごとに送信してきます。sensor_msgs::msg::Image型はheaderとimageで構成されています。header.stampには時間、header.frame_idには座標系、imageにはcv_bridgeで変換した後の画像データが格納されます。

　データを全て格納したら、image_pub->publish(*msg)で公開します。

　全てのフレームを切り出したら、勝手にノードが終了します。

気になるところ・追加したい機能

  • frame_idはどのように宣言すべきか：適当なフレーム名だとエラーになってしまったので/mapとしたが、本来はどうあるべきなのか。Parameterで指定したほうがよさそう。

  • ループ再生や逆再生・早送りを実装したい：Qtで実装すればよさそう。その場合は、フレームをカウントする必要がありそう。