ROS中动态调整参数的原理

我们在现实中有个需求：我们希望可以改变参数服务器中的参数用于调试机器人，而且我们不希望每改一遍参数之后必须重启一次相关节点才可以生效，这样耗时耗力太麻烦。我们期望的是：我们既可以改变参数服务器中的参数用于调试，也可以使我们刷新的参数服务器中的数据立刻起作用无需再次启动。

在ROS中，当参数服务器中的参数发生了改变，通信双方是无法知晓的，因为我们从参数服务器的工作原理就可以发现（以话题通信架构为例）：

我们重点关注从参数服务器回传的数据会发现参数服务器通信方式的以下缺点：

1. 参数服务器和客户端/服务器间的通信是异步的，没有实时交互；

2. 客户端/服务器要想获取参数服务器中的参数必须向参数服务器发出请求才可以；

3. 客户端/服务器只有在节点启动时会主动读取/刷新参数服务器中的数据；

上述三点使得：在C/S通信过程中，参数服务器中的参数发生了改变，通信双方也不会被参数服务器主动告知“参数服务器中的参数被刷新，各个通信节点速来读取”的信息。

参数服务器既然不能主动告知各个节点参数已被刷新，那我们利用C/S通信的特点（因为参数服务器的刷新并不会一直持续）来额外添加一个通信节点作为参数刷新的发起者以及“参数已刷新”信息的告知者，具体通信逻辑如下所示：

 动态重配置的重点是提供一种标准的方法，将节点的一个子集参数（你想要动态调试那一部分参数）公开给外部重配置。使用客户端程序，例如GUI，可以向节点查询一组可动态配置的参数，包括它们的名称、类型、范围，并向用户提供一个自定义接口。

动态配置C/S的实现

项目的文件结构：

 ① 动态配置客户端的实现

动态配置C/S服务通信的架构中客户端是基于XXX.cfg文件实现，其实这个文件的本质是python中的.py脚本文件，其内容如下：

# import package  
from dynamic_reconfigure.parameter_generator_catkin import *  
# create parameter generator  
gen = ParameterGenerator()  
# add parameters into parameter generator  
gen.add("int_param",int_t,0,"int type",0,0,100)  
gen.add("double_param",double_t,1,"double type",1.57,0,3.14)  
gen.add("bool_param",bool_t,2,"bool type",True)  
measure = gen.enum([gen.const("small",int_t,0,"small"),  
        gen.const("medium",int_t,1,"medium"),  
        gen.const("big",int_t,2,"big")],"choice")  
gen.add("list_param",int_t,3,"alternative choices",0,0,2,edit_method=measure)  
  
# generate intermediate file  
exit(gen.generate("demo01","config","dr1"))  

如果向我项目文件中建立的dr1.cfg文件写入上述代码会没有提示，反正.cfg文件和.py脚本文件的语法一致，我们可以先将dr1.cfg重命名为dr1.py，这样就有了内容提示，编写程序会轻松很多，编写完成后，我们在将dr1.py重命名为dr1.cfg（如果不恢复.py后缀则会编译失败）。编写客户端主要遵循以下步骤：

1. 导入函数库：

# import package  
from dynamic_reconfigure.parameter_generator_catkin import *

*的含义是将dynamic_reconfigure.parameter_generator_catkin库中的全部函数导入该文件。

2. 创建参数生成器：

# create parameter generator  
gen = ParameterGenerator()  

3. 向参数生成器中添加参数：

# add parameters into parameter generator  
gen.add("int_param",int_t,1,"int type",0,0,100)  // level=1
gen.add("double_param",double_t,2,"double type",1.57,0,3.14)  // level=2
gen.add("bool_param",bool_t,3,"bool type",True)  // level=3
measure = gen.enum([gen.const("small",int_t,0,"small"),  
        gen.const("medium",int_t,1,"medium"),  
        gen.const("big",int_t,2,"big")],"choice")  
gen.add("list_param",int_t,4,"alternative choices",0,0,2,edit_method=measure)  // level=4

参数生成器可以添加int、double、bool、enum、string这几种类型的参数。

1）add函数参数原型如下：add(name, paramtype, level, description, default=None, min=None, max=None, edit_method="")，其中

Name：参数名称；

Paramtype：参数类型（int_t，str_t，bool_t，double_t）共四种类型；

Level：参数掩码，其实就是一个整形常量，表征到底哪个参数是否被修改，相当于给参数起的数字编号；

Description：字符串类型，用于说明参数的细节；

Default：默认值；

Min/max：最大值/最小值；

edit_method：用于接收enum函数返回的句柄；

2）const函数原型参数如下：const(name, type, value, description)

Name：参数名称；

Type：参数类型（int_t，str_t，bool_t，double_t）共四种类型；

Value：常值参数的值；

Description：字符串类型，用于说明参数的细节；

