ros中rqt_reconfigure源码解读及其C++实现

Posted 2022-12-06 lwlv

ROS中参数如需实时更新得采用dynamic_reconfigure机制，这样用户在客户端修改参数后不需要重启master，直接向服务端发送请求，然后服务端通过回调函数确认来完成参数的动态重配置，具体服务端、客户端的写法可参见dynamic_reconfigure/Tutorials和《ROS动态调参(dynamic reconfigure)客户端服务端之C++ Python实现》。此外，ROS还提供了一个很好的工具便于我们查看和实时修改参数，这就是今天的主题——rqt_reconfugre。

rqt_reconfugre是用python实现，基于rqt_plugin框架，详见http://wiki.ros.org/rqt/Tutorials教程，rqt_reconfugre源代码可从https://github.com/ros-visualization/rqt_reconfigure下载到。

 

一、初始化

首先是写一个plugin.xml文件用于描述该插件，然后在param_plugin.py文件中有一个ParamPlugin类用于初始化插件的基本信息，并加载主界面类ParamWidget。

ParamWidget类的实现在param_widget.py中，主要作用是加载子模块，并实现页面布局。

如上图所示，在红框中的局部为水平splitter，左边为节点选择和过滤文本框，即NodeSelectorWidget类和TextFilterWidget类；右边是ParameditWidget类。其余部分为rqt_plugin自动生成或者调用得到的。

各子模块的信号与槽的绑定也在ParamWidget类中声明了，主要有以下四个，看注释可以很明显知道含义。

# Signal from paramedit widget to node selector widget.
reconf_widget.sig_node_disabled_selected.connect(self._nodesel_widget.node_deselected)

# Pass name of node to editor widget
self._nodesel_widget.sig_node_selected.connect(reconf_widget.show_reconf)

#Connect filter signal-slots.
self._text_filter.filter_changed_signal.connect(self._filter_key_changed)

# Open any clients indicated from command line
self.sig_selected.connect(self._nodesel_widget.node_selected)

此外，还有一些界面辅助功能，保存退出时位置等。

二、NodeSelectorWidget类

NodeSelectorWidget类的实现在node_selector_widget.py中，并加载node_selector.ui界面，页面布局有三个按钮和一个QTreeView。也就说具备dynamic reconfigure功能的节点将以树形结构的形式呈现在QTreeView中，按钮Collapse和Expand分别用于折叠和展开树。而按钮Refresh则用于刷新具有dynamic reconfigure功能的节点。

刷新功能在_update_nodetree_pernode函数中实现，之所以能得到dynamic reconfigure功能的节点还是得益于ros python的库写得好，即find_reconfigure_services()函数，这个函数ros cpp里面没有。后来专门查看了一下这个函数，原来ros master开启后是可以查询到所有活跃的topic和service。在rosservice中，有个getSystemState函数可以返回所有ros master的系统状态。

返回类型的结构为：[[str,[str]], [str,[str]], [str,[str]]]，第一个数组为发布的topic，第二个数组为订阅的topic，第三个数组为开启的服务。

具体结构如下：

System state is in list representation::
  [publishers, subscribers, services].
        
publishers is of the form::
  [ [topic1, [topic1Publisher1...topic1PublisherN]] ... ]
        
