ROS入门（四）：使用参数服务器

当我们需要控制一个包含很多可移动部件的机器人时，每个部件的位置应该存储在参数服务器以方便其它节点在校正位置时调取。本节将介绍两种设置参数服务器的方法以及如何在ROS运行过程中通过可视化界面动态调整参数。两种方法分别参考rospy_tutorials示例006_parameters以及ros wiki教程.

进入学习之前，我们先查询一下rospy中有哪些涉及参数的函数：
$ pydoc rospy | grep _param

• delete_param(param_name)
• get_param(param_name, default= )
• get_param_names()
• has_param(param_name)
• search_param(param_name)
• set_param(param_name, param_value)

从函数命名和参数定义看，主要涉及参数的删除、取值、查询是否存在、新增几个功能。
其中search_param和has_param都是查询参数是否存在。依据help文档，参数名存在时，search_param返回参数键名(看代码更像是返回键值啊？），若不存在，search_param返回None.而has_param返回的是bool值。

这是rospy/client中的源代码。search_param 实际上调用了msproxy 中的search.param函数

def search_param(param_name):
    """
    Search for a parameter on the param server

    NOTE: this method is thread-safe.

    @param param_name: parameter name
    @type  param_name: str
    @return: key of matching parameter or None if no matching parameter. 
    @rtype: str
    @raise ROSException: if parameter server reports an error
    """
    _init_param_server()
    return _param_server.search_param(param_name)

def has_param(param_name):
    """
    Test if parameter exists on the param server

    NOTE: this method is thread-safe.

    @param param_name: parameter name
    @type  param_name: str
    @raise ROSException: if parameter server reports an error
    """
    _init_param_server()
    return param_name in _param_server #MasterProxy does all the magic for us

msproxy.search_param

def search_param(self, key):
        """
        Search for a parameter matching key on the parameter server
        @return: found key or None if search did not succeed
        @rtype: str
        @raise ROSException: if parameter server reports an error
        """
        # #1810 searchParam has to use unresolved form of mappings
        mappings = rospy.names.get_mappings()
        if key in mappings:
            key = mappings[key]
        with self._lock:
            code, msg, val = self.target.searchParam(rospy.names.get_caller_id(), key)
        if code == 1:
            return val
        elif code == -1:
            return None
        else:
            raise rospy.exceptions.ROSException("cannot search for parameter: %s"%msg)

实在太迂回了，决定等有空的时候再追踪下去。估计在client层_param_server存储的是一个字典，如果在这里搜索不到的话，就到别的域进行搜索。

有点跑偏了，我们回到如何编写和参数服务器通讯的节点。

---

1. 直接编写设置参数服务器的节点

param_talk.launch

launch文件在参数服务器上登记了四层参数（global->foo->gains->param_talker），其中节点param_talker在最里层gains中。如果有兴趣详细了解launch文件，请看官方文档

<launch>

  
  <param name="global_example" value="global value" />

  <group ns="foo">

    
    <param name="utterance" value="Hello World" />

    <param name="to_delete" value="Delete Me" />

    
    <group ns="gains">
      <param name="P" value="1.0" />
      <param name="I" value="2.0" />
      <param name="D" value="3.0" /> 
    group>

    <node pkg="rospy_tutorials" name="param_talker" type="param_talker.py" output="screen">

      
      <param name="topic_name" value="chatter" />

    node>

  group>

launch>

param_talker.py

该示例主要涉及：
- 获取global域的参数
- 获取父域foo中的参数（父与foo同音纯属巧合）
- 获取私有域的参数
- 安全设置：get_param的预设值
- 获取一组参数值
- 设置、删除、搜索参数值

param_talker节点输出

获取单个参数：以全名获取或者只以名字获取

# 采用全路径名获取global域的参数值
    global_example = rospy.get_param("/global_example")

# 只用参数名获取父域参数值
    utterance = rospy.get_param('utterance')

# 暂时没搞懂私有域（～参数名）的逻辑，先记住这个用法吧
    topic_name = rospy.get_param('~topic_name')

设置get_param的预设值参数

当查询的参数名不存在时返回预设值，以免后面的程序因格式不符或激起Exception错误终止整个进程

    default_param = rospy.get_param('default_param', 'default_value')

获取一整个GROUP的参数

示例说用GROUP名或者一个dictionary来提取一组参数。dictionary的用法还未实验。

    gains = rospy.get_param('gains')
    p, i, d = gains['P'], gains['I'], gains['D']

参数的设置、删除及查找

 # 参数设置：参数名与上述单个参数获取时名称相同
    rospy.set_param('list_of_floats', [1., 2., 3., 4.])
    rospy.set_param('bool_True', True)
    rospy.set_param('~private_bar', 1+2)
    rospy.set_param('to_delete', 'baz')


# 参数删除：删除之前先确认参数存在，以防激发exception终止进程
    if rospy.has_param('to_delete'):
        rospy.delete_param('to_delete')
        rospy.loginfo("deleted %s parameter"%rospy.resolve_name('to_delete'))
    else:
        rospy.loginfo('parameter %s was already deleted'%rospy.resolve_name('to_delete'))

# 参数查找：
    param_name = rospy.search_param('global_example')
    rospy.loginfo('found global_example parameter under key: %s'%param_name)

2. 编写cfg文件后设置

本部分主要参考动态参数设置的官方文档

当我在python里面查询ParamenterGenerator函数用法时，第一次输入命令有误，结果直接终止了我的python进程：

from dynamic_reconfigure.parameter_generator_catkin import *
help(ParameterGenerator())
# 正确查询方式为help(ParameterGenerator)

显示：

ahhhh! Unexpected command line syntax!

Are you trying to call a dynamic_reconfigure configuration generation script
directly? When you are using dynamic_reconfigure with python, you don't ever
need to invoke the configuration generator script yourself; it loads
automatically. If you are using dynamic_reconfigure from C++, you need to
add a call to generate_dynamic_reconfigure_options() in your CMakeLists.txt

For an example, see http://wiki.ros.org/dynamic_reconfigure/Tutorials

Have a nice day

想说这些开发ROS的人都这么有趣而欢脱的么…… 还 ”MasterProxy does all the magic”(client源码里重复了好几次），还 “ahhhh!”，最后看到 “Have a nice day” 就像被甩一个微笑表情。。

创建包及依赖项

catkin_create_pkg --rosdistro kinetic dynamic_tutorials rospy roscpp dynamic_reconfigure
mkdir cfg

我使用的ROS版本是kinetic，如果系统中只有一个版本的话，这里不需要指明

1. 编写cfg文件Tutorials.cfg

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# 指定包名
PACKAGE = "dynamic_tutorials"

from dynamic_reconfigure.parameter_generator_catkin import *

# 生成参数配置实例
gen = ParameterGenerator()

# 设置参数
# add(self, name, paramtype, level, description, default=None, min=None, max=None, edit_method='')
# @paramtype: 支持4类 int_t, double_t, str_t, bool_t
# @level: 还不太清楚这个参数的用法
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)

# 通过enum设置枚举型参数
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, "An 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)

# 生成必要文件并退出程序
# generate(pkgname, nodename, name)
# 第二个参数是使用这个配置文件的节点名（只是用来生成文档）
# 第三个参数必须与配置文件同名
exit(gen.generate(PACKAGE, "dynamic_tutorials", "Tutorials"))

2. 设置cfg文件权限并更新CMakeLists

[Terminal]

sudo chmod a+x cfg/Tutorials.cfg

更新CmakeLists.txt

## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
   cfg/Tutorials.cfg
 )

