ROS动态调参(dynamic reconfigure)客户端服务端之C++ Python实现

在ROS系统中，我们需要实时修改参数，并能马上看到运行效果。这一功能是通过ros dynamic_reconfigure包实现的。官网教程如下：dynamic_reconfigure/Tutorials。

具体操作流程可简述为如下：首先写一个.cfg文件，python语法。然后在CMakeLists.txt文件中添加generate_dynamic_reconfigure_options用于编译刚才写的.cfg文件，并给出正确路径，且一定记得用chmod a+x *.cfg将其变为可执行的文件，这步很关键。最后在源码中包含编译后的头文件，一般是Config.h结尾，就可以开始码代码了。

目前官网给出的例程实现dynamic_reconfigure的客户端只给出了python例程，而服务端给了c++和python两种方法的实现。在rqt_reconfigure中，其源代码也是通过python写的GUI实现的。后来查看了下dynamic_reconfigure的源码，发现在头文件中包含了client.h文件，仔细研究了下，并对比了server.h文件，发现包含该文件即可用C++实现dynamic_reconfigure的客户端。在ros indigo版本中，dynamic_reconfigure包里面可能还没有client.h头文件，建议更新至最新的dynamic_reconfigure包。

以下是具体示例：

在CMakeLists.txt中包含

find_package(catkin REQUIRED roscpp rospy dynamic_reconfigure)
generate_dynamic_reconfigure_options(cfg/config.cfg)    # .cfg文件的名字

...

# make sure configure headers are built before any node using them
add_dependencies({PROJECT_NAME}_node ${PROJECT_NAME}_gencfg)  #添加依赖项也很关键，否则找不到生成的*Config.h文件

所写的.cfg文件所下，共设置了5种类型的参数：

#!/usr/bin/env python
PACKAGE = "my_msgs"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("int_param", int_t, 0, "An Integer parameter", 50, 0, 100)
gen.add("double_param", double_t, 0, "A Double parameter", .5, 0, 1)
gen.add("str_param", str_t, 0, "A String parameter", "Hello World")
gen.add("bool_param", bool_t, 0, "A Boolean parameter", True)

size_enum = gen.enum([gen.const("Small", int_t, 0, "A small constant"),
                      gen.const("Medium", int_t, 1, "A medium constant"),
                      gen.const("Large", int_t, 2, "A large constant"),
                      gen.const("Extralarge", int_t, 3, "A extra large constant")],
                      "An enum to set size")
gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum)

exit(gen.generate(PACKAGE, "my_msgs", "tutorials"))

服务器端具体实现的关键代码：

    dynamic_reconfigure::Server<my_msgs::tutorialsConfig> server;
    dynamic_reconfigure::Server<my_msgs::tutorialsConfig>::CallbackType f;     //用于回调

    f = boost::bind(&callback, _1, _2);          //回调函数，用于获取最新参数
    server.setCallback(f);

客户端具体实现的关键代码（C++）：

    dynamic_reconfigure::Client<my_msgs::tutorialsConfig> client("dynamic_srv", dynCallBack);  //订阅服务，并设置回调函数用于回读改变后的最新参数
    my_msgs::tutorialsConfig config;

    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        c++;
        static bool ret = true;
        static int cnt = 0;
        config.bool_param = !ret;
        config.int_param = cnt;
        config.double_param = 1/((double)(cnt+1));
        client.setConfiguration(config);          //用于更新参数
        ret = !ret;

        ros::spinOnce();
        loop_rate.sleep();
    }

注：用C++实现的话，如果所用版本dynamic_reconfigure包中没有client.h头文件，得从github上下载源码，然后将client.h加入到工程当中。

客户端具体实现的关键代码（python）：

    rospy.init_node("dynamic_client")
    rospy.wait_for_service("dynamic_srv/set_parameters")
    client = dynamic_reconfigure.client.Client("dynamic_srv", timeout=10, config_callback=callback)        //callback用于回读参数

    client.update_configuration({"int_param":x, "double_param":(1/(x+1)), "str_param":str(rospy.get_rostime()), "bool_param":b, "size": 1})    //update_configuration函数用于更新参数

在该客户端中，本人写了一个循环不断去改变参数的值，其效果类似于打开rqt_reconfigure去调节参数，当然也可以做一个GUI界面去改变参数的值。

代码完成后，编译，运行roscore，并设置工作空间的环境变量。

开启服务器端节点，再开启客户端节点（python和c++程序开启一个即可）。

用rqt_configure查看，可以发现里面的参数值在不断变化。截图如下：

工程源码下载地址：

https://github.com/WelinLee/ROS_dynamic_reconfig

Enjoy!