├── .gitignore ├── src ├── README.md └── roslibpy │ ├── comm │ ├── __init__.py │ ├── comm.py │ ├── comm_autobahn.py │ └── comm_cli.py │ ├── __version__.py │ ├── __init__.py │ ├── __main__.py │ ├── tf.py │ ├── event_emitter.py │ ├── actionlib.py │ ├── core.py │ └── ros.py ├── docs ├── examples │ ├── robots.jpg │ ├── 01_debug_logging.rst │ ├── 04_publish_image.rst │ ├── 05_subscribe_to_images.rst │ ├── 01_debug_logging.py │ ├── 03_slow_consumer.rst │ ├── 04_publish_image.py │ ├── 05_subscribe_to_images.py │ ├── 02_check_latency.py │ ├── 02_check_latency.rst │ └── 03_slow_consumer.py ├── files │ ├── ros-hello-world.py │ ├── ros-hello-world-run-forever.py │ ├── ros-service-call-logger.py │ ├── ros-service-call-set-bool.py │ ├── ros-hello-world-listener.py │ ├── ros-hello-world-talker.py │ ├── ros-service.py │ ├── ros-action-client.py │ └── ros-action-server.py ├── api.rst ├── authors.rst ├── epilogue.rst ├── index.rst ├── contribution.rst ├── introduction.rst ├── changelog.rst ├── conf.py └── examples.rst ├── requirements.txt ├── README.md ├── Makefile ├── .readthedocs.yml └── make.bat /.gitignore: -------------------------------------------------------------------------------- 1 | /build/ 2 | **/__pycache__/ -------------------------------------------------------------------------------- /src/README.md: -------------------------------------------------------------------------------- 1 | # 源码说明 2 | 3 | 该文件夹下为 **roslibpy 1.1.0** 的源码，对其中 docstring 部分进行了翻译，其它部分未作修改。 4 | -------------------------------------------------------------------------------- /docs/examples/robots.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XinArkh/roslibpy-docs-zh/HEAD/docs/examples/robots.jpg -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | Sphinx==1.8.5 2 | sphinx-rtd-theme==0.4.3 3 | autobahn==20.12.3 4 | Twisted>=20.3.0 5 | -------------------------------------------------------------------------------- /docs/files/ros-hello-world.py: -------------------------------------------------------------------------------- 1 | import roslibpy 2 | 3 | client = roslibpy.Ros(host='localhost', port=9090) 4 | client.run() 5 | print('Is ROS connected?', client.is_connected) 6 | client.terminate() 7 | -------------------------------------------------------------------------------- /docs/api.rst: -------------------------------------------------------------------------------- 1 | .. _ros-api-reference: 2 | 3 | API 文档 4 | ======== 5 | 6 | 7 | .. testsetup:: 8 | 9 | from roslibpy import * 10 | 11 | .. automodule:: roslibpy 12 | .. automodule:: roslibpy.actionlib 13 | .. automodule:: roslibpy.tf 14 | -------------------------------------------------------------------------------- /docs/examples/01_debug_logging.rst: -------------------------------------------------------------------------------- 1 | Enable debug logging 2 | ==================== 3 | 4 | This example shows how to enable debugging output using Python ``logging`` infrastructure. 5 | 6 | .. literalinclude :: 01_debug_logging.py 7 | :language: python 8 | -------------------------------------------------------------------------------- /docs/examples/04_publish_image.rst: -------------------------------------------------------------------------------- 1 | Publish images 2 | ============== 3 | 4 | This example shows how to publish images using the 5 | built-in ``sensor_msgs/CompressedImage`` message type. 6 | 7 | .. literalinclude :: 04_publish_image.py 8 | :language: python 9 | -------------------------------------------------------------------------------- /docs/files/ros-hello-world-run-forever.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import roslibpy 3 | 4 | client = roslibpy.Ros(host='localhost', port=9090) 5 | client.on_ready(lambda: print('Is ROS connected?', client.is_connected)) 6 | client.run_forever() 7 | -------------------------------------------------------------------------------- /docs/examples/05_subscribe_to_images.rst: -------------------------------------------------------------------------------- 1 | Subscribe to images 2 | =================== 3 | 4 | This example shows how to subscribe to a topic of images 5 | using the built-in ``sensor_msgs/CompressedImage`` message type. 6 | 7 | .. literalinclude :: 05_subscribe_to_images.py 8 | :language: python 9 | -------------------------------------------------------------------------------- /docs/files/ros-service-call-logger.py: -------------------------------------------------------------------------------- 1 | import roslibpy 2 | 3 | client = roslibpy.Ros(host='localhost', port=9090) 4 | client.run() 5 | 6 | service = roslibpy.Service(client, '/rosout/get_loggers', 'roscpp/GetLoggers') 7 | request = roslibpy.ServiceRequest() 8 | 9 | print('Calling service...') 10 | result = service.call(request) 11 | print('Service response: {}'.format(result['loggers'])) 12 | 13 | client.terminate() 14 | -------------------------------------------------------------------------------- /docs/authors.rst: -------------------------------------------------------------------------------- 1 | 关于作者 2 | ======== 3 | 4 | 5 | 官方文档（英文）作者 6 | ------------------------- 7 | 8 | * Gramazio Kohler Research `@gramaziokohler `_ 9 | * Gonzalo Casas `@gonzalocasas `_ 10 | * Mathias Lüdtke `@ipa-mdl `_ 11 | 12 | 13 | 本文档作者 14 | ---------- 15 | 16 | * Wu Xin `@XinArkh `_ -------------------------------------------------------------------------------- /docs/files/ros-service-call-set-bool.py: -------------------------------------------------------------------------------- 1 | import roslibpy 2 | 3 | client = roslibpy.Ros(host='localhost', port=9090) 4 | client.run() 5 | 6 | service = roslibpy.Service(client, '/set_ludicrous_speed', 'std_srvs/SetBool') 7 | request = roslibpy.ServiceRequest({'data': True}) 8 | 9 | print('Calling service...') 10 | result = service.call(request) 11 | print('Service response: {}'.format(result)) 12 | 13 | client.terminate() 14 | -------------------------------------------------------------------------------- /docs/examples/01_debug_logging.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | import roslibpy 4 | 5 | # Configure logging to high verbosity (DEBUG) 6 | fmt = '%(asctime)s %(levelname)8s: %(message)s' 7 | logging.basicConfig(format=fmt, level=logging.DEBUG) 8 | log = logging.getLogger(__name__) 9 | 10 | client = roslibpy.Ros(host='127.0.0.1', port=9090) 11 | client.on_ready(lambda: log.info('On ready has been triggered')) 12 | 13 | client.run_forever() 14 | -------------------------------------------------------------------------------- /src/roslibpy/comm/__init__.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | from .comm import RosBridgeException 4 | from .comm import RosBridgeProtocol 5 | 6 | if sys.platform == 'cli': 7 | from .comm_cli import CliRosBridgeClientFactory as RosBridgeClientFactory 8 | else: 9 | from .comm_autobahn import AutobahnRosBridgeClientFactory as RosBridgeClientFactory 10 | 11 | __all__ = ['RosBridgeException', 'RosBridgeProtocol', 'RosBridgeClientFactory'] 12 | -------------------------------------------------------------------------------- /docs/files/ros-hello-world-listener.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import roslibpy 3 | 4 | client = roslibpy.Ros(host='localhost', port=9090) 5 | client.run() 6 | 7 | listener = roslibpy.Topic(client, '/chatter', 'std_msgs/String') 8 | listener.subscribe(lambda message: print('Heard talking: ' + message['data'])) 9 | 10 | try: 11 | while True: 12 | pass 13 | except KeyboardInterrupt: 14 | client.terminate() 15 | -------------------------------------------------------------------------------- /docs/files/ros-hello-world-talker.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | import roslibpy 4 | 5 | client = roslibpy.Ros(host='localhost', port=9090) 6 | client.run() 7 | 8 | talker = roslibpy.Topic(client, '/chatter', 'std_msgs/String') 9 | 10 | while client.is_connected: 11 | talker.publish(roslibpy.Message({'data': 'Hello World!'})) 12 | print('Sending message...') 13 | time.sleep(1) 14 | 15 | talker.unadvertise() 16 | 17 | client.terminate() 18 | -------------------------------------------------------------------------------- /docs/files/ros-service.py: -------------------------------------------------------------------------------- 1 | import roslibpy 2 | 3 | def handler(request, response): 4 | print('Setting speed to {}'.format(request['data'])) 5 | response['success'] = True 6 | return True 7 | 8 | client = roslibpy.Ros(host='localhost', port=9090) 9 | 10 | service = roslibpy.Service(client, '/set_ludicrous_speed', 'std_srvs/SetBool') 11 | service.advertise(handler) 12 | print('Service advertised.') 13 | 14 | client.run_forever() 15 | client.terminate() 16 | -------------------------------------------------------------------------------- /src/roslibpy/__version__.py: -------------------------------------------------------------------------------- 1 | # 72 6F 73 6C 69 62 70 79 2 | # 47 72 61 6D 61 7A 69 6F 4B 6F 68 6C 65 72 52 65 73 65 61 72 63 68 3 | 4 | __title__ = 'roslibpy' 5 | __description__ = 'Python ROS Bridge library.' 6 | __url__ = 'https://github.com/gramaziokohler/roslibpy' 7 | __version__ = '1.1.0' 8 | __author__ = 'Gramazio Kohler Research' 9 | __author_email__ = 'gramaziokohler@arch.ethz.ch' 10 | __license__ = 'MIT license' 11 | __copyright__ = 'Copyright 2018 Gramazio Kohler Research' 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # roslibpy-docs-zh 2 | 3 | 项目[roslibpy](https://github.com/gramaziokohler/roslibpy)的**非官方**中文文档。 4 | 5 | 完整版文档页面可[在此处](https://roslibpy-docs-zh.readthedocs.io/zh/latest/)浏览。 6 | 7 | roslibpy是一个连接Python与ROS环境的工具，安装roslibpy的主机无需再安装ROS环境便可以与ROS Master进行通信。 8 | 9 | ## 本地构建 10 | 11 | Clone 该仓库到本地，然后进入项目根目录。 12 | 13 | 按照`requirements.txt`的要求自行安装所需要的 Python 依赖库。 14 | 15 | ### Linux 16 | 17 | ```shell 18 | make html 19 | ``` 20 | 21 | ### Windows 22 | 23 | ```shell 24 | ./make.bat html 25 | ``` 26 | 27 | 构建好的html文档位于`root/build/html`。 -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SOURCEDIR = docs 8 | BUILDDIR = build 9 | 10 | # Put it first so that "make" without argument is like "make help". 11 | help: 12 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 13 | 14 | .PHONY: help Makefile 15 | 16 | # Catch-all target: route all unknown targets to Sphinx using the new 17 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 18 | %: Makefile 19 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /.readthedocs.yml: -------------------------------------------------------------------------------- 1 | # .readthedocs.yml 2 | # Read the Docs configuration file 3 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 4 | 5 | # Required 6 | version: 2 7 | 8 | # Build documentation in the docs/ directory with Sphinx 9 | sphinx: 10 | configuration: docs/conf.py 11 | 12 | # Build documentation with MkDocs 13 | #mkdocs: 14 | # configuration: mkdocs.yml 15 | 16 | # Optionally build your docs in additional formats such as PDF and ePub 17 | formats: all 18 | 19 | # Optionally set the version of Python and requirements required to build your docs 20 | python: 21 | version: 3.6 22 | install: 23 | - requirements: ./requirements.txt -------------------------------------------------------------------------------- /docs/files/ros-action-client.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import roslibpy 3 | import roslibpy.actionlib 4 | 5 | client = roslibpy.Ros(host='localhost', port=9090) 6 | client.run() 7 | 8 | action_client = roslibpy.actionlib.ActionClient(client, 9 | '/fibonacci', 10 | 'actionlib_tutorials/FibonacciAction') 11 | 12 | goal = roslibpy.actionlib.Goal(action_client, 13 | roslibpy.Message({'order': 8})) 14 | 15 | goal.on('feedback', lambda f: print(f['sequence'])) 16 | goal.send() 17 | result = goal.wait(10) 18 | action_client.dispose() 19 | 20 | print('Result: {}'.format(result['sequence'])) 21 | -------------------------------------------------------------------------------- /docs/files/ros-action-server.py: -------------------------------------------------------------------------------- 1 | import roslibpy 2 | import roslibpy.actionlib 3 | 4 | client = roslibpy.Ros(host='localhost', port=9090) 5 | server = roslibpy.actionlib.SimpleActionServer(client, '/fibonacci', 'actionlib_tutorials/FibonacciAction') 6 | 7 | def execute(goal): 8 | print('Received new fibonacci goal: {}'.format(goal['order'])) 9 | 10 | seq = [0, 1] 11 | 12 | for i in range(1, goal['order']): 13 | if server.is_preempt_requested(): 14 | server.set_preempted() 15 | return 16 | 17 | seq.append(seq[i] + seq[i - 1]) 18 | server.send_feedback({'sequence': seq}) 19 | 20 | server.set_succeeded({'sequence': seq}) 21 | 22 | 23 | server.start(execute) 24 | client.run_forever() 25 | -------------------------------------------------------------------------------- /docs/examples/03_slow_consumer.rst: -------------------------------------------------------------------------------- 1 | Throttle messages for a slow consumer 2 | ===================================== 3 | 4 | This example shows how to throttle messages that are published are a rate faster than 5 | what a slow consumer (subscribed) can process. In this example, only the newest messages 6 | are preserved, messages that cannot be consumed on time are dropped. 7 | 8 | .. literalinclude :: 03_slow_consumer.py 9 | :language: python 10 | 11 | In the console, you should see gaps in the sequence of messages, because the publisher is 12 | producing messages every 0.001 seconds, but we configure a queue of length 1, with a 13 | throttling of 600ms to give time to our slow consumer. Without this throttling, the consumer 14 | would process increasingly old messages. 15 | -------------------------------------------------------------------------------- /docs/examples/04_publish_image.py: -------------------------------------------------------------------------------- 1 | import base64 2 | import logging 3 | import time 4 | 5 | import roslibpy 6 | 7 | # Configure logging 8 | fmt = '%(asctime)s %(levelname)8s: %(message)s' 9 | logging.basicConfig(format=fmt, level=logging.INFO) 10 | log = logging.getLogger(__name__) 11 | 12 | client = roslibpy.Ros(host='127.0.0.1', port=9090) 13 | 14 | publisher = roslibpy.Topic(client, '/camera/image/compressed', 'sensor_msgs/CompressedImage') 15 | publisher.advertise() 16 | 17 | def publish_image(): 18 | with open('robots.jpg', 'rb') as image_file: 19 | image_bytes = image_file.read() 20 | encoded = base64.b64encode(image_bytes).decode('ascii') 21 | 22 | publisher.publish(dict(format='jpeg', data=encoded)) 23 | 24 | client.on_ready(publish_image) 25 | client.run_forever() 26 | -------------------------------------------------------------------------------- /docs/epilogue.rst: -------------------------------------------------------------------------------- 1 | 后记 2 | ===== 3 | 4 | 5 | 从发现并使用 **roslibpy** 这个库，到决定把它的文档翻译成中文，其实\ 6 | 心血来潮的成分更大一点。最初是有两个动机，一是发现中文互联网上有关这个\ 7 | 库的资料少得可怜，只有寥寥几条语焉不详的教程，所以有了通过写文档来将其\ 8 | 介绍给更多中文用户的想法；二是恰好想折腾一番，一直觉得写技术文档是一个\ 9 | 很神秘的事情，想通过这个项目学习一下。 10 | 11 | 断断续续地构建这个文档，前后恰好花了一周的时间，其中大部分时间消耗在了 12 | Sphinx + reStructuredText + readthedocs 的各种挖坑与踩坑上，剩下的\ 13 | 小部分时间才是用在文档翻译上面的。经过一周的速成，现在基本是个半吊子\ 14 | 水平，以后如果再写技术文档，相信会顺利很多。 15 | 16 | 参与社区建设是一个正反馈的过程，我的工作虽然不大，但是可能会帮助到无数\ 17 | 的人，所以我认为是有意义的。最后，如果这个文档帮助到了你，希望你也能够\ 18 | 到我的 `GitHub 仓库 `_ 给\ 19 | 这个文档点一个 star，也算作对我的鼓励吧。 20 | 21 | 22 | >> **roslibpy** 官方仓库：\ `roslibpy `_ << 23 | 24 | >> 本文档仓库：\ `roslibpy-docs-zh `_ << 25 | 26 | 27 | Wu Xin 28 | 29 | 2019.4.26. 30 | 31 | 2019.6.8. edited. 32 | 33 | 2019.7.8. edited. -------------------------------------------------------------------------------- /docs/examples/05_subscribe_to_images.py: -------------------------------------------------------------------------------- 1 | import base64 2 | import logging 3 | import time 4 | 5 | import roslibpy 6 | 7 | # Configure logging 8 | fmt = '%(asctime)s %(levelname)8s: %(message)s' 9 | logging.basicConfig(format=fmt, level=logging.INFO) 10 | log = logging.getLogger(__name__) 11 | 12 | client = roslibpy.Ros(host='127.0.0.1', port=9090) 13 | 14 | def receive_image(msg): 15 | log.info('Received image seq=%d', msg['header']['seq']) 16 | base64_bytes = msg['data'].encode('ascii') 17 | image_bytes = base64.b64decode(base64_bytes) 18 | with open('received-image-{}.{}'.format(msg['header']['seq'], msg['format']) , 'wb') as image_file: 19 | image_file.write(image_bytes) 20 | 21 | subscriber = roslibpy.Topic(client, '/camera/image/compressed', 'sensor_msgs/CompressedImage') 22 | subscriber.subscribe(receive_image) 23 | 24 | client.run_forever() 25 | -------------------------------------------------------------------------------- /docs/examples/02_check_latency.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import time 3 | 4 | import roslibpy 5 | 6 | # Configure logging 7 | fmt = '%(asctime)s %(levelname)8s: %(message)s' 8 | logging.basicConfig(format=fmt, level=logging.INFO) 9 | log = logging.getLogger(__name__) 10 | 11 | client = roslibpy.Ros(host='127.0.0.1', port=9090) 12 | 13 | def receive_message(msg): 14 | age = int(time.time() * 1000) - msg['data'] 15 | log.info('Age of message: %6dms', age) 16 | 17 | publisher = roslibpy.Topic(client, '/check_latency', 'std_msgs/UInt64') 18 | publisher.advertise() 19 | 20 | subscriber = roslibpy.Topic(client, '/check_latency', 'std_msgs/UInt64') 21 | subscriber.subscribe(receive_message) 22 | 23 | def publish_message(): 24 | publisher.publish(dict(data=int(time.time() * 1000))) 25 | client.call_later(.5, publish_message) 26 | 27 | client.on_ready(publish_message) 28 | client.run_forever() 29 | -------------------------------------------------------------------------------- /make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=docs 11 | set BUILDDIR=build 12 | 13 | if "%1" == "" goto help 14 | 15 | %SPHINXBUILD% >NUL 2>NUL 16 | if errorlevel 9009 ( 17 | echo. 18 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 19 | echo.installed, then set the SPHINXBUILD environment variable to point 20 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 21 | echo.may add the Sphinx directory to PATH. 22 | echo. 23 | echo.If you don't have Sphinx installed, grab it from 24 | echo.http://sphinx-doc.org/ 25 | exit /b 1 26 | ) 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /docs/examples/02_check_latency.rst: -------------------------------------------------------------------------------- 1 | Check roundtrip message latency 2 | =============================== 3 | 4 | This example shows how to check roundtrip message latency on your system. 5 | 6 | .. literalinclude :: 02_check_latency.py 7 | :language: python 8 | 9 | The output on the console should look similar to the following:: 10 | 11 | $ python 02_check_latency.py 12 | 2020-04-09 07:45:49,909 INFO: Connection to ROS MASTER ready. 13 | 2020-04-09 07:45:50,431 INFO: Age of message: 2ms 14 | 2020-04-09 07:45:50,932 INFO: Age of message: 2ms 15 | 2020-04-09 07:45:51,431 INFO: Age of message: 1ms 16 | 2020-04-09 07:45:51,932 INFO: Age of message: 2ms 17 | 2020-04-09 07:45:52,434 INFO: Age of message: 3ms 18 | 2020-04-09 07:45:52,934 INFO: Age of message: 2ms 19 | 2020-04-09 07:45:53,435 INFO: Age of message: 3ms 20 | 2020-04-09 07:45:53,934 INFO: Age of message: 1ms 21 | 2020-04-09 07:45:54,436 INFO: Age of message: 2ms 22 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | ****************** 2 | roslibpy 中文文档 3 | ****************** 4 | 5 | 6 | :作者: `Gramazio Kohler Research `_ 7 | 8 | :译者: `Wu Xin `_ 9 | 10 | :授权: 该（翻译）文档遵循 `CC BY-SA 4.0 `_ 许可协议。 11 | 12 | .. note:: 13 | 14 | 该文档为\ **非官方**\ 中文文档。 官方文档可以\ `在此获得 `_。 15 | 16 | **Python ROS Bridge library**，简称 **roslibpy**，提供了用 Python 或 IronPython 与开源机器人平台 `ROS `_ 进行通信的一个途径。\ 17 | 该项目使用 WebSockets 与 `rosbridge 2.0 `_ 建立连接，提供了 publishing、subscribing、service calls、actionlib、TF 等 ROS 中的基本功能。 18 | 19 | 与 `rospy `_ 不同，该项目\ **不需要在本地搭建 ROS 环境**，为 Linux 以外的平台使用 ROS 带来了方便。 20 | 21 | **roslibpy** 的 API 构建参考了 `roslibjs `_ 的结构。 22 | 23 | 截至 2020 年 7 月 8 日，\ **roslibpy** 的版本更新至 1.1.0，也是该中文文档所对应的版本。早期版本在 `GitHub 仓库 `_\ 的分支中有记录，请自行查阅。 24 | 25 | 26 | .. toctree:: 27 | :maxdepth: 2 28 | :caption: 目录 29 | :numbered: 30 | 31 | introduction 32 | examples 33 | api 34 | contribution 35 | authors 36 | changelog 37 | epilogue 38 | 39 | 40 | 索引 41 | ===== 42 | 43 | * :ref:`genindex` 44 | * :ref:`modindex` 45 | * :ref:`search` 46 | -------------------------------------------------------------------------------- /docs/contribution.rst: -------------------------------------------------------------------------------- 1 | .. _contribution: 2 | 3 | 贡献者指南 4 | ========== 5 | 6 | 7 | 我们非常欢迎更多贡献者的参与到这个项目中来。 8 | 9 | 10 | 代码贡献 11 | --------- 12 | 13 | 我们欢迎任何人 pull request！\ 14 | 这里有一份改善代码的快速指南： 15 | 16 | 1. Fork并 clone \ `这个仓库 `_\ ； 17 | 2. 用你喜欢的工具创建一个虚拟环境（如\ ``virtualenv``\ 、\ ``conda``\ 等）； 18 | 3. 安装开发依赖:: 19 | 20 | pip install -r requirements-dev.txt 21 | 22 | 4. 确保所有的测试通过:: 23 | 24 | invoke test 25 | 26 | 5. 把你的更改更新到主分支（或从主分支分叉的其它分支）； 27 | 6. 再次确保所有的测试通过:: 28 | 29 | invoke test 30 | 31 | 7. 把你的名字添加到\ ``AUTHORS.rst``\ 里面； 32 | 8. Commit+push你的改变到 GitHub 仓库； 33 | 9. 通过 GitHub 页面新建一个\ `pull request `_\ 。 34 | 35 | 在开发过程中，使用 `pyinvoke `_ 来简化重复操作： 36 | 37 | * ``invoke clean``: 清除所有生成的内容； 38 | * ``invoke check``: 运行多种代码和文档风格检查； 39 | * ``invoke docs``: 生成文档； 40 | * ``invoke test``: 用一个命令迅速运行所有的测试和检查； 41 | * ``invoke``: 显示可供调用的任务。 42 | 43 | 44 | 文档贡献 45 | -------- 46 | 47 | 文档总是多多益善，无论是作为介绍/示例/使用文档的一部分，还是 docstrings 里面的 API 文档。 48 | 49 | 文档使用 `reStructuredText `_ 撰写，\ 50 | 并用 `Sphinx `_ 生成 HTML 格式的输出。 51 | 52 | 在你更改本地文档之后，运行以下命令来重新生成它:: 53 | 54 | invoke docs 55 | 56 | 57 | 报告 BUG 58 | --------- 59 | 60 | 在你想要\ `报告一个 BUG `_ 的时候，请包括以下内容： 61 | 62 | * 操作系统名称及版本； 63 | * ROS 版本； 64 | * 任何可能有助于排除故障的、有关你的本地配置的细节； 65 | * 重现这个 BUG 的详细步骤。 66 | 67 | 68 | Feature 的建议与反馈 69 | -------------------- 70 | 71 | 提供反馈最好的方式是新开一个 `GitHub issue `_。\ 72 | 如果你提出一个新的 feature，请： 73 | 74 | * 详细解释它的工作机理； 75 | * 使其涉及的范围尽可能小，以便实现。 76 | -------------------------------------------------------------------------------- /docs/examples/03_slow_consumer.py: -------------------------------------------------------------------------------- 1 | import time 2 | import logging 3 | 4 | import roslibpy 5 | 6 | # Configure logging 7 | fmt = '%(asctime)s %(levelname)8s: %(message)s' 8 | logging.basicConfig(format=fmt, level=logging.INFO) 9 | log = logging.getLogger(__name__) 10 | 11 | client = roslibpy.Ros(host='127.0.0.1', port=9090) 12 | 13 | def to_epoch(stamp): 14 | stamp_secs = stamp['secs'] 15 | stamp_nsecs = stamp['nsecs'] 16 | return stamp_secs + stamp_nsecs*1e-9 17 | 18 | def from_epoch(stamp): 19 | stamp_secs = int(stamp) 20 | stamp_nsecs = (stamp - stamp_secs) * 1e9 21 | return {'secs': stamp_secs, 'nsecs': stamp_nsecs} 22 | 23 | def receive_message(msg): 24 | age = time.time() - to_epoch(msg['stamp']) 25 | fmt = 'Age of message (sequence #%d): %6.3f seconds' 26 | log.info(fmt, msg['seq'], age) 27 | # Simulate a very slow consumer 28 | time.sleep(.5) 29 | 30 | publisher = roslibpy.Topic(client, '/slow_consumer', 'std_msgs/Header') 31 | publisher.advertise() 32 | 33 | # Queue length needs to be used in combination with throttle rate (in ms) 34 | # This value must be tuned to the expected duration of the slow consumer 35 | # and ideally bigger than the max of it, 36 | # otherwise message will be older than expected (up to a limit) 37 | subscriber = roslibpy.Topic(client, '/slow_consumer', 'std_msgs/Header', 38 | queue_length=1, throttle_rate=600) 39 | subscriber.subscribe(receive_message) 40 | 41 | seq = 0 42 | def publish_message(): 43 | global seq 44 | seq += 1 45 | header = dict(frame_id='', seq=seq, stamp=from_epoch(time.time())) 46 | publisher.publish(header) 47 | client.call_later(.001, publish_message) 48 | 49 | client.on_ready(publish_message) 50 | client.run_forever() 51 | -------------------------------------------------------------------------------- /docs/introduction.rst: -------------------------------------------------------------------------------- 1 | 简介 2 | ==== 3 | 4 | 5 | **Python ROS Bridge library**，简称 **roslibpy**，提供了用 Python 或 IronPython 与开源机器人平台 `ROS `_ 进行通信的一个途径。\ 6 | 该项目使用 WebSockets 与 `rosbridge 2.0 `_ 建立连接，提供了 publishing、subscribing、service calls、actionlib、TF 等 ROS 中的基本功能。 7 | 8 | 与 `rospy `_ 不同，该项目\ **不需要在本地搭建 ROS 环境**，为 Linux 以外的平台使用 ROS 带来了方便。 9 | 10 | **roslibpy** 的 API 构建参考了 `roslibjs`_ 的结构。 11 | 12 | 13 | 主要特性 14 | -------- 15 | 16 | * 话题 (Topic) 的发布和订阅； 17 | * 服务 (Service) 的发布和查询； 18 | * ROS 参数管理 (get/set/delete)； 19 | * ROS API 服务； 20 | * Actionlib 支持； 21 | * 通过 `tf2_web_republisher `_ 实现 TF Client。 22 | 23 | **Roslibpy** 可以在 Python 2.7、Python 3.x 和 IronPython 2.7 上运行。 24 | 25 | 26 | 安装 27 | ---- 28 | 29 | 使用\ ``pip``\ 命令安装 **roslibpy**:: 30 | 31 | pip install roslibpy 32 | 33 | 对于 IronPython 来说，\ ``pip``\ 命令略有不同:: 34 | 35 | ipy -X:Frames -m pip install --user roslibpy 36 | 37 | 38 | 文档 39 | ---- 40 | 41 | 完整版英文官方文档\ `见此 `_。 42 | 43 | `本文档 `_\ 为中文非官方文档。 44 | 45 | 46 | 贡献 47 | ---- 48 | 49 | 确保您的本地环境配置正确： 50 | 51 | * 将仓库 `roslibpy `_ 克隆到本地； 52 | * 创建一个虚拟环境； 53 | * 安装开发依赖:: 54 | 55 | pip install -r requirements-dev.txt 56 | 57 | **现在你可以开始编程了！** 58 | 59 | 在开发过程中，使用 `pyinvoke `_ 来简化重复操作： 60 | 61 | * ``invoke clean``: 清除所有生成的内容； 62 | * ``invoke check``: 运行多种代码和文档风格检查； 63 | * ``invoke docs``: 生成文档； 64 | * ``invoke test``: 用一个命令迅速运行所有的测试和检查； 65 | * ``invoke``: 显示可供调用的任务。 66 | 67 | 更多的细节请参考\ :ref:`contribution`\ 。 68 | 69 | 70 | 发布项目 71 | -------- 72 | 73 | 准备好发布 **roslibpy** 的新版本了吗？接下来是发布新版本的步骤： 74 | 75 | * 我们使用 `semver `_，即我们按照如下方式迭代版本： 76 | 77 | * ``patch``: 修复 BUG； 78 | * ``minor``: 增加向下兼容的 Feature； 79 | * ``major``: 向下兼容的修改。 80 | 81 | * 所有的更改都要记录在 ``CHANGELOG.rst`` 中！ 82 | * 准备好了吗？用下面的命令来发布新版本:: 83 | 84 | invoke release [patch|minor|major] 85 | 86 | * Profit! 87 | 88 | 89 | Credits 90 | ------- 91 | 92 | 本模块是基于 `roslibjs`_ 实现的，在很大程度上，它是到 Python 的逐行移植，只在其它惯用代码范式更有意义的地方才进行修改，\ 93 | 所以很大一部分功劳要归于 `roslibjs 的作者 `_\ 。 94 | 95 | .. _roslibjs: http://wiki.ros.org/roslibjs 96 | -------------------------------------------------------------------------------- /src/roslibpy/__init__.py: -------------------------------------------------------------------------------- 1 | r""" 2 | 3 | 此模块依赖 Robot Web Tools 所开发的 `ROS bridge suite `_\ ， 4 | 通过 WebSockets 实现与 ROS 的交互。 5 | 6 | `ROS bridge protocol `_ 7 | 使用 JSON 格式信息传输 8 | publishing, subscribing, service calls, actionlib, TF 等 ROS 中的功能。 9 | 10 | .. _ros-setup: 11 | 12 | ROS 设置 13 | -------- 14 | 15 | 使用本模块，必须在 ROS 环境中配置并运行\ ``rosbridge``\ 。 16 | 17 | 首次连接之前，在 ROS master所在环境用以下命令安装\ **rosbridge suite**:: 18 | 19 | sudo apt-get install -y ros-kinetic-rosbridge-server 20 | sudo apt-get install -y ros-kinetic-tf2-web-republisher 21 | 22 | 在以后的每次连接之前，都要确保预先开启了以下服务:: 23 | 24 | roslaunch rosbridge_server rosbridge_websocket.launch 25 | rosrun tf2_web_republisher tf2_web_republisher 26 | 27 | .. note:: 28 | 29 | 执行\ ``roslaunch``\ 命令时，会自动运行\ ``roscore``\ 命令，因此使用启动文件运行节点时不需要新开终端运行\ ``roscore``\ 。 30 | 31 | 32 | 连接 ROS 33 | -------- 34 | 35 | 与 ROS 的连接是由\ :class:`Ros`\ 类来处理的。除了连接和信息处理，在需要的时候它也会自动重连。 36 | 37 | 其他需要与 ROS 进行连接的类会将此实例作为其构造函数的参数接收。 38 | 39 | .. autoclass:: Ros 40 | :members: 41 | 42 | 43 | ROS 主要概念 44 | -------------- 45 | 46 | 话题 47 | ^^^^^ 48 | 49 | ROS 是一个通信框架。 在 ROS 里面, 不同的 **节点** 通过 messages 与其它节点通信。\ 50 | **ROS messages** 在\ :class:`Message`\ 类中实现，\ 51 | 并通过\ :class:`Topics `\ 的\ **发布/订阅**\ 模型来传输。 52 | 53 | .. autoclass:: Message 54 | :members: 55 | .. autoclass:: Topic 56 | :members: 57 | 58 | 服务 59 | ^^^^^ 60 | 61 | 除了话题的发布/订阅模型，ROS 还通过\ :class:`Services `\ 类提供了一套\ **请求/响应**\ 模型。 62 | 63 | .. autoclass:: Service 64 | :members: 65 | .. autoclass:: ServiceRequest 66 | :members: 67 | .. autoclass:: ServiceResponse 68 | :members: 69 | 70 | 71 | 参数服务器 72 | ^^^^^^^^^^ 73 | 74 | ROS 提供了用于在不同节点之间分享数据的参数服务器。\ 75 | 该服务通过\ :class:`Param`\ 类来实现。 76 | 77 | .. autoclass:: Param 78 | :members: 79 | 80 | """ 81 | 82 | from .__version__ import __author__ 83 | from .__version__ import __author_email__ 84 | from .__version__ import __copyright__ 85 | from .__version__ import __description__ 86 | from .__version__ import __license__ 87 | from .__version__ import __title__ 88 | from .__version__ import __url__ 89 | from .__version__ import __version__ 90 | from .core import Message 91 | from .core import Param 92 | from .core import Service 93 | from .core import ServiceRequest 94 | from .core import ServiceResponse 95 | from .core import Topic 96 | from .ros import Ros 97 | 98 | __all__ = ['Ros', 'Message', 'Param', 'Service', 'ServiceRequest', 'ServiceResponse', 'Topic', '__author__', 99 | '__author__', '__author_email__', '__copyright__', '__description__', '__license__', '__title__', '__url__', '__version__'] 100 | -------------------------------------------------------------------------------- /docs/changelog.rst: -------------------------------------------------------------------------------- 1 | 更新日志 2 | ======== 3 | 4 | 5 | 对该项目的所有显著更改都将记录在此文件中。 6 | 7 | 该 changelog 的格式基于 `Keep a Changelog `_\ 。\ 8 | 本项目遵守\ `语义版本控制 (Semantic Versioning) `_\ 。 9 | 10 | Unreleased 11 | ---------- 12 | 13 | **Changed** 14 | 15 | **Added** 16 | 17 | **Fixed** 18 | 19 | 1.1.0 20 | ---------- 21 | 22 | **Added** 23 | 24 | * Added ``set_initial_delay``, ``set_max_delay`` and ``set_max_retries`` to ``RosBridgeClientFactory`` to control reconnection parameters. 25 | * Added ``closing`` event to ``Ros`` class that gets triggered right before closing the connection. 26 | 27 | 1.0.0 28 | ---------- 29 | 30 | **Changed** 31 | 32 | * Changed behavior: Topics automatically reconnect when websockets is reconnected 33 | 34 | **Added** 35 | 36 | * Added blocking behavior to more ROS API methods: ``ros.get_nodes`` and ``ros.get_node_details``. 37 | * Added reconnection support to IronPython implementation of websockets 38 | * Added automatic topic reconnection support for both subscribers and publishers 39 | 40 | **Fixed** 41 | 42 | * Fixed reconnection issues on the Twisted/Autobahn-based implementation of websockets 43 | 44 | 0.7.1 45 | ---------- 46 | 47 | **Fixed** 48 | 49 | * Fixed blocking service calls for Mac OS 50 | 51 | 0.7.0 52 | ---------- 53 | 54 | **Changed** 55 | 56 | * The non-blocking event loop runner ``run()`` now defaults to 10 seconds timeout before raising an exception. 57 | 58 | **Added** 59 | 60 | * Added blocking behavior to ROS API methods, e.g. ``ros.get_topics``. 61 | * Added command-line mode to ROS API, e.g. ``roslibpy topic list``. 62 | * Added blocking behavior to the ``Param`` class. 63 | * Added parameter manipulation methods to ``Ros`` class: ``get_param``, ``set_param``, ``delete_param``. 64 | 65 | 0.6.0 66 | ---------- 67 | 68 | **Changed** 69 | 70 | * For consistency, ``timeout`` parameter of ``Goal.send()`` is now expressed in **seconds**, instead of milliseconds. 71 | 72 | **Deprecated** 73 | 74 | * The ``timeout`` parameter of ``ActionClient()`` is ignored in favor of blocking until the connection is established. 75 | 76 | **Fixed** 77 | 78 | * Raise exceptions when timeouts expire on ROS connection or service calls. 79 | 80 | **Added** 81 | 82 | * Support for calling a function in a thread from the Ros client. 83 | * Added implementation of a Simple Action Server. 84 | 85 | 0.5.0 86 | ---------- 87 | 88 | **Changed** 89 | 90 | * The non-blocking event loop runner now waits for the connection to be established in order to minimize the need for ``on_ready`` handlers. 91 | 92 | **Added** 93 | 94 | * Support blocking and non-blocking service calls. 95 | 96 | **Fixed** 97 | 98 | * Fixed an internal unsubscribing issue. 99 | 100 | 0.4.1 101 | ---------- 102 | 103 | **Fixed** 104 | 105 | * Resolve reconnection issues. 106 | 107 | 0.4.0 108 | ---------- 109 | 110 | **Added** 111 | 112 | * Add a non-blocking event loop runner 113 | 114 | 0.3.0 115 | ---------- 116 | 117 | **Changed** 118 | 119 | * Unsubscribing from a listener no longer requires the original callback to be passed. 120 | 121 | 0.2.1 122 | ---------- 123 | 124 | **Fixed** 125 | 126 | * Fix JSON serialization error on TF Client (on Python 3.x) 127 | 128 | 0.2.0 129 | ---------- 130 | 131 | **Added** 132 | 133 | * Add support for IronPython 2.7 134 | 135 | **Changed** 136 | 137 | * Handler ``on_ready`` now defaults to run the callback in thread 138 | 139 | **Deprecated** 140 | 141 | * Rename ``run_event_loop`` to the more fitting ``run_forever`` 142 | 143 | 0.1.1 144 | ---------- 145 | 146 | **Fixed** 147 | 148 | * Minimal documentation fixes 149 | 150 | 0.1.0 151 | ---------- 152 | 153 | **Added** 154 | 155 | * Initial version -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # http://www.sphinx-doc.org/en/master/config 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | 13 | import os 14 | import sys 15 | sys.path.insert(0, os.path.abspath('../src')) 16 | 17 | 18 | # -- Project information ----------------------------------------------------- 19 | 20 | project = u'roslibpy-docs-zh' 21 | copyright = u'2020, Wu Xin' 22 | author = u'Wu Xin' 23 | 24 | # The full version, including alpha/beta/rc tags 25 | release = '1.1.0' 26 | 27 | 28 | # -- General configuration --------------------------------------------------- 29 | 30 | # Add any Sphinx extension module names here, as strings. They can be 31 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 32 | # ones. 33 | extensions = [ 34 | 'sphinx.ext.autodoc', 35 | 'sphinx.ext.doctest', 36 | ] 37 | 38 | # Add any paths that contain templates here, relative to this directory. 39 | templates_path = ['_templates'] 40 | 41 | # The suffix of source filenames. 42 | source_suffix = '.rst' 43 | 44 | # The encoding of source files. 45 | #source_encoding = 'utf-8-sig' 46 | 47 | # The master toctree document. 48 | master_doc = 'index' 49 | 50 | # The language for content autogenerated by Sphinx. Refer to documentation 51 | # for a list of supported languages. 52 | # 53 | # This is also used if you do content translation via gettext catalogs. 54 | # Usually you set "language" from the command line for these cases. 55 | language = 'zh_CN' 56 | 57 | # List of patterns, relative to source directory, that match files and 58 | # directories to ignore when looking for source files. 59 | # This pattern also affects html_static_path and html_extra_path. 60 | exclude_patterns = [] 61 | 62 | # The name of the Pygments (syntax highlighting) style to use. 63 | pygments_style = 'friendly' # or 'sphinx' 64 | 65 | 66 | # -- Options for HTML output ------------------------------------------------- 67 | 68 | # The theme to use for HTML and HTML Help pages. See the documentation for 69 | # a list of builtin themes. 70 | html_theme = 'sphinx_rtd_theme' 71 | 72 | 73 | # Add any paths that contain custom static files (such as style sheets) here, 74 | # relative to this directory. They are copied after the builtin static files, 75 | # so a file named "default.css" will overwrite the builtin "default.css". 76 | html_static_path = ['_static'] 77 | 78 | 79 | # -- Options for Latex output ------------------------------------------------- 80 | latex_engine = 'xelatex' 81 | 82 | latex_elements = { 83 | # The paper size ('letterpaper' or 'a4paper'). 84 | #'papersize': 'letterpaper', 85 | 86 | # The font size ('10pt', '11pt' or '12pt'). 87 | #'pointsize': '10pt', 88 | 89 | # Additional stuff for the LaTeX preamble. 90 | #'preamble': '', 91 | 'preamble': r''' 92 | \hypersetup{unicode=true} 93 | \usepackage{CJKutf8} 94 | \DeclareUnicodeCharacter{00A0}{\nobreakspace} 95 | \DeclareUnicodeCharacter{2203}{\ensuremath{\exists}} 96 | \DeclareUnicodeCharacter{2200}{\ensuremath{\forall}} 97 | \DeclareUnicodeCharacter{2286}{\ensuremath{\subseteq}} 98 | \DeclareUnicodeCharacter{2713}{x} 99 | \DeclareUnicodeCharacter{27FA}{\ensuremath{\Longleftrightarrow}} 100 | \DeclareUnicodeCharacter{221A}{\ensuremath{\sqrt{}}} 101 | \DeclareUnicodeCharacter{221B}{\ensuremath{\sqrt[3]{}}} 102 | \DeclareUnicodeCharacter{2295}{\ensuremath{\oplus}} 103 | \DeclareUnicodeCharacter{2297}{\ensuremath{\otimes}} 104 | \begin{CJK}{UTF8}{gbsn} 105 | \AtEndDocument{\end{CJK}} 106 | ''', 107 | } -------------------------------------------------------------------------------- /src/roslibpy/comm/comm.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import json 4 | import logging 5 | 6 | from .. import Message 7 | from .. import ServiceResponse 8 | 9 | LOGGER = logging.getLogger('roslibpy') 10 | 11 | 12 | class RosBridgeException(Exception): 13 | """Exception raised on the ROS bridge communication.""" 14 | pass 15 | 16 | 17 | class RosBridgeProtocol(object): 18 | """Implements the websocket client protocol to encode/decode JSON ROS Bridge messages.""" 19 | 20 | def __init__(self, *args, **kwargs): 21 | super(RosBridgeProtocol, self).__init__(*args, **kwargs) 22 | self.factory = None 23 | self._pending_service_requests = {} 24 | self._message_handlers = { 25 | 'publish': self._handle_publish, 26 | 'service_response': self._handle_service_response, 27 | 'call_service': self._handle_service_request, 28 | } 29 | # TODO: add handlers for op: status 30 | 31 | def on_message(self, payload): 32 | message = Message(json.loads(payload.decode('utf8'))) 33 | handler = self._message_handlers.get(message['op'], None) 34 | if not handler: 35 | raise RosBridgeException( 36 | 'No handler registered for operation "%s"' % message['op']) 37 | 38 | handler(message) 39 | 40 | def send_ros_message(self, message): 41 | """Encode and serialize ROS Bridge protocol message. 42 | 43 | Args: 44 | message (:class:`.Message`): ROS Bridge Message to send. 45 | """ 46 | try: 47 | json_message = json.dumps(dict(message)).encode('utf8') 48 | LOGGER.debug('Sending ROS message|
%s
', json_message) 49 | 50 | self.send_message(json_message) 51 | except Exception as exception: 52 | # TODO: Check if it makes sense to raise exception again here 53 | # Since this is wrapped in many layers of indirection 54 | LOGGER.exception('Failed to send message, %s', exception) 55 | 56 | def register_message_handlers(self, operation, handler): 57 | """Register a message handler for a specific operation type. 58 | 59 | Args: 60 | operation (:obj:`str`): ROS Bridge operation. 61 | handler: Callback to handle the message. 62 | """ 63 | if operation in self._message_handlers: 64 | raise RosBridgeException( 65 | 'Only one handler can be registered per operation') 66 | 67 | self._message_handlers[operation] = handler 68 | 69 | def send_ros_service_request(self, message, callback, errback): 70 | """Initiate a ROS service request through the ROS Bridge. 71 | 72 | Args: 73 | message (:class:`.Message`): ROS Bridge Message containing the service request. 74 | callback: Callback invoked on successful execution. 75 | errback: Callback invoked on error. 76 | """ 77 | request_id = message['id'] 78 | self._pending_service_requests[request_id] = (callback, errback) 79 | 80 | json_message = json.dumps(dict(message)).encode('utf8') 81 | LOGGER.debug('Sending ROS service request: %s', json_message) 82 | 83 | self.send_message(json_message) 84 | 85 | def _handle_publish(self, message): 86 | self.factory.emit(message['topic'], message['msg']) 87 | 88 | def _handle_service_response(self, message): 89 | request_id = message['id'] 90 | service_handlers = self._pending_service_requests.get(request_id, None) 91 | 92 | if not service_handlers: 93 | raise RosBridgeException( 94 | 'No handler registered for service request ID: "%s"' % request_id) 95 | 96 | callback, errback = service_handlers 97 | del self._pending_service_requests[request_id] 98 | 99 | if 'result' in message and message['result'] is False: 100 | if errback: 101 | errback(message['values']) 102 | else: 103 | if callback: 104 | callback(ServiceResponse(message['values'])) 105 | 106 | def _handle_service_request(self, message): 107 | if 'service' not in message: 108 | raise ValueError( 109 | 'Expected service name missing in service request') 110 | 111 | self.factory.emit(message['service'], message) 112 | -------------------------------------------------------------------------------- /src/roslibpy/__main__.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | import roslibpy 4 | 5 | 6 | def rostopic_list(ros, **kwargs): 7 | for topic in ros.get_topics(): 8 | print(topic) 9 | 10 | 11 | def rostopic_type(ros, topic, **kwargs): 12 | topic_type = ros.get_topic_type(topic) 13 | print(topic_type) 14 | 15 | 16 | def rostopic_find(ros, type, **kwargs): 17 | for topic in ros.get_topics_for_type(type): 18 | print(topic) 19 | 20 | 21 | def rosmsg_info(ros, type, **kwargs): 22 | typedef = ros.get_message_details(type) 23 | _print_type(typedef) 24 | 25 | 26 | def rosservice_list(ros, **kwargs): 27 | for service in ros.get_services(): 28 | print(service) 29 | 30 | 31 | def rosservice_type(ros, service, **kwargs): 32 | service_type = ros.get_service_type(service) 33 | print(service_type) 34 | 35 | 36 | def rosservice_find(ros, type, **kwargs): 37 | for service in ros.get_services_for_type(type): 38 | print(service) 39 | 40 | 41 | def rossrv_info(ros, type, **kwargs): 42 | _print_type(ros.get_service_request_details(type)) 43 | print('---') 44 | _print_type(ros.get_service_response_details(type)) 45 | 46 | 47 | def rosservice_info(ros, service, **kwargs): 48 | type_name = ros.get_service_type(service) 49 | 50 | print('Type: %s\n' % type_name) 51 | print('Message definition') 52 | print('------------------') 53 | 54 | rossrv_info(ros, type_name) 55 | 56 | 57 | def rosparam_list(ros, **kwargs): 58 | for param in ros.get_params(): 59 | print(param) 60 | 61 | 62 | def rosparam_set(ros, param, value, **kwargs): 63 | ros.set_param(param, json.loads(value)) 64 | 65 | 66 | def rosparam_get(ros, param, **kwargs): 67 | print(ros.get_param(param)) 68 | 69 | 70 | def rosparam_delete(ros, param, **kwargs): 71 | ros.delete_param(param) 72 | 73 | 74 | def _print_typedef(typedef, def_map, level): 75 | defs = def_map[typedef] 76 | for fname, ftype, flen in zip(defs['fieldnames'], defs['fieldtypes'], defs['fieldarraylen']): 77 | if flen == -1: 78 | ftype_info = ftype 79 | elif flen == 0: 80 | ftype_info = ftype + '[]' 81 | else: 82 | ftype_info = '%s[%d]' % (ftype, flen) 83 | 84 | print('%s%s %s' % (' ' * level, ftype_info, fname)) 85 | if ftype in def_map: 86 | _print_typedef(ftype, def_map, level + 1) 87 | 88 | 89 | def _print_type(typedata): 90 | if len(typedata['typedefs']) == 0: 91 | return 92 | 93 | main_type = typedata['typedefs'][0]['type'] 94 | def_map = {typedef['type']: typedef for typedef in typedata['typedefs']} 95 | _print_typedef(main_type, def_map, 0) 96 | 97 | 98 | def main(): 99 | import argparse 100 | 101 | parser = argparse.ArgumentParser(description='roslibpy command-line utility') 102 | 103 | commands = parser.add_subparsers(help='commands') 104 | commands.dest = 'command' 105 | commands.required = True 106 | 107 | # Command: topic 108 | topic_command = commands.add_parser('topic', help='ROS Topics') 109 | topic_subcommands = topic_command.add_subparsers(help='ROS topic commands') 110 | topic_subcommands.dest = 'subcommand' 111 | topic_subcommands.required = True 112 | 113 | topic_list_parser = topic_subcommands.add_parser('list', help='List available ROS topics') 114 | topic_list_parser.set_defaults(func=rostopic_list) 115 | 116 | topic_type_parser = topic_subcommands.add_parser('type', help='ROS topic type') 117 | topic_type_parser.add_argument('topic', action='store', type=str, help='Topic name') 118 | topic_type_parser.set_defaults(func=rostopic_type) 119 | 120 | topic_find_parser = topic_subcommands.add_parser('find', help='ROS topics by type') 121 | topic_find_parser.add_argument('type', action='store', type=str, help='Type name') 122 | topic_find_parser.set_defaults(func=rostopic_find) 123 | 124 | # Command: msg 125 | msg_command = commands.add_parser('msg', help='ROS Message type information') 126 | msg_subcommands = msg_command.add_subparsers(help='ROS Message type commands') 127 | msg_subcommands.dest = 'subcommand' 128 | msg_subcommands.required = True 129 | 130 | msg_info_parser = msg_subcommands.add_parser('info', help='ROS message type information') 131 | msg_info_parser.add_argument('type', action='store', type=str, help='Message type') 132 | msg_info_parser.set_defaults(func=rosmsg_info) 133 | 134 | # Command: service 135 | service_command = commands.add_parser('service', help='ROS Services') 136 | service_subcommands = service_command.add_subparsers(help='ROS service commands') 137 | service_subcommands.dest = 'subcommand' 138 | service_subcommands.required = True 139 | 140 | service_list_parser = service_subcommands.add_parser('list', help='List available ROS services') 141 | service_list_parser.set_defaults(func=rosservice_list) 142 | 143 | service_type_parser = service_subcommands.add_parser('type', help='ROS service type') 144 | service_type_parser.add_argument('service', action='store', type=str, help='Service name') 145 | service_type_parser.set_defaults(func=rosservice_type) 146 | 147 | service_find_parser = service_subcommands.add_parser('find', help='ROS services by type') 148 | service_find_parser.add_argument('type', action='store', type=str, help='Type name') 149 | service_find_parser.set_defaults(func=rosservice_find) 150 | 151 | service_info_parser = service_subcommands.add_parser('info', help='ROS service information') 152 | service_info_parser.add_argument('service', action='store', type=str, help='Service name') 153 | service_info_parser.set_defaults(func=rosservice_info) 154 | 155 | # Command: srv 156 | srv_command = commands.add_parser('srv', help='ROS Service type information') 157 | srv_subcommands = srv_command.add_subparsers(help='ROS service type commands') 158 | srv_subcommands.dest = 'subcommand' 159 | srv_subcommands.required = True 160 | 161 | srv_info_parser = srv_subcommands.add_parser('info', help='ROS service type information') 162 | srv_info_parser.add_argument('type', action='store', type=str, help='Service type') 163 | srv_info_parser.set_defaults(func=rossrv_info) 164 | 165 | # Command: param 166 | param_command = commands.add_parser('param', help='ROS Params') 167 | param_subcommands = param_command.add_subparsers(help='ROS parameter commands') 168 | param_subcommands.dest = 'subcommand' 169 | param_subcommands.required = True 170 | 171 | param_list_parser = param_subcommands.add_parser('list', help='List available ROS parameters') 172 | param_list_parser.set_defaults(func=rosparam_list) 173 | 174 | param_set_parser = param_subcommands.add_parser('set', help='Set ROS param value') 175 | param_set_parser.add_argument('param', action='store', type=str, help='Param name') 176 | param_set_parser.add_argument('value', action='store', type=str, help='Param value') 177 | param_set_parser.set_defaults(func=rosparam_set) 178 | 179 | param_get_parser = param_subcommands.add_parser('get', help='Get ROS param value') 180 | param_get_parser.add_argument('param', action='store', type=str, help='Param name') 181 | param_get_parser.set_defaults(func=rosparam_get) 182 | 183 | param_delete_parser = param_subcommands.add_parser('delete', help='Delete ROS param') 184 | param_delete_parser.add_argument('param', action='store', type=str, help='Param name') 185 | param_delete_parser.set_defaults(func=rosparam_delete) 186 | 187 | # Invoke 188 | args = parser.parse_args() 189 | ros = roslibpy.Ros('localhost', 9090) 190 | 191 | try: 192 | ros.run() 193 | args.func(ros, **vars(args)) 194 | finally: 195 | ros.terminate() 196 | 197 | 198 | if __name__ == '__main__': 199 | main() 200 | -------------------------------------------------------------------------------- /src/roslibpy/tf.py: -------------------------------------------------------------------------------- 1 | r""" 2 | TF 3 | --- 4 | 5 | ROS 提供了一个非常强大的转换库，叫做 `TF2 `_，\ 6 | 允许用户随时跟踪多个坐标系。 7 | **roslibpy** 库通过\ :class:`TFClient`\ 类与 8 | \ `tf2_web_republisher `_\ 9 | 连接来提供对它的访问。 10 | 11 | .. autoclass:: TFClient 12 | :members: 13 | 14 | """ 15 | from __future__ import print_function 16 | 17 | import logging 18 | import math 19 | 20 | from . import Service 21 | from . import ServiceRequest 22 | from . import Topic 23 | 24 | __all__ = ['TFClient'] 25 | 26 | LOGGER = logging.getLogger('roslibpy.tf') 27 | 28 | 29 | class TFClient(object): 30 | """A TF Client that listens to TFs from tf2_web_republisher. 31 | 32 | Args: 33 | ros (:class:`.Ros`): Instance of the ROS connection. 34 | fixed_frame (:obj:`str`): Fixed frame, e.g. ``/base_link``. 35 | angular_threshold (:obj:`float`): Angular threshold for the TF republisher. 36 | translation_threshold (:obj:`float`): Translation threshold for the TF republisher. 37 | rate (:obj:`float`): Rate for the TF republisher. 38 | update_delay (:obj:`int`): Time expressed in milliseconds to wait after a new subscription to update the TF republisher's list of TFs. 39 | topic_timeout (:obj:`int`): Timeout parameter for the TF republisher expressed in milliseconds. 40 | repub_service_name (:obj:`str`): Name of the republish tfs service, e.g. ``/republish_tfs``. 41 | """ 42 | 43 | def __init__(self, ros, fixed_frame='/base_link', angular_threshold=2.0, translation_threshold=0.01, rate=10.0, update_delay=50, topic_timeout=2000.0, 44 | server_name='/tf2_web_republisher', repub_service_name='/republish_tfs'): 45 | self.ros = ros 46 | self.fixed_frame = fixed_frame 47 | self.angular_threshold = angular_threshold 48 | self.translation_threshold = translation_threshold 49 | self.rate = rate 50 | self.update_delay = update_delay 51 | 52 | seconds = topic_timeout / 1000. 53 | secs = math.floor(seconds) 54 | nsecs = math.floor((seconds - secs) * 1000000000) 55 | self.topic_timeout = dict(secs=secs, nsecs=nsecs) 56 | self.repub_service_name = repub_service_name 57 | 58 | self.current_topic = False 59 | self.frame_info = {} 60 | self.republisher_update_requested = False 61 | 62 | self.service_client = Service(ros, self.repub_service_name, 63 | 'tf2_web_republisher/RepublishTFs') 64 | 65 | def _process_tf_array(self, tf): 66 | """Process an incoming TF message and send it out using the callback functions. 67 | 68 | Args: 69 | tf (:obj:`list`): TF message from the server. 70 | """ 71 | # TODO: Test this function 72 | for transform in tf['transforms']: 73 | frame_id = self._normalize_frame_id(transform['child_frame_id']) 74 | frame = self.frame_info.get(frame_id, None) 75 | 76 | if frame: 77 | frame['transform'] = dict( 78 | translation=transform['transform']['translation'], 79 | rotation=transform['transform']['rotation'] 80 | ) 81 | for callback in frame['cbs']: 82 | callback(frame['transform']) 83 | 84 | def update_goal(self): 85 | """Send a new service request to the tf2_web_republisher based on the current list of TFs.""" 86 | message = dict(source_frames=list(self.frame_info.keys()), 87 | target_frame=self.fixed_frame, 88 | angular_thres=self.angular_threshold, 89 | trans_thres=self.translation_threshold, 90 | rate=self.rate) 91 | 92 | # In contrast to roslibjs, we do not support groovy compatibility mode 93 | # and only use the service interface to the TF republisher 94 | message['timeout'] = self.topic_timeout 95 | request = ServiceRequest(message) 96 | 97 | self.service_client.call( 98 | request, self._process_response, self._process_error) 99 | 100 | self.republisher_update_requested = False 101 | 102 | def _process_error(self, response): 103 | LOGGER.error( 104 | 'The TF republisher service interface returned an error. %s', str(response)) 105 | 106 | def _process_response(self, response): 107 | """Process the service response and subscribe to the tf republisher topic.""" 108 | LOGGER.info('Received response from TF Republisher service interface') 109 | 110 | if self.current_topic: 111 | self.current_topic.unsubscribe() 112 | 113 | self.current_topic = Topic( 114 | self.ros, response['topic_name'], 'tf2_web_republisher/TFArray') 115 | self.current_topic.subscribe(self._process_tf_array) 116 | 117 | def _normalize_frame_id(self, frame_id): 118 | # Remove leading slash, if it's there 119 | if frame_id[0] == '/': 120 | return frame_id[1:] 121 | 122 | return frame_id 123 | 124 | def subscribe(self, frame_id, callback): 125 | """Subscribe to the given TF frame. 126 | 127 | Args: 128 | frame_id (:obj:`str`): TF frame identifier to subscribe to. 129 | callback (:obj:`callable`): A callable functions receiving one parameter with `transform` data. 130 | """ 131 | 132 | frame_id = self._normalize_frame_id(frame_id) 133 | frame = self.frame_info.get(frame_id, None) 134 | 135 | # If there is no callback registered for the given frame, create emtpy callback list 136 | if not frame: 137 | frame = dict(cbs=[]) 138 | self.frame_info[frame_id] = frame 139 | 140 | if not self.republisher_update_requested: 141 | self.ros.call_later(self.update_delay / 1000., self.update_goal) 142 | self.republisher_update_requested = True 143 | else: 144 | # If we already have a transform, call back immediately 145 | if 'transform' in frame: 146 | callback(frame['transform']) 147 | 148 | frame['cbs'].append(callback) 149 | 150 | def unsubscribe(self, frame_id, callback): 151 | """Unsubscribe from the given TF frame. 152 | 153 | Args: 154 | frame_id (:obj:`str`): TF frame identifier to unsubscribe from. 155 | callback (:obj:`callable`): The callback function to remove. 156 | """ 157 | 158 | frame_id = self._normalize_frame_id(frame_id) 159 | frame = self.frame_info.get(frame_id, None) 160 | 161 | if 'cbs' in frame: 162 | frame['cbs'].pop(callback) 163 | 164 | if not callback or ('cbs' in frame and len(frame['cbs']) == 0): 165 | self.frame_info.pop(frame_id) 166 | 167 | def dispose(self): 168 | """Unsubscribe and unadvertise all topics associated with this instance.""" 169 | if self.current_topic: 170 | self.current_topic.unsubscribe() 171 | 172 | 173 | if __name__ == '__main__': 174 | from roslibpy import Ros 175 | 176 | FORMAT = '%(asctime)-15s [%(levelname)s] %(message)s' 177 | logging.basicConfig(level=logging.DEBUG, format=FORMAT) 178 | 179 | ros_client = Ros('127.0.0.1', 9090) 180 | 181 | def run_tf_example(): 182 | tfclient = TFClient(ros_client, fixed_frame='world', 183 | angular_threshold=0.01, translation_threshold=0.01) 184 | 185 | tfclient.subscribe('turtle2', print) 186 | 187 | def dispose_server(): 188 | tfclient.dispose() 189 | 190 | ros_client.call_later(10, dispose_server) 191 | ros_client.call_later(11, ros_client.close) 192 | ros_client.call_later(12, ros_client.terminate) 193 | 194 | run_tf_example() 195 | ros_client.run_forever() 196 | -------------------------------------------------------------------------------- /docs/examples.rst: -------------------------------------------------------------------------------- 1 | 快速上手 2 | ======== 3 | 4 | 5 | **roslibpy** 上手非常简单。在接下来的例子中你将看到如何使用它与一个 ROS 环境相连接。 6 | 7 | .. note:: 8 | 9 | 在连接的过程中，确保在你网络中的 ROS 服务器配置并打开了 **rosbridge server** 和 **TF2 web republisher**\ （具体请戳 :ref:`ros-setup`\ ） 10 | 11 | 这些例子假定了 ROS 服务器运行在同一台主机上。对于不在同一台主机的情况，只需要将\ ``host``\ 参数从\ ``'localhost'``\ 改为 ROS master 的 *IP 地址*\ 。 12 | 13 | .. note:: 14 | 15 | ``port``\ 参数必须设定为\ ``9090``\ ，因为\ ``rosbridge``\ 的默认端口号是\ ``9090``\ 。如果想改变端口号，\ 16 | 可参考\ `这里 `_\ 。 17 | 18 | 19 | 第一个例子 20 | ---------- 21 | 22 | 用以下命令导入\ ``roslibpy``:: 23 | 24 | >>> import roslibpy 25 | 26 | 用以下命令初始化连接:: 27 | 28 | >>> ros = roslibpy.Ros(host='localhost', port=9090) 29 | >>> ros.run() 30 | 31 | 是不是很简单？ 32 | 33 | 让我们检查一下连接状态:: 34 | 35 | >>> ros.is_connected 36 | True 37 | 38 | **耶( •̀ ω •́ )y 我们的第一个例子成功跑通！** 39 | 40 | 41 | 融会贯通 42 | -------- 43 | 44 | 让我们以 Python 文件的形式建立一个完整的例子。 45 | 46 | 新建一个文件并命名为\ ``ros-hello-world.py``\ ，然后复制粘贴以下内容： 47 | 48 | .. literalinclude:: files/ros-hello-world.py 49 | :language: python 50 | 51 | 在命令行中输入以下命令运行该程序:: 52 | 53 | $ python ros-hello-world.py 54 | 55 | 这个程序运行起来之后，会尝试建立与 ROS 的连接，连接建立后打印输出信息，并终止连接。 56 | 57 | 58 | 控制事件循环 59 | ------------ 60 | 61 | 在之前的例子里，我们通过调用\ ``run()``\ 来开启与 ROS 的连接，这样会在后台开启一个事件循环。在某些情况下，\ 62 | 我们希望在前台更明确地处理主事件循环，\ :class:`roslibpy.Ros` 提供了一个\ ``run_forever()``\ 方法来做这件事。 63 | 64 | 如果我们如果使用这个方法启动事件循环，则需要事先设置好所有连接处理配置。我们使用\ :meth:`roslibpy.Ros.on_ready`\ 来做这件事。 65 | 66 | 67 | 接下来的代码片段用\ ``run_forever()``\ 和\ ``on_ready()``\ 实现了与之前的例子同样的功能： 68 | 69 | .. literalinclude:: files/ros-hello-world-run-forever.py 70 | :language: python 71 | 72 | .. note:: 73 | 74 | \ ``run()``\ 与\ ``run_forever()``\ 的区别在于，前者新开一个单独的线程来处理事件，而后者会阻塞当前线程。 75 | 76 | 77 | 断开连接 78 | ----------- 79 | 80 | 在你的任务完成后，你应该干净地断开与 ``rosbridge`` 的连接。有两个相关的方法可以实现这个事情： 81 | 82 | - :meth:`roslibpy.Ros.close`: 断开 websocket 连接. 在连接被关闭后，通过调用 :meth:`roslibpy.Ros.connect`: 还可以再次连接。 83 | - :meth:`roslibpy.Ros.terminate`: 终止主事件循环。如果连接还是打开着的，那么就会首先关闭它。 84 | 85 | .. note:: 86 | 87 | 当使用 ``twisted/authbahn`` 循环的时候，终止主事件循环是一个不可逆的行为，因为 ``twisted`` 反应器 (reacters) 不能被重启。这个操作应该保留到你程序的最后面再使用。 88 | 89 | 重新连接 90 | ------------ 91 | 92 | 如果在开启连接的时候 ``rosbridge`` 不响应，或者一个已建立的连接意外断开的时候， ``roslibpy`` 将会自动尝试重新连接，并且重新连接话题的订阅者和发布者。重新连接尝试时间的间隔是指数递减的。 93 | 94 | 95 | Hello World: 话题（Topics） 96 | --------------------------- 97 | 98 | ROS 中的\ ``Hello World``\ 例子是开启两个节点，并利用话题的订阅/发布来建立通讯。这两个节点（一个 talker 和一个 listener）非常简单，\ 99 | 但是透过它们可以便于理解在 ROS 框架下的分布式系统中，两个进程之间的通信是如何工作的。 100 | 101 | 接下来，我们用 **roslibpy** 来建立一个简单的话题通讯。 102 | 103 | 104 | 一个 talker 节点 105 | ^^^^^^^^^^^^^^^^ 106 | 107 | 接下来的例子是开启一个 ROS 节点并循环发布信息（按下\ ``Ctrl+C``\ 来终止）。 108 | 109 | 110 | .. literalinclude:: files/ros-hello-world-talker.py 111 | :language: python 112 | 113 | * :download:`ros-hello-world-talker.py ` 114 | 115 | .. note:: 116 | 117 | 这里以 ROS 中的 `std_msgs/String `_ 消息类型为例，其它消息类型\ 118 | 也可以利用 Python 字典的方式构建，参考\ `这个例子 `_\ 。 119 | 120 | 121 | 一个 listener 节点 122 | ^^^^^^^^^^^^^^^^^^ 123 | 124 | Listener 端的代码如下： 125 | 126 | .. literalinclude:: files/ros-hello-world-listener.py 127 | :language: python 128 | 129 | * :download:`ros-hello-world-listener.py ` 130 | 131 | 132 | 运行例程 133 | ^^^^^^^^^^^ 134 | 135 | 打开一个终端，开启 talker 节点:: 136 | 137 | $ python ros-hello-world-talker.py 138 | 139 | 打开另一个终端，开启 listener 节点:: 140 | 141 | $ python ros-hello-world-listener.py 142 | 143 | .. note:: 144 | 145 | 两个文件的位置不必在一起，它们可以在不同的路径、甚至不同的计算机中，只要保证是同一个 Ros master 即可。 146 | 147 | 使用服务（Services） 148 | -------------------- 149 | 150 | 节点之间的另一种通讯方式是通过 ROS 服务来进行。 151 | 152 | 服务一般需要定义请求和回应的类型，为了简单，下面的例子使用了现成的\ ``get_loggers``\ 服务： 153 | 154 | .. literalinclude :: files/ros-service-call-logger.py 155 | :language: python 156 | 157 | * :download:`ros-service-call-logger.py ` 158 | 159 | 创建服务（Services） 160 | ------------------------- 161 | 162 | 只要服务类型的定义存在于您的 ROS 环境中，就可以创建新服务。 163 | 164 | 下面的例子展示了如何创建一个简单的服务，它使用ROS 中定义的标准服务类型之一（``std_srvs/SetBool``）: 165 | 166 | .. literalinclude :: files/ros-service.py 167 | :language: python 168 | 169 | * :download:`ros-service.py ` 170 | 171 | 下载该脚本，并输入如下命令:: 172 | 173 | $ python ros-service.py 174 | 175 | 程序开始运行，期间服务会一直处于活动状态（按下\ ``Ctrl+C``\ 来终止）。 176 | 177 | 不要关闭这个服务，下载并运行以下代码示例来调用服务，以验证服务是否正常工作: 178 | 179 | * :download:`ros-service-call-set-bool.py ` 180 | 181 | 下载后在一个新的终端中键入以下命令:: 182 | 183 | $ python ros-service-call-set-bool.py 184 | 185 | .. note:: 186 | 187 | 现在您已经掌握了 **roslibpy** 的基础知识，更多细节请查看 :ref:`ros-api-reference`\ 。 188 | 189 | Actions 190 | -------- 191 | 192 | 除了话题和服务之外，ROS还提供 **Actions**，它们被用于长时间运行的任务，比如导航，因为它们是非阻塞的，并且允许任务执行时被抢占和取消。 193 | 194 | **roslibpy** 既支持 action 客户端，也可以通过 :class:`roslibpy.actionlib.SimpleActionServer`\ 提供 action 服务器。 195 | 196 | 下面的例子使用\ **斐波那契** action，该 action 的定义可在 `actionlib_tutorials `_ 中查看。 197 | 198 | Action 服务器 199 | ^^^^^^^^^^^^^^^^ 200 | 201 | 让我们从斐波那契服务器的定义开始： 202 | 203 | .. literalinclude :: files/ros-action-server.py 204 | :language: python 205 | 206 | * :download:`ros-action-server.py ` 207 | 208 | 下载后键入以下命令:: 209 | 210 | $ python ros-action-server.py 211 | 212 | 在程序运行时，\ action 服务器将保持活动状态（按下\ ``Ctrl+C``\ 来终止）。 213 | 214 | 如果您想在下一个示例中测试这个窗口，请不要关闭它。 215 | 216 | Action 客户端 217 | ^^^^^^^^^^^^^^^^^ 218 | 219 | 现在，让我们看看如何为新创建的服务器编写一个 action 客户端。 220 | 221 | 下面的程序显示了一个简单的 action 客户端： 222 | 223 | .. literalinclude :: files/ros-action-client.py 224 | :language: python 225 | 226 | * :download:`ros-action-client.py ` 227 | 228 | 下载后键入以下命令:: 229 | 230 | $ python ros-action-client.py 231 | 232 | 您将立即看到我们的 action 服务器的所有中间计算，并在最后一行显示生成的斐波那契数列。 233 | 234 | 这个例子非常简单，使用了 :meth:`roslibpy.actionlib.Goal.wait` 函数，以使代码更易于阅读。一个更鲁棒的处理方法是使用回调把结果连接到 ``result`` 事件。 235 | 236 | 查询 ROS API 237 | -------------- 238 | 239 | ROS 提供了一系列 API 用来查询话题、服务、节点等。这些 API 可以用 Python 代码以编程的方式使用，也可以通过命令行调用。 240 | 241 | 通过命令行使用 242 | ^^^^^^^^^^^^^^^^^ 243 | 244 | 命令行的方式模仿了 ROS 本身的调用过程。 245 | 246 | 下列的命令都是可用的:: 247 | 248 | $ roslibpy topic list 249 | $ roslibpy topic type /rosout 250 | $ roslibpy topic find std_msgs/Int32 251 | $ roslibpy msg info rosgraph_msgs/Log 252 | 253 | $ roslibpy service list 254 | $ roslibpy service type /rosout/get_loggers 255 | $ roslibpy service find roscpp/GetLoggers 256 | $ roslibpy srv info roscpp/GetLoggers 257 | 258 | $ roslibpy param list 259 | $ roslibpy param set /foo "[\"1\", 1, 1.0]" 260 | $ roslibpy param get /foo 261 | $ roslibpy param delete /foo 262 | 263 | 通过 Python 代码使用 264 | ^^^^^^^^^^^^^^^^^^^^^^^^ 265 | 266 | 与上面的方式相对应，下列方法允许通过 Python 代码来查询 ROS API： 267 | 268 | 话题 269 | """""" 270 | 271 | * :meth:`roslibpy.Ros.get_topics` 272 | * :meth:`roslibpy.Ros.get_topic_type` 273 | * :meth:`roslibpy.Ros.get_topics_for_type` 274 | * :meth:`roslibpy.Ros.get_message_details` 275 | 276 | 服务 277 | """""" 278 | 279 | * :meth:`roslibpy.Ros.get_services` 280 | * :meth:`roslibpy.Ros.get_service_type` 281 | * :meth:`roslibpy.Ros.get_services_for_type` 282 | * :meth:`roslibpy.Ros.get_service_request_details` 283 | * :meth:`roslibpy.Ros.get_service_response_details` 284 | 285 | 参数 286 | """""" 287 | 288 | * :meth:`roslibpy.Ros.get_params` 289 | * :meth:`roslibpy.Ros.get_param` 290 | * :meth:`roslibpy.Ros.set_param` 291 | * :meth:`roslibpy.Ros.delete_param` 292 | 293 | 294 | 进阶例程 295 | -------------- 296 | 297 | 下列是一些更复杂的使用 ``roslibpy`` 的例子。 298 | 299 | .. toctree:: 300 | :maxdepth: 2 301 | :glob: 302 | 303 | examples/* 304 | 305 | 我们鼓励大家通过 pull request 或 issue tracker 来提交更多的例程。 306 | -------------------------------------------------------------------------------- /src/roslibpy/event_emitter.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | # The following code is derivative work of [pyee](https://github.com/jfhbrook/pyee) 4 | # Copyrighted by Joshua Holbrook with a MIT license 5 | 6 | 7 | # The MIT License(MIT) 8 | 9 | # Copyright(c) 2015 Joshua Holbrook 10 | 11 | # Permission is hereby granted, free of charge, to any person obtaining a copy 12 | # of this software and associated documentation files(the "Software"), to deal 13 | # in the Software without restriction, including without limitation the rights 14 | # to use, copy, modify, merge, publish, distribute, sublicense, and / or sell 15 | # copies of the Software, and to permit persons to whom the Software is 16 | # furnished to do so, subject to the following conditions: 17 | 18 | # The above copyright notice and this permission notice shall be included in 19 | # all copies or substantial portions of the Software. 20 | 21 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 26 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 27 | # THE SOFTWARE. 28 | 29 | """ 30 | ``EventEmitterMixin`` is similar to the ``EventEmitter`` of Node.js. 31 | It supports both synchronous callbacks and asyncio coroutines. 32 | """ 33 | 34 | try: 35 | from asyncio import iscoroutine, ensure_future 36 | except ImportError: 37 | iscoroutine = None 38 | ensure_future = None 39 | 40 | from collections import OrderedDict 41 | from collections import defaultdict 42 | from threading import RLock 43 | 44 | __all__ = ['EventEmitterMixin', 'EventEmitterException'] 45 | 46 | 47 | class EventEmitterException(Exception): 48 | """An internal exception.""" 49 | pass 50 | 51 | 52 | class EventEmitterMixin(object): 53 | """Mixin to add event emitter features to a class. 54 | 55 | For interoperation with asyncio, one can specify the scheduler and 56 | the event loop. The scheduler defaults to ``asyncio.ensure_future``, 57 | and the loop defaults to ``None``. When used with the default scheduler, 58 | this will schedule the coroutine onto asyncio's default loop. 59 | 60 | This should also be compatible with recent versions of twisted by 61 | setting ``scheduler=twisted.internet.defer.ensureDeferred``. 62 | 63 | Most events are registered with EventEmitterMixin via the ``on`` and ``once`` 64 | methods. However, event emitters have two *special* events: 65 | 66 | - ``new_listener``: Fires whenever a new listener is created. Listeners for 67 | this event do not fire upon their own creation. 68 | 69 | - ``error``: When emitted raises an Exception by default, behavior can be 70 | overriden by attaching callback to the event. 71 | 72 | For example:: 73 | 74 | @ee.on('error') 75 | def onError(message): 76 | logging.err(message) 77 | 78 | ee.emit('error', Exception('something blew up')) 79 | 80 | For synchronous callbacks, exceptions are **not** handled for you--- 81 | you must catch your own exceptions inside synchronous ``on`` handlers. 82 | However, when wrapping **async** functions, errors will be intercepted 83 | and emitted under the ``error`` event. **This behavior for async 84 | functions is inconsistent with node.js**, which unlike this package has 85 | no facilities for handling returned Promises from handlers. 86 | """ 87 | 88 | def __init__(self, *args, **kwargs): 89 | super(EventEmitterMixin, self).__init__(*args, **kwargs) 90 | self._events = defaultdict(OrderedDict) 91 | self._schedule = kwargs.get('scheduler', ensure_future) 92 | self._loop = kwargs.get('loop', None) 93 | self._event_lock = RLock() 94 | 95 | def on(self, event, f=None): 96 | """Registers the function (or optionally an asyncio coroutine function) 97 | ``f`` to the event name ``event``. 98 | 99 | If ``f`` isn't provided, this method returns a function that 100 | takes ``f`` as a callback; in other words, you can use this method 101 | as a decorator, like so:: 102 | 103 | @ee.on('data') 104 | def data_handler(data): 105 | print(data) 106 | 107 | As mentioned, this method can also take an asyncio coroutine function:: 108 | 109 | @ee.on('data') 110 | async def data_handler(data) 111 | await do_async_thing(data) 112 | 113 | 114 | This will automatically schedule the coroutine using the configured 115 | scheduling function (defaults to ``asyncio.ensure_future``) and the 116 | configured event loop (defaults to ``asyncio.get_event_loop()``). 117 | 118 | In both the decorated and undecorated forms, the event handler is 119 | returned. The upshot of this is that you can call decorated handlers 120 | directly, as well as use them in remove_listener calls. 121 | """ 122 | 123 | with self._event_lock: 124 | def _on(f): 125 | self._add_event_handler(event, f, f) 126 | return f 127 | 128 | if f is None: 129 | return _on 130 | else: 131 | return _on(f) 132 | 133 | def _add_event_handler(self, event, k, v): 134 | # Fire 'new_listener' *before* adding the new listener! 135 | self.emit('new_listener', event, k) 136 | 137 | # Add the necessary function 138 | # Note that k and v are the same for `on` handlers, but 139 | # different for `once` handlers, where v is a wrapped version 140 | # of k which removes itself before calling k 141 | self._events[event][k] = v 142 | 143 | def emit(self, event, *args, **kwargs): 144 | """Emit ``event``, passing ``*args`` and ``**kwargs`` to each attached 145 | function. Returns ``True`` if any functions are attached to ``event``; 146 | otherwise returns ``False``. 147 | 148 | Example:: 149 | 150 | ee.emit('data', '00101001') 151 | 152 | Assuming ``data`` is an attached function, this will call 153 | ``data('00101001')'``. 154 | 155 | For coroutine event handlers, calling emit is non-blocking. In other 156 | words, you do not have to await any results from emit, and the 157 | coroutine is scheduled in a fire-and-forget fashion. 158 | """ 159 | handled = False 160 | 161 | with self._event_lock: 162 | for f in list(self._events[event].values()): 163 | result = f(*args, **kwargs) 164 | 165 | # If f was a coroutine function, we need to schedule it and 166 | # handle potential errors 167 | if iscoroutine and iscoroutine(result): 168 | if self._loop: 169 | d = self._schedule(result, loop=self._loop) 170 | else: 171 | d = self._schedule(result) 172 | 173 | # scheduler gave us an asyncio Future 174 | if hasattr(d, 'add_done_callback'): 175 | @d.add_done_callback 176 | def _callback(f): 177 | exc = f.exception() 178 | if exc: 179 | self.emit('error', exc) 180 | 181 | # scheduler gave us a twisted Deferred 182 | elif hasattr(d, 'addErrback'): 183 | @d.addErrback 184 | def _callback(exc): 185 | self.emit('error', exc) 186 | handled = True 187 | 188 | if not handled and event == 'error': 189 | if args: 190 | raise args[0] 191 | else: 192 | raise EventEmitterException( 193 | "Uncaught, unspecified 'error' event.") 194 | 195 | return handled 196 | 197 | def once(self, event, f=None): 198 | """The same as ``ee.on``, except that the listener is automatically 199 | removed after being called. 200 | """ 201 | 202 | with self._event_lock: 203 | def _wrapper(f): 204 | def g(*args, **kwargs): 205 | self.remove_listener(event, f) 206 | # f may return a coroutine, so we need to return that 207 | # result here so that emit can schedule it 208 | return f(*args, **kwargs) 209 | 210 | self._add_event_handler(event, f, g) 211 | return f 212 | 213 | if f is None: 214 | return _wrapper 215 | else: 216 | return _wrapper(f) 217 | 218 | def off(self, event, f): 219 | """Removes the function ``f`` from ``event``.""" 220 | self._events[event].pop(f) 221 | 222 | def remove_listener(self, event, f): 223 | """Removes the function ``f`` from ``event``.""" 224 | self._events[event].pop(f) 225 | 226 | def remove_all_listeners(self, event=None): 227 | """Remove all listeners attached to ``event``. 228 | If ``event`` is ``None``, remove all listeners on all events. 229 | """ 230 | with self._event_lock: 231 | if event is not None: 232 | self._events[event] = OrderedDict() 233 | else: 234 | self._events = defaultdict(OrderedDict) 235 | 236 | def listeners(self, event): 237 | """Returns a list of all listeners registered to the ``event``. 238 | """ 239 | return list(self._events[event].keys()) 240 | -------------------------------------------------------------------------------- /src/roslibpy/comm/comm_autobahn.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import logging 4 | import threading 5 | 6 | from autobahn.twisted.websocket import WebSocketClientFactory 7 | from autobahn.twisted.websocket import WebSocketClientProtocol 8 | from autobahn.twisted.websocket import connectWS 9 | from autobahn.websocket.util import create_url 10 | from twisted.internet import defer 11 | from twisted.internet import reactor 12 | from twisted.internet import threads 13 | from twisted.internet.protocol import ReconnectingClientFactory 14 | from twisted.python import log 15 | 16 | from ..event_emitter import EventEmitterMixin 17 | from . import RosBridgeProtocol 18 | 19 | LOGGER = logging.getLogger('roslibpy') 20 | 21 | 22 | class AutobahnRosBridgeProtocol(RosBridgeProtocol, WebSocketClientProtocol): 23 | def __init__(self, *args, **kwargs): 24 | super(AutobahnRosBridgeProtocol, self).__init__(*args, **kwargs) 25 | 26 | def onConnect(self, response): 27 | LOGGER.debug('Server connected: %s', response.peer) 28 | 29 | def onOpen(self): 30 | LOGGER.info('Connection to ROS MASTER ready.') 31 | self._manual_disconnect = False 32 | self.factory.ready(self) 33 | 34 | def onMessage(self, payload, isBinary): 35 | if isBinary: 36 | raise NotImplementedError('Add support for binary messages') 37 | 38 | try: 39 | self.on_message(payload) 40 | except Exception: 41 | LOGGER.exception('Exception on start_listening while trying to handle message received.' + 42 | 'It could indicate a bug in user code on message handlers. Message skipped.') 43 | 44 | def onClose(self, wasClean, code, reason): 45 | LOGGER.info('WebSocket connection closed: Code=%s, Reason=%s', str(code), reason) 46 | 47 | def send_message(self, payload): 48 | return self.sendMessage(payload, isBinary=False, fragmentSize=None, sync=False, doNotCompress=False) 49 | 50 | def send_close(self): 51 | self._manual_disconnect = True 52 | self.sendClose() 53 | 54 | 55 | class AutobahnRosBridgeClientFactory(EventEmitterMixin, ReconnectingClientFactory, WebSocketClientFactory): 56 | """Factory to create instances of the ROS Bridge protocol built on top of Autobahn/Twisted.""" 57 | protocol = AutobahnRosBridgeProtocol 58 | 59 | def __init__(self, *args, **kwargs): 60 | super(AutobahnRosBridgeClientFactory, self).__init__(*args, **kwargs) 61 | self._proto = None 62 | self._manager = None 63 | self.connector = None 64 | self.setProtocolOptions(closeHandshakeTimeout=5) 65 | 66 | def connect(self): 67 | """Establish WebSocket connection to the ROS server defined for this factory.""" 68 | self.connector = connectWS(self) 69 | 70 | @property 71 | def is_connected(self): 72 | """Indicate if the WebSocket connection is open or not. 73 | 74 | Returns: 75 | bool: True if WebSocket is connected, False otherwise. 76 | """ 77 | return self.connector and self.connector.state == 'connected' 78 | 79 | def on_ready(self, callback): 80 | if self._proto: 81 | callback(self._proto) 82 | else: 83 | self.once('ready', callback) 84 | 85 | def ready(self, proto): 86 | self.resetDelay() 87 | self._proto = proto 88 | self.emit('ready', proto) 89 | 90 | def startedConnecting(self, connector): 91 | LOGGER.debug('Started to connect...') 92 | 93 | def clientConnectionLost(self, connector, reason): 94 | LOGGER.debug('Lost connection. Reason: %s', reason) 95 | self.emit('close', self._proto) 96 | 97 | if not self._proto or (self._proto and not self._proto._manual_disconnect): 98 | ReconnectingClientFactory.clientConnectionLost(self, connector, reason) 99 | self._proto = None 100 | 101 | def clientConnectionFailed(self, connector, reason): 102 | LOGGER.debug('Connection failed. Reason: %s', reason) 103 | ReconnectingClientFactory.clientConnectionFailed( 104 | self, connector, reason) 105 | self._proto = None 106 | 107 | @property 108 | def manager(self): 109 | """Get an instance of the event loop manager for this factory.""" 110 | if not self._manager: 111 | self._manager = TwistedEventLoopManager() 112 | 113 | return self._manager 114 | 115 | @classmethod 116 | def create_url(cls, host, port=None, is_secure=False): 117 | url = host if port is None else create_url(host, port, is_secure) 118 | return url 119 | 120 | @classmethod 121 | def set_max_delay(cls, max_delay): 122 | """Set the maximum delay in seconds for reconnecting to rosbridge (3600 seconds by default). 123 | 124 | Args: 125 | max_delay: The new maximum delay, in seconds. 126 | """ 127 | LOGGER.debug('Updating max delay to {} seconds'.format(max_delay)) 128 | # See https://twistedmatrix.com/documents/19.10.0/api/twisted.internet.protocol.ReconnectingClientFactory.html 129 | cls.maxDelay = max_delay 130 | 131 | @classmethod 132 | def set_initial_delay(cls, initial_delay): 133 | """Set the initial delay in seconds for reconnecting to rosbridge (1 second by default). 134 | 135 | Args: 136 | initial_delay: The new initial delay, in seconds. 137 | """ 138 | LOGGER.debug('Updating initial delay to {} seconds'.format(initial_delay)) 139 | # See https://twistedmatrix.com/documents/19.10.0/api/twisted.internet.protocol.ReconnectingClientFactory.html 140 | cls.initialDelay = initial_delay 141 | 142 | @classmethod 143 | def set_max_retries(cls, max_retries): 144 | """Set the maximum number or connection retries when the rosbridge connection is lost (no limit by default). 145 | 146 | Args: 147 | max_retries: The new maximum number of retries. 148 | """ 149 | LOGGER.debug('Updating max retries to {}'.format(max_retries)) 150 | # See https://twistedmatrix.com/documents/19.10.0/api/twisted.internet.protocol.ReconnectingClientFactory.html 151 | cls.maxRetries = max_retries 152 | 153 | 154 | class TwistedEventLoopManager(object): 155 | """Manage the main event loop using Twisted reactor. 156 | 157 | The event loop is a Twisted application is a very opinionated 158 | management strategy. Other communication layers use different 159 | event loop handlers that might be more fitting for different 160 | execution environments. 161 | """ 162 | def __init__(self): 163 | self._log_observer = log.PythonLoggingObserver() 164 | self._log_observer.start() 165 | 166 | def run(self): 167 | """Kick-starts a non-blocking event loop. 168 | 169 | This implementation starts the Twisted Reactor 170 | on a separate thread to avoid blocking.""" 171 | 172 | if reactor.running: 173 | return 174 | 175 | self._thread = threading.Thread(target=reactor.run, args=(False,)) 176 | self._thread.daemon = True 177 | self._thread.start() 178 | 179 | def run_forever(self): 180 | """Kick-starts the main event loop of the ROS client. 181 | 182 | This implementation relies on Twisted Reactors 183 | to control the event loop.""" 184 | reactor.run() 185 | 186 | def call_later(self, delay, callback): 187 | """Call the given function after a certain period of time has passed. 188 | 189 | Args: 190 | delay (:obj:`int`): Number of seconds to wait before invoking the callback. 191 | callback (:obj:`callable`): Callable function to be invoked when the delay has elapsed. 192 | """ 193 | reactor.callLater(delay, callback) 194 | 195 | def call_in_thread(self, callback): 196 | """Call the given function on a thread. 197 | 198 | Args: 199 | callback (:obj:`callable`): Callable function to be invoked in a thread. 200 | """ 201 | reactor.callInThread(callback) 202 | 203 | def blocking_call_from_thread(self, callback, timeout): 204 | """Call the given function from a thread, and wait for the result synchronously 205 | for as long as the timeout will allow. 206 | 207 | Args: 208 | callback: Callable function to be invoked from the thread. 209 | timeout (:obj: int): Number of seconds to wait for the response before 210 | raising an exception. 211 | 212 | Returns: 213 | The results from the callback, or a timeout exception. 214 | """ 215 | result_placeholder = defer.Deferred() 216 | if timeout: 217 | result_placeholder.addTimeout(timeout, reactor, onTimeoutCancel=self.raise_timeout_exception) 218 | return threads.blockingCallFromThread(reactor, callback, result_placeholder) 219 | 220 | def raise_timeout_exception(self, _result=None, _timeout=None): 221 | """Callback called on timeout. 222 | 223 | Args: 224 | _result: Unused--required by Twister. 225 | _timeout: Unused--required by Twister. 226 | 227 | Raises: 228 | An exception. 229 | """ 230 | raise Exception('No service response received') 231 | 232 | def get_inner_callback(self, result_placeholder): 233 | """Get the callback which, when called, provides result_placeholder with the result. 234 | 235 | Args: 236 | result_placeholder: (:obj: Deferred): Object in which to store the result. 237 | 238 | Returns: 239 | A callable which provides result_placeholder with the result in the case of success. 240 | """ 241 | def inner_callback(result): 242 | result_placeholder.callback({'result': result}) 243 | return inner_callback 244 | 245 | def get_inner_errback(self, result_placeholder): 246 | """Get the errback which, when called, provides result_placeholder with the error. 247 | 248 | Args: 249 | result_placeholder: (:obj: Deferred): Object in which to store the result. 250 | 251 | Returns: 252 | A callable which provides result_placeholder with the error in the case of failure. 253 | """ 254 | def inner_errback(error): 255 | result_placeholder.callback({'exception': error}) 256 | return inner_errback 257 | 258 | def terminate(self): 259 | """Signals the termination of the main event loop.""" 260 | if reactor.running: 261 | reactor.stop() 262 | 263 | self._log_observer.stop() 264 | -------------------------------------------------------------------------------- /src/roslibpy/actionlib.py: -------------------------------------------------------------------------------- 1 | r""" 2 | Actionlib 3 | ---------- 4 | 5 | 另一个与 ROS 建立通讯的方法是通过 **actionlib** 栈。\ 6 | ROS 中的 Actions 允许执行可抢占任务，即可以被客户端中断的任务。 7 | 8 | Actions 通过\ :class:`ActionClient`\ 类来使用，它可以添加\ :class:`Goals `\ 类。\ 9 | 每个 goal 都可以发射出可侦听的事件，以便接收者对来自 Action 服务器的更新作出反应。\ 10 | 可被发射的事件有四种：\ **status**\ ，\ **result**\ ，\ **feedback** 和 **timeout**\ 。 11 | 12 | .. autoclass:: Goal 13 | :members: 14 | .. autoclass:: ActionClient 15 | :members: 16 | .. autoclass:: SimpleActionServer 17 | :members: 18 | .. autoclass:: GoalStatus 19 | :members: 20 | 21 | """ 22 | from __future__ import print_function 23 | 24 | import logging 25 | import math 26 | import random 27 | import threading 28 | import time 29 | 30 | from . import Message 31 | from . import Topic 32 | from .event_emitter import EventEmitterMixin 33 | 34 | __all__ = ['Goal', 'GoalStatus', 'ActionClient', 'SimpleActionServer'] 35 | 36 | 37 | LOGGER = logging.getLogger('roslibpy') 38 | DEFAULT_CONNECTION_TIMEOUT = 3 # in seconds 39 | 40 | 41 | def _is_earlier(t1, t2): 42 | """Compares two timestamps.""" 43 | if t1['secs'] > t2['secs']: 44 | return False 45 | elif t1['secs'] < t2['secs']: 46 | return True 47 | elif t1['nsecs'] < t2['nsecs']: 48 | return True 49 | 50 | return False 51 | 52 | 53 | class GoalStatus: 54 | """Valid goal statuses.""" 55 | PENDING = 0 56 | ACTIVE = 1 57 | PREEMPTED = 2 58 | SUCCEEDED = 3 59 | ABORTED = 4 60 | REJECTED = 5 61 | PREEMPTING = 6 62 | RECALLING = 7 63 | RECALLED = 8 64 | LOST = 9 65 | 66 | 67 | class Goal(EventEmitterMixin): 68 | """Goal for an action server. 69 | 70 | After an event has been added to an action client, it will emit different 71 | events to indicate its progress: 72 | 73 | * ``status``: fires to notify clients on the current state of the goal. 74 | * ``feedback``: fires to send clients periodic auxiliary information of the goal. 75 | * ``result``: fires to send clients the result upon completion of the goal. 76 | * ``timeout``: fires when the goal did not complete in the specified timeout window. 77 | 78 | Args: 79 | action_client (:class:`.ActionClient`): Instance of the action client associated with the goal. 80 | goal_message (:class:`.Message`): Goal for the action server. 81 | """ 82 | 83 | def __init__(self, action_client, goal_message): 84 | super(Goal, self).__init__() 85 | 86 | self.action_client = action_client 87 | self.goal_message = goal_message 88 | self.goal_id = 'goal_%s_%d' % (random.random(), time.time() * 1000) 89 | 90 | self.wait_result = threading.Event() 91 | self.result = None 92 | self.status = None 93 | self.feedback = None 94 | 95 | self.goal_message = Message({ 96 | 'goal_id': { 97 | 'stamp': { 98 | 'secs': 0, 99 | 'nsecs': 0 100 | }, 101 | 'id': self.goal_id 102 | }, 103 | 'goal': dict(self.goal_message) 104 | }) 105 | 106 | self.action_client.add_goal(self) 107 | 108 | self.on('status', self._set_status) 109 | self.on('result', self._set_result) 110 | self.on('feedback', self._set_feedback) 111 | 112 | def send(self, result_callback=None, timeout=None): 113 | """Send goal to the action server. 114 | 115 | Args: 116 | timeout (:obj:`int`): Timeout for the goal's result expressed in seconds. 117 | callback (:obj:`callable`): Function to be called when a result is received. It is a shorthand for hooking on the ``result`` event. 118 | """ 119 | if result_callback: 120 | self.on('result', result_callback) 121 | 122 | self.action_client.goal_topic.publish(self.goal_message) 123 | if timeout: 124 | self.action_client.ros.call_later(timeout, self._trigger_timeout) 125 | 126 | def cancel(self): 127 | """Cancel the current goal.""" 128 | self.action_client.cancel_topic.publish(Message({'id': self.goal_id})) 129 | 130 | def wait(self, timeout=None): 131 | """Block until the result is available. 132 | 133 | If ``timeout`` is ``None``, it will wait indefinitely. 134 | 135 | Args: 136 | timeout (:obj:`int`): Timeout to wait for the result expressed in seconds. 137 | 138 | Returns: 139 | Result of the goal. 140 | """ 141 | if not self.wait_result.wait(timeout): 142 | raise Exception('Goal failed to receive result') 143 | 144 | return self.result 145 | 146 | def _trigger_timeout(self): 147 | if not self.is_finished: 148 | self.emit('timeout') 149 | 150 | def _set_status(self, status): 151 | self.status = status 152 | 153 | def _set_result(self, result): 154 | self.result = result 155 | self.wait_result.set() 156 | 157 | def _set_feedback(self, feedback): 158 | self.feedback = feedback 159 | 160 | @property 161 | def is_finished(self): 162 | """Indicate if the goal is finished or not. 163 | 164 | Returns: 165 | bool: True if finished, False otherwise. 166 | """ 167 | return self.result is not None 168 | 169 | 170 | class ActionClient(EventEmitterMixin): 171 | """Client to use ROS actions. 172 | 173 | Args: 174 | ros (:class:`.Ros`): Instance of the ROS connection. 175 | server_name (:obj:`str`): Action server name, e.g. ``/fibonacci``. 176 | action_name (:obj:`str`): Action message name, e.g. ``actionlib_tutorials/FibonacciAction``. 177 | timeout (:obj:`int`): **Deprecated.** Connection timeout, expressed in seconds. 178 | """ 179 | 180 | def __init__(self, ros, server_name, action_name, timeout=None, 181 | omit_feedback=False, omit_status=False, omit_result=False): 182 | super(ActionClient, self).__init__() 183 | self.ros = ros 184 | self.server_name = server_name 185 | self.action_name = action_name 186 | self.omit_feedback = omit_feedback 187 | self.omit_status = omit_status 188 | self.omit_result = omit_result 189 | self.goals = {} 190 | 191 | # Create the topics associated with actionlib 192 | self.feedback_listener = Topic(ros, server_name + '/feedback', action_name + 'Feedback') 193 | self.status_listener = Topic(ros, server_name + '/status', 'actionlib_msgs/GoalStatusArray') 194 | self.result_listener = Topic(ros, server_name + '/result', action_name + 'Result') 195 | self.goal_topic = Topic(ros, server_name + '/goal', action_name + 'Goal') 196 | self.cancel_topic = Topic(ros, server_name + '/cancel', 'actionlib_msgs/GoalID') 197 | 198 | # Advertise the goal and cancel topics 199 | self.goal_topic.advertise() 200 | self.cancel_topic.advertise() 201 | 202 | # Subscribe to the status topic 203 | if not self.omit_status: 204 | self.status_listener.subscribe(self._on_status_message) 205 | 206 | # Subscribe to the feedback topic 207 | if not self.omit_feedback: 208 | self.feedback_listener.subscribe(self._on_feedback_message) 209 | 210 | # Subscribe to the result topic 211 | if not self.omit_result: 212 | self.result_listener.subscribe(self._on_result_message) 213 | 214 | if timeout: 215 | LOGGER.warn( 216 | 'Deprecation warning: timeout parameter is ignored, and replaced by the DEFAULT_CONNECTION_TIMEOUT constant.') 217 | 218 | self.wait_status = threading.Event() 219 | 220 | if not self.wait_status.wait(DEFAULT_CONNECTION_TIMEOUT): 221 | raise Exception('Action client failed to connect, no status received.') 222 | 223 | def _on_status_message(self, message): 224 | self.wait_status.set() 225 | 226 | for status in message['status_list']: 227 | goal_id = status['goal_id']['id'] 228 | goal = self.goals.get(goal_id, None) 229 | 230 | if goal: 231 | goal.emit('status', status) 232 | 233 | def _on_feedback_message(self, message): 234 | goal_id = message['status']['goal_id']['id'] 235 | goal = self.goals.get(goal_id, None) 236 | 237 | if goal: 238 | goal.emit('status', message['status']) 239 | goal.emit('feedback', message['feedback']) 240 | 241 | def _on_result_message(self, message): 242 | goal_id = message['status']['goal_id']['id'] 243 | goal = self.goals.get(goal_id, None) 244 | 245 | if goal: 246 | goal.emit('status', message['status']) 247 | goal.emit('result', message['result']) 248 | 249 | def add_goal(self, goal): 250 | """Add a goal to this action client. 251 | 252 | Args: 253 | goal (:class:`.Goal`): Goal to add. 254 | """ 255 | self.goals[goal.goal_id] = goal 256 | 257 | def cancel(self): 258 | """Cancel all goals associated with this action client.""" 259 | self.cancel_topic.publish(Message()) 260 | 261 | def dispose(self): 262 | """Unsubscribe and unadvertise all topics associated with this action client.""" 263 | self.goal_topic.unadvertise() 264 | self.cancel_topic.unadvertise() 265 | 266 | if not self.omit_status: 267 | self.status_listener.unsubscribe() 268 | if not self.omit_feedback: 269 | self.feedback_listener.unsubscribe() 270 | if not self.omit_result: 271 | self.result_listener.unsubscribe() 272 | 273 | 274 | class SimpleActionServer(EventEmitterMixin): 275 | """Implementation of the simple action server. 276 | 277 | The server emits the following events: 278 | 279 | * ``goal``: fires when a new goal has been received by the server. 280 | * ``cancel``: fires when the client has requested the cancellation of the action. 281 | 282 | Args: 283 | ros (:class:`.Ros`): Instance of the ROS connection. 284 | server_name (:obj:`str`): Action server name, e.g. ``/fibonacci``. 285 | action_name (:obj:`str`): Action message name, e.g. ``actionlib_tutorials/FibonacciAction``. 286 | """ 287 | STATUS_PUBLISH_INTERVAL = 0.5 # In seconds 288 | 289 | def __init__(self, ros, server_name, action_name): 290 | super(SimpleActionServer, self).__init__() 291 | 292 | self.ros = ros 293 | self.server_name = server_name 294 | self.action_name = action_name 295 | self._lock = threading.Lock() 296 | 297 | # Create all required publishers and listeners 298 | self.feedback_publisher = Topic(ros, server_name + '/feedback', action_name + 'Feedback') 299 | self.status_publisher = Topic(ros, server_name + '/status', 'actionlib_msgs/GoalStatusArray') 300 | self.result_publisher = Topic(ros, server_name + '/result', action_name + 'Result') 301 | self.goal_listener = Topic(ros, server_name + '/goal', action_name + 'Goal') 302 | self.cancel_listener = Topic(ros, server_name + '/cancel', 'actionlib_msgs/GoalID') 303 | 304 | # Advertise all publishers 305 | self.feedback_publisher.advertise() 306 | self.status_publisher.advertise() 307 | self.result_publisher.advertise() 308 | 309 | # Track the goals and their status in order to publish status 310 | self.status_message = Message(dict( 311 | header=dict( 312 | stamp=dict(secs=0, nsecs=100), 313 | frame_id='' 314 | ), 315 | status_list=[] 316 | )) 317 | 318 | # Needed for handling preemption prompted by a new goal being received 319 | self.current_goal = None # currently tracked goal 320 | self.next_goal = None # the one that'll be preempting 321 | self.preempt_request = False 322 | 323 | self.goal_listener.subscribe(self._on_goal_message) 324 | self.cancel_listener.subscribe(self._on_cancel_message) 325 | 326 | # Intentionally not publishing immediately and instead 327 | # waiting one interval for the first message 328 | self.ros.call_later(self.STATUS_PUBLISH_INTERVAL, self._publish_status) 329 | 330 | def start(self, action_callback): 331 | """Start the action server. 332 | 333 | Args: 334 | action_callback: Callable function to be invoked when a new goal is received. It takes one paramter containing the goal message. 335 | """ 336 | LOGGER.info('Action server {} started'.format(self.server_name)) 337 | 338 | def _internal_goal_callback(goal): 339 | LOGGER.info('Action server {} received new goal'.format(self.server_name)) 340 | self.ros.call_in_thread(lambda: action_callback(goal)) 341 | 342 | def _internal_preempt_callback(): 343 | LOGGER.info('Action server {} received preemption request'.format(self.server_name)) 344 | self.preempt_request = True 345 | 346 | self.on('goal', _internal_goal_callback) 347 | self.on('cancel', _internal_preempt_callback) 348 | 349 | def _publish_status(self): 350 | # Status publishing is required for clients to know they've connected 351 | with self._lock: 352 | current_time = time.time() 353 | secs = int(math.floor(current_time)) 354 | nsecs = int(round(1e9 * (current_time - secs))) 355 | 356 | self.status_message['header']['stamp']['secs'] = secs 357 | self.status_message['header']['stamp']['nsecs'] = nsecs 358 | self.status_publisher.publish(self.status_message) 359 | 360 | # Invoke again in the defined interval 361 | self.ros.call_later(self.STATUS_PUBLISH_INTERVAL, self._publish_status) 362 | 363 | def _on_goal_message(self, message): 364 | will_cancel = False 365 | will_emit_goal = None 366 | 367 | with self._lock: 368 | if self.current_goal: 369 | self.next_goal = message 370 | # needs to happen AFTER rest is set up 371 | will_cancel = True 372 | else: 373 | self.status_message['status_list'] = [ 374 | dict(goal_id=message['goal_id'], status=GoalStatus.ACTIVE)] 375 | self.current_goal = message 376 | will_emit_goal = message['goal'] 377 | 378 | if will_cancel: 379 | self.emit('cancel') 380 | if will_emit_goal: 381 | self.emit('goal', will_emit_goal) 382 | 383 | def _on_cancel_message(self, message): 384 | # As described in the comments of the original roslibjs code 385 | # this may be more complicated than necessary, since it's 386 | # not sure the callbacks can ever wind up with a scenario 387 | # where we've been preempted by a next goal, it hasn't finished 388 | # processing, and then we get a cancel message. 389 | 390 | will_cancel = False 391 | 392 | with self._lock: 393 | message_id = message['id'] 394 | message_stamp = message['stamp'] 395 | secs = message['stamp']['secs'] 396 | 397 | if secs == 0 and secs == 0 and message_id == '': 398 | self.next_goal = None 399 | if self.current_goal: 400 | will_cancel = True 401 | 402 | else: # treat id and stamp independently 403 | if self.current_goal and message_id == self.current_goal['goal_id']['id']: 404 | will_cancel = True 405 | elif self.next_goal and message_id == self.next_goal['goal_id']['id']: 406 | self.next_goal = None 407 | 408 | if self.next_goal and _is_earlier(self.next_goal['goal_id']['stamp'], message_stamp): 409 | self.next_goal = None 410 | if self.current_goal and _is_earlier(self.current_goal['goal_id']['stamp'], message_stamp): 411 | will_cancel = True 412 | 413 | if will_cancel: 414 | self.emit('cancel') 415 | 416 | def is_preempt_requested(self): 417 | """Indicate whether the client has requested preemption of the current goal.""" 418 | with self._lock: 419 | return self.preempt_request 420 | 421 | def set_succeeded(self, result): 422 | """Set the current action state to succeeded. 423 | 424 | Args: 425 | result (:obj:`dict`): Dictionary of key/values to set as the result of the action. 426 | """ 427 | LOGGER.info( 428 | 'Action server {} setting current goal to SUCCEEDED'.format(self.server_name)) 429 | 430 | with self._lock: 431 | result_message = Message({ 432 | 'status': { 433 | 'goal_id': self.current_goal['goal_id'], 434 | 'status': GoalStatus.SUCCEEDED 435 | }, 436 | 'result': result 437 | }) 438 | 439 | self.result_publisher.publish(result_message) 440 | 441 | self.status_message['status_list'] = [] 442 | 443 | if self.next_goal: 444 | self.current_goal = self.next_goal 445 | self.next_goal = None 446 | else: 447 | self.current_goal = None 448 | 449 | self.preempt_request = False 450 | 451 | # If there's a new current goal assigned, emit it 452 | if self.current_goal: 453 | self.emit('goal', self.current_goal['goal']) 454 | 455 | def send_feedback(self, feedback): 456 | """Send feedback. 457 | 458 | Args: 459 | feedback (:obj:`dict`): Dictionary of key/values of the feedback message. 460 | """ 461 | feedback_message = Message({ 462 | 'status': { 463 | 'goal_id': self.current_goal['goal_id'], 464 | 'status': GoalStatus.ACTIVE 465 | }, 466 | 'feedback': feedback 467 | }) 468 | 469 | self.feedback_publisher.publish(feedback_message) 470 | 471 | def set_preempted(self): 472 | """Set the current action to preempted (cancelled).""" 473 | LOGGER.info('Action server {} preempting current goal'.format(self.server_name)) 474 | 475 | with self._lock: 476 | self.status_message['status_list'] = [] 477 | result_message = Message({ 478 | 'status': { 479 | 'goal_id': self.current_goal['goal_id'], 480 | 'status': GoalStatus.PREEMPTED 481 | } 482 | }) 483 | 484 | self.result_publisher.publish(result_message) 485 | 486 | if self.next_goal: 487 | self.current_goal = self.next_goal 488 | self.next_goal = None 489 | else: 490 | self.current_goal = None 491 | 492 | # Preemption completed 493 | self.preempt_request = False 494 | 495 | # If there's a new current goal assigned, emit it 496 | if self.current_goal: 497 | self.emit('goal', self.current_goal['goal']) 498 | -------------------------------------------------------------------------------- /src/roslibpy/core.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import json 4 | import logging 5 | 6 | # Python 2/3 compatibility import list 7 | try: 8 | from collections import UserDict 9 | except ImportError: 10 | from UserDict import UserDict 11 | 12 | LOGGER = logging.getLogger('roslibpy') 13 | 14 | __all__ = ['Message', 15 | 'ServiceRequest', 16 | 'ServiceResponse', 17 | 'Topic', 18 | 'Service', 19 | 'Param'] 20 | 21 | 22 | class Message(UserDict): 23 | """Message objects used for publishing and subscribing to/from topics. 24 | 25 | A message is fundamentally a dictionary and behaves as one.""" 26 | 27 | def __init__(self, values=None): 28 | self.data = {} 29 | if values is not None: 30 | self.update(values) 31 | 32 | 33 | class ServiceRequest(UserDict): 34 | """Request for a service call.""" 35 | 36 | def __init__(self, values=None): 37 | self.data = {} 38 | if values is not None: 39 | self.update(values) 40 | 41 | 42 | class ServiceResponse(UserDict): 43 | """Response returned from a service call.""" 44 | 45 | def __init__(self, values=None): 46 | self.data = {} 47 | if values is not None: 48 | self.update(values) 49 | 50 | 51 | class Topic(object): 52 | """Publish and/or subscribe to a topic in ROS. 53 | 54 | Args: 55 | ros (:class:`.Ros`): Instance of the ROS connection. 56 | name (:obj:`str`): Topic name, e.g. ``/cmd_vel``. 57 | message_type (:obj:`str`): Message type, e.g. ``std_msgs/String``. 58 | compression (:obj:`str`): Type of compression to use, e.g. `png`. Defaults to `None`. 59 | throttle_rate (:obj:`int`): Rate (in ms between messages) at which to throttle the topics. 60 | queue_size (:obj:`int`): Queue size created at bridge side for re-publishing webtopics. 61 | latch (:obj:`bool`): True to latch the topic when publishing, False otherwise. 62 | queue_length (:obj:`int`): Queue length at bridge side used when subscribing. 63 | reconnect_on_close (:obj:`bool`): Reconnect the topic (both for publisher and subscribers) if a reconnection is detected. 64 | """ 65 | SUPPORTED_COMPRESSION_TYPES = ('png', 'none') 66 | 67 | def __init__(self, ros, name, message_type, compression=None, latch=False, throttle_rate=0, 68 | queue_size=100, queue_length=0, reconnect_on_close=True): 69 | self.ros = ros 70 | self.name = name 71 | self.message_type = message_type 72 | self.compression = compression 73 | self.latch = latch 74 | self.throttle_rate = throttle_rate 75 | self.queue_size = queue_size 76 | self.queue_length = queue_length 77 | 78 | self._subscribe_id = None 79 | self._advertise_id = None 80 | 81 | if self.compression is None: 82 | self.compression = 'none' 83 | 84 | if self.compression not in self.SUPPORTED_COMPRESSION_TYPES: 85 | raise ValueError( 86 | 'Unsupported compression type. Must be one of: ' + str(self.SUPPORTED_COMPRESSION_TYPES)) 87 | 88 | self.reconnect_on_close = reconnect_on_close 89 | 90 | @property 91 | def is_advertised(self): 92 | """Indicate if the topic is currently advertised or not. 93 | 94 | Returns: 95 | bool: True if advertised as publisher of this topic, False otherwise. 96 | """ 97 | return self._advertise_id is not None 98 | 99 | @property 100 | def is_subscribed(self): 101 | """Indicate if the topic is currently subscribed or not. 102 | 103 | Returns: 104 | bool: True if subscribed to this topic, False otherwise. 105 | """ 106 | return self._subscribe_id is not None 107 | 108 | def subscribe(self, callback): 109 | """Register a subscription to the topic. 110 | 111 | Every time a message is published for the given topic, 112 | the callback will be called with the message object. 113 | 114 | Args: 115 | callback: Function to be called when messages of this topic are published. 116 | """ 117 | # Avoid duplicate subscription 118 | if self._subscribe_id: 119 | return 120 | 121 | self._subscribe_id = 'subscribe:%s:%d' % ( 122 | self.name, self.ros.id_counter) 123 | 124 | self.ros.on(self.name, callback) 125 | self._connect_topic(Message({ 126 | 'op': 'subscribe', 127 | 'id': self._subscribe_id, 128 | 'type': self.message_type, 129 | 'topic': self.name, 130 | 'compression': self.compression, 131 | 'throttle_rate': self.throttle_rate, 132 | 'queue_length': self.queue_length 133 | })) 134 | 135 | def unsubscribe(self): 136 | """Unregister from a subscribed the topic.""" 137 | if not self._subscribe_id: 138 | return 139 | 140 | # Do not try to reconnect when manually unsubscribing 141 | if self.reconnect_on_close: 142 | self.ros.off('close', self._reconnect_topic) 143 | 144 | self.ros.off(self.name) 145 | self.ros.send_on_ready(Message({ 146 | 'op': 'unsubscribe', 147 | 'id': self._subscribe_id, 148 | 'topic': self.name 149 | })) 150 | self._subscribe_id = None 151 | 152 | def publish(self, message): 153 | """Publish a message to the topic. 154 | 155 | Args: 156 | message (:class:`.Message`): ROS Bridge Message to publish. 157 | """ 158 | if not self.is_advertised: 159 | self.advertise() 160 | 161 | self.ros.send_on_ready(Message({ 162 | 'op': 'publish', 163 | 'id': 'publish:%s:%d' % (self.name, self.ros.id_counter), 164 | 'topic': self.name, 165 | 'msg': dict(message), 166 | 'latch': self.latch 167 | })) 168 | 169 | def advertise(self): 170 | """Register as a publisher for the topic.""" 171 | if self.is_advertised: 172 | return 173 | 174 | self._advertise_id = 'advertise:%s:%d' % ( 175 | self.name, self.ros.id_counter) 176 | 177 | self._connect_topic(Message({ 178 | 'op': 'advertise', 179 | 'id': self._advertise_id, 180 | 'type': self.message_type, 181 | 'topic': self.name, 182 | 'latch': self.latch, 183 | 'queue_size': self.queue_size 184 | })) 185 | 186 | if not self.reconnect_on_close: 187 | self.ros.on('close', self._reset_advertise_id) 188 | 189 | def _reset_advertise_id(self, _proto): 190 | self._advertise_id = None 191 | 192 | def _connect_topic(self, message): 193 | self._connect_message = message 194 | self.ros.send_on_ready(message) 195 | 196 | if self.reconnect_on_close: 197 | self.ros.on('close', self._reconnect_topic) 198 | 199 | def _reconnect_topic(self, _proto): 200 | # Delay a bit the event hookup because 201 | # 1) _proto is not yet nullified, and 202 | # 2) reconnect anyway takes a few seconds 203 | self.ros.call_later(1, lambda: self.ros.send_on_ready(self._connect_message)) 204 | 205 | def unadvertise(self): 206 | """Unregister as a publisher for the topic.""" 207 | if not self.is_advertised: 208 | return 209 | 210 | # Do not try to reconnect when manually unadvertising 211 | if self.reconnect_on_close: 212 | self.ros.off('close', self._reconnect_topic) 213 | 214 | self.ros.send_on_ready(Message({ 215 | 'op': 'unadvertise', 216 | 'id': self._advertise_id, 217 | 'topic': self.name, 218 | })) 219 | 220 | self._advertise_id = None 221 | 222 | 223 | class Service(object): 224 | """Client/server of ROS services. 225 | 226 | This class can be used both to consume other ROS services as a client, 227 | or to provide ROS services as a server. 228 | 229 | Args: 230 | ros (:class:`.Ros`): Instance of the ROS connection. 231 | name (:obj:`str`): Service name, e.g. ``/add_two_ints``. 232 | service_type (:obj:`str`): Service type, e.g. ``rospy_tutorials/AddTwoInts``. 233 | """ 234 | 235 | def __init__(self, ros, name, service_type): 236 | self.ros = ros 237 | self.name = name 238 | self.service_type = service_type 239 | 240 | self._service_callback = None 241 | self._is_advertised = False 242 | 243 | @property 244 | def is_advertised(self): 245 | """Service servers are registered as advertised on ROS. 246 | 247 | This class can be used to be a service client or a server. 248 | 249 | Returns: 250 | bool: True if this is a server, False otherwise. 251 | """ 252 | return self._is_advertised 253 | 254 | def call(self, request, callback=None, errback=None, timeout=None): 255 | """Start a service call. 256 | 257 | Note: 258 | The service can be used either as blocking or non-blocking. 259 | If the ``callback`` parameter is ``None``, then the call will 260 | block until receiving a response. Otherwise, the service response 261 | will be returned in the callback. 262 | 263 | Args: 264 | request (:class:`.ServiceRequest`): Service request. 265 | callback: Callback invoked on successful execution. 266 | errback: Callback invoked on error. 267 | timeout: Timeout for the operation, in seconds. Only used if blocking. 268 | 269 | Returns: 270 | object: Service response if used as a blocking call, otherwise ``None``. 271 | """ 272 | if self.is_advertised: 273 | return 274 | 275 | service_call_id = 'call_service:%s:%d' % ( 276 | self.name, self.ros.id_counter) 277 | 278 | message = Message({ 279 | 'op': 'call_service', 280 | 'id': service_call_id, 281 | 'service': self.name, 282 | 'args': dict(request), 283 | }) 284 | 285 | # Non-blocking mode 286 | if callback: 287 | self.ros.call_async_service(message, callback, errback) 288 | return 289 | 290 | # Blocking mode 291 | call_results = self.ros.call_sync_service(message, timeout) 292 | if 'exception' in call_results: 293 | raise Exception(call_results['exception']) 294 | 295 | return call_results['result'] 296 | 297 | def advertise(self, callback): 298 | """Start advertising the service. 299 | 300 | This turns the instance from a client into a server. The callback will be 301 | invoked with every request that is made to the service. 302 | 303 | If the service is already advertised, this call does nothing. 304 | 305 | Args: 306 | callback: Callback invoked on every service call. It should accept two parameters: `service_request` and 307 | `service_response`. It should return `True` if executed correctly, otherwise `False`. 308 | """ 309 | if self.is_advertised: 310 | return 311 | 312 | if not callable(callback): 313 | raise ValueError('Callback is not a valid callable') 314 | 315 | self._service_callback = callback 316 | self.ros.on(self.name, self._service_response_handler) 317 | self.ros.send_on_ready(Message({ 318 | 'op': 'advertise_service', 319 | 'type': self.service_type, 320 | 'service': self.name 321 | })) 322 | self._is_advertised = True 323 | 324 | def unadvertise(self): 325 | """Unregister as a service server.""" 326 | if not self.is_advertised: 327 | return 328 | 329 | self.ros.send_on_ready(Message({ 330 | 'op': 'unadvertise_service', 331 | 'service': self.name, 332 | })) 333 | self.ros.off(self.name, self._service_response_handler) 334 | 335 | self._is_advertised = False 336 | 337 | def _service_response_handler(self, request): 338 | response = ServiceResponse() 339 | success = self._service_callback(request['args'], response) 340 | 341 | call = Message({'op': 'service_response', 342 | 'service': self.name, 343 | 'values': dict(response), 344 | 'result': success 345 | }) 346 | 347 | if 'id' in request: 348 | call['id'] = request['id'] 349 | 350 | self.ros.send_on_ready(call) 351 | 352 | 353 | class Param(object): 354 | """A ROS parameter. 355 | 356 | Args: 357 | ros (:class:`.Ros`): Instance of the ROS connection. 358 | name (:obj:`str`): Parameter name, e.g. ``max_vel_x``. 359 | """ 360 | 361 | def __init__(self, ros, name): 362 | self.ros = ros 363 | self.name = name 364 | 365 | def get(self, callback=None, errback=None, timeout=None): 366 | """Fetch the current value of the parameter. 367 | 368 | Note: 369 | This method can be used either as blocking or non-blocking. 370 | If the ``callback`` parameter is ``None``, the call will 371 | block and return the parameter value. Otherwise, the parameter 372 | value will be passed on to the callback. 373 | 374 | Args: 375 | callback: Callable function to be invoked when the operation is completed. 376 | errback: Callback invoked on error. 377 | timeout: Timeout for the operation, in seconds. Only used if blocking. 378 | 379 | Returns: 380 | object: Parameter value if used as a blocking call, otherwise ``None``. 381 | """ 382 | client = Service(self.ros, '/rosapi/get_param', 'rosapi/GetParam') 383 | request = ServiceRequest({'name': self.name}) 384 | 385 | if not callback: 386 | result = client.call(request, timeout=timeout) 387 | return json.loads(result['value']) 388 | else: 389 | client.call(request, lambda result: callback( 390 | json.loads(result['value'])), errback) 391 | 392 | def set(self, value, callback=None, errback=None, timeout=None): 393 | """Set a new value to the parameter. 394 | 395 | Note: 396 | This method can be used either as blocking or non-blocking. 397 | If the ``callback`` parameter is ``None``, the call will 398 | block until completion. 399 | 400 | Args: 401 | callback: Callable function to be invoked when the operation is completed. 402 | errback: Callback invoked on error. 403 | timeout: Timeout for the operation, in seconds. Only used if blocking. 404 | """ 405 | client = Service(self.ros, '/rosapi/set_param', 'rosapi/SetParam') 406 | request = ServiceRequest( 407 | {'name': self.name, 'value': json.dumps(value)}) 408 | 409 | client.call(request, callback, errback, timeout=timeout) 410 | 411 | def delete(self, callback=None, errback=None, timeout=None): 412 | """Delete the parameter. 413 | 414 | Note: 415 | This method can be used either as blocking or non-blocking. 416 | If the ``callback`` parameter is ``None``, the call will 417 | block until completion. 418 | 419 | Args: 420 | callback: Callable function to be invoked when the operation is completed. 421 | errback: Callback invoked on error. 422 | timeout: Timeout for the operation, in seconds. Only used if blocking. 423 | """ 424 | client = Service(self.ros, '/rosapi/delete_param', 425 | 'rosapi/DeleteParam') 426 | request = ServiceRequest({'name': self.name}) 427 | 428 | client.call(request, callback, errback, timeout=timeout) 429 | 430 | 431 | if __name__ == '__main__': 432 | 433 | import time 434 | from . import Ros 435 | 436 | FORMAT = '%(asctime)-15s [%(levelname)s] %(message)s' 437 | logging.basicConfig(level=logging.DEBUG, format=FORMAT) 438 | 439 | ros_client = Ros('127.0.0.1', 9090) 440 | 441 | def run_subscriber_example(): 442 | listener = Topic(ros_client, '/chatter', 'std_msgs/String') 443 | listener.subscribe(lambda message: LOGGER.info( 444 | 'Received message on: %s', message['data'])) 445 | 446 | def run_unsubscriber_example(): 447 | listener = Topic(ros_client, '/chatter', 'std_msgs/String') 448 | 449 | def print_message(message): 450 | LOGGER.info('Received message on: %s', message['data']) 451 | 452 | listener.subscribe(print_message) 453 | 454 | ros_client.call_later(5, lambda: listener.unsubscribe()) 455 | ros_client.call_later(10, lambda: listener.subscribe(print_message)) 456 | 457 | def run_publisher_example(): 458 | publisher = Topic(ros_client, '/chatter', 459 | 'std_msgs/String', compression='png') 460 | 461 | def start_sending(): 462 | i = 0 463 | while ros_client.is_connected and i < 5: 464 | i += 1 465 | message = Message({'data': 'test'}) 466 | LOGGER.info('Publishing message to /chatter. %s', message) 467 | publisher.publish(message) 468 | 469 | time.sleep(0.75) 470 | 471 | publisher.unadvertise() 472 | 473 | ros_client.on_ready(start_sending, run_in_thread=True) 474 | 475 | def run_service_example(): 476 | def h1(x): 477 | print('ok', x) 478 | 479 | def h2(x): 480 | print('error', x) 481 | 482 | service = Service(ros_client, '/turtle1/teleport_relative', 483 | 'turtlesim/TeleportRelative') 484 | service.call(ServiceRequest({'linear': 2, 'angular': 2}), h1, h2) 485 | 486 | def run_turtle_subscriber_example(): 487 | listener = Topic(ros_client, '/turtle1/pose', 488 | 'turtlesim/Pose', throttle_rate=500) 489 | 490 | def print_message(message): 491 | LOGGER.info('Received message on: %s', message) 492 | 493 | listener.subscribe(print_message) 494 | 495 | def run_get_example(): 496 | param = Param(ros_client, 'run_id') 497 | param.get(print) 498 | 499 | def run_set_example(): 500 | param = Param(ros_client, 'test_param') 501 | param.set('test_value') 502 | 503 | def run_delete_example(): 504 | param = Param(ros_client, 'test_param') 505 | param.delete() 506 | 507 | def run_server_example(): 508 | service = Service(ros_client, '/test_server', 509 | 'rospy_tutorials/AddTwoInts') 510 | 511 | def dispose_server(): 512 | service.unadvertise() 513 | ros_client.call_later(1, service.ros.terminate) 514 | 515 | def add_two_ints(request, response): 516 | response['sum'] = request['a'] + request['b'] 517 | if response['sum'] == 42: 518 | ros_client.call_later(2, dispose_server) 519 | 520 | return True 521 | 522 | service.advertise(add_two_ints) 523 | 524 | run_server_example() 525 | ros_client.run_forever() 526 | -------------------------------------------------------------------------------- /src/roslibpy/ros.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import logging 4 | import threading 5 | 6 | from . import Message 7 | from . import Param 8 | from . import Service 9 | from . import ServiceRequest 10 | from .comm import RosBridgeClientFactory 11 | 12 | __all__ = ['Ros'] 13 | 14 | LOGGER = logging.getLogger('roslibpy') 15 | CONNECTION_TIMEOUT = 10 16 | ROSAPI_TIMEOUT = 3 17 | 18 | 19 | class Ros(object): 20 | """Connection manager to ROS server. 21 | 22 | Args: 23 | host (:obj:`str`): Name or IP address of the ROS bridge host, e.g. ``127.0.0.1``. 24 | port (:obj:`int`): ROS bridge port, e.g. ``9090``. 25 | is_secure (:obj:`bool`): ``True`` to use a secure web sockets connection, otherwise ``False``. 26 | """ 27 | 28 | def __init__(self, host, port=None, is_secure=False): 29 | self._id_counter = 0 30 | url = RosBridgeClientFactory.create_url(host, port, is_secure) 31 | self.factory = RosBridgeClientFactory(url) 32 | self.is_connecting = False 33 | self.connect() 34 | 35 | @property 36 | def id_counter(self): 37 | """Generate an auto-incremental ID starting from 1. 38 | 39 | Returns: 40 | int: An auto-incremented ID. 41 | """ 42 | self._id_counter += 1 43 | return self._id_counter 44 | 45 | @property 46 | def is_connected(self): 47 | """Indicate if the ROS connection is open or not. 48 | 49 | Returns: 50 | bool: True if connected to ROS, False otherwise. 51 | """ 52 | return self.factory.is_connected 53 | 54 | def connect(self): 55 | """Connect to ROS master.""" 56 | # Don't try to reconnect if already connected. 57 | if self.is_connected or self.is_connecting: 58 | return 59 | 60 | self.is_connecting = True 61 | 62 | def _unset_connecting_flag(*args): 63 | self.is_connecting = False 64 | 65 | self.factory.on_ready(_unset_connecting_flag) 66 | self.factory.connect() 67 | 68 | def close(self): 69 | """Disconnect from ROS master.""" 70 | if self.is_connected: 71 | def _wrapper_callback(proto): 72 | self.emit('closing') 73 | proto.send_close() 74 | return proto 75 | 76 | self.factory.on_ready(_wrapper_callback) 77 | 78 | def run(self, timeout=CONNECTION_TIMEOUT): 79 | """Kick-starts a non-blocking event loop. 80 | 81 | Args: 82 | timeout: Timeout to wait until connection is ready. 83 | """ 84 | self.factory.manager.run() 85 | 86 | wait_connect = threading.Event() 87 | self.factory.on_ready(lambda _: wait_connect.set()) 88 | 89 | if not wait_connect.wait(timeout): 90 | raise Exception('Failed to connect to ROS') 91 | 92 | def run_forever(self): 93 | """Kick-starts a blocking loop to wait for events. 94 | 95 | Depending on the implementations, and the client applications, 96 | running this might be required or not. 97 | """ 98 | self.factory.manager.run_forever() 99 | 100 | def run_event_loop(self): 101 | LOGGER.warn( 102 | 'Deprecation warning: use run_forever instead of run_event_loop ') 103 | self.run_forever() 104 | 105 | def call_in_thread(self, callback): 106 | """Call the given function in a thread. 107 | 108 | The threading implementation is deferred to the factory. 109 | 110 | Args: 111 | callback (:obj:`callable`): Callable function to be invoked. 112 | """ 113 | self.factory.manager.call_in_thread(callback) 114 | 115 | def call_later(self, delay, callback): 116 | """Call the given function after a certain period of time has passed. 117 | 118 | Args: 119 | delay (:obj:`int`): Number of seconds to wait before invoking the callback. 120 | callback (:obj:`callable`): Callable function to be invoked when ROS connection is ready. 121 | """ 122 | self.factory.manager.call_later(delay, callback) 123 | 124 | def terminate(self): 125 | """Signals the termination of the main event loop.""" 126 | if self.is_connected: 127 | self.close() 128 | 129 | self.factory.manager.terminate() 130 | 131 | def on(self, event_name, callback): 132 | """Add a callback to an arbitrary named event. 133 | 134 | Args: 135 | event_name (:obj:`str`): Name of the event to which to subscribe. 136 | callback: Callable function to be executed when the event is triggered. 137 | """ 138 | self.factory.on(event_name, callback) 139 | 140 | def off(self, event_name, callback=None): 141 | """Remove a callback from an arbitrary named event. 142 | 143 | Args: 144 | event_name (:obj:`str`): Name of the event from which to unsubscribe. 145 | callback: Callable function. If ``None``, all callbacks of the event 146 | will be removed. 147 | """ 148 | if callback: 149 | self.factory.off(event_name, callback) 150 | else: 151 | self.factory.remove_all_listeners(event_name) 152 | 153 | def emit(self, event_name, *args): 154 | """Trigger a named event.""" 155 | self.factory.emit(event_name, *args) 156 | 157 | def on_ready(self, callback, run_in_thread=True): 158 | """Add a callback to be executed when the connection is established. 159 | 160 | If a connection to ROS is already available, the callback is executed immediately. 161 | 162 | Args: 163 | callback: Callable function to be invoked when ROS connection is ready. 164 | run_in_thread (:obj:`bool`): True to run the callback in a separate thread, False otherwise. 165 | """ 166 | def _wrapper_callback(proto): 167 | if run_in_thread: 168 | self.factory.manager.call_in_thread(callback) 169 | else: 170 | callback() 171 | 172 | return proto 173 | 174 | self.factory.on_ready(_wrapper_callback) 175 | 176 | def send_on_ready(self, message): 177 | """Send message to the ROS Master once the connection is established. 178 | 179 | If a connection to ROS is already available, the message is sent immediately. 180 | 181 | Args: 182 | message (:class:`.Message`): ROS Bridge Message to send. 183 | """ 184 | def _send_internal(proto): 185 | proto.send_ros_message(message) 186 | return proto 187 | 188 | self.factory.on_ready(_send_internal) 189 | 190 | def blocking_call_from_thread(self, callback, timeout): 191 | """Call the given function from a thread, and wait for the result synchronously 192 | for as long as the timeout will allow. 193 | 194 | Args: 195 | callback: Callable function to be invoked from the thread. 196 | timeout (:obj: int): Number of seconds to wait for the response before 197 | raising an exception. 198 | 199 | Returns: 200 | The results from the callback, or a timeout exception. 201 | """ 202 | return self.factory.manager.blocking_call_from_thread(callback, timeout) 203 | 204 | def get_service_request_callback(self, message): 205 | """Get the callback which, when called, sends the service request. 206 | 207 | Args: 208 | message (:class:`.Message`): ROS Bridge Message containing the request. 209 | 210 | Returns: 211 | A callable which makes the service request. 212 | """ 213 | def get_call_results(result_placeholder): 214 | 215 | inner_callback = self.factory.manager.get_inner_callback(result_placeholder) 216 | 217 | inner_errback = self.factory.manager.get_inner_errback(result_placeholder) 218 | 219 | self.call_async_service(message, inner_callback, inner_errback) 220 | 221 | return result_placeholder 222 | 223 | return get_call_results 224 | 225 | def call_sync_service(self, message, timeout): 226 | """Send a blocking service request to the ROS Master once the connection is established, 227 | waiting for the result to be return. 228 | 229 | If a connection to ROS is already available, the request is sent immediately. 230 | 231 | Args: 232 | message (:class:`.Message`): ROS Bridge Message containing the request. 233 | timeout (:obj: int): Number of seconds to wait for the response before 234 | raising an exception. 235 | Returns: 236 | Either returns the service request results or raises a timeout exception. 237 | """ 238 | get_call_results = self.get_service_request_callback(message) 239 | return self.blocking_call_from_thread(get_call_results, timeout) 240 | 241 | def call_async_service(self, message, callback, errback): 242 | """Send a service request to the ROS Master once the connection is established. 243 | 244 | If a connection to ROS is already available, the request is sent immediately. 245 | 246 | Args: 247 | message (:class:`.Message`): ROS Bridge Message containing the request. 248 | callback: Callback invoked on successful execution. 249 | errback: Callback invoked on error. 250 | """ 251 | def _send_internal(proto): 252 | proto.send_ros_service_request(message, callback, errback) 253 | return proto 254 | 255 | self.factory.on_ready(_send_internal) 256 | 257 | def set_status_level(self, level, identifier): 258 | level_message = Message({ 259 | 'op': 'set_level', 260 | 'level': level, 261 | 'id': identifier 262 | }) 263 | 264 | self.send_on_ready(level_message) 265 | 266 | def get_topics(self, callback=None, errback=None): 267 | """Retrieve list of topics in ROS. 268 | 269 | Note: 270 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 271 | 272 | Returns: 273 | list: List of topics if blocking, otherwise ``None``. 274 | """ 275 | service = Service(self, '/rosapi/topics', 'rosapi/Topics') 276 | 277 | result = service.call(ServiceRequest(), callback, 278 | errback, timeout=ROSAPI_TIMEOUT) 279 | 280 | if callback: 281 | return 282 | 283 | assert 'topics' in result 284 | return result['topics'] 285 | 286 | def get_topic_type(self, topic, callback=None, errback=None): 287 | """Retrieve the type of a topic in ROS. 288 | 289 | Note: 290 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 291 | 292 | Returns: 293 | str: Topic type if blocking, otherwise ``None``. 294 | """ 295 | service = Service(self, '/rosapi/topic_type', 296 | 'rosapi/TopicType') 297 | 298 | result = service.call(ServiceRequest({'topic': topic}), 299 | callback, errback, timeout=ROSAPI_TIMEOUT) 300 | 301 | if callback: 302 | return 303 | 304 | assert 'type' in result 305 | return result['type'] 306 | 307 | def get_topics_for_type(self, topic_type, callback=None, errback=None): 308 | """Retrieve list of topics in ROS matching the specified type. 309 | 310 | Note: 311 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 312 | 313 | Returns: 314 | list: List of topics matching the specified type if blocking, otherwise ``None``. 315 | """ 316 | service = Service(self, '/rosapi/topics_for_type', 317 | 'rosapi/TopicsForType') 318 | 319 | result = service.call(ServiceRequest({'type': topic_type}), 320 | callback, errback, timeout=ROSAPI_TIMEOUT) 321 | 322 | if callback: 323 | return 324 | 325 | assert 'topics' in result 326 | return result['topics'] 327 | 328 | def get_services(self, callback=None, errback=None): 329 | """Retrieve list of active service names in ROS. 330 | 331 | Note: 332 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 333 | 334 | Returns: 335 | list: List of services if blocking, otherwise ``None``. 336 | """ 337 | service = Service(self, '/rosapi/services', 338 | 'rosapi/Services') 339 | 340 | result = service.call(ServiceRequest(), callback, 341 | errback, timeout=ROSAPI_TIMEOUT) 342 | 343 | if callback: 344 | return 345 | 346 | assert 'services' in result 347 | return result['services'] 348 | 349 | def get_service_type(self, service_name, callback=None, errback=None): 350 | """Retrieve the type of a service in ROS. 351 | 352 | Note: 353 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 354 | 355 | Returns: 356 | str: Service type if blocking, otherwise ``None``. 357 | """ 358 | service = Service(self, '/rosapi/service_type', 359 | 'rosapi/ServiceType') 360 | 361 | result = service.call(ServiceRequest( 362 | {'service': service_name}), callback, errback, timeout=ROSAPI_TIMEOUT) 363 | 364 | if callback: 365 | return 366 | 367 | assert 'type' in result 368 | return result['type'] 369 | 370 | def get_services_for_type(self, service_type, callback=None, errback=None): 371 | """Retrieve list of services in ROS matching the specified type. 372 | 373 | Note: 374 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 375 | 376 | Returns: 377 | list: List of services matching the specified type if blocking, otherwise ``None``. 378 | """ 379 | service = Service(self, '/rosapi/services_for_type', 380 | 'rosapi/ServicesForType') 381 | 382 | result = service.call(ServiceRequest({'type': service_type}), 383 | callback, errback, timeout=ROSAPI_TIMEOUT) 384 | 385 | if callback: 386 | return 387 | 388 | assert 'services' in result 389 | return result['services'] 390 | 391 | def get_service_request_details(self, type, callback=None, errback=None): 392 | """Retrieve details of a ROS Service Request. 393 | 394 | Note: 395 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 396 | 397 | Returns: 398 | Service Request details if blocking, otherwise ``None``. 399 | """ 400 | service = Service(self, '/rosapi/service_request_details', 401 | 'rosapi/ServiceRequestDetails') 402 | 403 | result = service.call(ServiceRequest({'type': type}), 404 | callback, errback, timeout=ROSAPI_TIMEOUT) 405 | 406 | if callback: 407 | return 408 | 409 | return result 410 | 411 | def get_service_response_details(self, type, callback=None, errback=None): 412 | """Retrieve details of a ROS Service Response. 413 | 414 | Note: 415 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 416 | 417 | Returns: 418 | Service Response details if blocking, otherwise ``None``. 419 | """ 420 | service = Service(self, '/rosapi/service_response_details', 421 | 'rosapi/ServiceResponseDetails') 422 | 423 | result = service.call(ServiceRequest({'type': type}), 424 | callback, errback, timeout=ROSAPI_TIMEOUT) 425 | 426 | if callback: 427 | return 428 | 429 | return result 430 | 431 | def get_message_details(self, message_type, callback=None, errback=None): 432 | """Retrieve details of a message type in ROS. 433 | 434 | Note: 435 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 436 | 437 | Returns: 438 | Message type details if blocking, otherwise ``None``. 439 | """ 440 | service = Service(self, '/rosapi/message_details', 441 | 'rosapi/MessageDetails') 442 | 443 | result = service.call(ServiceRequest( 444 | {'type': message_type}), callback, errback, timeout=ROSAPI_TIMEOUT) 445 | 446 | if callback: 447 | return 448 | 449 | return result 450 | 451 | def get_params(self, callback=None, errback=None): 452 | """Retrieve list of param names from the ROS Parameter Server. 453 | 454 | Note: 455 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 456 | 457 | Returns: 458 | list: List of parameters if blocking, otherwise ``None``. 459 | """ 460 | service = Service(self, '/rosapi/get_param_names', 461 | 'rosapi/GetParamNames') 462 | 463 | result = service.call(ServiceRequest(), callback, 464 | errback, timeout=ROSAPI_TIMEOUT) 465 | 466 | if callback: 467 | return 468 | 469 | assert 'names' in result 470 | return result['names'] 471 | 472 | def get_param(self, name, callback=None, errback=None): 473 | """Get the value of a parameter from the ROS Parameter Server. 474 | 475 | Note: 476 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 477 | 478 | Returns: 479 | Parameter value if blocking, otherwise ``None``. 480 | """ 481 | param = Param(self, name) 482 | return param.get(callback, errback, timeout=ROSAPI_TIMEOUT) 483 | 484 | def set_param(self, name, value, callback=None, errback=None): 485 | """Set the value of a parameter from the ROS Parameter Server. 486 | 487 | Note: 488 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 489 | """ 490 | param = Param(self, name) 491 | param.set(value, callback, errback, timeout=ROSAPI_TIMEOUT) 492 | 493 | def delete_param(self, name, callback=None, errback=None): 494 | """Delete parameter from the ROS Parameter Server. 495 | 496 | Note: 497 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 498 | """ 499 | param = Param(self, name) 500 | param.delete(callback, errback, timeout=ROSAPI_TIMEOUT) 501 | 502 | def get_action_servers(self, callback, errback=None): 503 | """Retrieve list of action servers in ROS.""" 504 | service = Service(self, '/rosapi/action_servers', 505 | 'rosapi/GetActionServers') 506 | 507 | service.call(ServiceRequest(), callback, errback) 508 | 509 | def get_nodes(self, callback=None, errback=None): 510 | """Retrieve list of active node names in ROS. 511 | 512 | Note: 513 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 514 | """ 515 | service = Service(self, '/rosapi/nodes', 516 | 'rosapi/Nodes') 517 | 518 | result = service.call(ServiceRequest(), callback, errback) 519 | 520 | if callback: 521 | return 522 | 523 | assert 'nodes' in result 524 | return result['nodes'] 525 | 526 | def get_node_details(self, node, callback=None, errback=None): 527 | """Retrieve list subscribed topics, publishing topics and services of a specific node name. 528 | 529 | Note: 530 | To make this a blocking call, pass ``None`` to the ``callback`` parameter . 531 | """ 532 | service = Service(self, '/rosapi/node_details', 533 | 'rosapi/NodeDetails') 534 | 535 | result = service.call(ServiceRequest({'node': node}), callback, errback) 536 | if callback: 537 | return 538 | 539 | output = { 540 | 'services': result['services'], 541 | 'subscribing': result['subscribing'], 542 | 'publishing': result['publishing'] 543 | } 544 | 545 | return output 546 | 547 | 548 | if __name__ == '__main__': 549 | FORMAT = '%(asctime)-15s [%(levelname)s] %(message)s' 550 | logging.basicConfig(level=logging.DEBUG, format=FORMAT) 551 | 552 | ros_client = Ros('127.0.0.1', 9090) 553 | 554 | ros_client.on_ready(lambda: ros_client.get_topics(print)) 555 | ros_client.call_later(3, ros_client.close) 556 | ros_client.call_later(5, ros_client.terminate) 557 | 558 | ros_client.run_forever() 559 | -------------------------------------------------------------------------------- /src/roslibpy/comm/comm_cli.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import logging 4 | import math 5 | 6 | from System import Action 7 | from System import Array 8 | from System import ArraySegment 9 | from System import Byte 10 | from System import TimeSpan 11 | from System import Uri 12 | from System import UriBuilder 13 | from System.Net.WebSockets import ClientWebSocket 14 | from System.Net.WebSockets import WebSocketCloseStatus 15 | from System.Net.WebSockets import WebSocketError 16 | from System.Net.WebSockets import WebSocketMessageType 17 | from System.Net.WebSockets import WebSocketReceiveResult 18 | from System.Net.WebSockets import WebSocketState 19 | from System.Text import Encoding 20 | from System.Threading import CancellationToken 21 | from System.Threading import CancellationTokenSource 22 | from System.Threading import ManualResetEventSlim 23 | from System.Threading import SemaphoreSlim 24 | from System.Threading import Thread 25 | from System.Threading import ThreadPool 26 | from System.Threading import WaitCallback 27 | from System.Threading.Tasks import Task 28 | 29 | from ..event_emitter import EventEmitterMixin 30 | from . import RosBridgeException 31 | from . import RosBridgeProtocol 32 | 33 | LOGGER = logging.getLogger('roslibpy') 34 | RECEIVE_CHUNK_SIZE = 1024 35 | SEND_CHUNK_SIZE = 1024 36 | 37 | 38 | def _unwrap_exception(task): 39 | exception = task.Exception 40 | if exception.InnerException: 41 | exception = exception.InnerException 42 | 43 | if hasattr(exception, 'WebSocketErrorCode'): 44 | return (exception.WebSocketErrorCode, exception.Message) 45 | 46 | return (None, exception.Message) 47 | 48 | 49 | class CliRosBridgeProtocol(RosBridgeProtocol): 50 | """Implements the ROS Bridge protocol on top of CLI WebSockets. 51 | 52 | This implementation is mainly intended to be used on IronPython 53 | implementations and makes use of the Tasks library of .NET for 54 | most internal scheduling and cancellation signals.""" 55 | def __init__(self, factory, socket, *args, **kwargs): 56 | super(CliRosBridgeProtocol, self).__init__(*args, **kwargs) 57 | self.factory = factory 58 | self.socket = socket 59 | # According to docs, exactly one send and one receive is supported on each ClientWebSocket object in parallel. 60 | # https://msdn.microsoft.com/en-us/library/system.net.websockets.clientwebsocket.receiveasync(v=vs.110).aspx 61 | # So we configure the semaphore to allow for 2 concurrent requests 62 | # User-code might still end up in a race if multiple requests are triggered from different threads 63 | self.semaphore = SemaphoreSlim(2) 64 | 65 | def on_open(self, task): 66 | """Triggered when the socket connection has been established. 67 | 68 | This will kick-start the listening thread.""" 69 | if task.IsFaulted: 70 | err_code, err_desc = _unwrap_exception(task) 71 | self.factory.client_connection_failed(self, err_code, err_desc) 72 | return 73 | 74 | LOGGER.info('Connection to ROS MASTER ready.') 75 | self._manual_disconnect = False 76 | self.factory.ready(self) 77 | self.factory.manager.call_in_thread(self.start_listening) 78 | 79 | def receive_chunk_async(self, task, context): 80 | """Handle the reception of a message chuck asynchronously.""" 81 | try: 82 | if task: 83 | 84 | if task.IsFaulted: 85 | err_code, err_desc = _unwrap_exception(task) 86 | self.factory.client_connection_lost(self, err_code, err_desc) 87 | return 88 | 89 | result = task.Result 90 | 91 | if result.MessageType == WebSocketMessageType.Close: 92 | LOGGER.info('WebSocket connection closed: [Code=%s] Description=%s', 93 | result.CloseStatus, result.CloseStatusDescription) 94 | 95 | try: 96 | # Socket might be already disposed or nullified 97 | if not self.socket: 98 | return 99 | 100 | # If not, we try to be good citizens and finalize the close handshake 101 | return self.socket.CloseOutputAsync(result.CloseStatus, 102 | result.CloseStatusDescription, CancellationToken.None) # noqa: E999 (disable flake8 error, which incorrectly parses None as the python keyword) 103 | except: # noqa: E722 104 | # But it could also fail (eg. the socket was just disposed) we just warn and return then 105 | LOGGER.warn('Unable to send close output. Socket might be already disposed.') 106 | return 107 | else: 108 | chunk = Encoding.UTF8.GetString(context['buffer'], 0, result.Count) 109 | context['content'].append(chunk) 110 | 111 | # Signal the listener thread if we're done parsing chunks 112 | if result.EndOfMessage: 113 | # NOTE: Once we reach the end of the message 114 | # we release the lock (Semaphore) 115 | self.semaphore.Release() 116 | 117 | # And signal the manual reset event 118 | context['mre'].Set() 119 | return task 120 | 121 | # NOTE: We will enter the lock (Semaphore) at the start of receive 122 | # to make sure we're accessing the socket read/writes at most from 123 | # two threads, one for receiving and one for sending 124 | if not task: 125 | self.semaphore.Wait(self.factory.manager.cancellation_token) 126 | 127 | receive_task = self.socket.ReceiveAsync(ArraySegment[Byte]( 128 | context['buffer']), self.factory.manager.cancellation_token) 129 | receive_task.ContinueWith.Overloads[Action[Task[WebSocketReceiveResult], object], object]( 130 | self.receive_chunk_async, context) 131 | except Exception: 132 | error_message = 'Exception on receive_chunk_async, processing will be aborted' 133 | if task: 134 | error_message += '; Task status: {}, Inner exception: {}'.format(task.Status, task.Exception) 135 | LOGGER.exception(error_message) 136 | raise 137 | 138 | def start_listening(self): 139 | """Starts listening asynchronously while the socket is open. 140 | 141 | The inter-thread synchronization between this and the async 142 | reception threads is sync'd with a manual reset event.""" 143 | try: 144 | LOGGER.debug( 145 | 'About to start listening, socket state: %s', self.socket.State) 146 | 147 | while self.socket and self.socket.State == WebSocketState.Open: 148 | mre = ManualResetEventSlim(False) 149 | content = [] 150 | buffer = Array.CreateInstance(Byte, RECEIVE_CHUNK_SIZE) 151 | 152 | self.receive_chunk_async(None, dict( 153 | buffer=buffer, content=content, mre=mre)) 154 | 155 | LOGGER.debug('Waiting for messages...') 156 | try: 157 | mre.Wait(self.factory.manager.cancellation_token) 158 | except SystemError: 159 | LOGGER.debug('Cancellation detected on listening thread, exiting...') 160 | break 161 | 162 | try: 163 | message_payload = ''.join(content) 164 | LOGGER.debug('Message reception completed|
%s
', message_payload) 165 | self.on_message(message_payload) 166 | except Exception: 167 | LOGGER.exception('Exception on start_listening while trying to handle message received.' + 168 | 'It could indicate a bug in user code on message handlers. Message skipped.') 169 | except Exception: 170 | LOGGER.exception( 171 | 'Exception on start_listening, processing will be aborted') 172 | raise 173 | finally: 174 | LOGGER.debug('Leaving the listening thread') 175 | 176 | def send_close(self): 177 | """Trigger the closure of the websocket indicating normal closing process.""" 178 | self._manual_disconnect = True 179 | 180 | err_desc = '' 181 | err_code = WebSocketCloseStatus.NormalClosure 182 | 183 | close_task = self.socket.CloseAsync(err_code, 184 | err_desc, CancellationToken.None) if self.socket else None # noqa: E999 (disable flake8 error, which incorrectly parses None as the python keyword) 185 | 186 | self.factory.client_connection_lost(self, err_code, err_desc) 187 | 188 | return close_task 189 | 190 | def send_chunk_async(self, task, message_data): 191 | """Send a message chuck asynchronously.""" 192 | try: 193 | if not task: 194 | self.semaphore.Wait(self.factory.manager.cancellation_token) 195 | 196 | message_buffer, message_length, chunks_count, i = message_data 197 | 198 | offset = SEND_CHUNK_SIZE * i 199 | is_last_message = (i == chunks_count - 1) 200 | 201 | if is_last_message: 202 | count = message_length - offset 203 | else: 204 | count = SEND_CHUNK_SIZE 205 | 206 | message_chunk = ArraySegment[Byte](message_buffer, offset, count) 207 | LOGGER.debug('Chunk %d of %d|From offset=%d, byte count=%d, Is last=%s', 208 | i + 1, chunks_count, offset, count, str(is_last_message)) 209 | task = self.socket.SendAsync( 210 | message_chunk, WebSocketMessageType.Text, is_last_message, self.factory.manager.cancellation_token) 211 | 212 | if not is_last_message: 213 | task.ContinueWith(self.send_chunk_async, [ 214 | message_buffer, message_length, chunks_count, i + 1]) 215 | else: 216 | # NOTE: If we've reached the last chunk of the message 217 | # we can release the lock (Semaphore) again. 218 | task.ContinueWith(lambda _res: self.semaphore.Release()) 219 | 220 | return task 221 | except Exception: 222 | LOGGER.exception('Exception while on send_chunk_async') 223 | raise 224 | 225 | def send_message(self, payload): 226 | """Start sending a message over the websocket asynchronously.""" 227 | 228 | if self.socket.State != WebSocketState.Open: 229 | raise RosBridgeException( 230 | 'Connection is not open. Socket state: %s' % self.socket.State) 231 | 232 | try: 233 | message_buffer = Encoding.UTF8.GetBytes(payload) 234 | message_length = len(message_buffer) 235 | chunks_count = int(math.ceil(float(message_length) / SEND_CHUNK_SIZE)) 236 | 237 | send_task = self.send_chunk_async( 238 | None, [message_buffer, message_length, chunks_count, 0]) 239 | 240 | return send_task 241 | except Exception: 242 | LOGGER.exception('Exception while sending message') 243 | raise 244 | 245 | def dispose(self, *args): 246 | """Dispose the resources held by this protocol instance, i.e. socket.""" 247 | if self.socket: 248 | self.socket.Dispose() 249 | self.socket = None 250 | LOGGER.debug('Websocket disposed') 251 | 252 | def __del__(self): 253 | """Dispose correctly the connection.""" 254 | self.dispose() 255 | 256 | 257 | class CliRosBridgeClientFactory(EventEmitterMixin): 258 | """Factory to create instances of the ROS Bridge protocol built on top of .NET WebSockets.""" 259 | 260 | max_delay = 3600.0 261 | initial_delay = 1.0 262 | max_retries = None 263 | 264 | # NOTE: The following factor was taken from Twisted's reconnecting factory: 265 | # https://github.com/twisted/twisted/blob/6ac66416c0238f403a8dc1d42924fb3ba2a2a686/src/twisted/internet/protocol.py#L369 266 | factor = 2.7182818284590451 # (math.e) 267 | 268 | def __init__(self, url, *args, **kwargs): 269 | super(CliRosBridgeClientFactory, self).__init__(*args, **kwargs) 270 | self._manager = CliEventLoopManager() 271 | self.proto = None 272 | self.url = url 273 | self.delay = self.initial_delay 274 | self.retries = 0 275 | 276 | @property 277 | def is_connected(self): 278 | """Indicate if the WebSocket connection is open or not. 279 | 280 | Returns: 281 | bool: True if WebSocket is connected, False otherwise. 282 | """ 283 | if self.proto and self.proto.socket: 284 | return self.proto.socket.State == WebSocketState.Open 285 | 286 | return False 287 | 288 | def connect(self): 289 | """Establish WebSocket connection to the ROS server defined for this factory. 290 | 291 | Returns: 292 | async_task: The async task for the connection. 293 | """ 294 | LOGGER.debug('Started to connect...') 295 | socket = ClientWebSocket() 296 | connect_task = socket.ConnectAsync( 297 | self.url, self.manager.cancellation_token) 298 | 299 | protocol = CliRosBridgeProtocol(self, socket) 300 | connect_task.ContinueWith(protocol.on_open) 301 | 302 | return connect_task 303 | 304 | def reset_delay(self): 305 | """ 306 | Call this method after a successful connection to reset the delay. 307 | """ 308 | self.delay = self.initial_delay 309 | 310 | def _reconnect_if_needed(self): 311 | if self.proto and self.proto._manual_disconnect: 312 | return 313 | 314 | if self.max_retries is not None and (self.retries >= self.max_retries): 315 | LOGGER.info('Abandonning after {} retries'.format(self.retries)) 316 | return 317 | 318 | self.retries += 1 319 | self.delay = min(self.delay * self.factor, self.max_delay) 320 | LOGGER.info('Connection manager will retry in {} seconds'.format(int(self.delay))) 321 | 322 | self.manager.call_later(self.delay, self.connect) 323 | 324 | def client_connection_lost(self, connector, reason_code, reason_description): 325 | LOGGER.debug('Lost connection. Code: %s, Reason: %s', reason_code, reason_description) 326 | self.emit('close', self.proto) 327 | self._reconnect_if_needed() 328 | 329 | if self.proto: 330 | self.proto.dispose() 331 | self.proto = None 332 | 333 | def client_connection_failed(self, connector, reason_code, reason_description): 334 | LOGGER.debug('Connection failed. Reason: %s', reason_description) 335 | self._reconnect_if_needed() 336 | 337 | if self.proto: 338 | self.proto.dispose() 339 | self.proto = None 340 | 341 | def ready(self, proto): 342 | self.proto = proto 343 | self.reset_delay() 344 | self.emit('ready', proto) 345 | 346 | def on_ready(self, callback): 347 | if self.proto: 348 | callback(self.proto) 349 | else: 350 | self.once('ready', callback) 351 | 352 | @property 353 | def manager(self): 354 | """Get an instance of the event loop manager for this factory.""" 355 | return self._manager 356 | 357 | @classmethod 358 | def create_url(cls, host, port=None, is_secure=False): 359 | if port is None: 360 | return Uri(host) 361 | else: 362 | scheme = 'wss' if is_secure else 'ws' 363 | builder = UriBuilder(scheme, host, port) 364 | return builder.Uri 365 | 366 | @classmethod 367 | def set_max_delay(cls, max_delay): 368 | """Set the maximum delay in seconds for reconnecting to rosbridge (3600 seconds by default). 369 | 370 | Args: 371 | max_delay: The new maximum delay, in seconds. 372 | """ 373 | LOGGER.debug('Updating max delay to {} seconds'.format(max_delay)) 374 | cls.max_delay = max_delay 375 | 376 | @classmethod 377 | def set_initial_delay(cls, initial_delay): 378 | """Set the initial delay in seconds for reconnecting to rosbridge (1 second by default). 379 | 380 | Args: 381 | initial_delay: The new initial delay, in seconds. 382 | """ 383 | LOGGER.debug('Updating initial delay to {} seconds'.format(max_delay)) 384 | cls.initial_delay = initial_delay 385 | 386 | @classmethod 387 | def set_max_retries(cls, max_retries): 388 | """Set the maximum number or connection retries when the rosbridge connection is lost (no limit by default). 389 | 390 | Args: 391 | max_retries: The new maximum number of retries. 392 | """ 393 | LOGGER.debug('Updating max retries to {}'.format(max_retries)) 394 | cls.max_retries = max_retries 395 | 396 | 397 | class CliEventLoopManager(object): 398 | """Manage the main event loop using .NET threads. 399 | 400 | For the time being, this implementation is pretty light 401 | and mostly relies on .NET async doing "the right thing(tm)" 402 | with a sprinkle of threading here and there. 403 | """ 404 | 405 | def __init__(self): 406 | self._init_cancellation() 407 | self._disconnect_event = ManualResetEventSlim(False) 408 | 409 | def _init_cancellation(self): 410 | """Initialize the cancellation source and token.""" 411 | self.cancellation_token_source = CancellationTokenSource() 412 | self.cancellation_token = self.cancellation_token_source.Token 413 | self.cancellation_token.Register(lambda: LOGGER.debug('Started token cancellation')) 414 | 415 | def run(self): 416 | """Kick-starts a non-blocking event loop. 417 | 418 | In this implementation, this is a no-op.""" 419 | pass 420 | 421 | def run_forever(self): 422 | """Kick-starts a blocking loop while the ROS client is connected.""" 423 | self._disconnect_event.Wait(self.cancellation_token) 424 | LOGGER.debug('Received disconnect event on main loop') 425 | 426 | def call_later(self, delay, callback): 427 | """Call the given function after a certain period of time has passed. 428 | 429 | Args: 430 | delay (:obj:`int`): Number of seconds to wait before invoking the callback. 431 | callback (:obj:`callable`): Callable function to be invoked when the delay has elapsed. 432 | """ 433 | # NOTE: Maybe there's a more elegant way of doing this 434 | def closure(): 435 | Thread.Sleep(delay * 1000) 436 | callback() 437 | 438 | Task.Factory.StartNew(closure, self.cancellation_token) 439 | 440 | def call_in_thread(self, callback): 441 | """Call the given function on a thread. 442 | 443 | Args: 444 | callback (:obj:`callable`): Callable function to be invoked in a thread. 445 | """ 446 | Task.Factory.StartNew(callback, self.cancellation_token) 447 | 448 | def blocking_call_from_thread(self, callback, timeout): 449 | """Call the given function from a thread, and wait for the result synchronously 450 | for as long as the timeout will allow. 451 | 452 | Args: 453 | callback: Callable function to be invoked from the thread. 454 | timeout (:obj: int): Number of seconds to wait for the response before 455 | raising an exception. 456 | 457 | Returns: 458 | The results from the callback, or a timeout exception. 459 | """ 460 | manual_event = ManualResetEventSlim(False) 461 | result_placeholder = {'manual_event': manual_event} 462 | ThreadPool.QueueUserWorkItem(WaitCallback(callback), result_placeholder) 463 | 464 | # Wait with timeout results bool indicating success/failure 465 | if timeout and manual_event.Wait(timeout * 1000, self.cancellation_token): 466 | return result_placeholder 467 | 468 | # Wait without timeouts raises in case of failure triggered by cancellation 469 | if not timeout: 470 | manual_event.Wait(self.cancellation_token) 471 | return result_placeholder 472 | 473 | self.raise_timeout_exception() 474 | 475 | def raise_timeout_exception(self, _result=None, _timeout=None): 476 | """Callback called on timeout. 477 | 478 | Args: 479 | _result: Unused--required by Twister. 480 | _timeout: Unused--required by Twister. 481 | 482 | Raises: 483 | An exception. 484 | """ 485 | raise Exception('No service response received') 486 | 487 | def get_inner_callback(self, result_placeholder): 488 | """Get the callback which, when called, provides result_placeholder with the result. 489 | 490 | Args: 491 | result_placeholder: (:obj: dict): Object in which to store the result. 492 | 493 | Returns: 494 | A callable which provides result_placeholder with the result in the case of success. 495 | """ 496 | def inner_callback(result): 497 | result_placeholder['result'] = result 498 | result_placeholder['manual_event'].Set() 499 | return inner_callback 500 | 501 | def get_inner_errback(self, result_placeholder): 502 | """Get the errback which, when called, provides result_placeholder with the error. 503 | 504 | Args: 505 | result_placeholder: (:obj: dict): Object in which to store the result. 506 | 507 | Returns: 508 | A callable which provides result_placeholder with the error in the case of failure. 509 | """ 510 | def inner_errback(error): 511 | result_placeholder['exception'] = error 512 | result_placeholder['manual_event'].Set() 513 | return inner_errback 514 | 515 | def terminate(self): 516 | """Signals the termination of the main event loop.""" 517 | self._disconnect_event.Set() 518 | 519 | if self.cancellation_token_source: 520 | self.cancellation_token_source.Cancel() 521 | 522 | # Renew to allow re-connects 523 | self._init_cancellation() 524 | --------------------------------------------------------------------------------