【ROS】subscribeとpublishを同じノードで行う

同じノード内で、トピックをsubscribeした後その内容に応じてトピックをpublishしたいという状況はよくあると思います。

ROS Wiki ではこの方法についてあまり触れていないので、ここにまとめておきます。

同じノードで行うためには、subscriberが呼び出すcallback関数の中でpublisherを使えるようにする必要があります。

そのため、以下のようにクラスを定義します。

#include "ros/ros.h"
#include "sensor_msgs/Image.h"

class SubPub
{
private:
  ros::NodeHandle n_;
  ros::Publisher pub_;
  ros::Subscriber sub_;

public:
  void callback(const sensor_msgs::Image &msg_sub);

  SubPub()
  {
    sub_ = n_.subscribe("/topic_sub", 1000, &SubPub::callback, this);
    pub_ = n_.advertise<sensor_msgs::Image>("/topic_pub", 1000);
  }
};

void SubPub::callback(const sensor_msgs::Image &msg_sub)
{
  sensor_msgs::Image msg_pub;

  // 処理

  pub_.publish(msg_pub);
}


int main(int argc, char *argv[])
{
  ros::init(argc, argv, "node_subpub");
  SubPub subpub;

  ros::spin();
}

/topic_sub というトピックをsubscribeし、/topic_pub というトピックをpublishするようにしています。

また、メッセージ型としては、sensor_msgs::Image をsubscribeし、同じ型をpublishするようにしています。

トピック名やメッセージ型は適宜変更してください。

上記コードのように、subscriberとpublisherをクラスのプライベートメンバ変数として持つことで、メンバ関数のcallbackがpublisherにアクセスできるようにしています。

実際に使うときには、上記main関数のように、クラスのインスタンスを1つ生成すればOKです。