This tutorial covers how to write a publisher and subscriber node in C++.

Note: This tutorial assumes that you have completed the previous tutorials: ROSのサービスとパラメータを理解する.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

シンプルな配信者(Publisher)と購読者(Subscriber)を書く(C++)

Description: このチュートリアルでは, 配信者(Publisher)と購読者(Subscriber)のC++での書き方について学びます

Tutorial Level: BEGINNER

Next Tutorial: シンプルなPublisherとSubscriberの実行

配信者(Publisher)ノードの作成

"Node" は ROS のネットワークにつながった実行可能なものを表す ROS 用語です. ここでは, 絶え間なくメッセージを発信する 配信者(Publisher)"talker" のノードを作ってみましょう.

creating a rosbuild package で作った beginner_tutorials のディレクトリに移動しましょう.

roscd beginner_tutorials

creating a packageで作った beginner_tutorials のディレクトリに移動しましょう.:

cd ~/catkin_ws/src/beginner_tutorials

ソースコード

src のディレクトリを beginner_tutorials のパッケージのディレクトリで作成します.

mkdir -p ~/catkin_ws/src/beginner_tutorials/src

このディレクトリは, beginner_tutorials のパッケージのためのあらゆるソースを入れていきます.

beginner_tutorials の中にsrc/talker.cpp ファイルを作り,以下のものを張り付けてください.

https://raw.github.com/ros/ros_tutorials/hydro-devel/roscpp_tutorials/talker/talker.cpp

  27 #include "ros/ros.h"
  28 #include "std_msgs/String.h"
  29 
  30 #include <sstream>
  31 
  32 /**
  33  * This tutorial demonstrates simple sending of messages over the ROS system.
  34  */
  35 int main(int argc, char **argv)
  36 {
  37   /**
  38    * The ros::init() function needs to see argc and argv so that it can perform
  39    * any ROS arguments and name remapping that were provided at the command line. For programmatic
  40    * remappings you can use a different version of init() which takes remappings
  41    * directly, but for most command-line programs, passing argc and argv is the easiest
  42    * way to do it.  The third argument to init() is the name of the node.
  43    *
  44    * You must call one of the versions of ros::init() before using any other
  45    * part of the ROS system.
  46    */
  47   ros::init(argc, argv, "talker");
  48 
  49   /**
  50    * NodeHandle is the main access point to communications with the ROS system.
  51    * The first NodeHandle constructed will fully initialize this node, and the last
  52    * NodeHandle destructed will close down the node.
  53    */
  54   ros::NodeHandle n;
  55 
  56   /**
  57    * The advertise() function is how you tell ROS that you want to
  58    * publish on a given topic name. This invokes a call to the ROS
  59    * master node, which keeps a registry of who is publishing and who
  60    * is subscribing. After this advertise() call is made, the master
  61    * node will notify anyone who is trying to subscribe to this topic name,
  62    * and they will in turn negotiate a peer-to-peer connection with this
  63    * node.  advertise() returns a Publisher object which allows you to
  64    * publish messages on that topic through a call to publish().  Once
  65    * all copies of the returned Publisher object are destroyed, the topic
  66    * will be automatically unadvertised.
  67    *
  68    * The second parameter to advertise() is the size of the message queue
  69    * used for publishing messages.  If messages are published more quickly
  70    * than we can send them, the number here specifies how many messages to
  71    * buffer up before throwing some away.
  72    */
  73   ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  74 
  75   ros::Rate loop_rate(10);
  76 
  77   /**
  78    * A count of how many messages we have sent. This is used to create
  79    * a unique string for each message.
  80    */
  81   int count = 0;
  82   while (ros::ok())
  83   {
  84     /**
  85      * This is a message object. You stuff it with data, and then publish it.
  86      */
  87     std_msgs::String msg;
  88 
  89     std::stringstream ss;
  90     ss << "hello world " << count;
  91     msg.data = ss.str();
  92 
  93     ROS_INFO("%s", msg.data.c_str());
  94 
  95     /**
  96      * The publish() function is how you send messages. The parameter
  97      * is the message object. The type of this object must agree with the type
  98      * given as a template parameter to the advertise<>() call, as was done
  99      * in the constructor above.
 100      */
 101     chatter_pub.publish(msg);
 102 
 103     ros::spinOnce();
 104 
 105     loop_rate.sleep();
 106     ++count;
 107   }
 108 
 109 
 110   return 0;
 111 }

