关于ROS下的Dynamic Reconfig的学习探索

Dynamic Reconfig是ROS比较厉害的一个地方，可以在程序运行的时候动态调整参数，ROS官网关于Dynamic Reconfig的讲解主要有三处

1. http://wiki.ros.org/dynamic_reconfigure/Tutorials

2. http://wiki.ros.org/roscpp_tutorials/Tutorials 最下面

3. http://wiki.ros.org/ROSNodeTutorialC++ 最下面

个人感觉第一个好一点，另外两个比较杂乱。下面就介绍自己关于Dynamic Reconfig学习过程，这里假定大家都做完了，Beginner tutorials的学习，即写一个Publisher ，subscriber，自定义的service（两个数字的加法）以及Msg的学习。

主要参考：

http://wiki.ros.org/dynamic_reconfigure/Tutorials/SettingUpDynamicReconfigureForANode

我的最终的文件目录如图

1.定义cfg文件

首先在catkin_ws/src/beginner_tutorials目录下建立一个名为cfg的文件夹，放入一个名为MyStuff.cfg的文件，这个文件的作用就是告诉ROS 到时侯我们需要动态调整的参数有哪些。

#! /usr/bin/env python 
# Forearm camera configuration

PACKAGE='beginner_tutorials'
import roslib
roslib.load_manifest(PACKAGE)

from math import pi

from driver_base.msg import SensorLevels
from dynamic_reconfigure.parameter_generator import *

gen = ParameterGenerator()
angles = gen.add_group("Angles")
#       Name                    Type      Reconfiguration level
#       Description
#       Default  Min  Max
angles.add("min_ang",           double_t, SensorLevels.RECONFIGURE_STOP,
           "The angle of the first range measurement. The unit depends on ~ang_radians.",
           -pi/2,-pi, pi)
angles.add("max_ang",           double_t, SensorLevels.RECONFIGURE_STOP,
           "The angle of the first range measurement. The unit depends on ~ang_radians.",
            pi/2, -pi, pi)
gen.add("intensity",            bool_t,   SensorLevels.RECONFIGURE_STOP,
        "Whether or not the hokuyo returns intensity values.",
         False)
gen.add("cluster",              int_t,    SensorLevels.RECONFIGURE_STOP,
        "The number of adjacent range measurements to cluster into a single reading",
         1, 0, 99)
gen.add("skip",                 int_t,    SensorLevels.RECONFIGURE_STOP,
        "The number of scans to skip between each measured scan",
         0, 0,  9)
gen.add("port",                 str_t,    SensorLevels.RECONFIGURE_CLOSE,
        "The serial port where the hokuyo device can be found",
        "/dev/ttyACM0")
gen.add("calibrate_time",       bool_t,   SensorLevels.RECONFIGURE_CLOSE,
        "Whether the node should calibrate the hokuyo's time offset",
         True)
gen.add("frame_id",             str_t,    SensorLevels.RECONFIGURE_RUNNING,
        "The frame in which laser scans will be returned",
        "laser")
gen.add("time_offset",          double_t, SensorLevels.RECONFIGURE_RUNNING,
        "An offet to add to the timestamp before publication of a scan",
         0, -0.25, 0.25)
gen.add("allow_unsafe_settings",bool_t,   SensorLevels.RECONFIGURE_CLOSE,
        "Turn this on if you wish to use the UTM-30LX with an unsafe angular range. Turning this option on may cause occasional crashes or bad data. This option is a tempory workaround that will hopefully be removed in an upcoming driver version.",
         False)

exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "MyStuff"))

这里尤其要注意最后一句的第三个参数，必须与config文件同名，我们的config文件名为MyStuff.cfg所以这里为Mytuff。而且第三个参数也决定着我们的config头文件的名称，后面埋个伏笔，头文件生成的名称为MyStuffConfig.h也即，第三个参数加上Config.h.

修改cfg文件的权限

chmod a+x MyStuff.cfg