3. 编写配置参数服务器的节点

server.py 源代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy

from dynamic_reconfigure.server import Server
from dynamic_tutorials.cfg import TutorialsConfig
# TutorialsConfig 在运行上面cfg文件中generate()会自动生成

#当参数配置更新后调用的函数
def callback(config, level):
    rospy.loginfo("""Reconfigure Request: {int_param}, {double_param},\ 
          {str_param}, {bool_param}, {size}""".format(**config))
    return config

if __name__ == "__main__":
    rospy.init_node("dynamic_tutorials", anonymous = True)

    srv = Server(TutorialsConfig, callback)
    rospy.spin()

4. 运行节点！

# 设置节点文件权限
chmod +x nodes/server.py
# 确认rqt_reconfigure已经安装
sudo apt-get install ros-kinetic-rqt-reconfigure
# 编译包
cd ~/catkin_ws
catkin_make --pkg dynamic_tutorials
# 运行节点
rosrun dynamic_tutorials server.py
# 可视化动态调整窗口
rosrun rqt_gui rqt_gui -s reconfigure

TAH-DAH!! 可视化调整窗口出来了!!

5. 更新参数服务器的client节点

官方文档给出一个每隔十秒更新参数的client节点。但是我运行的时候，启用Client服务时都显示启用set_parameters服务超时。

