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. |
Setting up Dynamic Reconfigure for a Node(cpp)
Description: How to make a node dynamically reconfigureable in C++.Tutorial Level: INTERMEDIATE
Next Tutorial: Using the Dynamic Reconfigure Python Client
Contents
The Code
Since we have already created a cfg file in the last tutorial we'll dive right in to the code. Go back to our dynamic_tutorials package, create a new directory called nodes, and create a file called server.cpp. Drop the following code into 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 }
The Breakdown
Let take a closer look at server.cpp.
Here we just include the necessary header files for our node. Take note of dynamic_tutorials/TutorialConfig.h this is the header file generated by dynamic_reconfigure from our config file.
This is the callback that will get called when the dynamic_reconfigure server is sent a new configuration. It takes two parameters, the first is the new config. The second is the level, which is the result of ORing together all of level values of the parameters that have changed. What you want to do with the level value is entirely up to you, for now it is unnecessary. All we are going to do in the callback is print out the configuration.
In our main function we simply initialize our node and define the dynamic_reconfigure server, passing it our config type. As long as the server lives (in this case until the end of main()), the node listens to reconfigure requests.
Next we define a variable to represent our callback and then send it to the server. Now when the server gets a reconfiguration request it will call our callback function.
NOTE:If the callback if a member function of class use f = boost::bind(&callback, x, _1, _2) instead, where x is an instance of the class (or this if called from inside the class).
Lastly we spin the node.
Run It!
Compile the node(Don't forget to add the executable in CMakeLists.txt!). Rosrun it and the launch the rqt_reconfigure (or reconfigure_gui if you're on fuerte or earlier) using:
$ rosrun rqt_reconfigure rqt_reconfigure $ rosrun dynamic_reconfigure reconfigure_gui ## On Fuerte or earlier
You should see a window that looks something like this pop up.
And you're done! You've just built your first dynamically reconfigure-able node.