Principle of dynamically adjusting parameters in ROS

We have a requirement in reality: we hope to change the parameters in the parameter server for debugging the robot, and we don't want to restart the relevant nodes every time we change the parameters, which is time-consuming, labor-consuming and troublesome. What we expect is that we can not only change the parameters in the parameter server for debugging, but also make the data in our refreshed parameter server work immediately without starting again.

In ROS, when the parameters in the parameter server are changed, the communication parties cannot know, because we can find from the working principle of the parameter server (take the topic communication architecture as an example):

Focusing on the data returned from the parameter server, we will find the following disadvantages of the communication mode of the parameter server:

1. The communication between parameter server and client / server is asynchronous without real-time interaction;

2. If the client / server wants to obtain the parameters in the parameter server, it must send a request to the parameter server;

3. The client / server will actively read / refresh the data in the parameter server only when the node is started;

The above three points make: in the process of C/S communication, the parameters in the parameter server have changed, and the communication parties will not be actively informed by the parameter server that "the parameters in the parameter server are refreshed, and each communication node will read it as soon as possible".

Since the parameter server cannot actively inform each node that the parameters have been refreshed, we use the characteristics of C/S Communication (because the refresh of the parameter server will not continue all the time) to add an additional communication node as the initiator of parameter refresh and the informer of "parameter refreshed" information. The specific communication logic is as follows:

The focus of dynamic reconfiguration is to provide a standard method to expose a subset of node parameters (the part of parameters you want to debug dynamically) to external reconfiguration. Using the client program, such as GUI, you can query a set of dynamically configurable parameters from the node, including their name, type and range, and provide a user-defined interface to the user.

Implementation of dynamic configuration C/S

File structure of the project:

① implementation of dynamic configuration client

In the architecture of dynamically configuring C/S service communication, the client is based on XXX Cfg file implementation, in fact, the essence of this file is in python py script file, the contents of which are as follows:

If you create dr1.0 in my project file cfg file will write the above code without prompt, anyway cfg files and The syntax of the py script file is the same. We can first add DR1 cfg renamed DR1 py, so that there is a content prompt, and it will be much easier to write the program. After writing, we will DR1 py renamed DR1 cfg (if the. py suffix is not restored, the compilation will fail). The following steps are mainly followed in writing the client:

1. Import function library:

*The meaning of dynamic_ reconfigure. parameter_ generator_ All functions in the catkin library are imported into this file.

2. Create parameter generator:

3. Add parameters to the parameter generator:

The parameter generator can add int, double, bool, enum and string parameters.

1) The prototype of the add function parameter is as follows: add(name, paramtype, level, description, default=None, min=None, max=None, edit_method = ""), where

Name: parameter name;

Paramtype: four types of parameter types (int_t, str_t, bool_t, double_t);

Level: parameter mask is actually an integer constant, which indicates which parameter is modified or not, which is equivalent to the numerical number of the parameter;

Description: string type, used to describe the details of parameters;

Default: default value;

Min/max: maximum / minimum;

edit_method: used to receive the handle returned by enum function;

2) The prototype parameters of const function are as follows: const(name, type, value, description)

Name: parameter name;

Type: four types of parameter types (int_t, str_t, bool_t, double_t);

Value: value of constant value parameter;

Description: string type, used to describe the details of parameters;

3) The prototype parameters of enum function are as follows: enum(constants, description)

Constants: array of constants [...];

Description: string type, used to describe the details of parameters;

How to write different types of enum s:

Among them, the string/bool type does not need to specify min/max. When you're done After the cfg file, be sure to open the executable permissions for python script files in the following Linux systems. Only based on the script executable permissions can the compilation pass and the GUI be generated:

The chmod command is used to modify the executable permissions of files. The executable permissions are divided into the following three categories:

Let's right-click XXX Cfg file, click "open in integration terminal", then enter chmod +x XXX.cfg, and then use l -sl command to view the executable permissions of XXX.cfg file:

The first character represents the file type. d stands for directory, - stands for non directory. Next, every three characters are a group of permissions, which are divided into three groups, representing owner permissions, user permissions in the same group and other user permissions in turn. The three characters of each group of permissions in turn represent whether they are readable, writable and executable:

R: Indicates that you have read permission

W: Indicates that you have write permission

10: Indicates that you have executable permissions

-: indicates that you do not have this permission

Let's look at the second to fourth letters in the first line: rwx - the file is readable, writable and executable. For the meanings of all parameters of chmod command, see: Linux chmod command | rookie tutorial (runoob.com)https://www.runoob.com/linux/linux-comm-chmod.html

4) Exit and generate intermediate files

package_name indicates that the name is package_name's feature pack, dynamic_server_name indicates the node name of the dynamic parameter adjustment server, cfg_name indicates the cfg file name.

Then, the function package is built by importing the following dependencies: roscpp and std_msgs,rospy,dynamic_reconfigure. Package. After importing dependencies The XML code is as follows:

