创建 ROS rqt 插件 topic service

本机系统：Ubuntu 16.04, ROS Luna

更新20191023：Ubutnu 18.04 LTS ROS Melodic

 

所有文件可在https://github.com/huyaoyu/rqt_my_plugin获取。

 

关于ROS rqt custom plugin的官方文档在

http://wiki.ros.org/rqt/Tutorials/Create%20your%20new%20rqt%20plugin

http://wiki.ros.org/rqt/Tutorials/Writing%20a%20Python%20Plugin

相关源码在

https://github.com/lucasw/rqt_mypkg

本文在综合上述信息的基础上，进行了尝试，这里把过程留下。

更新20191023：在实例中增加了topic的publisher和subscriber，增加了service 的server和client。

 

创建新ROS package

• 在catkin的workspace/src下，创建新的ROS package。

catkin_create_pkg rqt_my_plugin rospy rqt_gui rqt_gui_py

• 修改rqt_my_plugin/package.xml文件

变更package名称（尚未完善测试，这里是为了防止python import过程中出现命名冲突）

<package><name>元素从qrt_my_plugin修改为my_plugin

 

在<package>element下增加如下内容

<export>
    <rqt_gui plugin="${prefix}/plugin.xml" />
</export>

为了使用service，在<package>元素内增加

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

 

创建plugin.xml文件。plugin.xml文件同样位于rqt_my_plugin文件夹下。

<library path="src">
  <class name="My plugin" type="plugin.my_module.MyPlugin" base_class_type="rqt_gui_py::Plugin">
    <description>
      A Python GUI plugin for test the functionalities.
    </description>
    <qtgui>
      <group>
        <label>Logging</label>
        <icon type="theme">folder</icon>
        <statustip>Plugins related to logging.</statustip>
      </group>
      <label>My plugin label</label>
      <icon type="theme">applications-other</icon>
      <statustip>A Python GUI plugin for test the functionalities.</statustip>
    </qtgui>
  </class>
</library>

其中，<class>的type属性是后面要创建的python脚本内定义的类名。<group>element表示将该插件置于rqt的plugin目录的哪一个子目录下。

setup.py文件

在同样位置创建setup.py文件，内容如下。

from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
  
d = generate_distutils_setup(
    packages=['plugin'],
    package_dir={'': 'src'},
)

setup(**d)

创建UI resource

在package的文件夹内创建resource文件夹，在该文件夹内放置Qt designer输出的.ui文件。.ui文件实际上是xml文件，这里使用的实例如下，命名为MyPlugin.ui。

<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
 <class>Class name</class>
 <widget class="QWidget" name="Form">
  <property name="geometry">
   <rect>
    <x>0</x>
    <y>0</y>
    <width>400</width>
    <height>300</height>
   </rect>
  </property>
  <property name="windowTitle">
   <string>My plugin window title</string>
  </property>
  <widget class="QPushButton" name="button_test">
   <property name="geometry">
    <rect>
     <x>120</x>
     <y>70</y>
     <width>98</width>
     <height>27</height>
    </rect>
   </property>
   <property name="text">
    <string>Click me!</string>
   </property>
  </widget>
 </widget>
 <resources/>
 <connections/>
</ui>

在目前的UI上，仅有一个PushButton。

 

my_module.py

接下来创建my_module.py，该文件的文件名必须也plugin.xml文件中的library/class@type属性描述一致。在package的src文件夹下创建文件夹plugin，然后在src/plugin内创建__init__.py文件。__init__.py文件内容为空。接下来在src/plugin内创建my_module.py文件，内容如下。

import os
import rospy
import rospkg
from std_msgs.msg import String

from qt_gui.plugin import Plugin
from python_qt_binding import loadUi
from python_qt_binding.QtCore import Qt, Slot
from python_qt_binding.QtWidgets import QWidget

from my_plugin.srv import AddTwoInts, AddTwoIntsResponse