2.修改依赖项

在manifest.xml文件中加入依赖项即：

<build_depend>dynamic_reconfigure</build_depend>
<build_depend>driver_base</build_depend>

<run_depend>dynamic_reconfigure</run_depend>
<run_depend>driver_base</run_depend>

3.创建server主函数

在src文件夹下建立一个名为dynamic_reconfigure_node.cpp的文件，代码如下，尤其注意这里的第二个头文件的包含，因为我发现编译的时候，生成的config的头文件编译时放在了cfg文件夹下的cpp文件夹中，这个是catkin_make（catkin_make __force-cmake）的时候创建的，但是按照官网的路径它查找不到，我们干脆拷贝到src文件夹下

#include <dynamic_reconfigure/server.h>
#include "MyStuffConfig.h"
#include <ros/ros.h>

void callback(beginner_tutorials::MyStuffConfig &config, uint32_t level)
{
  ROS_INFO("Reconfigure request : %f %f %i %i %i %s %i %s %f %i",
           config.groups.angles.min_ang,
           config.groups.angles.max_ang,
           (int)config.intensity,
           config.cluster,
           config.skip,
           config.port.c_str(),
           (int)config.calibrate_time,
           config.frame_id.c_str(),
           config.time_offset,
           (int)config.allow_unsafe_settings);

  // do nothing for now
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "dynamic_reconfigure_node");
  dynamic_reconfigure::Server<beginner_tutorials::MyStuffConfig> srv;
  dynamic_reconfigure::Server<beginner_tutorials::MyStuffConfig>::CallbackType f;
  f = boost::bind(&callback, _1, _2);
  srv.setCallback(f);
  ROS_INFO("Starting to spin...");
  ros::spin();
  return 0;
}

4.关于CMakeList.txt的修改

这个文件相信大家不陌生，但是也是最头疼的地方，编译的错误好多都是设置不当引起的。我直接贴上来

cmake_minimum_required(VERSION 2.8.3)
project(beginner_tutorials)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  dynamic_reconfigure
)

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependencies might have been
##     pulled in transitively but can be declared for certainty nonetheless:
##     * add a build_depend tag for "message_generation"
##     * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
 add_message_files(
  FILES
  Num.msg
 )

## Generate services in the 'srv' folder
 add_service_files(
   FILES
   AddTwoInts.srv
   
 )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
 )

# for dynamic reconfigure
generate_dynamic_reconfigure_options(
  cfg/MyStuff.cfg
  
)

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package( CATKIN_DEPENDS message_runtime)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)

include_directories(${catkin_INCLUDE_DIRS})

## Declare a cpp library
# add_library(beginner_tutorials
#   src/${PROJECT_NAME}/beginner_tutorials.cpp
# )

## Declare a cpp executable
# add_executable(beginner_tutorials_node src/beginner_tutorials_node.cpp)

add_executable(talker src/talker.cpp)
add_executable(listener src/listener.cpp)

# for dynamic reconfigure
add_executable(dynamic_reconfigure_node src/dynamic_reconfigure_node.cpp)


add_executable(add_two_ints_server src/add_two_ints_server.cpp)
add_executable(add_two_ints_client src/add_two_ints_client.cpp)

## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(beginner_tutorials_node beginner_tutorials_generate_messages_cpp)

# for dynamic reconfigure
add_dependencies(dynamic_reconfigure_node beginner_tutorials_gencfg)
# for python dynamic client
add_dependencies(dynamic_reconfigure_node beginner_tutorials_generate_messages_cpp)

add_dependencies(talker beginner_tutorials_generate_messages_cpp)
add_dependencies(listener beginner_tutorials_generate_messages_cpp)

add_dependencies(add_two_ints_server beginner_tutorials_gencpp)
add_dependencies(add_two_ints_client beginner_tutorials_gencpp)

## Specify libraries to link a library or executable target against
# target_link_libraries(beginner_tutorials_node ${catkin_LIBRARIES} )