コード解説

さて,コードを読み解いて行きましょう.

  27 #include "ros/ros.h"
  28 

ros/ros.h は, ROS のシステムでよく用いる部分を使うのに必要なヘッダーをすべて含むことができる便利なファイルです.

  28 #include "std_msgs/String.h"
  29 

これは std_msgs パッケージに含まれる std_msgs/String のメッセージを含みます.これは,そのパッケージの Strings.msg から自動的に生成されるヘッダーです.メッセージの定義についてのより詳しい情報は msg を参照してください.

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

ROS の初期化をします.これは ROS が,コマンドラインを通してリマップを行うことを可能にします(今はわからなくてもいいです).これは,このノードの名前を特定する箇所でもあります. ノードの名前は,このシステムの中で唯一無二でなくてはなりません.

ここで使われている名前は base name である必要があり,/を含んではいけません.

  54   ros::NodeHandle n;

このプロセスのノードへのハンドラを作成します.初めの NodeHandle は ノードの初期化を行い,最後のハンドラが破棄されるときに, ノードが使っていたリソースをすべてクリアします.

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

ノードは Master に chatter という名前のトピックに std_msgs/String というタイプのメッセージを送ることを伝えます.これで Master は今から発信しようとしている chatter から情報を得ようとしているすべての ノードに chatter というトピックができたことを知らせます.この2つ目の引数には発信するキューの大きさを記します.この場合,もしこの ノードはあまりにも早く発信をしてしまうと,古い情報を捨てる前に最大のバッファーサイズの1000に到達してしまいます.

NodeHandle::advertise() の戻り値である ros::Publisher オブジェクトは,二つの目的を果たします.1) トピックに対してメッセージを発信できる publish() メソッドを含んでいます.2) スコープから出ると,自動的に発信をしなくなります.

  75   ros::Rate loop_rate(10);

ros::Rate オブジェクトは,ループ頻度を設定が可能です.最後に Rate::sleep() を呼び出してからどれだけ経過したかを常に管理し,正確な時間になるまでスリープします.

