1. 程式人生 > >ROS動態調參(dynamic reconfigure)客戶端服務端之C++ Python實現

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檢視,可以發現裡面的引數值在不斷變化。截圖如下:

工程原始碼下載地址:

Enjoy!