After compilation, configure cmakelist Txt file:

After compilation, the results are as follows:

In fact, the parameter generator essentially packages the parameters of the client into a C + + custom class type, which is our topic type in C/S service communication.

② Implementation of dynamic configuration server

1. Add the required header file

demo01/dr1Config.h: . cfg generated user-defined class type file;

dynamic_reconfigure/server.h: Files related to server in dynamic configuration parameters;

2. Node initialization:

3. server creation:

4. Creation of callback function:

Where Demo01:: dr1config & configtype_ Obj is the topic type; level is the mask of the parameter (32-bit long integer variable), which indicates whether the parameter has been changed or not. It is just a numeric name for the parameter.

5. Setting of callback function:

The prototype of the callback function object is as follows:

This parameter must be a function object of type boost:: function <... >. In order to convert ordinary function pointers into function objects in the boost library, we should use the boost::bind binding function to convert:

6. Use the roundabout function to callback the callback function:

Our running results are as follows:

1. Run the client:

2. Run rosrun rqt_reconfigure rqt_reconfigure:

 3. Dynamically changing parameters:

We can see that when we change different parameters, the level value will also be different, that is, different level values represent different parameters we change. Note: when compiling, we need to add. / vscode/tasks. Replace the JSON code with the following code:

And compiled cfg/. After the MSG file, in C_ cpp_ properties. In the includepath of the JSON file, add/ The path of devel/include directory is shown in the following figure after adding:

After adding, you can call the header file transformed from the custom file in the file, and you will not report it wrong.

Topic communication and dynamic parameter configuration

The document structure is as follows:

The cfg/cpp folder is generated during compilation. We don't need to create this folder and its C + + header file.

From the above structure, we can see that node A plays two roles: the server of dynamic configuration parameters and the publisher of topic communication. Therefore, the program should also be divided into two parts. The program design flow chart is as follows:

① Common code part

② Code of dynamic configuration parameters

③ while polling gets the parameters in the parameter server and the subscriber publishing information associated with it

In this way, the publisher can read the information in the parameter server every time before publishing. We should pay special attention to the parameter name in the parameter server. If we are not sure, we can write the code of "dynamic parameter adjustment" and run it. Use rosparam list to check the name of the parameter reported by "dynamic parameter adjustment client" to the parameter server:

We add "/ dr /..." to each parameter, It's too troublesome. We can add a namespace to the node handle to achieve this purpose:

#include "ros/ros.h"
#include "demo01/dr1Config.h"
#include "demo01/sub_msg.h"
#include "dynamic_reconfigure/server.h"

// void(ConfigType &, uint32_t level)
void CallbackFunc(demo01::dr1Config& ConfigType_obj, uint32_t level)
{
    ROS_INFO("param:%d,%f,%d,%d\n\t",ConfigType_obj.int_param,
    ConfigType_obj.double_param,
    ConfigType_obj.bool_param,
    ConfigType_obj.list_param);
    ROS_INFO("which parameter is changed::%d\n\t",level+1);
}

int main(int argc,char* argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dr");
    dynamic_reconfigure::Server<demo01::dr1Config> server;
    dynamic_reconfigure::Server<demo01::dr1Config>::CallbackType Callback;
    // boost::bind return a function object
    Callback = boost::bind(&CallbackFunc,_1,_2);
    server.setCallback(Callback);

    ros::NodeHandle nh("dr"); // Add dr namespace
    ros::Publisher pub = nh.advertise<demo01::sub_msg>("chatter",10,true);
    demo01::sub_msg msg_obj;
        
    ros::Rate rate(1);
    while(ros::ok())
    {
        msg_obj.bool_param = nh.param("/dr/bool_param",false);
        msg_obj.double_param = nh.param("/dr/double_param",0.0);
        msg_obj.int_param = nh.param("/dr/int_param",0);
        msg_obj.list_param = nh.param("/dr/list_param",0);
        pub.publish(msg_obj);

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

However, at this time, the publisher's topic changes to / dr/chatter, while the subscriber's topic is still / chatter. In this way, add the same namespace as the publisher to the subscriber's node handle, so that the topic topic names of the two can remain the same again:

#include "demo01/sub_msg.h"
#include "ros/ros.h"

// void (*fp)(const boost::shared_ptr<const M> &)
void CallbackFunc(const demo01::sub_msg::ConstPtr& ptr)
{
    ROS_INFO("sub_param:%d,%d,%f,%d\n\t",ptr->int_param,ptr->list_param,ptr->double_param,ptr->bool_param);
}

int main(int argc,char* argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"sub");
    ros::NodeHandle nh("dr"); // Add dr namespace
    ros::Subscriber sub = nh.subscribe<demo01::sub_msg>("chatter",10,CallbackFunc);
    ros::spin();
    return 0;
}

CMakelist.txt file to add configuration information:

Package. Add configuration information to XML file:

The operation results are as follows:

At this point, we can change the information subscribed by subscribers at any time through the GUI.