この場合,10Hzの周波数で実行されます.

  81   int count = 0;
  82   while (ros::ok())
  83   {

デフォルトでは, roscpp は Ctrl-C を押すと ros::ok() が false を返却するような SIGINT ハンドルをインストールしています.

ros::ok() は以下のような時に false を返します.

  • SIGINT シグナル を受け取ったとき (Ctrl-C)
  • 同じ名前のノードができたために,ネットワークから外されたとき
  • アプリケーションの他の部分でros::shutdown()が呼ばれたとき

  • 全ての ros::NodeHandles が破棄されたとき

一度ros::ok() が false を返すと、すべての ROS の呼び出しは失敗します.

  87     std_msgs::String msg;
  88 
  89     std::stringstream ss;
  90     ss << "hello world " << count;
  91     msg.data = ss.str();

一般的に msg file から作られるメッセージ適用(message-adapted)クラスを使って, ROS でのメッセージを発信します.もっと複雑なデータ型を持つものも可能ですが,今は"data" のみをメンバーとして持つ標準的なString メッセージを使います.

 101     chatter_pub.publish(msg);

ここは、つながっているどのノードに対しても実際にメッセージを発信しています.

  93     ROS_INFO("%s", msg.data.c_str());

ROS_INFO とその類は printf/cout に代わるものです.より詳しいことは rosconsole documentation を参照してください.

 103     ros::spinOnce();

ここで呼んでいる ros::spinOnce() はこのシンプルなプログラムでは必要ありません.というのも,コールバックをここでは呼んでいないからです.しかし,もしこのアプリケーションに購読の機能を加えた場合、ここにros::spinOnce()が無いと,コールバックは決して呼ばれません.ですので,ここで加えておいた方がよいでしょう.

 105     loop_rate.sleep();

ros::Rate オブジェクトを 10Hz の発信が行えるように残り時間をスリープするために使います.

以下に何が起こっているかの要約を残しておきます.

  • ROS を初期化する
  • chatter というトピックに std_msgs/String のメッセージを配信する事をMasterに告知する

  • chatter に 1秒 に 10回 メッセージを発信しながらループする

さて,次にメッセージを受け取るノードを書く必要があります.

購読するノードを作る

ソースコード

beginner_tutorials パッケージの中に src/listener.cpp を作り,以下のコードをコピー&ペーストしてください.

https://raw.github.com/ros/ros_tutorials/hydro-devel/roscpp_tutorials/listener/listener.cpp

  28 #include "ros/ros.h"
  29 #include "std_msgs/String.h"
  30 
  31 /**
  32  * This tutorial demonstrates simple receipt of messages over the ROS system.
  33  */
  34 void chatterCallback(const std_msgs::String::ConstPtr& msg)
  35 {
  36   ROS_INFO("I heard: [%s]", msg->data.c_str());
  37 }
  38 
  39 int main(int argc, char **argv)
  40 {
  41   /**
  42    * The ros::init() function needs to see argc and argv so that it can perform
  43    * any ROS arguments and name remapping that were provided at the command line. For programmatic
  44    * remappings you can use a different version of init() which takes remappings
  45    * directly, but for most command-line programs, passing argc and argv is the easiest
  46    * way to do it.  The third argument to init() is the name of the node.
  47    *
  48    * You must call one of the versions of ros::init() before using any other
  49    * part of the ROS system.
  50    */
  51   ros::init(argc, argv, "listener");
  52 
  53   /**
  54    * NodeHandle is the main access point to communications with the ROS system.
  55    * The first NodeHandle constructed will fully initialize this node, and the last
  56    * NodeHandle destructed will close down the node.
  57    */
  58   ros::NodeHandle n;
  59 
  60   /**
  61    * The subscribe() call is how you tell ROS that you want to receive messages
  62    * on a given topic.  This invokes a call to the ROS
  63    * master node, which keeps a registry of who is publishing and who
  64    * is subscribing.  Messages are passed to a callback function, here
  65    * called chatterCallback.  subscribe() returns a Subscriber object that you
  66    * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
  67    * object go out of scope, this callback will automatically be unsubscribed from
  68    * this topic.
  69    *
  70    * The second parameter to the subscribe() function is the size of the message
  71    * queue.  If messages are arriving faster than they are being processed, this
  72    * is the number of messages that will be buffered up before beginning to throw
  73    * away the oldest ones.
  74    */
  75   ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  76 
  77   /**
  78    * ros::spin() will enter a loop, pumping callbacks.  With this version, all
  79    * callbacks will be called from within this thread (the main one).  ros::spin()
  80    * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
  81    */
  82   ros::spin();
  83 
  84   return 0;
  85 }

コード解説

では,先ほど上で解説した部分を除いて、細かく見ていきましょう.

  34 void chatterCallback(const std_msgs::String::ConstPtr& msg)
  35 {
  36   ROS_INFO("I heard: [%s]", msg->data.c_str());
  37 }

これはコールバック関数で,新しいメッセージが chatter トピックに届くとこのコールバックが呼ばれます.このメッセージは [http://www.boost.org/doc/libs/1_37_0/libs/smart_ptr/shared_ptr.htm|boost shared_ptr]] に届きます。すなわちこれは自分が見えないところでメッセージが勝手に消えるのを心配せず,基礎をなすデータをコピーせずに、必要な時に分離してメッセージを保管できることを意味します.

  75   ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

Matser によって chatter のトピックを購読します. ROS は chatterCallback() を新しいメッセージがくる度に呼び出します. 2つ目の引数はキューのサイズで,メッセージが早すぎて処理できないときのためです.この場合1000メッセージにキューが達すると新しいメッセージがくる度に古いものを捨てていきます.

NodeHandle::subscribe()ros::Subscriber オブジェクトを返し、購読を解除するまでこのオブジェクトは保持しなければなりません。 購読者(Subscriber)オブジェクトが破棄されるとき,chatterトピックからの購読を自動的に解除します.

NodeHandle::subscribe()関数はバージョンがあり、クラスのメンバ関数や, Boost.Function オブジェクトから呼ばれるものさえ特定することが可能です。 roscpp overviewは、より多くの情報を含んでいます.

  82   ros::spin();

ros::spin() がループに入ると、できるだけはやくメッセージのコールバック関数を呼びます.でも心配しないでください.もし特にやることがなければCPUを大して使うことはありません。一度ros::ok()がfalseを返すとros::spin()は終了します。これはros::shutdown()が呼ばれていることを意味し、Ctrl-Cが押された時のデフォルトのハンドラや、Masterがshutdownするよう命じたり、または実行者による手動の終了も同様です。

他にもコールバックを呼び出す方法はありますが,ここでは置いておきましょう. roscpp_tutorials パッケージはこれのデモをするサンプルアプリがあります. roscpp overview にもまた多くの情報があります.

またここでは何が起こったかのまとめをしておきます.

  • ROSのシステムを初期化する
  • chatter のトピックから購読する

  • Spin でメッセージがくるのを待つ
  • メッセージが届いたときに chatterCallback()関数が呼ばれる

node をビルドする

roscreate-pkg はパッケージに必要なデフォルトの Makefile と CMakeLists.txt を作ります.

$ rosed beginner_tutorials CMakeLists.txt 

それは以下の様に見えると思います.

  • cmake_minimum_required(VERSION 2.4.6)
    include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
    
    # Set the build type.  Options are:
    #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
    #  Debug          : w/ debug symbols, w/o optimization
    #  Release        : w/o debug symbols, w/ optimization
    #  RelWithDebInfo : w/ debug symbols, w/ optimization
    #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
    #set(ROS_BUILD_TYPE RelWithDebInfo)
    
    rosbuild_init()
    
    #set the default path for built executables to the "bin" directory
    set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
    #set the default path for built libraries to the "lib" directory
    set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
    
    #uncomment if you have defined messages
    #rosbuild_genmsg()
    #uncomment if you have defined services
    #rosbuild_gensrv()
    
    #common commands for building c++ executables and libraries
    #rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
    #target_link_libraries(${PROJECT_NAME} another_library)
    #rosbuild_add_boost_directories()
    #rosbuild_link_boost(${PROJECT_NAME} thread)
    #rosbuild_add_executable(example examples/example.cpp)
    #target_link_libraries(example ${PROJECT_NAME})

以下のコードを上のコードの一番下に加えてください.

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

これはデフォルトでは, bin フォルダに入れられる2つの実行可能ファイル talkerlistener をつくります.

より詳しい ROS での CMake の使いかたは CMakeLists を参照してください. さて, make をしてみてください.

$ make

ノードをビルドする

前章でpackage.xmlCMakeLists.txtファイルを自分のパッケージ用に作る際、catkin_create_pkgを使いました。

生成したCMakeLists.txtはこのように見えているでしょう(Creating Msgs and Srvsチュートリアルでの変更、未使用コメントや例を外した状態):

https://raw.github.com/ros/catkin_tutorials/master/create_package_modified/catkin_ws/src/beginner_tutorials/CMakeLists.txt

   1 cmake_minimum_required(VERSION 2.8.3)
   2 project(beginner_tutorials)
   3 
   4 ## Find catkin and any catkin packages
   5 find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
   6 
   7 ## Declare ROS messages and services
   8 add_message_files(DIRECTORY msg FILES Num.msg)
   9 add_service_files(DIRECTORY srv FILES AddTwoInts.srv)
  10 
  11 ## Generate added messages and services
  12 generate_messages(DEPENDENCIES std_msgs)
  13 
  14 ## Declare a catkin package
  15 catkin_package()

(#)でコメントアウトされた例の部分の編集については気にする必要はありません。単に以下の数行を自分のCMakeLists.txtの下の方に追加します:

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})

仕上がった自分のCMakeLists.txtファイルは以下のようになっているでしょう:

https://raw.github.com/ros/catkin_tutorials/master/create_package_pubsub/catkin_ws/src/beginner_tutorials/CMakeLists.txt

   1 cmake_minimum_required(VERSION 2.8.3)
   2 project(beginner_tutorials)
   3 
   4 ## Find catkin and any catkin packages
   5 find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
   6 
   7 ## Declare ROS messages and services
   8 add_message_files(FILES Num.msg)
   9 add_service_files(FILES AddTwoInts.srv)
  10 
  11 ## Generate added messages and services
  12 generate_messages(DEPENDENCIES std_msgs)
  13 
  14 ## Declare a catkin package
  15 catkin_package()
  16 
  17 ## Build talker and listener
  18 include_directories(include ${catkin_INCLUDE_DIRS})
  19 
  20 add_executable(talker src/talker.cpp)
  21 target_link_libraries(talker ${catkin_LIBRARIES})
  22 add_dependencies(talker beginner_tutorials_generate_messages_cpp)
  23 
  24 add_executable(listener src/listener.cpp)
  25 target_link_libraries(listener ${catkin_LIBRARIES})
  26 add_dependencies(listener beginner_tutorials_generate_messages_cpp)

これでtalkerlistenerの2つの実行ファイルが生成されます。そしてこれは、 自分のdevel spaceにデフォルトで入り、~/catkin_ws/devel/lib/share/<package name>に置かれます。

実行ファイル用のメッセージ生成対象の依存を追加しなければならないことに注意してください:

add_dependencies(talker beginner_tutorials_generate_messages_cpp)

これで、使用前にこのパッケージのメッセージヘッダが確かに生成されます。もし自分のcatkinワークスペースの中の、他のパッケージのメッセージを使う場合は、使うパッケージのそれぞれのメッセージ生成の依存も追加する必要があります。なぜならcatkinは全てのプロジェクトを同時にビルドするからです。全ての必要な対象の依存を追加するために、以下の変数を使うことができます。

add_dependencies(talker ${catkin_EXPORTED_TARGETS})

実行ファイルを直接呼び出すか、またはrosrunで実行することができます。システムにパッケージをインストールする時PATHが汚くなるので、実行ファイルは'<prefix>/bin'には置かれていません。もしインストール時に自分の実行ファイルをPATH上にしたいのであれば、インストールターゲットを設定することができます。catkin/CMakeLists.txtを参照してください。

CMakeLists.txt ファイルについてのより詳細な説明は、catkin/CMakeLists.txtを参照してください。

では、catkin_makeを実行しましょう:

#自分のcatkinワークスペースの中で
catkin_make

Note: ビルド済みの環境に新しいパッケージとして追加しているのであれば、--force-cmakeオプションで、catkinに強制的にmakeの実行を指定をする必要があるでしょう。Building Packages in a catkin Workspace With catkin_makeを見てください。

これでシンプルな配信者と購読者を書くことが出来ました。シンプルなPublisherとSubscriberの実行に進みましょう.

Wiki: ja/ROS/Tutorials/WritingPublisherSubscriber(c++) (last edited 2014-09-22 12:50:20 by Moirai)