# for dynamic reconfigure
target_link_libraries(dynamic_reconfigure_node ${catkin_LIBRARIES})

target_link_libraries(talker ${catkin_LIBRARIES})
target_link_libraries(listener ${catkin_LIBRARIES})

target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables and/or libraries for installation
# install(TARGETS beginner_tutorials beginner_tutorials_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_beginner_tutorials.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

关于dynamic reconfig的主要几句是

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  dynamic_reconfigure
)

generate_dynamic_reconfigure_options(
  cfg/MyStuff.cfg
 
)

include_directories(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)

add_executable(dynamic_reconfigure_node src/dynamic_reconfigure_node.cpp)这里生成可执行的node文件，node名字是dynamic_reconfigure_node

add_dependencies(dynamic_reconfigure_node beginner_tutorials_gencfg)也有写成${PROJECT_NAME}_gencfg的

target_link_libraries(dynamic_reconfigure_node ${catkin_LIBRARIES})生成可执行的node文件，node名字是dynamic_reconfigure_node

5.测试用的程序

测试用到的程序，其实我在这里仿照了http://wiki.ros.org/dynamic_reconfigure/Tutorials/中的client.py的写法，但是它的有个缺点，按照ros.rate(0.1)即0.1Hz也即10s一次，他会复位，就是你调整的参数10s后就由默认值替代了，我这里修改了一下，让他不会复位

在src中放入一个名为yakeclient.py的文件，这里就是客户端了

#!/usr/bin/env python

PACKAGE = 'beginner_tutorials'
import roslib;roslib.load_manifest(PACKAGE)
import rospy

import dynamic_reconfigure.client

def callback(config):
    rospy.loginfo("Config set to {min_ang}, {max_ang}, {intensity}, {cluster}, {skip}, {port}, {calibrate_time}, {frame_id}, {time_offset},{allow_unsafe_settings}".format(**config))
   

if __name__ == "__main__":
    rospy.init_node("yakeclient")

# Client first argument is the node names
    client = dynamic_reconfigure.client.Client("dynamic_reconfigure_node", timeout=30, config_callback=callback)

    r = rospy.Rate(0.1)
    #x = 0
    #b = False 
    while not rospy.is_shutdown():
        #x = x+1
        #if x>10:
        #    x=0
        #b = not b
       # client.update_configuration({"int_param":x, "double_param":(1/(x+1)), "str_param":str(rospy.get_rostime()), "bool_param":b, "size":1})
    	r.sleep()

6.编译并启动程序

7.对比官方教程

这里我们可以看到其实结构都是一样的，最主要的区别还是在于config文件的定义，而他是完全自由的，完全由设计者考虑，需要动态调整一些什么样子的参数。

从下面的这个详细的节点关系图，我们可以看出，其实rqt_reconfig也即图中的rqt_gui_py_node_9301，就是一个client。那个9507是rqt_graph

关掉我们自己的yakeclient如图所示。因此自己的那个client目前只有显示的作用，（即只能看到设定的值）。而rqt_reconfig则包含了设置与显示。所以一般情况下，我们所要做的就是写dynamic server，利用rqt_reconfig作为client来设定目标值。

8.总结

这些内容写起来容易，但是自己花了差不多两天才解决掉，其中主要卡在了cfg.h上，因为系统编译老是提示找不到它，如果你也有这样的问题，首先看看CMakeList.txt内容是否正确，二，就像我之前那样，生成的头文件居然放在cfg文件夹里了。还有就是Client.py文件的写法。我当时起得名字叫yakeclient.py，观察rqt_graph时的节点信息老是订阅不了topic

像上图中的一样，然后仔细查看才发现python中的文件第17行

这里我想当然理解为了package name其实它应该是publisher 的名称，也就是server的名称，这里我们的server节点是dynamic_reconfigure_node所以改完以后就Ok了。引以为戒。