raise ROSException("timeout exceeded while waiting for service %s"%resolved_name)
rospy.exceptions.ROSException: timeout exceeded while waiting for service /dynamic_tutorials/set_parameters

这里还是附上官方的代码待后期查看

#!/usr/bin/env python

import rospy

import dynamic_reconfigure.client

def callback(config):
    rospy.loginfo("Config set to {int_param}, {double_param}, {str_param}, {bool_param}, {size}".format(**config))

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

    client = dynamic_reconfigure.client.Client("dynamic_tutorials", 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()

附：006_parameters/param_talker.py完整代码

#!/usr/bin/env python
import rospy
from std_msgs.msg import String

def param_talker():
    rospy.init_node('param_talker')

    # Fetch values from the Parameter Server. In this example, we fetch
    # parameters from three different namespaces:
    #
    # 1) global (/global_example)
    # 2) parent (/foo/utterance)
    # 3) private (/foo/param_talker/topic_name)

    # fetch a /global parameter
    global_example = rospy.get_param("/global_example")
    rospy.loginfo("%s is %s", rospy.resolve_name('/global_example'), global_example)

    # fetch the utterance parameter from our parent namespace
    utterance = rospy.get_param('utterance')
    rospy.loginfo("%s is %s", rospy.resolve_name('utterance'), utterance)

    # fetch topic_name from the ~private namespace
    topic_name = rospy.get_param('~topic_name')
    rospy.loginfo("%s is %s", rospy.resolve_name('~topic_name'), topic_name)

    # fetch a parameter, using 'default_value' if it doesn't exist
    default_param = rospy.get_param('default_param', 'default_value')
    rospy.loginfo('%s is %s', rospy.resolve_name('default_param'), default_param)

    # fetch a group (dictionary) of parameters
    gains = rospy.get_param('gains')
    p, i, d = gains['P'], gains['I'], gains['D']
    rospy.loginfo("gains are %s, %s, %s", p, i, d)    

    # set some parameters
    rospy.loginfo('setting parameters...')
    rospy.set_param('list_of_floats', [1., 2., 3., 4.])
    rospy.set_param('bool_True', True)
    rospy.set_param('~private_bar', 1+2)
    rospy.set_param('to_delete', 'baz')
    rospy.loginfo('...parameters have been set')

    # delete a parameter
    if rospy.has_param('to_delete'):
        rospy.delete_param('to_delete')
        rospy.loginfo("deleted %s parameter"%rospy.resolve_name('to_delete'))
    else:
        rospy.loginfo('parameter %s was already deleted'%rospy.resolve_name('to_delete'))

    # search for a parameter
    param_name = rospy.search_param('global_example')
    rospy.loginfo('found global_example parameter under key: %s'%param_name)

    # publish the value of utterance repeatedly
    pub = rospy.Publisher(topic_name, String, queue_size=10)
    while not rospy.is_shutdown():
        pub.publish(utterance)
        rospy.loginfo(utterance)
        rospy.sleep(1)

if __name__ == '__main__':
    try:
        param_talker()
    except rospy.ROSInterruptException: pass

第一部分代码版权归属： Copyright (c) 2008, Willow Garage, Inc.
第二部分代码参考：http://wiki.ros.org/dynamic_reconfigure/Tutorials

转载请注明出处