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

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!

 

  • 7
    点赞
  • 20
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值