ホーム / 技術系 /

【ROS】SubscribeするTopicを任意のタイミングで切り替える方法

2020年5月15日金曜日

ROS プログラミング 技術系

t f B! P L

宣言済みSubscriberを途中で上書きすることで、別のTopicを受信することができます

ROSにおいて、すでに宣言したTopicの受信(購読)を止めて、別のTopicの受信（購読）に切り替える方法。

ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/hoge", 10000, callback);

上記のように宣言された'/hoge'を受け取るSubscriber'sub'に対して

sub = n.subscribe("/hoge2",1000,callback2);

上記のようにコード途中で'/hoge2'を受け取るように上書きすることで、 'ros::spin()'などを実行した際に '/hoge'の受信を行わず、'/hoge2'のみを受け取る ように出来ます（出来てしまいます）。

サンプルコード

publisher

publisherサンプルソースコード(クリックで開きます)
#include "ros/ros.h"
#include "std_msgs/Int16.h"

int main(int argc, char **argv){
    ros::init(argc, argv, "publisher");
    ros::NodeHandle n;
    /*切り替え確認のため2つのパブリッシャーを宣言*/
    ros::Publisher pub = n.advertise<std_msgs::Int16>("/data", 10);
    ros::Publisher pub2 = n.advertise<std_msgs::Int16>("/data2", 10);
    /*10Hz間隔でデータを送信する*/
    ros::Rate loop_rate(10);
    int count = 0;
    
    while(ros::ok()){
        std_msgs::Int16 pub_data;
        std_msgs::Int16 pub_data2;
        /*'/data'ではcountの値を、'/data2'ではcountの２乗を送信する*/
        pub_data.data = count;
        pub_data2.data = count * count;
        pub.publish(pub_data);
        pub2.publish(pub_data2);
        /*countの値をコンソール表示*/
        ROS_INFO("publish:[%d]", count);
        count++;
        loop_rate.sleep();
    }
    return 0;
}

10Hz間隔である整数と、その整数を２乗した値をそれぞれ別々のTopic'/data'、'/data2'として送信します。

subscriber

subscriberサンプルソースコード(クリックで開きます)
#include "ros/ros.h"
#include "std_msgs/Int16.h"

/*コールバック関数*/
void callback(const std_msgs::Int16::ConstPtr& msg){
    ROS_INFO("subscribe:[%d]", msg->data);
}

int main(int argc, char **argv){
    ros::init(argc, argv, "listener");
    ros::NodeHandle n;
    /*サブスクライバーsubを宣言し、'/data'Topicを購読するよう設定*/
    ros::Subscriber sub = n.subscribe("/data", 1, callback);    
    
    /*10回ループしたら（≠10回Topicを受信したら）受信トピックを切り替える*/
    for(int i=0; i< 10;i++){   
        ros::spinOnce();            /*topicを受信したらコールバックを呼び出す*/
        ROS_INFO("roop:[%d]",i);    /*今のループ回数表示*/      
        ros::Duration(0.1).sleep(); /*0.1秒待機*/
    }
    ROS_INFO("change subscribe");
    /*subに'/data2'を購読するよう再設定*/
    sub = n.subscribe("/data2", 1, callback);
    ros::spin();        /*再設定したtopicを受信*/
    return 0;
}

はじめの1秒間は約0.1秒間隔で'/data'Topicを受信し、表示します。1秒経過後に受信するTopicを'/data2'に切り替え、受信した値を表示します。

実行結果

publisher側標準出力(クリックで開きます)
[ INFO] [1589533527.849628060]: publish:[0]
[ INFO] [1589533527.949853763]: publish:[1]
[ INFO] [1589533528.049791612]: publish:[2]
[ INFO] [1589533528.149761918]: publish:[3]
[ INFO] [1589533528.249714665]: publish:[4]
[ INFO] [1589533528.349856617]: publish:[5]
[ INFO] [1589533528.449682766]: publish:[6]
[ INFO] [1589533528.549819517]: publish:[7]
[ INFO] [1589533528.649704980]: publish:[8]
[ INFO] [1589533528.749832312]: publish:[9]
[ INFO] [1589533528.849759482]: publish:[10]
[ INFO] [1589533528.949942352]: publish:[11]
[ INFO] [1589533529.049720363]: publish:[12]
subscriber側標準出力(クリックで開きます)
[ INFO] [1589533527.573784060]: roop:[0]
[ INFO] [1589533527.674647858]: roop:[1]
[ INFO] [1589533527.774866759]: roop:[2]
[ INFO] [1589533527.875018502]: roop:[3]
[ INFO] [1589533527.975183442]: roop:[4]
[ INFO] [1589533528.075417228]: roop:[5]
[ INFO] [1589533528.175753042]: subscribe:[3]
[ INFO] [1589533528.175804911]: roop:[6]
[ INFO] [1589533528.276110992]: subscribe:[4]
[ INFO] [1589533528.276175996]: roop:[7]
[ INFO] [1589533528.376466394]: subscribe:[5]
[ INFO] [1589533528.376641115]: roop:[8]
[ INFO] [1589533528.477003028]: subscribe:[6]
[ INFO] [1589533528.477168722]: roop:[9]
[ INFO] [1589533528.577353580]: change subscribe
[ INFO] [1589533528.750486725]: subscribe:[81]
[ INFO] [1589533528.849946445]: subscribe:[100]
[ INFO] [1589533528.951332036]: subscribe:[121]
[ INFO] [1589533529.049989415]: subscribe:[144]

起動タイミングを合わせていないためか、'/data'の1,2が受信出来ていませんがループ10回目までは'/data'を、その後は'/data2'を受信し、表示することが出来ています。

雑感

直感的にはエラーになりそうですが、エラーにならず問題なく動きます。

おそらくTopicのRemapを実現するためにこのような仕様になっていると思いますが、特にクラスを用いてSubscriberを実装した際などに、うっかり同じ変数名の宣言や上書きなどをしてしまうと 問題なく実行されるがTopicが受信されない ということが発生しうるため注意が必要です。

逆に垂れ流しのセンサデータなどを任意のタイミングでのみ受信したい場合には有効活用できるかもしれません（そういった場合にはServiceを使ったり、Publisher側で管理をすべきだと思いますが・・・）。

気が向いたらコード見て、上書き時の動作を確認してみようと思います。

QooQ