ホーム / 技術系 /

【ROS】ROS node からrosbagの保存開始＋保存終了を自動（任意のタイミング）で実行する方法。

2021年10月25日月曜日

ROS プログラミング 技術系

t f B! P L

c++の標準関数であるsystem関数を用いて、rosbagの保存開始・終了を自動で（コマンド入力なしで）実行します。

'uninstd.h'の関数である'system()'を用いることで、'Ctrl + C'を使用しなくてもrosbagの記録開始、終了ができます。

具体的には'system()'関数で'rosbag record'と'rosnode kill'を実行することで、記録開始、終了ができます。

その際に以下の点に注意してください。

  • '__name:=hogehoge'を'rosbag record'の引数に与えないと'rosnode kill'の対象指定ができない。
  • 記録開始時にコマンドを並列実行可('system()'引数末尾に'&'を付ける)にしないとそのプロセスが次の処理に進めなくなる。

サンプルコード

publisher

publisherサンプルソースコード
#include "ros/ros.h"
#include "std_msgs/Int16.h"
#include <unistd.h>

int main(int argc, char **argv){
    ros::init(argc, argv, "publisher");
    ros::NodeHandle n;
    
    /*ROSbag開始、終了用パブリッシャー、記録用パブリッシャーを宣言*/
    ros::Publisher pub = n.advertise<std_msgs::Int16>("/start_rosbag", 10);
    ros::Publisher pub2 = n.advertise<std_msgs::Int16>("/finish_rosbag", 10);
    ros::Publisher pub3 = n.advertise<std_msgs::Int16>("/count_data", 10);
    
    /*100Hz間隔でデータを送信する*/
    ros::Rate loop_rate(100);
    int count = 0;   
    std_msgs::Int16 pub_data;
    std_msgs::Int16 pub_data2;
    /*データの送信のみでフラグとするのでフラグ用の値は適当*/
    pub_data.data = 1;
    pub_data2.data = 1;
    
    sleep(3);
    /*記録開始フラグを送信*/
    pub.publish(pub_data);
    sleep(3);
    
    while(count < 100){
        std_msgs::Int16 pub_data3;
        pub_data3.data = count;
        pub3.publish(pub_data3);
        /*ループの途中で記録終了フラグを送信*/
        if(count == 20){
            pub2.publish(pub_data2);
        }
        count++;
        loop_rate.sleep();
    }
    
    return 0;
}

ループ途中でrosbagの終了処理命令を送信しています。

subscriber

subscriberサンプルソースコード
#include "ros/ros.h"
#include "std_msgs/Int16.h"
#include <iostream>

/*記録開始コールバック関数*/
void bag_start_callback(const std_msgs::Int16::ConstPtr& msg){
    /*system関数を用いてrosbagの記録を開始
      次の処理に進ませるためにバックグラウンド処理*/
    system("rosbag record -a -O hoge.bag __name:=hoge_bag&");
    ROS_INFO("hoge_bag start");
}

/*記録終了コールバック関数*/
void bag_finish_callback(const std_msgs::Int16::ConstPtr& msg){
    /*system関数を用いてrosbagの記録を終了*/
    system("rosnode kill /hoge_bag&");
    ROS_INFO("hoge_bag finish");
}

int main(int argc, char **argv){
    ros::init(argc, argv, "listener");
    ros::NodeHandle n;

    /*各サブスクライバーにコールバック関数を割り当て*/
    ros::Subscriber sub1 = n.subscribe("/start_rosbag", 1, bag_start_callback);
    ros::Subscriber sub2 = n.subscribe("/finish_rosbag", 1, bag_finish_callback);
    /*topicを受信*/
    ros::spin();

    return 0;
}

それぞれのコールバック関数で'hoge.bag'への記録開始、終了を実行しています。

実行結果

実行後のrosbagの中身は以下のようになります。

保存したrosbagの情報
rosbag info hoge.bag
path:        hoge.bag
version:     2.0
duration:    3.1s
start:       Oct 24 2021 23:30:59.83 (1635085859.83)
end:         Oct 24 2021 23:31:02.91 (1635085862.91)
size:        16.5 KB
messages:    45
compression: none [1/1 chunks]
types:       rosgraph_msgs/Log [acffd30cd6b6de30f120938c17c593fb]
             std_msgs/Int16    [8524586e34fbd7cb1c08c5f5f1ca0e57]
topics:      /count_data      39 msgs    : std_msgs/Int16   
             /finish_rosbag    1 msg     : std_msgs/Int16   
             /rosout           4 msgs    : rosgraph_msgs/Log (2 connections)
             /rosout_agg       1 msg     : rosgraph_msgs/Log

rosbagの記録終了命令の送信から実際に記録終了するまでに遅延があるため、ちょうど20個ではありませんが、'/count_data'がループ回数の100個ではなく、39個となっている（＝途中で命令を受け取り記録停止している）ことがわかります。

また、生成されたbagファイルも記録中ファイル('hoge.bag.active')ではなくbagファイルが生成されました。

雑感

強引ではありますが、nodeから任意のタイミングでbagファイルの起動開始、終了を実行することができました。 もう少しうまく実施する方法がありそう(rosの機能で実現できそうな気がする)なので、そのあたりを追って調査したいです。

QooQ