ROS 1つのノードでパブリッシュとサブスクライブ C++

目的

センサーからの情報を常時配信しつつ、センサーの状態を変化させる必要があった。調べてみてもサブスクライブしてメッセージがきたら内容を処理してパブリッシュは多いけど、あまり無かったため投稿しました。

動作イメージ

AさんはBさんに対して1秒間隔で常に「Like」と言っている。

CさんはAさんに対して「Bさんの悪口」を任意のタインミングで言った。心の清らかなAさんは、Cさんに言われたことを鵜呑みして、Bさんに対する言う内容を「Dislike」と変化させた。

動作イメージ的にはこんな感じとなります。
ここで重要なのは、Cさんの任意のタイミングでBさんに対するAさんの配信内容を変更するということです。
これによって、起動初期だけでなく、動作中にもAさんの設定を変更することができます。

実際には、
Aさん　センサー
Bさん　センサーデータ配信先
Cさん　センサーのコントロール
という感じとなります。

処理

サブスクライブとパブリッシュでそれぞれスレッドを立てて処理を行います。

サブスクライブの内容
トピック名　control　、型　std_msgs/Int8

パブリッシュの内容
トピック名　/pubsub　、型　std_msgs/String

１秒間隔で、/pubsubに「Like」という文字列を配信する。
/controlに data: 0 と配信された場合に「Dislike」に文字列を置き換える。
/controlに data: N (N > 0)と配信された場合は、1秒間にN回の配信を行う。

コード

メイン

pubsubtest.cpp

# include "pubsubtest.h"

PubSubTest::PubSubTest(ros::NodeHandle &_nh){
    nh = _nh;
    pub = nh.advertise<std_msgs::String>("pubsub", 1);
    // Publishする間隔の初期値設定
    rate = new ros::Rate(1);

    // サブスクライブのスレッドを用意
    std::thread th([this]() {this->thSub(); });

    // Detachで実行
    th.detach();

    // Publishの処理
    run();
}

void PubSubTest::run(){
    while(ros::ok()){
        std::string s = "Like";
        // メンバ変数selによる場合分け
        switch(sel){
        case -1:
            s = "Dislike";
            break;
        default:
            break;
        }
        std_msgs::String data;
        data.data = s;
        //  文字列のパブリッシュ
        pub.publish(data);
        //  パブリッシュ間隔のためのsleep()
        rate->sleep();
    }
}

void PubSubTest::cbInt8(const std_msgs::Int8 &msg){
    int8_t data = msg.data;
    ROS_INFO("Get Control Data: %d", data);
    if(data > 0){
        // data > 0 以上ならパブリッシュ間隔の変更を行う
        rate = new ros::Rate(data);
    }else{
        // data <= 0 なら メンバ変数にdataを格納
        sel = data;
    }
}

void PubSubTest::thSub(){
    // Callbackの登録
    sub = nh.subscribe("control", 10, &PubSubTest::cbInt8, this);

    // これが無いとサブスクライブをしてくれません。
    ros::spin();
}

int main(int argc, char **argv){
    ros::init(argc, argv, "pubsubtest");
    ros::NodeHandle nh;
    PubSubTest pubsubtest(nh);
    return 0;
}

ヘッダー

pubsubtest.h

# ifndef PUBSUB_TEST_H_
# define PUBSUB_TEST_T_

# include "ros/ros.h"
# include "std_msgs/Int8.h"
# include "std_msgs/String.h"

# include <iostream>
# include <thread>

class PubSubTest
{
private:
    ros::NodeHandle nh;
    ros::Publisher pub;
    ros::Subscriber sub;
    int8_t sel;
    ros::Rate *rate;
public:
    PubSubTest(ros::NodeHandle &_nh);
    // Subscribe Thread Function
    void thSub();
    // Subscribe Function
    void cbInt8(const std_msgs::Int8 &msg);
    // Publish Function
    void run();
};

# endif

まとめ

コントロール用ノード（GUI兼用ノードなどで）で各ノードの設定を制御したい場合などには使えると思います。