class MyPlugin(Plugin):

    def __init__(self, context):
        super(MyPlugin, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('MyPlugin')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q", "--quiet", action="store_true",
                      dest="quiet",
                      help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        if not args.quiet:
            print 'arguments: ', args
            print 'unknowns: ', unknowns

        # Create QWidget
        self._widget = QWidget()
        # Get path to UI file which should be in the "resource" folder of this package
        ui_file = os.path.join(rospkg.RosPack().get_path('my_plugin'), 'resource', 'MyPlugin.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('MyPluginUi')
        # Show _widget.windowTitle on left-top of each plugin (when 
        # it's set in _widget). This is useful when you open multiple 
        # plugins at once. Also if you open multiple instances of your 
        # plugin at once, these lines add number to make it easy to 
        # tell from pane to pane.
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        
        # Push button.
        self._widget.button_test.clicked.connect(self.on_button_test_clicked)
        
        # Add widget to the user interface
        context.add_widget(self._widget)

        # Simple topic publisher from ROS tutorial.
        self.rosPub = rospy.Publisher('my_plugin_pub', String, queue_size=10)
        self.rosPubCount = 0

        # Simple topic subscriber.
        self.rosSub = rospy.Subscriber("my_plugin_sub", String, self.ros_string_handler)

        # Start simple ROS server.
        self.rosSrv = rospy.Service("add_two_ints", AddTwoInts, self.handle_add_two_ints)

    def ros_string_handler(self, data):
        rospy.loginfo(rospy.get_caller_id() + ": Subscriber receives %s", data.data)

    def handle_add_two_ints(self, req):
        rospy.loginfo( "Returning [%s + %s = %s]" % (req.a, req.b, (req.a + req.b)) )
        return AddTwoIntsResponse(req.a + req.b)

    @Slot()
    def on_button_test_clicked(self):
        rospy.loginfo("button_test gets clicked. Send request to itself.")

        self.rosPub.publish("rosPubCount = %d. " % (self.rosPubCount))
        self.rosPubCount += 1

        # Send service request.
        try:
            rospy.wait_for_service("add_two_ints", timeout=5)

            # Set the actual request.
            add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
            resp = add_two_ints(self.rosPubCount, 100)

            rospy.loginfo("add_two_ints responses with %d. " % ( resp.sum ))
        except rospy.ROSException as e:
            rospy.loginfo("Service add_two_ints unavailable for 5 seconds.")
        except rospy.ServiceException as e:
            rospy.logerr("Service request to add_two_ints failed.")

    def shutdown_plugin(self):
        # TODO unregister all publishers here
        self.rosSrv.shutdown()
        rospy.loginfo("rosSrv shutdown. ")

        self.rosSub.unregister()
        rospy.loginfo("rosSub unregistered. ")

        self.rosPub.unregister()
        rospy.loginfo("rosPub unregistered. ")

    def save_settings(self, plugin_settings, instance_settings):
        # TODO save intrinsic configuration, usually using:
        # instance_settings.set_value(k, v)
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        # TODO restore intrinsic configuration, usually using:
        # v = instance_settings.value(k)
        pass

    #def trigger_configuration(self):
        # Comment in to signal that the plugin has a way to configure
        # This will enable a setting button (gear icon) in each dock widget title bar
        # Usually used to open a modal configuration dialog
        

这里my_moduel.py中，注意__init__函数中loadUi( )的使用。loadUi( )执行过之后，self._widget对象会根据MyPlugin.ui文件的描述，自动创建新的成员变量，例如我们的按钮将会成为self._widget.button_test。在__init__( )函数中同样注册好buttion_test的onClicked事件的回调函数，或者通过

self._widget.button_test.clicked.connect(self.on_button_test_clicked)

实现。on_button_test_clicked( )函数即为该回调函数。这里@Slot修饰符其实可以不用。观察on_button_test_clicked( )函数可知，当我们点击button_test按钮后，将会产生一组有关简单loginfo, topic和service的示例操作 。

 

首先，程序向my_plugin_pub topic 发送了一个std_msgs/String类型的数据。之后，程序试图连接add_two_ints service，若5秒钟内连接成功则向该service发送请求。请求成功处理后会显示response结果。若请求超时或者请求返回异常，会有对应的处理。 这里所用到的topic publisher和service server都声明在MyPlugin的__init__()函数中，分别是self.rosPub和self.rosSrv。所以本实例中，add_two_ints service的提供方是自己，这只是作为例子而已，现实中server和client若都是一个node貌似没有什么用。

 

此外，作为实例的一部分，MyPlugin类中声明了一个topic subscriber，注册用于监听my_plugin_sub topic。可通过rostopic pub命令进行测试。

 

本实例中所用到的topic publisher, subscriber和 service server, client的逻辑都取自ROS的官方教程。

ROS文档中提到，rqt插件不需要用户显式调用init_node( )，但是在shut_down( )函数中需要unsubscribe所有已经订阅过的topic, 注销自身的publisher，释放所有自身的timer。

 

创建scripts文件夹

根据ROS的推荐方案，在package文件夹下创建scripts文件夹，将启动plugin的python脚本放入其中。脚本名为my_plugin，注意该文件名与src文件夹中的源码文件是同名的，但是省略了.py扩展名。通过chmod命令修改my_plugin的权限，将其变为可执行文件。my_plugin的内容如下。

#!/usr/bin/env python

import sys

from plugin.my_module import MyPlugin
from rqt_gui.main import Main

plugin = 'my_plugin'
main = Main(filename=plugin)
sys.exit(main.main(standalone=plugin))

 

创建service

在<catkin_ws>/src/rqt_my_plugin/下创建srv目录。在srv目录内创建文本文件AddTwoInts.srv，其内容如下

int64 a
int64 b
---
int64 sum

 

配置CMakeLists.txt

修改文件最开始project()指令为project(my_plugin)。

取消catkin_python_setup( )行的注释，取消注释或增加如下内容

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  rospy
  rqt_gui
  rqt_gui_py
  std_msgs
  message_generation
)

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

# Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs  # Or other packages containing msgs
)

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

