Note: This tutorial assumes that you have completed the previous tutorials: How to Write Your First cfg File.
(!) 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.

nodeのためのDynamic Reconfigureを設定する(cpp)

Description: C++でどのように、動的再設定可能なnodeを作るかを学びます。

Tutorial Level: INTERMEDIATE

Next Tutorial: Using the Dynamic Reconfigure Python Client

コード

すでに、前回のチュートリアルで、cfgファイルを作成しているので、さっそく取り掛かりましょう。dynamic_tutorialsのパッケージに戻り、新しくnodesと呼ばれるディレクトリと、server.cppと呼ばれるファイルを作成し、server.cppに以下のコードを貼り付けてください。:

   1 #include <ros/ros.h>
   2 
   3 #include <dynamic_reconfigure/server.h>
   4 #include <dynamic_tutorials/TutorialsConfig.h>
   5 
   6 void callback(dynamic_tutorials::TutorialsConfig &config, uint32_t level) {
   7   ROS_INFO("Reconfigure Request: %d %f %s %s %d", 
   8             config.int_param, config.double_param, 
   9             config.str_param.c_str(), 
  10             config.bool_param?"True":"False", 
  11             config.size);
  12 }
  13 
  14 int main(int argc, char **argv) {
  15   ros::init(argc, argv, "dynamic_tutorials");
  16 
  17   dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig> server;
  18   dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig>::CallbackType f;
  19 
  20   f = boost::bind(&callback, _1, _2);
  21   server.setCallback(f);
  22 
  23   ROS_INFO("Spinning node");
  24   ros::spin();
  25   return 0;
  26 }

コード解説

詳しくserver.cppをみていきましょう.

   1 #include <ros/ros.h>
   2 
   3 #include <dynamic_reconfigure/server.h>
   4 #include <dynamic_tutorials/TutorialsConfig.h>
   5 

ここでは、nodeに必要なヘッダーファイルをインクルードしているだけです。dynamic_tutorials/TutorialConfig.hは、我々のconfigファイルからdynamic_reconfigureによって作成されたヘッダーファイルです。

   1 void callback(dynamic_tutorials::TutorialsConfig &config, uint32_t level) {
   2   ROS_INFO("Reconfigure Request: %d %f %s %s %d", 
   3             config.int_param, config.double_param, 
   4             config.str_param.c_str(), 
   5             config.bool_param?"True":"False", 
   6             config.size);
   7 }

これは、dynamic_reconfigureサーバが新しい設定を送られてきたときに呼ばれるコールバックです。2つのパラメータを必要とし、一つ目は、新しい設定です。2つ目は、変更のあったパラメータの準位値のすべての論理輪の結果が入っています。この準位値をつかって何をするかは全部あなた次第です。今回は必要ありません。コードバックで行うことは、設定を表示するだけです。

   1 int main(int argc, char **argv) {
   2   ros::init(argc, argv, "dynamic_tutorials");
   3 
   4   dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig> server;

main関数の中では、単にnodeを初期化して、dynamic_reconfigureサーバを設定、それにconfigのタイプを渡しています。サーバが起動している限り、nodeはreconfigureの要求をリスンしつづけます。(今回の場合は、mainの最後にいたるまで)

   1 dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig>::CallbackType f;
   2 
   3   f = boost::bind(&callback, _1, _2);
   4   server.setCallback(f);

次に、コールバックを表す変数を定義し、サーバに送っています。これで、サーバがreconfigureの要求を受け取るたびに、コールバックを呼び出すようになります。

   1   ROS_INFO("Spinning node");
   2   ros::spin();
   3   return 0;
   4 }

最後に、nodeをspinします。

実行

nodeをコンパイルしましょう。(CMakeLists.txtの中に、実行可能にする行を付け加えるのを忘れないでください。) rosrunで実行し、rqt_reconfigureをlaunchしましょう。 (もしfuerteかそれ以前ならreconfigure_gui ) 以下のように:

$ rosrun rqt_reconfigure rqt_reconfigure
$ rosrun dynamic_reconfigure reconfigure_gui ## On Fuerte or earlier

以下のポップアップのようなウィンドウが見えると思います。 Screenshot-Reconfigure.png

これで、終わりです。初めての動的再設定可能なnodeを作れました。

Wiki: ja/dynamic_reconfigure/Tutorials/SettingUpDynamicReconfigureForANode(cpp) (last edited 2013-03-14 14:14:05 by Yuto Inagaki)