목적

센서로부터의 정보를 상시 전달하면서 센서의 상태를 변화시킬 필요가 있었다. 조사해도 구독하고 메시지가 오면 내용을 처리하고 게시는 많지만 별로 없었기 때문에 투고했습니다.

동작 이미지

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

1 초 간격으로/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 겸용 노드등에서)로 각 노드의 설정을 제어하고 싶은 경우 등에는 사용할 수 있다고 생각합니다.