3）enum函数原型参数如下：enum(constants, description)

Constants：常量组成的数组[…]；

Description：字符串类型，用于说明参数的细节；

关于不同类型的enum的写法：

// 字符串类型的枚举  
str_choice = gen.enum([gen.const("small",str_t,"A","small"),  
        gen.const("medium",str_t,"B","medium"),  
        gen.const("big",str_t,"C","big")],"str_choice")  
gen.add("list_param",str_t,0,"alternative choices","A",edit_method=str_choice)  
// 整形类型的枚举  
int_choice = gen.enum([gen.const("small",int_t,0,"small"),  
        gen.const("medium",int_t,1,"medium"),  
        gen.const("big",int_t,2,"big")],"int_choice")  
gen.add("list_param",int_t,0,"alternative choices",0,0,2,edit_method=int_choice)  
// 浮点型类型的枚举  
double_choice = gen.enum([gen.const("small",double_t,0.0,"small"),  
        gen.const("medium",double_t,1.0,"medium"),  
        gen.const("big",double_t,2.0,"big")],"int_choice")  
gen.add("list_param",double_t,0,"alternative choices",0.0,0.0,2.0,edit_method=double_choice)  
// bool类型的枚举  
bool_choice = gen.enum([gen.const("small",bool_t,true,"small"),  
        gen.const("medium",bool_t,false,"medium")],"int_choice")  
gen.add("list_param",bool_t,0,"alternative choices",true,edit_method=bool_choice)  

其中，string/bool类型都不用指明min/max。当编写完.cfg文件之后，一定要开启以下Linux系统中关于python脚本文件的可执行权限，只有基于脚本可执行权限，编译才可以通过，才可以生成GUI：

其中，chmod命令用于修改文件的可执行权限，可执行权限分为如下三类：

 我们右键单击XXX.cfg文件，点击“在集成终端中打开“，然后输入chmod +x XXX.cfg即可，然后在使用l -sl命令查看XXX.cfg文件的可执行权限：

 第一个字符代表文件类型。d代表目录,-代表非目录。接下来每三个字符为一组权限，分为三组，依次代表所有者权限，同组用户权限，其它用户权限。每组权限的三个字符依次代表是否可读，是否可写，是否可执行：

R：表示拥有读的权限

W：表示拥有写的权限

X：表示拥有可执行的权限

-：表示没有该权限

我们看第一行中的第2-第4个字母：rwx——文件已经可读可写可执行。关于chmod命令的所有参数含义详见：Linux chmod 命令 | 菜鸟教程 (runoob.com)https://www.runoob.com/linux/linux-comm-chmod.html

4）退出并生成中间文件

exit(gen.generate(package_name, "dynamic_server_name", "cfg_name"))

package_name表示在名为package_name的功能包，dynamic_server_name表示动态调参服务器的节点名称，cfg_name表示该.cfg的文件名。

然后，构建功能包是导入如下依赖项：roscpp、std_msgs、rospy、dynamic_reconfigure。导入依赖项之后得到的package.xml代码如下：

// 与动态配置调参相关的信息
<buildtool_depend>catkin</buildtool_depend>  
<build_depend>dynamic_reconfigure</build_depend>  
<build_depend>roscpp</build_depend>  
<build_depend>rospy</build_depend>  
<build_depend>std_msgs</build_depend>  
<build_export_depend>dynamic_reconfigure</build_export_depend>  
<build_export_depend>roscpp</build_export_depend>  
<build_export_depend>rospy</build_export_depend>  
<build_export_depend>std_msgs</build_export_depend>  
<exec_depend>dynamic_reconfigure</exec_depend>  
<exec_depend>roscpp</exec_depend>  
<exec_depend>rospy</exec_depend>  
<exec_depend>std_msgs</exec_depend>  

编译完成后配置CMakelist.txt文件：

// 与动态配置调参相关的配置信息
find_package(catkin REQUIRED COMPONENTS  
  dynamic_reconfigure  
  roscpp  
  rospy  
  std_msgs  
  message_generation  
)  
generate_dynamic_reconfigure_options(  
  cfg/dr1.cfg  
)  
catkin_package(  
#  INCLUDE_DIRS include  
#  LIBRARIES demo01  
 CATKIN_DEPENDS dynamic_reconfigure roscpp rospy std_msgs message_runtime  
#  DEPENDS system_lib  
)  
include_directories(  
# include  
  ${catkin_INCLUDE_DIRS}  
)  
add_executable(config src/config.cpp)  
add_dependencies(config ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS})  
target_link_libraries(config  
  ${catkin_LIBRARIES}  
) 

编译完成之后，结果如下所示：

  其实，参数生成器本质上是将客户端的参数打包成一个C++自定义类类型，这个类类型就是我们在C/S服务通信中的话题类型。

