創建 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的實現。