パブリッシャー（発行）とサブスクライバー（購読）のサンプルをC++で書く

今日はC++でPublisherとSubscriberを書きます。

個人的にC++はよく分からないので慎重にやります。

$ roscd beginner_tutorials
して
今回は
src/talker.cpp
として
以下を保存します。

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>
int main(int argc, char** argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 100);
  ros::Rate loop_rate(5);
  int count = 0;
  while (ros::ok())
  {
    std::stringstream ss;
    ss << "Hello there! This is message [" << count << "]";
    std_msgs::String msg;
    msg.data = ss.str();
    chatter_pub.publish(msg);
    ROS_INFO("I published [%s]", ss.str().c_str());
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
}

ではコードの中を見ていきましょう。

最初の一行、

#include <ros/ros.h>

はいいでしょうか？
お決まりでROSを使うときはこれを必ずしましょう。

#include <std_msgs/String.h>

はstd_msgsパッケージのStringメッセージを使うためにインクルードします。
String.msgファイルから自動生成されています。
rosls std_msgs/msg/cpp/std_msgs/
に実体がありますね。

ros::init(argc, argv, "talker");

ROSの初期化です。talkerという名前のノードとして宣言しています。

ros::NodeHandle n;

これでノードのハンドルを生成しています。
ハンドルでネームスペースを管理できます。
ここでノードの初期化処理が行われます。
これが解放されるときに終了処理が行われます。
（複数ros::NodeHandle作ると複数のネームスペースを使いやすくなります。）

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 100);

これでstd_msg::String型のメッセージで、chatterという名前のトピックを公開するという宣言をしています。テンプレート関数を使っています。
この宣言はMasterに送られます。

2つ目の引数の１００はキューの数でデータを最大１００蓄えることができます。
ros::Publisherのインスタンスはpublishメソッドによりトピックを更新するときに使います。

ros::Rate loop_rate(5);

これにより5Hzで駆動するためのタイマーができます。
つまり、loop_rate.sleep()すると処理時間を入れて0.2secスリープするということです。

while (ros::ok())

ROSではデフォルトでSIGINTをハンドリングするのでCtrl-Cが送られたときros::ok()はfalseを返すようになっています。
他にros::ok()がfalseになるのは同じ名前のノードが接続してきて切断されるとき、ros::shutdownをどこかのノードが発行したときです。

std::stringstream ss;
    ss << "Hello there! This is message [" << count << "]";
    std_msgs::String msg;
    msg.data = ss.str();

このあたりは大丈夫でしょうか。
Stringメッセージのdataという変数にss.str()を入れています。

chatter_pub.publish(msg);

これでmsgがchatterというトピックとして発行されます。

ROS_INFO("I published [%s]", ss.str().c_str());

ROS_INFOでINFOという警告レベルでconsoleに書き出します。
このへんに詳しく書いてあるようです。
ROS_DEBUG, ROS_ERRORなどがありますね。

ros::spinOnce();

コールバックを受ける必要があるノードの場合はこれを呼びます。
今回はコールバックないので必要ないのですが、基本なのでいれています。

loop_rate.sleep();

さっきでてきましたがこれで5Hz用のsleepです。

処理が0.2sec以上だったらどうなるんでしょうかね。

これでパブリッシャーは終了です。

次はサブスクライバーです。
以下をsrc/listener.cppとして保存しましょう。

#include <ros/ros.h>
#include <std_msgs/String.h>
void chatterCallback(const std_msgs::StringConstPtr& msg)
{
  ROS_INFO("Received [%s]", msg->data.c_str());
}
int main(int argc, char** argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber chatter_sub = n.subscribe("chatter", 100, chatterCallback);
  ros::spin();
}

なんかもうほとんど分かっちゃいますね。

void chatterCallback(const std_msgs::StringConstPtr& msg)
{
  ROS_INFO("Received [%s]", msg->data.c_str());
}

コールバックの引数に渡されるのはboost::shared_prt(要するに参照カウンタ付きポインタ）です。

ros::Subscriber chatter_sub = n.subscribe("chatter", 100, chatterCallback);

これで購読するトピックを指定して、更新があったときに呼び出されるコールバック関数を指定しています。
これも同様に100個までデータを蓄えます。
ros::Subscriberのオブジェクトが存在する限りコールバックが呼ばれます。
このインスタンスを解放するとコールバックが呼ばれなくなります。

subscribe()の引数には、Boost.Functionのオブジェクトや、クラスのメンバ関数も入れられます。

ros::spin();

でコールバックを待ちます。
これだけでwhile(ros::ok()) ros::spinOnce();と同じで、Ctrl-Cを拾います。

これでサブスクライバーも終わりです。

次はビルドします。
CMakeLists.txtを編集しましょう。

$ rosed beginner_tutorials CMakeLists.txt
とするのもいいでしょう。

このファイルの最後に以下の２行を加えます。

rosbuild_add_executable(talker src/talker.cpp)
rosbuild_add_executable(listener src/listener.cpp)

意味は大体分かりますよね？

src/talker.cppを使ってtalkerというバイナリをbin/以下に作る、ということです。

もっと詳しく知りたい人はこちらを見てください。

$ make
とするとできましたね。
$ ls bin/
listener talker
お、できてます。

では試してみましょう。


$ roscore
して、新しいターミナルで
$ rosrun beginner_tutorials talker __name:=talker
すると

[ INFO] 1251943083.656415000: I published [Hello there! This is message [0]]

という感じで流れ出しますね。

もいっこ新しいターミナルで、

$ rosrun beginner_tutorials listener __name:=listener

すると、

[ INFO] 1251943144.400553000: Received [Hello there! This is message [1]]

という感じのメッセージが流れます。

ためしに、
$ rosrun beginner_tutorials listener.py __name:=listener
として、Pythonでつないでみましたが、ちゃんと動きますね。
異なる言語がさくっと繋がりました。

では次回はサービスをC++で書きますよ。

roscppのチュートリアルでもっと詳しく学べるようです。