② 动态配置服务器的实现

1. 添加所需头文件

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

demo01/dr1Config.h：.cfg生成的自定义类类型文件；

dynamic_reconfigure/server.h：动态配置参数中与server相关的文件；

2. 节点初始化：

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

3. server的创建：

dynamic_reconfigure::Server<demo01::dr1Config> server;  

4. 回调函数的创建：

// 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);  
}  

其中，demo01::dr1Config& ConfigType_obj为话题类型；level为参数的掩码（32位的长整型变量），表征到底哪个参数是否被改变，给参数起的一个数字名称而已。

5. 回调函数的设置：

dynamic_reconfigure::Server<demo01::dr1Config>::CallbackType Callback;  
// boost::bind return a function object  
Callback = boost::bind(&CallbackFunc,_1,_2);  
server.setCallback(Callback);  

回调函数对象的原型如下：

boost::function<void(ConfigType &, uint32_t level)> CallbackType;  

这个参数必须是一个boost::function<…>类型的函数对象，为了将普通函数指针转化为boost库中的函数对象，我们应当使用boost::bind绑定函数进行转化：

Callback = boost::bind(&CallbackFunc,_1,_2);  

6. 使用回旋函数进行回调函数的回调：

ros::spin();  

我们运行结果如下所示：

1. 运行客户端：

2. 运行rosrun rqt_reconfigure rqt_reconfigure：

 3. 动态改变参数：

 我们看到当我们改变不同参数时，level值也会不同，即不同的level值代表着我们改变的参数不同。注意：我们在编译时，要将./.vscode/tasks.json的代码替换成如下代码：

{  
// 有关 tasks.json 格式的文档，请参见  
    // https://go.microsoft.com/fwlink/?LinkId=733558  
    "version": "2.0.0",  
    "tasks": [  
        {  
            "label": "catkin_make:debug", //代表提示的描述性信息  
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行  
            "command": "catkin_make",//这个是我们需要运行的命令  
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”  
            "group": {"kind":"build","isDefault":true},  
            "presentation": {  
                "reveal": "always"//可选always或者silence，代表是否输出信息  
            },  
            "problemMatcher": "$msCompile"  
        }  
    ]  
}  

并且编译完.cfg/.msg文件之后，要在c_cpp_properties.json文件的includepath中添加，./devel/include目录的路径，添加完后如下图所示：

"includePath": [  
  "/opt/ros/noetic/include/**",  
  "/usr/include/**",  
  "/home/rosnetic/test01/devel/include/**" // 改行为自己添加目录
] 

添加了之后，你再在文件中调用由自定义文件转化而来的头文件就不会报错了。

话题通信与动态参数配置

文件结构如下：

其中，cfg/cpp文件夹是编译时期生成的，我们无需创建该文件夹以及其中C++头文件。

从上述结构中，我们看到节点A充当两个角色：动态配置参数的服务器&话题通信的发布者，因此，程序也应当分为两部分去实现，程序设计流程图如下所示：

① 公共代码部分

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);  
// 动态配置参数的回调函数
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);  
} 

③ while轮询获取参数服务器中的参数并且与之相联的订阅者发布信息

ros::NodeHandle nh;  
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();  
}  

这样就可以使得发布者每次发布之前都读取一次参数服务器中的信息，我们特别的要注意参数服务器中的参数名称，我们不确定的话，可以编写完“动态调参“部分代码之后运行一下，使用rosparam list查看一下”动态调参客户端“上报至参数服务器中参数的名称：

我们给每个参数都加上"/dr/..."，太麻烦了，我们可以给节点句柄添加命名空间达到这一目的：

#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"); // 添加dr命名空间
    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;
}

 但是，此时发布者的话题变为了/dr/chatter，而订阅者的话题依旧为/chatter，这样也给订阅者的节点句柄添加与发布者同样的命名空间即可使两者的topic话题名称重新一直：

#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"); // 添加dr命名空间
    ros::Subscriber sub = nh.subscribe<demo01::sub_msg>("chatter",10,CallbackFunc);
    ros::spin();
    return 0;
}

CMakelist.txt文件新增配置信息：

// 与话题通信相关的配置
add_message_files(  
  FILES  
  sub_msg.msg  
)  
generate_messages(  
  DEPENDENCIES  
  std_msgs  
)  
add_executable(sub src/sub.cpp)  
add_dependencies(sub ${PROJECT_NAME}_gencpp ${catkin_EXPORTED_TARGETS})  
target_link_libraries(sub  
  ${catkin_LIBRARIES}  
)  

Package.xml文件新增配置信息：

// 与话题通信相关的配置信息
<build_depend>message_generation</build_depend>  
<exec_depend>message_runtime</exec_depend>  

运行结果如下所示：

 至此，我们可以通过GUI随时改变订阅者订阅到的信息了。