subscribers is of the form::
  [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
        
services is of the form::
  [ [service1, [service1Provider1...service1ProviderN]] ... ]

实际上dynamic reconfigure也属于ros service，其形式是在后面加了"set_parameters"后缀。find_reconfigure_services的作用就是寻找带有"set_parameters"后缀的service。

如果想通过C++实现，还是得调用getSystemState函数，对应roscpp中的ros::master::execute。具体实现如下：

bool find_dynmaic_services( std::vector<std::string> &srvls)

	srvls.clear();

	XmlRpc::XmlRpcValue args, result, payload;
	args[0] = ros::this_node::getName();

	if (!ros::master::execute("getSystemState", args, result, payload, true))
	
		ROS_ERROR("Execute ros system wrong!");
		return false;
	

	//std::cout << "payload[2] (services) size: " << payload[2].size() << " \\n";
	for (int i = 0; i < payload[2].size(); ++i)
	
		XmlRpc::XmlRpcValue val = payload[2][i];
		std::string service_name = val[0];
		int index = service_name.find("/set_parameters");
		if(index > 0)    //not found index = -1
		
			std::string dynsrv_name = service_name.assign(service_name.c_str(), index);
			srvls.push_back(dynsrv_name);
		
	
	return true;

以上，通过该函数就可获取dynamic reconfigure服务的列表了。

 

对于获取的dynamic reconfigure服务，由于有的是在pluginlib中开启，会存在多级的情况，类似于"/.../.../..."。_update_nodetree_pernode，_add_children_treenode（添加子节点）和_prune_nodetree_pernode（裁剪树形结构）函数就是为了实现该功能，从而形成树形结构。

在Qt中，TreeView控件只是视图，用于显示，还得加入模型，rqt_reconfigure又专门写了两个类TreenodeItemModel和TreenodeQstdItem用于加载模型。在TreenodeQstdItem类中，另专门开有一个线程connect_param_server用于不断连接树根节点的服务，即dynamic_reconfigure.client.Client，并将该客户端类型传入到DynreconfClientWidget中。

对于QTreeView中选择的节点，点击选择节点则相应的参数会显示在左右的框，再点一次该节点，则相当于不再选择该节点，由_selection_deselected和_selection_selected实现。

三、ParameditWidget类

ParameditWidget类用于显示dynamic reconfigure服务的参数列表，并实时刷新。在该类中会加载paramedit_pane.ui界面。该界面由QScrollArea和QWidget组成，并接受从ParamWidget发来的信号，用于显示参数列表，即show_reconf函数。对于选中的节点，显示客户端widget是有颜色交叉变化的，详见代码如下：

from rqt_py_common.layout_util import LayoutUtil

LayoutUtil.alternate_color(
    self._dynreconf_clients.values(),
    [self.palette().window().color().lighter(125),
    self.palette().window().color().darker(125)])

下面着重说DynreconfClientWidget类，该类继承GroupWidget。首先还是进行一个布局，最上面是保存、载入、标题和关闭控件。python的库果然还是强大，对于参数保存或加载为yaml格式，没想到调用yaml模块中的dump和load_all即可。

DynreconfClientWidget类中调用ParamUpdater类，开启线程不断更新参数，用的dynamic_reconfigre.client中的update_configuration函数。在self.reconf.config_callback = self.config_callback中，则是通过回调函数来加载参数。dynamic reconfigre的参数分两种，一种是在一个group中，一种是独立的，参数共五种类型：布尔，整型，浮点型，字符型和枚举型。

EditorWidget类在param_editors.py模块中实现，主要是根据参数类型加载不同的参数UI，根据config类型得到每个参数的描述，default，最大值，最小值，并增加了右键菜单栏。同时更新数据update_value和参数update_configuration。

四、TextFilter类

textfiler类主要是为了方便检索tree view里面的节点。过滤原理在于使用了QSortFilterProxyModel，由FilterChildrenModel类实现。

 

基于以上解读，可以在C++写一个类似的界面框架，关键在于找到开启了dynamic reconfigre服务的节点，其余都关于界面方面的操作。不过对于dynamic reconfigre的c++客户端，得知道config具体类型才能设置参数服务。不过可以通过调用开启dynamic reconfigure服务的parameter_updates topic知道更新后的数值。

整理后的软件截图如下：

学得还是挺像的，只不过不能想python一样能动态调节参数，监测参数及其参数的变化还是可以的。

 

源码实现在https://github.com/WelinLee/ROS_dynamic_reconfig中的dyn_cfg_gui包。

Enjoy!