install(DIRECTORY
  resource
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

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

 

catkin_make

使用

catkin_make --pkg my_plugin

编译package。事实上我们并没有编写任何C++代码，所以catkin_make并没有实际的编译连接过程发生，但由于声明了service，所以会有相关的生成过程。catkin_make之后，可以通过

rossrv show my_plugin/AddTwoInts

来显示service的配置，结果如图所示

 

测试

可以使用如下几种方法进行测试

（1）rosrun my_plugin my_plugin

（2）rqt --standalone my_plugin

（3）先启动rqt，然后用Plugins->Logging->My plugin label菜单在rqt的框架内添加新一个my_plugin实例。

 

（1）和（2）将看到相同的GUI，如下图所示。

（注意：由于我使用了屏幕缩放，所以GUI的比例可能不协调。我加大了按钮的尺寸来容纳屏幕缩放时自动放大的字体。）

方法（3）将在rqt内显示我们的my_plugin，可以自由移动my_plugin到rqt的docking place或者开启多个my_plugin实例。

通过点击 “Click me!” 按钮，启动button_test的回调函数，此时在terminal内将看到我们输出的字符串和service的工作情况，如上图所示。

 

在另外一个terminal中通过

rostopic pub /my_plugin_sub std_msgs/String "Test topic string." -r 1

命令以1Hz频率向my_plugin_sub topic发送数据，可以看到plugin所在的terminal内的输出

 

注意，在回调函数中，不能对Qt的Widget进行操作，原因是rqt将在子线程中运行插件，每个插件实例都有自己的线程。可在回调函数中emit 一个Qt singal，从在slot内而实现对Widget的操作（尚未测试）。

 

更新20191023

• 为了减少python import的难度，避免出现命名重复，修改了部分对象的命名，在此处列出（注意：前文相关位置都已进行了修正，前文所列配置和源码均为最新状态）
• 修改rqt_my_plugin/package.xml文件，变更package名称。<package><name>元素从qrt_my_plugin修改为my_plugin
• 重命名<catkin_ws>/src/rqt_my_plugin/src/rqt_my_plugin 文件夹为 <catkin_ws>/src/rqt_my_plugin/src/plugin
• 重命名<catkin_ws>/src/rqt_my_plugin/scripts/rqt_my_plugin 文件为<catkin_ws>/src/rqt_my_plugin/scripts/my_plugin
• 修改plugin.xml文件中<library><class>元素的type属性，从type="rqt_my_plugin.my_module.MyPlugin" 改为 type="plugin.my_module.MyPlugin"
• 修改setup.py文件，将

d = generate_distutils_setup(
    packages=['rqt_my_plugin'],
    package_dir={'': 'src'},
)

改为

d = generate_distutils_setup(
    packages=['plugin'],
    package_dir={'': 'src'},
)

• 修改<catkin_ws>/src/rqt_my_plugin/src/plugin/my_module.py 中

ui_file = os.path.join(rospkg.RosPack().get_path('rqt_my_plugin'), 'resource', 'MyPlugin.ui')

为

ui_file = os.path.join(rospkg.RosPack().get_path('my_plugin'), 'resource', 'MyPlugin.ui')

• 修改<catkin_ws>/src/rqt_my_plugin/scripts/my_plugin文件内容

从from rqt_my_plugin.my_module import MyPlugin 变为 from plugin.my_module import MyPlugin

从plugin = 'rqt_my_plugin' 变为 plugin = 'my_plugin'

• 修改CMakeLists.txt

修改project( )指令为project(my_plugin)

修改

install(PROGRAMS

  scripts/rqt_my_plugin

  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

)

为

install(PROGRAMS

  scripts/my_plugin

  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

)

其他修改见于CMakeLists.txt文件。

• 添加有关topic和service的实现。