├── .gitignore ├── .readthedocs.yaml ├── LICENSE ├── Makefile ├── README.md ├── build.os ├── make.bat ├── requirements.txt ├── source ├── conf.py ├── docs │ ├── README.md │ ├── ROS_VS_ROS2.md │ ├── fork_and_pr.md │ ├── 仿真器概述.md │ ├── 传感器.md │ ├── 写在前面.md │ ├── 可视化.md │ ├── 地图.md │ ├── 坐标系.md │ ├── 工作空间.md │ ├── 环境安装.md │ ├── 第一个全局路径规划器.md │ ├── 第一个启动脚本.md │ ├── 虚拟时钟.md │ ├── 路径规划仿真一.md │ ├── 路径规划仿真二.md │ ├── 遥控器.md │ ├── 里程计一.md │ ├── 里程计三.md │ ├── 里程计二.md │ └── 障碍物.md ├── images │ ├── costmap.png │ ├── cp.png │ ├── dwb_local_planner.png │ ├── fork.png │ ├── fork1.png │ ├── galactic.png │ ├── gbpl.gif │ ├── life_cycle_sm.png │ ├── listener.png │ ├── map.png │ ├── move.png │ ├── move.svg │ ├── move_rotate.svg │ ├── multi_car.gif │ ├── multi_topic.gif │ ├── nav2_architecture.png │ ├── navigation.gif │ ├── navigation_with_recovery_behaviours.gif │ ├── odom.gif │ ├── odom.png │ ├── odom.svg │ ├── odom_reloc.gif │ ├── odom_tf.gif │ ├── planner.png │ ├── planner_bridge.gif │ ├── planner_server.svg │ ├── plans.png │ ├── pull.png │ ├── pull1.png │ ├── pull2.png │ ├── reloc.gif │ ├── robot.svg │ ├── rotate.png │ ├── rviz.gif │ ├── rviz_map.png │ ├── sim_car.gif │ ├── simulator.png │ ├── single_topic.gif │ ├── smile.svg │ ├── straight.gif │ ├── straightline.gif │ ├── talker.png │ ├── tele_move.gif │ ├── teleop.gif │ ├── tf.png │ ├── tf_odom.png │ ├── tf_rviz.png │ ├── tf_simple.png │ ├── tracking.gif │ ├── transform.svg │ ├── vx.jpg │ └── wheels.png ├── index.rst └── uml │ ├── controller.puml │ └── costmap.puml └── src ├── planner ├── CMakeLists.txt ├── include │ └── planner │ │ └── straight_line.hpp ├── launch │ └── planner.launch.py ├── package.xml ├── params │ └── planner.yaml ├── planner_plugins.xml └── src │ └── straight_line.cpp └── simulator ├── CMakeLists.txt ├── include └── simulator │ └── transform.hpp ├── launch ├── planner_simulator.launch.py └── simulator.launch.py ├── maps ├── a.pgm ├── a.yaml ├── b.pgm └── b.yaml ├── package.xml ├── rviz └── planner.rviz └── src ├── clock_node.cpp ├── planner_bridge.cpp ├── telecontrol.cpp └── transform.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | # Byte-compiled / optimized / DLL files 3 | __pycache__/ 4 | *.py[cod] 5 | *$py.class 6 | 7 | # C extensions 8 | *.so 9 | 10 | # ROS 11 | log/ 12 | install/ 13 | # Distribution / packaging 14 | .Python 15 | build/ 16 | develop-eggs/ 17 | dist/ 18 | downloads/ 19 | eggs/ 20 | .eggs/ 21 | lib/ 22 | lib64/ 23 | parts/ 24 | sdist/ 25 | var/ 26 | wheels/ 27 | pip-wheel-metadata/ 28 | share/python-wheels/ 29 | *.egg-info/ 30 | .installed.cfg 31 | *.egg 32 | MANIFEST 33 | 34 | # PyInstaller 35 | # Usually these files are written by a python script from a template 36 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 37 | *.manifest 38 | *.spec 39 | 40 | # Installer logs 41 | pip-log.txt 42 | pip-delete-this-directory.txt 43 | 44 | # Unit test / coverage reports 45 | htmlcov/ 46 | .tox/ 47 | .nox/ 48 | .coverage 49 | .coverage.* 50 | .cache 51 | nosetests.xml 52 | coverage.xml 53 | *.cover 54 | *.py,cover 55 | .hypothesis/ 56 | .pytest_cache/ 57 | 58 | # Translations 59 | *.mo 60 | *.pot 61 | 62 | # Django stuff: 63 | *.log 64 | local_settings.py 65 | db.sqlite3 66 | db.sqlite3-journal 67 | 68 | # Flask stuff: 69 | instance/ 70 | .webassets-cache 71 | 72 | # Scrapy stuff: 73 | .scrapy 74 | 75 | # Sphinx documentation 76 | docs/_build/ 77 | 78 | # PyBuilder 79 | target/ 80 | 81 | # Jupyter Notebook 82 | .ipynb_checkpoints 83 | 84 | # IPython 85 | profile_default/ 86 | ipython_config.py 87 | 88 | # pyenv 89 | .python-version 90 | 91 | # pipenv 92 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 93 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 94 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 95 | # install all needed dependencies. 96 | #Pipfile.lock 97 | 98 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 99 | __pypackages__/ 100 | 101 | # Celery stuff 102 | celerybeat-schedule 103 | celerybeat.pid 104 | 105 | # SageMath parsed files 106 | *.sage.py 107 | 108 | # Environments 109 | .env 110 | .venv 111 | env/ 112 | venv/ 113 | ENV/ 114 | env.bak/ 115 | venv.bak/ 116 | 117 | # Spyder project settings 118 | .spyderproject 119 | .spyproject 120 | 121 | # Rope project settings 122 | .ropeproject 123 | 124 | # mkdocs documentation 125 | /site 126 | 127 | # mypy 128 | .mypy_cache/ 129 | .dmypy.json 130 | dmypy.json 131 | 132 | # Pyre type checker 133 | .pyre/ 134 | -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | # Read the Docs configuration file for Sphinx projects 2 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 3 | 4 | # Required 5 | version: 2 6 | 7 | # Set the OS, Python version and other tools you might need 8 | build: 9 | os: ubuntu-22.04 10 | tools: 11 | python: "3.8" 12 | # You can also specify other tool versions: 13 | # nodejs: "20" 14 | # rust: "1.70" 15 | # golang: "1.20" 16 | 17 | # Build documentation in the "docs/" directory with Sphinx 18 | sphinx: 19 | configuration: source/conf.py 20 | # You can configure Sphinx to use a different builder, for instance use the dirhtml builder for simpler URLs 21 | # builder: "dirhtml" 22 | # Fail on all warnings to avoid broken references 23 | # fail_on_warning: true 24 | 25 | # Optionally build your docs in additional formats such as PDF and ePub 26 | # formats: 27 | # - pdf 28 | # - epub 29 | 30 | # Optional but recommended, declare the Python requirements required 31 | # to build your documentation 32 | # See https://docs.readthedocs.io/en/stable/guides/reproducible-builds.html 33 | python: 34 | install: 35 | - requirements: ./requirements.txt 36 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 一起动手写ROS仿真器 2 | ![Simulator](source/images/simulator.png) 3 | 4 | 本仓库是《一起动手写ROS仿真器》系列教程的代码和示例集合。这个教程旨在帮助你从零开始，逐步添加功能，最终完整实现一个用于ROS平台**机器人导航规划控制功能**的ROS仿真器 5 | **备注: 虽然开发环境使用的galactic，但是整个工程在ROS2的各版本上是可以直接使用的，不必区分版本** 6 | 7 | ## 简介 8 | ROS（Robot Operating System）是一个广泛使用的机器人开发平台，提供了一系列工具和库，用于构建机器人应用程序。在本教程中，我们将使用ROS来开发一个仿真器，模拟机器人在虚拟环境中的运动和感知。 9 | 10 | ## 教程目录 11 | 12 | [bilibili](https://space.bilibili.com/554016964/channel/collectiondetail?sid=1560370) 13 | 14 | [在线文档](https://nav-simulator.readthedocs.io/en/latest/) 15 | 16 | 由于上面文档平台有一些公式的显示问题我没有搞定，如果对公式感兴趣，可以看下面的链接。内容都是一样的。 17 | [知乎](https://www.zhihu.com/column/c_1682327659298902016) 18 | 19 | ## 安装依赖 20 | 在开始教程之前，请确保教程所使用的环境： 21 | 22 | 操作系统： Ubuntu 20.04 23 | 24 | ROS:     ros2 25 | 26 | 27 | ## 使用方法 28 | 29 | ### 路径规划器仿真 30 | 31 | ``` 32 | # 编译 33 | git clone https://github.com/cf-zhang/nav_simulator 34 | cd nav_simulator 35 | colcon build 36 | source install/setup.bash 37 | # 启动仿真器 38 | ros2 launch simulator planner_simulator.launch.py 39 | # 另一个终端启动仿真器 40 | ros2 launch planner planner.launch.py 41 | ``` 42 | 43 | ![planner](./source/images/planner_bridge.gif) 44 | 45 | ### 导航全流程仿真 46 | 47 | 启动仿真器： 48 | 49 | ``` 50 | git clone git@github.com:cf-zhang/nav_simulator.git 51 | cd nav_simulator 52 | source /opt/ros/galactic/setup.bash 53 | colcon build 54 | source install/setup.bash 55 | ros2 launch simulator simulator.launch.py 56 | ``` 57 | 58 | 再打开一个终端，启动导航的功能节点： 59 | 60 | ``` 61 | ros2 launch nav2_bringup navigation_launch.py 62 | ``` 63 | 64 | 然后使用`2D Goal Pose`控件设置目标点，可以看到机器人的导航移动效果，具体如下图所示： 65 | 66 | ![navigation](./source/images/navigation.gif) 67 | 68 | ## 贡献 69 | `一个人走的快，众人走的远` 70 | 71 | 欢迎对本教程提出问题、反馈和改进建议。如果您发现任何错误或问题，请随时提交Issue或Pull Request。 72 | 73 | ## 许可证 74 | 本教程的代码遵循MIT许可证，详情请参见 LICENSE 文件。 75 | 76 | --- 77 | 78 | *本教程旨在帮助大家学习ROS仿真器的构建，如果有任何侵权行为，请及时联系作者删除。谢谢！* 79 | 80 | -------------------------------------------------------------------------------- /build.os: -------------------------------------------------------------------------------- 1 | version: 2 2 | build: 3 | os: "ubuntu-22.04" 4 | tools: 5 | python: "3.11" 6 | -------------------------------------------------------------------------------- /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=source 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% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | alabaster==0.7.12 2 | appdirs==1.4.4 3 | Babel==2.9.1 4 | beautifulsoup4==4.9.3 5 | bs4==0.0.1 6 | certifi==2020.12.5 7 | chardet==4.0.0 8 | colorama==0.4.4 9 | commonmark==0.9.1 10 | cpplint==1.5.4 11 | distlib==0.3.1 12 | docutils==0.16 13 | fake-useragent==0.1.11 14 | filelock==3.0.12 15 | fishbase==1.3 16 | flake8==3.9.2 17 | idna==2.10 18 | imagesize==1.2.0 19 | importlib-metadata==3.9.1 20 | importlib-resources==5.1.2 21 | install==1.3.4 22 | Jinja2==3.0.1 23 | livereload==2.6.3 24 | Markdown==3.3.4 25 | MarkupSafe==2.0.1 26 | mccabe==0.6.1 27 | packaging==21.0 28 | pycodestyle==2.7.0 29 | pyflakes==2.3.1 30 | Pygments==2.9.0 31 | pyparsing==2.4.7 32 | python-dateutil==2.8.1 33 | pytz==2021.1 34 | recommonmark==0.7.1 35 | redis==3.5.3 36 | requests==2.25.1 37 | six==1.15.0 38 | snowballstemmer==2.1.0 39 | soupsieve==2.2.1 40 | Sphinx==4.0.3 41 | sphinx-autobuild==2021.3.14 42 | sphinx-markdown-tables==0.0.15 43 | sphinx-rtd-theme==0.5.2 44 | sphinxcontrib-applehelp==1.0.2 45 | sphinxcontrib-devhelp==1.0.2 46 | sphinxcontrib-htmlhelp==2.0.0 47 | sphinxcontrib-jsmath==1.0.1 48 | sphinxcontrib-qthelp==1.0.3 49 | sphinxcontrib-serializinghtml==1.1.5 50 | tornado==6.1 51 | typing-extensions==3.7.4.3 52 | urllib3==1.26.5 53 | virtualenv==20.4.3 54 | zipp==3.4.1 55 | -------------------------------------------------------------------------------- /source/conf.py: -------------------------------------------------------------------------------- 1 | # encoding:utf-8 2 | ''' 3 | Author: your name 4 | Date: 2021-07-07 09:42:42 5 | LastEditTime: 2021-07-10 12:34:02 6 | LastEditors: Please set LastEditors 7 | Description: In User Settings Edit 8 | FilePath: \cpp_dictionary\source\conf.py 9 | ''' 10 | # Configuration file for the Sphinx documentation builder. 11 | # 12 | # This file only contains a selection of the most common options. For a full 13 | # list see the documentation: 14 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 15 | 16 | # -- Path setup -------------------------------------------------------------- 17 | 18 | # If extensions (or modules to document with autodoc) are in another directory, 19 | # add these directories to sys.path here. If the directory is relative to the 20 | # documentation root, use os.path.abspath to make it absolute, like shown here. 21 | # 22 | # import os 23 | # import sys 24 | # sys.path.insert(0, os.path.abspath('.')) 25 | 26 | 27 | # -- Project information ----------------------------------------------------- 28 | 29 | project = '一起动手写ROS仿真器' 30 | copyright = '2023, cf-zhang' 31 | author = 'Zhang Chuanfa' 32 | 33 | # The full version, including alpha/beta/rc tags 34 | release = 'v0.1' 35 | 36 | 37 | # -- General configuration --------------------------------------------------- 38 | 39 | # Add any Sphinx extension module names here, as strings. They can be 40 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 41 | # ones. 42 | # extensions = [ 43 | # ] 44 | extensions = ['recommonmark', 'sphinx_markdown_tables'] 45 | 46 | 47 | # Add any paths that contain templates here, relative to this directory. 48 | templates_path = ['_templates'] 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 | 63 | # -- Options for HTML output ------------------------------------------------- 64 | 65 | # The theme to use for HTML and HTML Help pages. See the documentation for 66 | # a list of builtin themes. 67 | # 68 | # html_theme = 'alabaster' 69 | html_theme = 'sphinx_rtd_theme' 70 | 71 | # Add any paths that contain custom static files (such as style sheets) here, 72 | # relative to this directory. They are copied after the builtin static files, 73 | # so a file named "default.css" will overwrite the builtin "default.css". 74 | html_static_path = [] -------------------------------------------------------------------------------- /source/docs/README.md: -------------------------------------------------------------------------------- 1 | # README 2 | 3 | -------------------------------------------------------------------------------- /source/docs/ROS_VS_ROS2.md: -------------------------------------------------------------------------------- 1 | # ROS与ROS2的区别 2 | ROS（Robot Operating System）和ROS2（Robot Operating System 2）是由Open Robotics开发的机器人操作系统。它们都是用于构建机器人应用程序的开源平台，提供了一系列工具、库和功能，用于编写、测试和运行机器人软件。虽然它们都具有相似的目标，但在设计和功能上有一些重要的区别。 3 | 4 | ## 架构与通信机制： 5 | 6 | - ROS使用自定义的中间件（"roscore"），基于发布-订阅模型、服务调用和参数服务器等通信机制。 7 | - ROS2使用DDS（Data Distribution Service）中间件，其中eProsima Fast RTPS和Cyclone DDS是常用的实现。ROS2引入了更灵活的通信机制，如实时数据流、快速通信等，支持分布式系统和实时性能。 8 | 9 | ## 可移植性： 10 | 11 | - ROS主要支持Ubuntu和其他Linux发行版。 12 | - ROS2不仅支持Ubuntu和其他Linux发行版，还增加了对Windows和macOS等操作系统的支持。 13 | 14 | ## 构建系统： 15 | 16 | - ROS使用catkin作为构建系统，它基于Python和C++。 17 | - ROS2使用colcon作为构建系统，同样基于Python和C++。colcon相对于catkin在构建速度和可扩展性上有所改进。 18 | 19 | ## 实时性能： 20 | 21 | - ROS在实时性能方面有一定的限制，特别是在高实时要求的机器人应用中。 22 | - ROS2在实时性能方面做出了改进，支持更复杂的实时任务。 23 | 24 | ## 可靠性和稳定性： 25 | 26 | - ROS在处理大规模系统和复杂通信拓扑时可能存在一些问题。 27 | - ROS2在处理复杂拓扑和大规模系统时更稳定可靠。 28 | 29 | ## 兼容性： 30 | 31 | - ROS和ROS2在接口和代码级别上不完全兼容。但ROS2提供了一些工具和桥接功能，以帮助用户从ROS迁移到ROS2。 32 | 33 | ## 安全性 34 | 35 | - ROS2引入了更多的安全机制，可以对通信进行加密和认证。 36 | 37 | 总体而言，ROS和ROS2是两个不同的机器人操作系统，每个系统都有自己的优势和适用场景。对于新项目，特别是需要高实时性、更复杂的通信机制或跨平台支持的项目，ROS2可能是更好的选择。对于已有的ROS项目，需要考虑迁移成本和兼容性问题。在选择ROS还是ROS2时，需要根据具体项目需求和现有资源进行权衡。 38 | 39 | ## ROS1和ROS2怎么选 40 | 选择ROS还是ROS2取决于项目的具体需求和情况。以下是一些考虑因素： 41 | 42 | 选择ROS（Robot Operating System）: 43 | 44 | 已有代码和生态系统：如果你的项目已经基于ROS开发，并且依赖于ROS的生态系统和工具库，可能继续使用ROS是最好的选择。 45 | 46 | 稳定性和成熟度：ROS是一个成熟且稳定的平台，已经在许多机器人项目中被广泛使用。 47 | 48 | 社区支持：ROS拥有庞大的开源社区，这意味着你可以从其他开发者那里获得帮助，共享知识，以及使用已经存在的ROS软件包。 49 | 50 | 编程语言：ROS主要使用C++和Python，如果你对这两种语言熟悉，并且不需要其他语言的支持，那么ROS可能是更合适的选择。 51 | 52 | 选择ROS2: 53 | 54 | 实时性：ROS2引入了一些实时通信机制，对于一些对实时性要求较高的应用场景，ROS2可能更适合。 55 | 56 | 安全性：ROS2对数据通信进行了加密和认证，对于一些对安全性要求较高的应用场景，ROS2可能更合适。 57 | 58 | 分布式系统：ROS2引入了一些分布式系统的特性，支持多机器人系统的开发。 59 | 60 | 多语言支持：ROS2支持多种编程语言，包括C++，Python，Java等。 61 | 62 | 长期支持：ROS2被设计为长期支持版本，未来的更新和维护将在ROS2基础上进行。 63 | 64 | 综上所述，如果你的项目已经在ROS上开发并且满足需求，继续使用ROS是一个不错的选择。如果你对实时性、安全性、多机器人系统或者其他ROS2的特性感兴趣，并且对ROS2生态系统的发展抱有信心，那么可以选择ROS2。最终的决定取决于项目的具体情况和需求。 65 | -------------------------------------------------------------------------------- /source/docs/fork_and_pr.md: -------------------------------------------------------------------------------- 1 | # Fork和Pull Request 2 | 3 | 在确定要参与到项目的情况下，请联系cf-zhang在本仓库中建立对应的ROS发布分支，然后按照如下的步骤参与进来。有任何问题，请及时在开发群里反馈沟通。 4 | 5 | 1. 在源仓库分支创建好之后，开发者首先Fork（复制）一个项目的仓库到自己的GitHub账号下。 6 | 7 | 打开： https://github.com/cf-zhang/nav_simulator 8 | 9 | 微信 10 | 微信 11 | 12 | 2. 在自己Fork的仓库的对应分支下进行修改和开发。 13 | 14 | 3. 当完成某个功能或修复时，可以发起Pull Request（PR）将自己的修改合并到原项目。 15 | 16 | 打开自己github的nav_simulator地址,通过如下步骤完成pull request. 17 | 18 | 微信 19 | 微信 20 | 微信 21 | 22 | -------------------------------------------------------------------------------- /source/docs/仿真器概述.md: -------------------------------------------------------------------------------- 1 | # 仿真器概述 2 | 3 | ## 引言 4 | 5 | 文档通篇致力于实现一个简单高效的仿真工具，用于2D移动机器人的导航功能开发 6 | ----------- 工欲善其事 必先利其器 7 | 8 | 9 | 在完成前面环境安装与开发环境搭建之后，正式进入仿真器阶段。在本小节，首先从仿真器到底在仿什么进行切入，尽可能的讲清楚即将实现的仿真器所拥有的模块，以及模块之间的关系。 10 | 11 | ## 什么是仿真器 12 | 13 | 仿真器： 一个通过计算机技术与建模技术所构建出来的虚拟的，可视化的，用于节约开发成本的工具。 14 | 15 | 解决机器人时间，地点，人物，事件的描述问题。 16 | 17 | ## 仿真器与实体机器人 18 | 19 | 主要通过机器人与仿真器之间的类比，来筛选出仿真器所需要的最小功能集合。 20 | 21 | ### 真实的机器人架构 22 | 23 | ![robot](./../images/robot.svg) 24 | 25 | ### 导航算法功能架构 26 | 27 | ![nav_simulator](./../images/nav2_architecture.png) 28 | 29 | ## Nav Simulator 30 | 31 | 导航算法的仿真的最小模块集合： 32 | 33 | - 虚拟时钟 34 | 35 | 在ROS2中，如果你的节点需要使用仿真时间来与其他节点同步，你可以将 use_sim_time 参数设置为 true。这样，节点会订阅 /clock 话题，并使用从该话题接收到的时间信息，而不是从系统时钟获取时间。这使得在仿真器中进行节点通信更加准确，因为所有节点都将使用仿真器提供的统一时间。 36 | - 地图 37 | ![map](./../images/map.png) 38 | 作为仿真器的机器人运行环境，在导航算法中，并不知道建筑物的概念，输入就只是地图，所以，只要有一张地图就可以满足规划需求。 39 | 40 | - 坐标系 41 | 42 | ![map](./../images/tf.png) 43 | 描述机器人与地图之间的坐标变换， 允许机器人系统中的不同组件在不同坐标系之间进行无缝的转换，以实现正确的坐标变换和传递。机器人通常由多个部件组成，例如传感器、执行器、末端执行器等，它们在不同的坐标系中工作。这些坐标系之间的关系可能是固定的，也可能是动态的。在导航功能里最小集合只需要map-->base_link之间的变化关系，中间还会插入一个odom坐标系用以位姿调整。ROS中提供了一个专用工具集：tf/tf2 44 | 45 | - 机器人 46 | 47 | 用来直观的体现导航功能的效果,需要把base_link所在位姿，理解成是机器人的位姿，并进行可视化处理。 48 | 49 | - 遥控器 50 | 51 | 用于自验证的工具 52 | 53 | - 里程计 54 | 55 | 里程计（odometry）是一种通过测量机器人的轮子旋转和移动距离来估计机器人的运动的技术。它是一种基于机器人轮子运动的相对定位方法，用于跟踪机器人在一个相对于起始位置的局部坐标系中的运动。里程计通常在机器人的局部区域内提供较为准确的位姿估计，但随着时间的推移，累积误差可能会导致精度下降。 56 | 在做定位矫正的时候，实际就是调整的odom与map之间的关系。 57 | 58 | - 传感器 59 | 60 | 用来感知障碍物，在机器人系统中，传感器是非常重要的组成部分，它们帮助机器人感知和理解周围环境。在仿真器的实现过程中，重点是如何将障碍物体现为传感器的数据形式。只以激光数据进行实现 61 | 62 | - 障碍物 63 | 64 | 用来做导航算法的避障效果测试，可以分为永久障碍与临时障碍，临时障碍又分为静态障碍和动态障碍物。在仿真器的实现过程中，重点是如何将障碍物与地图关系进行映射，并将映射后的数据交给传感器仿真器进行数据呈现。 65 | 66 | 67 | - 目标检测 68 | 69 | 用来做跟随功能，将多机器人仿真系统里的机器人A位姿输出，作为跟随机器人的目标进行跟踪。用于验证跟随算法效果。 70 | 71 | - 可视化 72 | 73 | 直观的感受导航效果，路径，代价图，控制轨迹，速度曲线等等。实现鼠标拖拽，方便快捷的重定位效果。 74 | 75 | 76 | 77 | 78 | ## ROS 仿真器的使用场景 79 | 80 | 通过上面所描述的仿真系统，后续可以完成如下的基础功能： 81 | ### 遥控器 82 | ![teleop](./../images/teleop.gif) 83 | ### 路径规划 84 | ![global planner](./../images/gbpl.gif) 85 | ### 机器人导航仿真 86 | ![global planner](./../images/sim_car.gif) 87 | 88 | ## ROS 仿真器的进阶功能 89 | 结合ros中的命名空间设计，可以完成进阶的功能仿真： 90 | ### 多机器人导航仿真 91 | ![multi_robot](./../images/multi_car.gif) 92 | ### 机器人跟随 93 | ![tracking](./../images/tracking.gif) 94 | 95 | ## 总结 96 | 97 | 概述仿真器在机器人ROS开发中的重要性和应用。并将仿真器的最小功能集进行筛选解释，后续会逐步实现每个模块，最终组合成一个完整的导航仿真系统。 -------------------------------------------------------------------------------- /source/docs/传感器.md: -------------------------------------------------------------------------------- 1 | # 传感器 2 | 3 | - 传感器 4 | 5 | 用来感知障碍物，在机器人系统中，传感器是非常重要的组成部分，它们帮助机器人感知和理解周围环境。在仿真器的实现过程中，重点是如何将障碍物体现为传感器的数据形式。只以激光数据进行实现 6 | 7 | -------------------------------------------------------------------------------- /source/docs/写在前面.md: -------------------------------------------------------------------------------- 1 | # 写在前面 2 | ![Simulator](./../images/simulator.png) 3 | ## 引言 4 | 5 |     机器人领域中，仿真器起着至关重要的作用。它们允许开发人员、研究人员和工程师在虚拟环境中构建、验证和优化机器人系统，而无需实际的物理硬件。通过仿真器，可以进行各种任务和实验，如路径规划、运动控制、感知算法评估等，以加快开发周期、降低成本并提高系统性能。 6 | 7 |     作为一个十年研发老兵，有着七年以上机器人行业经验，一直都在深耕机器人的导航规划控制算法领域， 所开发的产品，有面向定制的搬运服务机器人， 面向普通用户的陪伴机器人，运动形式上包含了轮式机器人和足式机器人。所有的开发过程中，前前后后使用过多个仿真器，深刻的认知到仿真器对于研发效率的重要性。 8 | 9 |     就所使用的各种仿真器中，普遍都是一个黑盒子，从网络上寻求到一份可以用的环境，一键启动之后，进行使用。对于整套系统其为什么能仿真，每个模块之间的关系也不清晰，就像在使用一个黑匣子。有改动需求的时候也很难做到得心应手。 10 | 11 |     同时作为一个机器人上层导航功能开发人员，在功能开发过程中其实并不深度关注机器人的SLAM效果如何，更需要的是一个完美定位情况进行导航层面的功能开发。在功能算法及流程都验证完成之后才会到实体机器上进行验证和调优。很多时候仿真器的物理引擎及slam反而成了制约我开发进度的重要因素，比如我在调试导航效果时候，会涉及到频繁的调整机器人在环境中的位置和朝向，这个时候，我就需要通过修改仿真器中机器人本体位置，再从rviz（机器人平台可视化工具）上进行观察效果，如果此时还加入了定位仿真，那么还要不断地进行重定位后再进行效果观察。 12 | 13 |     随着时间的推移，不断地尝试过多种仿真器之后，这种感觉愈发的强烈。最终通过对仿真系统的分析和理解，并结合导航功能开发的实际需求，决定自己开发一套趁手的仿真工具：一个专门用于机器人导航功能开发的仿真平台。它就是本书即将展开的内容。 14 | 15 | ## 目标读者 16 | 17 |     本书的核心目的就是从环境搭建开始逐步实现一个可以用于导航算法功能开发的仿真器，对于有志于从业机器人相关领域的高效学生和科研人员，机器人行业的从业人员都有帮助。通过本书的学习，可以从更高的维度上理解机器人系统设计，及模块之间的依赖关系。在后续的工作学习中能够对整个系统了如指掌，不再因为开发环境的复杂造成效率问题。 18 | 19 | ## 结构概述 20 | 21 |     虽然核心目标是写仿真器，但是为了保持内容的完整性和连续性，会在前四个章节进行仿真器开发的内容铺垫，从环境搭建，工作空间解析，到基础节点的开发都进行覆盖，针对后续仿真器开发需要用的知识进行展开。 22 | 23 |     对于有ROS基础的同学，可以选择性的略过前面几个章节，直接从第五章开始。 24 | 25 |     在第五章会对仿真器进行一个概述，描述仿真器与实体机器的异同，并将我们实际开发时所需要的模块进行抽象和描述。从第六章之后会将仿真器的每一个模块进行展开，从原理讲述到代码实践。 26 | 27 |     当单机器人导航仿真系统搭建完成之后，会将多机器人仿真的系统再进行多一个章节的阐述，这样整本书从内容的广度和深度都能得到保证。 28 | 29 | ## 学习前提 30 | 31 |     本书中开发环境用到的系统是Ubuntu20.04，所以希望使用本书做实践时，最好安装一样的系统，且同时能够具备基本的linux系统的操作能力。 32 | 33 |     ROS系统使用的是ROS2的galactic版本。阅读和使用本书，并不需要机器人相关知识，只需要对机器人有热情，能够一步一步坚持到结束。当然如果有相关知识最好，所以我会建议阅读网络上关于ROS和Navigation的开源Wiki资料: 34 | ``` 35 | ROS2: https://docs.ros.org/en/galactic/index.html 36 | 37 | navigation2: https://navigation.ros.org/ 38 | ``` 39 | navigation2相关的内容并不在本书中展开，知识将navigation2中的能力，在本书所实现的仿真器中进行功能验证。 40 | 41 |     在阅读本书之前，最好具备基本的C++编程功底，并不要求多么的精湛和多少行的代码量。因为在基础部分，会对用到的编程知识，工程特点做到尽可能多的覆盖，包括工作空间结构，代码构成，编译过程等。如果没有系统学习过C++，可以找一本谭浩强老师的C++教材看一遍即可。 42 | 43 |     书中还会涉及到一小部分用于系统启动的的python脚本，并不需要对python有很深的认识，甚至只知道python是一个语言即可，涉及到的模块和句式都较为固定，不涉及过多的逻辑。对于python不熟悉的读者，也没有什么影响。 44 | 45 | ## 本书特点 46 | 47 |     本书从理论开始，以实践为导向。力求每一个模块都能做到理论联系实际，做到一本书读完，就能具备独立实现ROS仿真器的能力。 48 | 49 |     通过这本书的理论学习，代码实践，工程部署，可以获得ROS相关的技术储备，对机器人的整体架构，设计思路能够做到清晰的认知。 50 | 51 | ## 阅读建议 52 | 53 |     有一定机器人研发经历的读者，在阅读本书时，可以直接阅读书籍，并根据书籍中对代码的描述和自己的理解去尝试复现整个仿真系统，针对每个章节所涉及到的模块进行工程化实践。 54 | 55 |     本书配套完整的视频教程，会在B站同步跟进。对于基础较弱的读者，在阅读本书时，建议先学习对应章节的视频教程，然后再巩固书中对应的章节后，再尝试动手实践。 56 | 57 |     对于只想使用仿真器，而不关系仿真器实现过程的读者，可以直接将本书托管到github上的源代码进行clone到本地进行编译使用即可。 58 | 59 | ## 结语 60 | 61 |     让我们从0开始搭建一个仿真器，逐渐解开机器人的面纱。做到知其然的同时，也知其所以然。 62 | 63 | ## 展望 64 |     或许在不久的将来，将这个仿真器完成之后，还可以基于该仿真器进行更高纬度的内容学习，实操。将导航相关的路径规划算法，控制规划算法，框架开发等进行展开。 -------------------------------------------------------------------------------- /source/docs/可视化.md: -------------------------------------------------------------------------------- 1 | # 可视化 2 | 3 | - 可视化 4 | 5 | 直观的感受导航效果，路径，代价图，控制轨迹，速度曲线等等。实现鼠标拖拽，方便快捷的重定位效果。 6 | 7 | -------------------------------------------------------------------------------- /source/docs/地图.md: -------------------------------------------------------------------------------- 1 | # 地图 2 | 3 | 作为仿真器的机器人运行环境，在导航算法中，并不知道建筑物的概念，输入就只是地图，所以，只要有一张地图就可以满足规划需求。 4 | 5 | ## nav2_map_server 6 | 7 | ## map 8 | 9 | map一般是通过建图工具进行生成，也可以通过绘图软件进行编辑。不管从哪里来的地图，我们关心的只有如何将地图加载并管理起来。 10 | 11 | ### 数据形式： 12 | 13 | 在ROS中，常用的地图数据表示方式是栅格地图，其中环境被划分为许多小的栅格（grid cell）。每个栅格可以表示一个小区域，记录有关该区域的信息，通常是关于是否为障碍物、自由空间或未知区域的信息。这种表示方法适用于静态环境，可以被导航系统用于路径规划和避障。提供该地图的方式为：一个图片文件代表地图（一般为png文件或者pgm文件），一个yaml配置文件说明地图关键参数。 14 | 15 | ### 参数配置 16 | 17 | yaml文件中相关参数及含义如下： 18 | ``` 19 | image: /path/to/map_image.png 20 | resolution: 0.05 21 | origin: [-10.0, -10.0, 0.0] 22 | negate: 0 23 | occupied_thresh: 0.65 24 | free_thresh: 0.196 25 | ``` 26 | - image： 指定地图图像文件的路径，通常是PNG格式的栅格地图图像。 27 | - resolution： 指定每个栅格的物理尺寸。 28 | - origin： 定义地图原点在物理空间中的位置。 29 | - negate： 如果设置为1，表示将栅格地图中的障碍物和自由空间取反。 30 | - occupied_thresh： 定义被视为障碍物的栅格状态阈值。 31 | - free_thresh： 定义被视为空闲区域的栅格状态阈值。 32 | 33 | ### 地图文件 34 | 项目提供两个地图放在`nav_simulator/src/simulator/`目录下maps目录中,后续的项目展开都会基于该地图进行。 35 | ``` 36 | . 37 | ├── CMakeLists.txt 38 | ├── include 39 | │   └── simulator 40 | ├── maps 41 | │   ├── a.pgm 42 | │   ├── a.yaml 43 | │   ├── b.pgm 44 | │   └── b.yaml 45 | ├── package.xml 46 | └── src 47 | ├── clock_node.cpp 48 | └── telecontrol.cpp 49 | 50 | 4 directories, 8 files 51 | ``` 52 | 此时，还需要修改CMakeList.txt，增加如下install条目，使得maps文件夹移动到编译产物目录下，方便后期的debian文件制作以及ros2相关启动命令进行文件索引： 53 | ``` 54 | ... 55 | install( 56 | DIRECTORY maps 57 | DESTINATION share/${PROJECT_NAME} 58 | ) 59 | ... 60 | ``` 61 | 62 | 63 | ## map server 64 | 在ROS 2（Robot Operating System 2）中，nav2_map_server 是与导航和路径规划相关的一个包。它负责管理和提供地图数据，以供ROS 2导航堆栈（Navigation Stack）中的其他节点使用。以下是关于nav2_map_server的概念、介绍和使用方法的详细信息： 65 | 66 | nav2_map_server 是ROS 2中的一个节点，用于加载、管理和发布地图数据。在移动机器人的导航任务中，地图是一个关键的组成部分，它描述了机器人所在环境的空间布局。地图通常以栅格地图（Grid Map）的形式表示，其中每个栅格代表一小块区域，并标识出障碍物、自由空间等信息。 67 | 68 | nav2_map_server节点的主要功能包括： 69 | 70 | - 加载地图： 它可以从文件系统中加载保存的地图文件，如图片格式的地图或地图元数据文件。 71 | 72 | - 管理地图数据： 一旦地图加载到节点中，nav2_map_server可以将其存储在内存中，以供其他导航节点查询和使用。 73 | 74 | - 发布地图数据： 通过ROS 2的地图数据消息，如nav_msgs/OccupancyGrid消息类型，nav2_map_server将地图数据发布到ROS 2网络中，以便导航堆栈中的其他节点（如路径规划器、感知节点等）使用。 75 | 76 | 使用方法： 77 | 78 | - 安装和配置： 首先，确保您已经安装了ROS 2以及nav2_map_server包。您可能需要在ROS 2工作空间中构建该包（前面navigation2的安装就已经完成了该步骤）。 79 | 80 | - 准备地图数据： 获取或创建您的地图数据。这可以是一个图片格式的栅格地图，或者其他适用的地图数据文件（会提供map相关文件到项目的map文件夹）。 81 | 82 | - 启动nav2_map_server节点： 在终端中执行命令以启动nav2_map_server节点，指定地图文件的路径和相关参数： 83 | 84 | ``` 85 | ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=/path/to/your/map.yaml 86 | ``` 87 | 其中/path/to/your/map.yaml是您地图的元数据文件的路径。 88 | 由于navigation2中的map_server是一个lifecycle node，所以需要执行生命周期的操作。另起一个terminal： 89 | ``` 90 | ros2 lifecycle set /map_server configure 91 | ros2 lifecycle set /map_server activate 92 | ``` 93 | 94 | - 订阅地图数据： 在导航堆栈的其他节点（一般指costmap2D），您可以通过订阅nav_msgs/OccupancyGrid消息来获取地图数据，然后用于路径规划、感知等任务。 95 | ``` 96 | ros2 topic echo --qos-durability transient_local --qos-reliability reliable /map 97 | ``` 98 | 通过以上命令，可以接收到地图数据，或者打开rviz2进行可视化验证，订阅map组件能看到如下正常的地图，就说明功能一切正常，可以继续向下进行。 99 | ![rviz-map](./../images/rviz_map.png) 100 | 101 | 需要注意的是，nav2_map_server只是导航系统的一部分，用于提供地图数据。要实现完整的导航功能，您还需要使用其他导航堆栈的节点，如路径规划器、控制器等，以及适配您的机器人硬件和环境。 102 | 103 | 请参阅ROS 2文档和nav2_map_server包的官方文档以获取更详细的信息、配置选项和使用示例：[map server](https://navigation.ros.org/configuration/packages/configuring-map-server.html) 104 | 105 | 相关源码部分可以参考 [code](https://github.com/ros-planning/navigation2/tree/galactic/nav2_map_server) 106 | -------------------------------------------------------------------------------- /source/docs/坐标系.md: -------------------------------------------------------------------------------- 1 | # 坐标系 2 | 3 | ## tf的介绍 4 | 在ROS（Robot Operating System）中，tf（Transform Library）是一个非常重要的工具，用于处理机器人坐标系之间的变换关系。它能够追踪和管理不同坐标系之间的变换关系，从而使得多个传感器和执行器的数据能够在统一的坐标系下进行协同处理和分析。以下是 tf 的功能和作用的概述： 5 | 6 | ![tf](./../images/tf_rviz.png) 7 | ### 功能： 8 | 9 | 坐标变换管理： tf 允许您在机器人系统中定义多个坐标系，然后跟踪和管理它们之间的变换关系。这些坐标系可以是车辆坐标系、传感器坐标系、全局地图坐标系等。 10 | 11 | 坐标变换广播： 通过 tf，您可以广播机器人各个坐标系之间的变换关系，使其他节点能够获取这些变换信息并进行数据转换。 12 | 13 | 坐标变换查询： tf 允许节点查询特定坐标系之间的变换关系，从而使数据在不同坐标系之间进行变换。这在传感器融合、导航和控制等任务中非常有用。 14 | 15 | 插值和平滑： tf 能够进行坐标变换的插值和平滑操作，使得坐标变换在连续时间范围内变得平滑，从而减少传感器噪声和不稳定性的影响。 16 | 17 | ### 作用： 18 | 19 | 传感器融合： 在机器人系统中，传感器通常会以不同的坐标系提供数据。tf 可以将这些数据转换到统一的坐标系下，以进行多传感器数据融合。 20 | 21 | 导航： 机器人的导航任务通常需要在全局坐标系和本地坐标系之间进行数据变换。tf 可以帮助实现这种变换，使机器人能够在不同坐标系下进行导航。 22 | 23 | 控制： 在执行器控制中，可能需要将控制指令从一个坐标系变换到另一个坐标系。tf 可以帮助进行这种变换，使控制指令适用于实际执行器所在的坐标系。 24 | 25 | 建图： 在进行SLAM（Simultaneous Localization and Mapping）建图时，不同传感器提供的数据需要在地图坐标系下进行处理。tf 可以实现这种数据变换，使数据能够在地图坐标系下正确对齐。 26 | 27 | tf 在ROS中的作用非常重要，它使机器人系统能够处理不同坐标系之间的数据变换，从而实现多传感器数据融合、导航、控制和建图等任务。通过 tf，机器人系统可以更加灵活和强大地处理多坐标系数据。 28 | 29 | ## tf的初步使用 30 | 描述机器人与地图之间的坐标变换， 允许机器人系统中的不同组件在不同坐标系之间进行无缝的转换，以实现正确的坐标变换和传递。机器人通常由多个部件组成，例如传感器、执行器、末端执行器等，它们在不同的坐标系中工作。这些坐标系之间的关系可能是固定的，也可能是动态的。在这里，我们暂时先试用导航功能里最小集合：map-->base_link之间的变化关系。 31 | 32 | ROS中提供了一个专用工具集：[tf](https://github.com/ros/geometry) / [tf2](https://github.com/ros2/geometry2) 33 | 同时也提供了一系列的tf工具。 34 | 我们先构建一个map->base_link的坐标关系，有了这个坐标关系，我们后续就可以进行机器人路径规划相关的算法开发了。 35 | 打开一个terminal，然后执行如下命令可以发布一个map->base_link的静态tf关系： 36 | ``` 37 | ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map base_link 38 | ``` 39 | 然后打开另一个terminal，执行如下命令，可以将当前系统中的tf关系可视化出来。 40 | ``` 41 | ros2 run tf2_tools view_frames 42 | ``` 43 | 这条命令会在当前目录生成两个文件：`frames.gv` `frames.pdf` 实际上属于同一个内容的不同表达形式，此时打开pdf文件可以看到如下内容： 44 | ![simple tf](./../images/tf_simple.png) 45 | -------------------------------------------------------------------------------- /source/docs/工作空间.md: -------------------------------------------------------------------------------- 1 | # 工作空间 2 | 3 | ## 引言 4 | 5 | ROS系统下开发的第一步，就是创建工作空间以及创建package。 6 | 7 | ## 工作空间的创建 8 | 9 | 解释什么是工作空间，为什么需要创建工作空间。 10 | 提供创建工作空间的步骤，包括选择工作空间的位置、创建和初始化工作空间等。 11 | 12 | 13 |     在ROS 2中，工作空间（Workspace）是用于组织和管理ROS2软件包的目录结构。工作空间是一个包含多个ROS2软件包的顶层目录，用于集中管理和编译这些软件包。工作空间的概念是为了方便开发者在同一环境中管理多个ROS2项目，并确保它们之间的依赖关系正确。 14 | 15 |     ROS2工作空间通常包含以下目录： 16 | 17 | - `src/`: 这是源码目录，用于存放所有的ROS 2软件包。在该目录下可以创建和管理多个ROS 2软件包，每个软件包都包含独立的功能或节点。 18 | 19 | - `build/`: 这是编译目录，用于存放编译生成的中间文件和构建产物。当您使用ROS 2构建系统编译软件包时，编译过程会生成构建文件，并将生成的库和可执行文件存放在这个目录下。 20 | 21 | - `install/`: 这是安装目录，用于存放已编译的ROS 2软件包和其它文件。在编译ROS 2软件包后，您可以通过执行`colcon install`等命令将软件包安装到这个目录下，然后ROS 2系统就可以在运行时加载和使用这些软件包。 22 | 23 |     通过如下的方式创建一个工作空间，其中dev_ws是工作空间的总入口 24 | ``` 25 | mkdir -p ~/dev_ws/src 26 | cd ~/dev_ws 27 | colcon build 28 | tree 29 | ``` 30 | 如果`tree`命令未安装，可以通过如下指令进行安装 31 | ``` 32 | sudo apt install tree 33 | ``` 34 | 可以看到如下的目录结构 35 | ``` 36 | . 37 | ├── build 38 | ├── install 39 | ├── log 40 | └── src 41 | ``` 42 | 43 | 使用工作空间的好处包括： 44 | 45 | - 将多个ROS2软件包组织在一个目录下，方便管理和版本控制。 46 | - 能够正确处理软件包之间的依赖关系，确保依赖的软件包都能正确编译和加载。 47 | - 在同一工作空间中可以集中管理多个项目，使得项目之间的代码和资源不会互相干扰。 48 | 49 |     要使用ROS2工作空间，需要在工作空间的顶层目录执行`colcon build`等构建命令，以编译所有的ROS2软件包。在工作空间中，您可以使用`colcon build`来构建软件包，使用`colcon test`来运行测试，使用`colcon install`来安装软件包，等等。 50 | 51 |     请注意，ROS2工作空间不同于ROS1的catkin工作空间。ROS2使用colcon作为主要的构建工具，而ROS1使用catkin。两者的工作空间结构和构建方式略有不同。 52 | 53 | ## Package 的概念 54 | 55 |     在ROS2中，package（软件包）是一种组织ROS代码和资源的方式。它是ROS2工程中的基本单元，用于将相关的功能、节点、消息、服务、动作、参数等相关内容组织在一起。每个package都有自己的文件夹，包含了用于构建和运行ROS 2节点的所有必要文件。 56 | 57 | 以下是ROS 2中package的主要概念和组成部分： 58 | 59 | - package.xml：每个ROS2 package都包含一个名为package.xml的XML文件，用于描述package的基本信息，如名称、版本、作者、依赖关系等。它还指定了package的构建和安装规则。 60 | 61 | - CMakeLists.txt：每个ROS2 package都包含一个名为CMakeLists.txt的CMake文件，用于配置构建过程。这个文件描述了如何编译package中的代码、链接库和可执行文件等。 62 | 63 | - src文件夹：在ROS2 package中，通常包含一个src文件夹，用于存放源代码文件。ROS节点、服务、动作等的实现通常放在这个文件夹下。 64 | 65 | - include文件夹：在ROS2 package中，通常包含一个include文件夹，用于存放头文件。这些头文件定义了ROS节点、服务、动作等的接口。 66 | 67 | - launch文件夹：在ROS2 package中，通常包含一个launch文件夹，用于存放启动文件。这些文件用于启动ROS节点和配置节点参数。 68 | 69 | - msg和srv文件夹：在ROS2 package中，通常包含msg和srv文件夹，分别用于存放自定义的消息和服务定义。 70 | 71 | - action文件夹：在ROS 2 package中，通常包含action文件夹，用于存放自定义的动作定义。 72 | 73 | - 参数文件：在ROS 2 package中，可能还包含参数文件，用于配置ROS节点的参数。 74 | 75 |     通过将相关功能和资源组织在一个package中，可以使代码更易于管理、复用和分发。在ROS2中，使用工具如colcon来构建和安装packages，使得创建和维护ROS2项目更加方便和高效。 76 | 77 | ## 创建 Package 78 | 79 | 使用如下命令创建package 80 | 81 | ``` 82 | ros2 pkg create --build-type ament_cmake --node-name my_node my_package 83 | ``` 84 | 其中： 85 | - `ros2`: ROS2命令行工具，用于管理ROS 2工程和操作。 86 | 87 | - `pkg create`: 这是ros2工具中用于创建ROS2软件包的子命令。 88 | 89 | - `--build-type ament_cmake`: 这是选项，用于指定创建的ROS2软件包的构建类型。ament_cmake是ROS2的构建系统之一，它是构建ROS2软件包的常用方式。除了ament_cmake，ROS2还支持其他构建系统，如ament_python和colcon。 90 | 91 | - `--node-name my_node`: 这是选项，用于指定在创建的ROS2软件包中要创建的节点（Node）的名称。Node是ROS2的基本执行单元，用于实现不同功能的模块化代码。在这里，我们为软件包创建一个名为my_node的节点。 92 | 93 | - `my_package`: 这是命令的最后一个参数，用于指定要创建的ROS2软件包的名称。在这里，我们为软件包取名为my_package。 94 | 95 |     综上所述，这个命令的作用是创建一个名为my_package的ROS2软件包，使用ament_cmake作为构建系统，并在该软件包中创建一个名为my_node的节点。创建软件包后，您可以在其中编写ROS2节点代码，并通过构建系统进行编译和构建。 96 | 97 | 可以使用如下命令查看更多的可选用参数 98 | ``` 99 | ros2 pkg create --help 100 | ``` 101 | 102 | ## Package 结构和文件 103 | 104 | ``` 105 | cd my_package/src/ 106 | ros2 pkg create --build-type ament_cmake --library-name topic --node-name pub --maintainer-name zcf --maintainer-email xxxx@162.com my_package 107 | tree 108 | ``` 109 | 使用`tree`指令进行查看当前目录的文件结构如下： 110 | ``` 111 | . 112 | ├── CMakeLists.txt 113 | ├── include 114 | │   └── my_package 115 | │   ├── topic.hpp 116 | │   └── visibility_control.h 117 | ├── package.xml 118 | └── src 119 | ├── pub.cpp 120 | └── topic.cpp 121 | 122 | 3 directories, 6 files 123 | ``` 124 | 125 | ### package.xml 126 |     如下是刚刚新建的ROS（Robot Operating System）的包（package）的XML文件，用于描述ROS包的基本信息和依赖关系。在ROS中，通过解析这个文件可以实现自动化构建和管理ROS包。相关条目以注释的形式进行说明： 127 | 128 | ``` 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | my_package 137 | 138 | 0.0.0 139 | 140 | TODO: Package description 141 | 142 | zcf 143 | 144 | TODO: License declaration 145 | 146 | ament_cmake 147 | 148 | ament_lint_auto 149 | ament_lint_common 150 | 151 | 152 | 153 | ament_cmake 154 | 155 | 156 | ``` 157 | 158 | ### CMakeList.txt 159 | 下面是刚创建包的CMakeLists.txt文件，是ROS2软件包的构建脚本，通过解析这个文件可以自动化地构建和管理ROS2软件包（package）。下面注释解释每个字段的含义： 160 | ``` 161 | # 指定需要的CMake版本。 162 | cmake_minimum_required(VERSION 3.8) 163 | # 指定项目名称为`my_package`，这是ROS2软件包的名称。 164 | project(my_package) 165 | # 检查编译器类型，如果是GNU C++编译器或者是Clang编译器，则添加编译选项`-Wall -Wextra -Wpedantic`。 166 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 167 | add_compile_options(-Wall -Wextra -Wpedantic) 168 | endif() 169 | 170 | # find dependencies 171 | # 查找ament_cmake包，这是ROS2构建系统的核心包，必须要求。 172 | find_package(ament_cmake REQUIRED) 173 | # 查找ament_cmake_ros包，这是用于ROS2软件包的CMake功能的额外包。 174 | find_package(ament_cmake_ros REQUIRED) 175 | # uncomment the following section in order to fill in 176 | # further dependencies manually. 177 | # find_package( REQUIRED) 178 | # 添加一个名为topic的库，并将源文件`src/topic.cpp`与之关联。 179 | add_library(topic src/topic.cpp) 180 | # 设置topic库的编译特性，要求支持C99和C++17。 181 | target_compile_features(topic PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 182 | # 设置topic库的包含目录，包括构建时和安装时的目录。 183 | target_include_directories(topic PUBLIC 184 | $ 185 | $) 186 | 187 | # Causes the visibility macros to use dllexport rather than dllimport, 188 | # which is appropriate when building the dll but not consuming it. 189 | # 设置topic库的编译定义，这里定义了一个名为`MY_PACKAGE_BUILDING_LIBRARY`的宏。 190 | target_compile_definitions(topic PRIVATE "MY_PACKAGE_BUILDING_LIBRARY") 191 | # 将include目录的内容安装到目标系统的include目录下。 192 | install( 193 | DIRECTORY include/ 194 | DESTINATION include 195 | ) 196 | # 安装topic库到目标系统的lib目录下。 197 | install( 198 | TARGETS topic 199 | EXPORT export_${PROJECT_NAME} 200 | ARCHIVE DESTINATION lib 201 | LIBRARY DESTINATION lib 202 | RUNTIME DESTINATION bin 203 | ) 204 | # 添加一个名为pub的可执行文件，并将源文件`src/pub.cpp`与之关联。 205 | add_executable(pub src/pub.cpp) 206 | target_include_directories(pub PUBLIC 207 | $ 208 | $) 209 | # 设置pub可执行文件链接到topic库。 210 | target_link_libraries(pub topic) 211 | # 安装pub可执行文件到目标系统的lib/my_package目录下。 212 | install(TARGETS pub 213 | DESTINATION lib/${PROJECT_NAME}) 214 | # 检查是否需要构建测试。 215 | if(BUILD_TESTING) 216 | find_package(ament_lint_auto REQUIRED) 217 | # the following line skips the linter which checks for copyrights 218 | # uncomment the line when a copyright and license is not present in all source files 219 | #set(ament_cmake_copyright_FOUND TRUE) 220 | # the following line skips cpplint (only works in a git repo) 221 | # uncomment the line when this package is not in a git repo 222 | #set(ament_cmake_cpplint_FOUND TRUE) 223 | ament_lint_auto_find_test_dependencies() 224 | endif() 225 | # 导出include目录，使其他软件包可以使用这个目录。 226 | ament_export_include_directories( 227 | include 228 | ) 229 | # 导出topic库，使其他软件包可以链接到这个库。 230 | ament_export_libraries( 231 | topic 232 | ) 233 | # 导出一个名为`export_my_package`的目标。 234 | ament_export_targets( 235 | export_${PROJECT_NAME} 236 | ) 237 | 完成ROS2软件包的构建配置，生成配置文件并导出软件包的元数据。 238 | ament_package() 239 | ``` 240 | 241 | 242 | ## 构建和编译 243 | 执行如下命令进行工作空间的编译 244 | ``` 245 | cd dev_ws 246 | colcon build 247 | ``` 248 | 会出现如下的终端日志 249 | ``` 250 | Starting >>> my_package 251 | Finished <<< my_package [0.92s] 252 | 253 | Summary: 1 package finished [1.04s] 254 | ``` 255 | 256 | ## 总结 257 | 258 | 完成创建工作空间和 package 的过程，并理解 package 的结构和作用。但是实际开发过程中，很少这么从头去建设一个package，一般都是从已有的工程中改，这样做的好处是可以减少很多不必要的工作量。 259 | -------------------------------------------------------------------------------- /source/docs/环境安装.md: -------------------------------------------------------------------------------- 1 | # 环境安装 2 | ![galactic](./../images/galactic.png) 3 | 4 | ## 引言 5 | 6 |     为了帮助读者快速开始使用ROS，并确保他们在正确的环境中配置和安装ROS系统与书中所使用的环境保持一致。在这一小节对ROS环境安装以及navigation2的安装进行学习。 7 | 8 |     将环境安装单独安排一个章节进行描述，可以帮助零基础读者迅速了解ROS的安装流程和基本要求。对于有相关基础的环境的读者可以在不浪费时间的情况下快速调到下一个章节，了解ROS系统的基本结构和功能。 9 | 10 |     ROS的安装和配置可能涉及多个步骤和依赖项。通过提供详细的安装指导，确保读者能够正确配置ROS环境，避免因配置错误而导致的问题和不必要的麻烦。 11 | 12 |     本小节的内容主要来自ROS2的Wiki，以及navigation2的wiki。会在下方贴出相关站点链接，英文功底扎实的同学可以直接前往官方文档进行安装后，跳过本小节。 13 | 14 | 均建议使用二进制安装包进行环境配置： 15 | 16 | ``` 17 | ROS2（galactic）： https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html 18 | 19 | navigation2（galactic）： https://navigation.ros.org/getting_started/index.html#installation 20 | ``` 21 | 22 | ## 操作系统和ROS版本 23 | 24 |     书中使用的操作系统为 ubuntu 20.04 25 | 26 |     ROS2版本为galactic 27 | 28 |     尽量避免虚拟机的形式安装ubuntu，可以考虑安装双系统。或者直接安排ubuntu系统在开发的PC上。(具体双系统怎么装，就不在此罗列了) 29 | 30 |     安装顺序是先安装完成ROS，再进行navigation2的功能包安装。 31 | 32 | ## 安装ROS 33 | 34 | 详细介绍在所选操作系统上安装ROS的步骤。 35 | 包括添加ROS软件源、使用包管理器安装ROS核心软件包、设置ROS环境变量等。 36 | 37 | `apt install` 是在 Ubuntu 或 Debian 等 Linux 系统中用于安装软件包的命令。其安装流程如下： 38 | 39 | 1. 更新软件包列表：首先，系统会检查软件包列表是否过时，如果是，则会从软件源服务器下载最新的软件包列表。 40 | 41 | 2. 搜索软件包：接下来，系统会根据你提供的软件包名称在软件包列表中搜索匹配项。 42 | 43 | 3. 解析依赖关系：如果找到了匹配的软件包，系统将检查该软件包的依赖关系，以确定它是否需要其他软件包来正常运行。如果需要，系统将自动下载并安装这些依赖项。 44 | 45 | 4. 下载软件包：一旦解析了所有依赖关系，系统将从软件源服务器上下载相应的软件包文件。 46 | 47 | 5. 安装软件包：下载完成后，系统会将软件包文件解压并安装到正确的目录中。 48 | 49 | 6. 配置软件包：一些软件包在安装后需要进行一些配置。系统将执行这些配置步骤，以确保软件能够正常运行。 50 | 51 | 7. 完成安装：一旦所有步骤都成功完成，`apt install` 命令就会结束，并且软件包现在已经安装在你的系统中。 52 | 53 | 请注意，为了执行 `apt install` 命令，你需要具有管理员权限，通常通过在命令前加上 `sudo` 来获得。例如：`sudo apt install 软件包名称`。 54 | 55 | ### 设置语言环境 56 | 57 | ``` 58 | # 用于显示计算机当前的本地化（locale）设置，包括语言、字符编码等相关信息。 59 | locale # check for UTF-8 60 | # 命令组合,用于更新包列表以确保软件包信息是最新的。接着装 locales 软件包，该软件包提供设置本地化的工具和文件。 61 | sudo apt update && sudo apt install locales 62 | # 生成 en_US 和 en_US.UTF-8 两种本地化，其中 en_US.UTF-8 表示以 UTF-8 字符编码的英语本地化 63 | sudo locale-gen en_US en_US.UTF-8 64 | # 更新系统的本地化设置，将 LC_ALL 和 LANG 设置为 en_US.UTF-8，即将系统的所有本地化相关配置都设为以 UTF-8 字符编码的英语本地化。 65 | sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 66 | # 设置当前会话（session）的 LANG 环境变量为 en_US.UTF-8，这样当前会话中所有使用 locale 的程序都将遵循 UTF-8 编码的英语本地化 67 | export LANG=en_US.UTF-8 68 | # 验证刚刚配置的本地化设置是否生效 69 | locale # verify settings 70 | ``` 71 | 执行这些命令后，计算机的本地化设置将被更新为以 UTF-8 字符编码的英语本地化。UTF-8 是一种支持全球多种字符的编码方式，这意味着计算机可以正确处理和显示包括中文、日文、俄文等非ASCII字符的文本和应用程序。通过配置 UTF-8 的本地化，确保系统能够正确处理不同字符集的文本，从而提高了多语言支持和国际化能力。这对于跨语言环境下的计算机和软件来说非常重要。 72 | ### 设置软件源 73 | 74 | 将apt软件仓库添加到系统中 75 | ``` 76 | # 安装软件包"software-properties-common"。"software-properties-common" 是一个包含常用软件源管理工具的软件包。通过安装它，你可以使用诸如 "add-apt-repository" 这样的命令来添加、删除和管理软件源，方便地进行软件安装和更新。 77 | sudo apt install software-properties-common 78 | # 添加 "universe" 软件源到你的系统中。在 Ubuntu 系统中，软件源分为 "main"、"restricted"、"universe" 和 "multiverse" 四个组件，其中 "universe" 组件包含了许多自由且开源的软件包，但不受官方支持。通过添加 "universe" 软件源，你可以访问更多的软件包，扩展系统的软件选择范围。 79 | sudo add-apt-repository universe 80 | ``` 81 | 添加 "universe" 软件源，你可以访问更多的自由且开源的软件包，增加了系统软件选择的可能性。 82 | 83 | 添加 ROS2的GPG密钥 84 | ``` 85 | # 更新计算机中的软件包列表。它会检查当前系统所配置的软件源，并获取最新的软件包信息，但不会安装或更新实际的软件包。 86 | # 安装 curl 工具。curl 是一个用于执行网络请求的强大工具，它可以用于下载文件，访问API等。安装 curl 后，你就可以在终端中使用 curl 命令。 87 | sudo apt update && sudo apt install curl 88 | # 下载 ROS 密钥文件，并将其保存到 /usr/share/keyrings/ros-archive-keyring.gpg 文件中。密钥文件是ROS软件源的认证文件，用于确保从ROS软件源下载的软件包是可信的。 89 | sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 90 | ``` 91 | 通过使用 ROS 的认证密钥，用户可以确保下载的 ROS 软件包是来自官方软件源的，不会被篡改或植入恶意代码。这有助于提高软件包的安全性和可靠性，保障了 ROS 系统的稳定运行和开发环境的安全性 92 | 93 | 然后将软件源添加到系统源列表中 94 | ``` 95 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null 96 | ``` 97 | - 将 ROS 2 软件源的地址写入到 /etc/apt/sources.list.d/ros2.list 文件中。在 Ubuntu 和 Debian 等 Linux 系统中，软件包管理器（APT）会从 /etc/apt/sources.list 和 /etc/apt/sources.list.d/ 目录下的文件读取软件源的信息，以确定从哪些位置下载软件包。 98 | - 通过写入 ROS 2 软件源的地址，系统的软件包管理器就能知道要从哪里获取 ROS 2 相关的软件包和更新。 99 | - ROS 2 软件源地址中包含了之前下载的 ROS 公钥文件的路径 /usr/share/keyrings/ros-archive-keyring.gpg，这样软件包管理器在下载软件包时，会使用这个公钥来验证软件包的合法性，以确保下载的软件包是来自 ROS 官方软件源的，不会被篡改或植入恶意代码。 100 | 通过这些步骤，用户就可以成功添加 ROS 2 软件源，从而可以通过软件包管理器安装 ROS 2 相关的软件包和更新，从而进行 ROS 2 的开发和运行。### 安装ROS2的功能包 101 | 102 | 完成软件源设置之后更新apt仓库的缓存 103 | ``` 104 | sudo apt update 105 | ``` 106 | 107 | 检查已安装软件包的更新，并将可用的更新版本下载并安装到系统中 108 | ``` 109 | sudo apt upgrade 110 | ``` 111 | 112 | 推荐安装Desktop版本，其包括了一系列可视化工具和示例程序 113 | ``` 114 | sudo apt install ros-galactic-desktop 115 | ``` 116 | 安装开发工具：编译器和其他构建ROS包的工具 117 | ``` 118 | sudo apt install ros-dev-tools 119 | ``` 120 | 121 | ### 设置环境变量 122 | 安装位置一般会位于/opt/ros/galactic/目录 123 | ``` 124 | # Replace ".bash" with your shell if you're not using bash 125 | # Possible values are: setup.bash, setup.sh, setup.zsh 126 | source /opt/ros/galactic/setup.bash 127 | ``` 128 | 129 | ### ROS2环境验证 130 | 打开一个terminal，运行如下指令： 131 | ``` 132 | source /opt/ros/galactic/setup.bash 133 | ros2 run demo_nodes_cpp talker 134 | ``` 135 | ![talker](./../images/talker.png) 136 | 137 | 打开另一个terminal，运行如下指令： 138 | ``` 139 | source /opt/ros/galactic/setup.bash 140 | ros2 run demo_nodes_py listener 141 | ``` 142 | ![listener](./../images/listener.png) 143 | 144 |     如果两个终端的指令执行和输出显示均如图所示，那么说明ROS2的环境已经安装OK，可以进行下一步的navigation2功能包的安装。 145 | 146 | ### ROS2卸载 147 |     如果决定退坑不玩儿了，可以选择保留环境或者卸载环境，卸载时按照如下指令执行即可。 148 | 卸载ROS2相关软件包 149 | ``` 150 | sudo apt remove ~nros-galactic-* && sudo apt autoremove 151 | ``` 152 | 删除ROS2相关的软件源 153 | ``` 154 | sudo rm /etc/apt/sources.list.d/ros2.list 155 | sudo apt update 156 | sudo apt autoremove 157 | # Consider upgrading for packages previously shadowed. 158 | sudo apt upgrade 159 | ``` 160 | 161 | ## 安装navigation2 162 | 163 |     在保证ROS2环境安装并配置成功之后，就可以进行navigation2的功能包安装了。 164 | 165 | ### 安装 166 | 安装Nav2相关的功能包 167 | ``` 168 | sudo apt install ros-galactic-navigation2 169 | sudo apt install ros-galactic-nav2-bringup 170 | ``` 171 | 172 | 安装Turtlebot3仿真包 173 | ``` 174 | sudo apt install ros-galactic-turtlebot3-gazebo 175 | ``` 176 | 177 | ### 功能验证 178 | 打开一个terminal，执行如下指令设置所需要的环境变量 179 | ``` 180 | source /opt/ros/galactic/setup.bash 181 | export TURTLEBOT3_MODEL=waffle 182 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/galactic/share/turtlebot3_gazebo/models 183 | ``` 184 | 185 | 然后执行如下指令 186 | ``` 187 | ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False 188 | ``` 189 | 190 | 如果能看到如下效果图，那么说明navigation2的功能包安装成功。 191 | ![navigation](./../images/navigation_with_recovery_behaviours.gif) 192 | 193 |     第一次启动功能，可能会由于gazebo模型加载需要等的久一些。但是我们这本书就是要写一个替代gazebo的仿真器来进行导航功能验证，所以如果实在无法启动 只要前面安装过程都是顺利无报错的，也不影响后续内容的进行。 194 | 195 | ## 常见安装问题和解决方法 196 | ``` 197 | source /opt/ros/galactic/setup.bash 198 | ``` 199 | 执行如上指令，是为了设置ROS2相关的环境变量。如果环境里只会用到刚安装的这一ROS版本，那么可以使用下面的指令，将该步骤写入到.bashrc中，避免每次启动终端都要先运行如上的source指令了。 200 | ``` 201 | echo 'source /opt/ros/galactic/setup.bash' >> ~/.bashrc 202 | ``` 203 | 204 |     在安装过程中，不可避免的会遇到一些奇奇怪怪的问题，要学会善于利用搜索工具，google或者baidu，还有ros社区等。 205 | 206 |     如果后续我建立沟通交流渠道的话，也可以在沟通群里寻求伙伴的帮助。 207 | 208 | ## 总结 209 | 210 |     成功安装和配置ROS环境和navigation2功能栈。 211 | 212 | -------------------------------------------------------------------------------- /source/docs/第一个全局路径规划器.md: -------------------------------------------------------------------------------- 1 | # 第一个全局路径规划器 2 | 文档翻译自： [Writing a New Planner Plugin](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html) 3 | 4 | ![strightline](./../images/straightline.gif) 5 | 6 | ## 概述 7 | 本教程介绍了如何创建自己的规划器插件（Planner Plugin）。 8 | 9 | ## 要求 10 | 11 | - ROS 2（二进制或从源代码构建） 12 | - Nav2（包括依赖项） 13 | 14 | ## 教程步骤 15 | 16 | ### 创建新的规划器插件 17 | 18 | 我们将创建一个简单的直线规划器（straight-line planner）。本教程中的代码示例可以在[nav_simulator](https://github.com/cf-zhang/nav_simulator)仓库中的planner找到。这个包可以被视为编写规划器插件的参考。 19 | 20 | 我们的示例插件继承自基类nav2_core::GlobalPlanner。基类提供了5个纯虚方法来实现规划器插件。规划器插件将被规划器服务器用于计算轨迹。让我们更详细地了解编写规划器插件所需的方法。 21 | 22 | | 虚函数 | 函数描述 | 需要重写？ | 23 | |---------|---------|---------| 24 | | configure() | 当planner server进入on_configure状态时调用该方法。理想情况下，这个方法应该执行ROS参数的声明和规划器成员变量的初始化。该方法接受4个输入参数：父节点的共享指针，规划器名称，tf缓冲区的指针以及代价地图的共享指针。 | 是 | 25 | | activate() | 当planner_server进入on_activate状态时调用该方法。理想情况下，这个方法应该实现规划器进入活动状态前必要的操作。 | 是 | 26 | | deactivate() | 当planner server进入on_deactivate状态时调用该方法。理想情况下，这个方法应该实现规划器进入非活动状态前必要的操作。| 是 | 27 | | cleanup() | 当planner server进入on_cleanup状态时调用该方法。理想情况下，这个方法应该清理为规划器创建的资源。| 是 | 28 | | createPlan() | 当planner server需要为指定的起始姿态和目标姿态生成全局规划时调用该方法。该方法返回携带全局规划的nav_msgs::msg::Path。该方法接受2个输入参数：起始姿态和目标姿态。| 是 | 29 | 30 | 对于本教程，我们将使用方法`StraightLine::configure()`和`StraightLine::createPlan()`来创建直线规划器。 31 | 32 | 在规划器中，configure()方法必须从ROS参数中设置成员变量并进行任何所需的初始化。 33 | 34 | ``` 35 | node_ = parent; 36 | tf_ = tf; 37 | name_ = name; 38 | costmap_ = costmap_ros->getCostmap(); 39 | global_frame_ = costmap_ros->getGlobalFrameID(); 40 | 41 | // Parameter initialization 42 | nav2_util::declare_parameter_if_not_declared(node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1)); 43 | node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_); 44 | ``` 45 | 46 | 在这里，name_ + ".interpolation_resolution" 获取了与我们的规划器特定的ROS参数`interpolation_resolution`。Nav2允许加载多个插件，并为了保持组织结构的清晰，每个插件都映射到某个ID或名称。现在，如果我们想要检索特定插件的参数，我们使用.，就像上面的代码片段中所做的那样。例如，我们的示例规划器映射到名称"GridBased"，要检索特定于"GridBased"的`interpolation_resolution`参数，我们使用GridBased.interpolation_resolution。换句话说，GridBased被用作插件特定参数的命名空间。当我们讨论参数文件（或参数文件）时，我们将更多地了解这一点。 47 | 48 | 在`createPlan()`方法中，我们需要根据给定的起始姿态和目标姿态创建一条路径。`StraightLine::createPlan()`被调用，使用起始姿态和目标姿态来解决全局路径规划问题。成功后，它将路径转换为`nav_msgs::msg::Path`并返回给规划器服务器。下面的注释显示了这个方法的实现。 49 | 50 | ``` 51 | nav_msgs::msg::Path global_path; 52 | 53 | // Checking if the goal and start state is in the global frame 54 | if (start.header.frame_id != global_frame_) { 55 | RCLCPP_ERROR( 56 | node_->get_logger(), "Planner will only except start position from %s frame", 57 | global_frame_.c_str()); 58 | return global_path; 59 | } 60 | 61 | if (goal.header.frame_id != global_frame_) { 62 | RCLCPP_INFO( 63 | node_->get_logger(), "Planner will only except goal position from %s frame", 64 | global_frame_.c_str()); 65 | return global_path; 66 | } 67 | 68 | global_path.poses.clear(); 69 | global_path.header.stamp = node_->now(); 70 | global_path.header.frame_id = global_frame_; 71 | // calculating the number of loops for current value of interpolation_resolution_ 72 | int total_number_of_loop = std::hypot( 73 | goal.pose.position.x - start.pose.position.x, 74 | goal.pose.position.y - start.pose.position.y) / 75 | interpolation_resolution_; 76 | double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop; 77 | double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop; 78 | 79 | for (int i = 0; i < total_number_of_loop; ++i) { 80 | geometry_msgs::msg::PoseStamped pose; 81 | pose.pose.position.x = start.pose.position.x + x_increment * i; 82 | pose.pose.position.y = start.pose.position.y + y_increment * i; 83 | pose.pose.position.z = 0.0; 84 | pose.pose.orientation.x = 0.0; 85 | pose.pose.orientation.y = 0.0; 86 | pose.pose.orientation.z = 0.0; 87 | pose.pose.orientation.w = 1.0; 88 | pose.header.stamp = node_->now(); 89 | pose.header.frame_id = global_frame_; 90 | global_path.poses.push_back(pose); 91 | } 92 | 93 | global_path.poses.push_back(goal); 94 | 95 | return global_path; 96 | ``` 97 | 98 | 剩下的方法虽然不被使用，但是必须覆盖它们。根据规则，我们确实覆盖了所有方法，只是将它们保留为空白。 99 | 100 | ### 导出规划器插件 101 | 现在我们已经创建了自定义规划器，我们需要导出我们的规划器插件，以便它对规划器服务器可见。插件在运行时加载，如果它们不可见，那么我们的规划器服务器将无法加载它们。在ROS 2中，插件的导出和加载由pluginlib处理。 102 | 103 | 回到我们的教程，类planner::StraightLine以nav2_core::GlobalPlanner的形式动态加载，这是我们的基类。 104 | 105 | 1. 要导出规划器，我们需要提供两行代码： 106 | ``` 107 | #include "pluginlib/class_list_macros.hpp" 108 | PLUGINLIB_EXPORT_CLASS(planner::StraightLine, nav2_core::GlobalPlanner) 109 | ``` 110 | 111 | 请注意，需要使用pluginlib来导出插件的类。Pluginlib将提供名为PLUGINLIB_EXPORT_CLASS的宏，它会执行导出的所有工作。 112 | 113 | 将这些行放在文件末尾是一种良好的做法，但在技术上，您也可以放在文件顶部。 114 | 115 | 2. 接下来，需要在包的根目录中创建插件的描述文件。例如，在我们的教程包中创建一个名为global_planner_plugin.xml的文件。该文件包含以下信息： 116 | 117 | - `library path`：插件的库名称及其位置。 118 | 119 | - `class name`：类的名称。 120 | 121 | - `class type`：类的类型。 122 | 123 | - `base class`：基类的名称。 124 | 125 | - `description`：插件的描述。 126 | 127 | ``` 128 | 129 | 130 | This is an example plugin which produces straight path. 131 | 132 | 133 | ``` 134 | 135 | 3. 下一步是使用CMakeLists.txt导出插件，使用cmake函数pluginlib_export_plugin_description_file()。此函数将插件描述文件安装到share目录，并设置ament索引以使其可发现。 136 | ``` 137 | pluginlib_export_plugin_description_file(nav2_core planner_plugins.xml) 138 | ``` 139 | 140 | 4. 插件描述文件也应该添加到package.xml文件中。 141 | 142 | ``` 143 | 144 | ament_cmake 145 | 146 | 147 | ``` 148 | 5. 编译并应该注册。接下来，我们将使用这个插件。 149 | 150 | ### 通过params文件传递插件名称 151 | 要启用插件，我们需要修改planner.yaml文件，以替换以下参数。 152 | 153 | 注意 154 | 155 | 对于Galactic或更高版本，plugin_names和plugin_types已被替换为插件名称的单个plugins字符串向量。类型现在在plugin_name命名空间中在plugin:字段中定义（例如，plugin: MyPlugin::Plugin）。代码块中的内联注释将帮助您完成此操作。 156 | 157 | ``` 158 | planner_server: 159 | ros__parameters: 160 | plugins: ["GridBased"] 161 | use_sim_time: True 162 | GridBased: 163 | plugin: "nav2_navfn_planner/NavfnPlanner" # For Foxy and later 164 | tolerance: 2.0 165 | use_astar: false 166 | allow_unknown: true 167 | ``` 168 | 替换为 169 | ``` 170 | planner_server: 171 | ros__parameters: 172 | plugins: ["GridBased", "straight"] 173 | use_sim_time: True 174 | GridBased: 175 | plugin: "nav2_navfn_planner/NavfnPlanner" # For Foxy and later 176 | tolerance: 2.0 177 | use_astar: false 178 | allow_unknown: true 179 | 180 | straight: 181 | plugin: "planner/StraightLine" 182 | interpolation_resolution: 0.1 183 | ``` 184 | 185 | 在上面的代码片段中，您可以观察 planner/StraightLine planner 到其 id GridBased 的映射。为了传递特定于插件的参数，我们使用了 .。 186 | 187 | 由于planner server可以加载多个规划器，所以planner bridge中设计了一个`planner_id`参数，可以通过planner_bridge节点的`planner_id`来指定该次测试是使用`GridBased`还是使用`straight`规划器。 188 | 189 | ### 运行直线插件 190 | 191 | 按照上一小节《路径规划仿真二》中的描述进行路径规划效果的验证，可以有如下的效果展示： 192 | 193 | ![straig line](./../images/straight.gif) 194 | -------------------------------------------------------------------------------- /source/docs/第一个启动脚本.md: -------------------------------------------------------------------------------- 1 | # launch 2 | 3 | ## 介绍 4 | 在ROS2中，`launch` 文件是用于管理和配置机器人系统中各种节点（ROS2程序）以及它们之间的关系的一种XML格式的配置文件。`launch` 文件允许您在一个地方定义和组织多个节点的启动、参数、命名空间、参数设置、命令行参数等，以便更方便地启动和管理整个机器人系统。 5 | 6 | 以下是关于ROS2中`launch`文件的一些重要概念和作用： 7 | 8 | - **组织节点启动：** 9 | 通过`launch`文件，您可以同时启动多个ROS 2节点，这些节点可以是传感器、执行器、算法、控制器等等。这使得整个系统的启动和管理变得更加方便。 10 | 11 | - **参数设置和配置：** 12 | 使用`launch`文件，您可以为每个节点设置参数和配置，以便在启动时提供定制化的设置。这使得节点的配置和参数管理变得更加集中和容易。 13 | 14 | - **命名空间管理：** 15 | `launch`文件允许您为每个节点设置命名空间，从而在启动时为节点提供独立的命名空间环境，防止节点之间的冲突。 16 | 17 | - **启动顺序和依赖：** 18 | 通过`launch`文件，您可以指定节点的启动顺序以及节点之间的依赖关系。这对于确保节点在正确的顺序启动以及正确处理依赖关系非常重要。 19 | 20 | - **可读性和可维护性：** 21 | 使用`launch`文件可以将节点的配置和启动参数集中在一个地方，使得系统的配置更加可读性和可维护性。 22 | 23 | - **条件启动：** 24 | `launch`文件还支持根据条件来启动特定的节点或一组节点，这在根据环境或任务需要灵活地调整系统行为时非常有用。 25 | 26 | - **可重用性：** 27 | 您可以将`launch`文件设计为可重用的模块，以便在不同的机器人系统中使用相似的配置。 28 | 29 | - **启动参数传递：** 30 | 使用`launch`文件，您可以将参数传递给节点，以便在启动时动态地配置节点的行为。 31 | 32 | 想了解更详细的内容，可以追本溯源： [Wiki](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Launch/Launch-Main.html) 33 | 34 | 35 | ## 仿真器的第一个启动脚本 36 | 在完成了地图的加载与map->base_link的tf关系发布之后，此时的系统状态就已经满足了机器人导航中的路径规划算法运行的条件，但是如果每次都手动启动各个节点，再进行算法验证，略显繁琐。尤其是当项目逐渐扩大之后，启动一次算法验证，要打开许多个终端界面，会让人崩溃。所以我们需要将所有依赖的节点都通过一个启动脚本文件进行管理，当需要做功能验证的时候，只需要一个命令就可以完成所有依赖加载。 37 | 38 | 接下来， 39 | - 在`nav_simulator/src/simulator`目录下创建`launch`文件夹，并创建一个`planner_simulator.launch.py`文件 40 | - 在`nav_simulator/src/simulator`目录下创建`rviz`文件夹，用于保存rviz的配置文件 41 | 然后将map_server、tf以及rviz都依次添加到脚本中，完成本小节内容之后，就具备了机器人导航路径开发的所有条件。 42 | 43 | 此时，还需要修改CMakeList.txt，修改如下install条目，使得`launch`文件夹移动到编译产物目录下，方便后期的debian文件制作以及ros2相关启动命令进行文件索引： 44 | ``` 45 | ... 46 | install( 47 | DIRECTORY maps launch 48 | DESTINATION share/${PROJECT_NAME} 49 | ) 50 | ... 51 | ``` 52 | 这里navigation2中引入了一个lifecycle的概念，lifecycle manager 负责管理 lifecycle node。用于管理节点进程的声明周期： configure， activate， deactivate， cleanup等。 53 | ![lifecycle](./../images/life_cycle_sm.png) 54 | 下面以注释的形式，对planner_simulator.launch.py文件内容进行解释 55 | ``` 56 | import os 57 | from launch import LaunchDescription 58 | from launch_ros.actions import Node 59 | from ament_index_python.packages import get_package_share_directory 60 | 61 | def generate_launch_description(): 62 | # 获取当前package的目录，并以此得到地图文件的文件名 63 | package_dir = get_package_share_directory('simulator') 64 | yaml_filename = os.path.join(package_dir, 'maps', 'a.yaml') 65 | # 设置rviz的加载配置文件位置 66 | rviz_config = os.path.join(package_dir, 'rviz', 'planner.rviz') 67 | # 设置需要被lifecycle manager管理的lifecycle node 68 | lifecycle_nodes = ['map_server'] 69 | return LaunchDescription([ 70 | # 启动仿真时间节点，在/clock话题中发布时钟 71 | Node( 72 | package='simulator', 73 | executable='clock_node', 74 | name='clock', 75 | output='screen' 76 | ), 77 | # 发布一个静态的坐标变换关系，用于描述map->base_link 78 | Node( 79 | package='tf2_ros', 80 | executable='static_transform_publisher', 81 | parameters=[{'use_sim_time': True}], 82 | arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link']), 83 | # 打开一个用于显示的rviz2终端 84 | Node( 85 | package='rviz2', 86 | executable='rviz2', 87 | name='rviz2', 88 | arguments=['-d', rviz_config], 89 | parameters=[{'use_sim_time': True}], 90 | ), 91 | # 启动map_server并加载上面变量中的map文件 92 | Node( 93 | package='nav2_map_server', 94 | executable='map_server', 95 | name='map_server', 96 | output='screen', 97 | parameters=[{'yaml_filename': yaml_filename}, 98 | {'use_sim_time': True}]), 99 | # 启动lifecycle manager节点，用于管理map_server的生命周期 100 | Node( 101 | package='nav2_lifecycle_manager', 102 | executable='lifecycle_manager', 103 | name='lifecycle_manager_simulator', 104 | output='screen', 105 | parameters=[{'use_sim_time': True}, 106 | {'autostart': True}, 107 | {'node_names': lifecycle_nodes}]) 108 | ]) 109 | ``` 110 | 111 | 到这里，就具备了ROS机器人导航中路径规划的全部条件。 112 | ![rviz](./../images/rviz.gif) 113 | -------------------------------------------------------------------------------- /source/docs/虚拟时钟.md: -------------------------------------------------------------------------------- 1 | # 虚拟时钟 2 | 3 | 在ROS2中，如果你的节点需要使用仿真时间来与其他节点同步，你可以将 use_sim_time 参数设置为 true。这样，节点会订阅 /clock 话题，并使用从该话题接收到的时间信息，而不是从系统时钟获取时间。这使得在仿真器中进行节点通信更加准确，因为所有节点都将使用仿真器提供的统一时间。 4 | ## 开发环境 5 | 以`nav_simulator`为工作空间进行开发，所以在`nav_simulator`目录中新建`src`目录。 6 | ``` 7 | cd nav_simulator 8 | mkdir src 9 | ``` 10 | 并修改.gitignore文件，将colcon编译系统的产物不加入git管理，编辑.gitignore文件，加入如下说明： 11 | ``` 12 | # ROS 13 | log/ 14 | install/ 15 | ``` 16 | 17 | 整个仿真器开发从虚拟时钟开始着手，所以首先需要建立一个package用于管理开发过程： 18 | ``` 19 | cd src 20 | 21 | ros2 pkg create --build-type ament_cmake --node-name clock_node --dependencies rclcpp rosgraph_msgs --maintainer-name zhangchuanfa --maintainer-email chuanfazhang1992@gmail.com --description "a simple simulator for navgation" --license MIT simulator 22 | ``` 23 | 24 | 并通过tree命令查看当前目录结构如下： 25 | ``` 26 | . 27 | └── simulator 28 | ├── CMakeLists.txt 29 | ├── include 30 | │   └── simulator 31 | ├── package.xml 32 | └── src 33 | └── clock_node.cpp 34 | 35 | 4 directories, 3 files 36 | ``` 37 | 此时返回到nav_simulator目录中，执行`colcon build`,进行第一次编译。 38 | ``` 39 | Starting >>> simulator 40 | Finished <<< simulator [1.48s] 41 | 42 | Summary: 1 package finished [1.59s] 43 | 44 | ``` 45 | 46 | 然后通过运行`ros2 run simulator clock_node`指令后可以得到如下输出： 47 | ``` 48 | hello world simulator package 49 | ``` 50 | 这是通过pkg创建package之后默认的打印效果， 51 | 52 | ## 代码与释意 53 | 此时基本开发环境就绪，可以进行虚拟时钟节点的编码, 54 | src/clock_node.cpp的内容如下： 55 | ``` 56 | #include 57 | #include 58 | #include 59 | #include 60 | 61 | using namespace std::chrono_literals; 62 | 63 | namespace simulator{ 64 | 65 | class ClockPublisher : public rclcpp::Node 66 | { 67 | public: 68 | ClockPublisher() 69 | : rclcpp::Node("clock_publisher"),message_(rosgraph_msgs::msg::Clock()) 70 | { 71 | 72 | publisher_ = this->create_publisher("/clock", 10); 73 | timer_ = this->create_wall_timer( 74 | 10ms, std::bind(&ClockPublisher::timer_callback, this)); 75 | } 76 | 77 | private: 78 | 79 | void timer_callback() 80 | { 81 | message_.clock.nanosec += 10000000; 82 | if(message_.clock.nanosec >= 1000000000){ 83 | message_.clock.sec += 1; 84 | message_.clock.nanosec = 0; 85 | } 86 | publisher_->publish(std::move(message_)); 87 | } 88 | rclcpp::TimerBase::SharedPtr timer_; 89 | rosgraph_msgs::msg::Clock message_; 90 | rclcpp::Publisher::SharedPtr publisher_; 91 | }; 92 | } 93 | int main(int argc, char * argv[]) 94 | { 95 | rclcpp::init(argc, argv); 96 | auto clock_node = std::make_shared(); 97 | rclcpp::spin(clock_node->get_node_base_interface()); 98 | rclcpp::shutdown(); 99 | return 0; 100 | } 101 | 102 | ``` 103 | 这段代码创建了一个ROS 2节点，该节点会定期发布模拟时钟消息，模拟时间的流逝。通过定时器回调函数，每隔10毫秒，时钟消息的时间会增加10毫秒，当纳秒部分达到1秒（1,000,000,000纳秒）时，秒部分会增加1，纳秒部分会归零。这种模拟可以用于ROS 2系统的仿真环境中，以模拟时间的推移。 104 | 105 | 下面逐段解释代码的各个部分： 106 | 107 | ``` 108 | #include 109 | #include 110 | #include 111 | #include 112 | ``` 113 | 这些是所需的头文件，包括了与时间、ROS 2节点、消息等相关的头文件。 114 | 115 | ``` 116 | using namespace std::chrono_literals; 117 | ``` 118 | 这一行允许在代码中使用诸如 10ms 这样的时间单位, 定义在`#include `。它简化了对时间的操作。 119 | ``` 120 | namespace simulator { 121 | // ... 122 | } 123 | 124 | ``` 125 | 定义项目的命名空间，这个命名空间包含了仿真程序的主要组件。 126 | 127 | ``` 128 | class ClockPublisher : public rclcpp::Node { 129 | ClockPublisher() 130 | : rclcpp::Node("clock_publisher"),message_(rosgraph_msgs::msg::Clock()) 131 | { 132 | 133 | publisher_ = this->create_publisher("/clock", 10); 134 | timer_ = this->create_wall_timer( 135 | 10ms, std::bind(&ClockPublisher::timer_callback, this)); 136 | } 137 | 138 | ``` 139 | 定义ClockPublisher类，继承自rclcpp::Node的类。它将作为一个ROS节点，负责发布模拟时钟消息。 140 | 在构造函数中，创建了一个话题发布器，用于向`clock`话题发布Clock类型的数据； 141 | 142 | 同时创建了一个用于定期触发的定时器。这个定时器将每隔10毫秒（10ms）触发一次，并且会调用 timer_callback 函数。std::bind 用于将成员函数与实际对象（this）绑定，以便正确调用。 143 | 144 | ``` 145 | void timer_callback() 146 | { 147 | message_.clock.nanosec += 10000000; 148 | if(message_.clock.nanosec >= 1000000000){ 149 | message_.clock.sec += 1; 150 | message_.clock.nanosec = 0; 151 | } 152 | publisher_->publish(std::move(message_)); 153 | } 154 | rclcpp::TimerBase::SharedPtr timer_; 155 | rosgraph_msgs::msg::Clock message_; 156 | rclcpp::Publisher::SharedPtr publisher_; 157 | }; 158 | ``` 159 | 这是在定时器触发时会被调用的回调函数。在这个函数内，模拟时钟消息的时间会被更新。 160 | 模拟时钟消息的类型`rosgraph_msgs/msg/Clock`如下： 161 | ``` 162 | # This message communicates the current time. 163 | # 164 | # For more information, see https://design.ros2.org/articles/clock_and_time.html. 165 | builtin_interfaces/Time clock 166 | int32 sec 167 | uint32 nanosec 168 | ``` 169 | 其中，sec代表秒，nanosec，纳秒（nanoseconds），是时间的一个单位，等于十亿分之一秒（1/1,000,000,000 秒）。 170 | 171 | ## 功能验证 172 | 回到nav_simulator目录，并执行编译成功后运行节点： 173 | ``` 174 | colcon build 175 | ros2 run simulator clock_node 176 | ``` 177 | 另起一个终端通过`ros2 topic echo /clock`可以订阅到如下消息，即完成模拟时钟部分。 178 | ``` 179 | --- 180 | clock: 181 | sec: 12 182 | nanosec: 480000000 183 | --- 184 | clock: 185 | sec: 12 186 | nanosec: 490000000 187 | --- 188 | clock: 189 | sec: 12 190 | nanosec: 500000000 191 | --- 192 | clock: 193 | sec: 12 194 | nanosec: 510000000 195 | . 196 | . 197 | . 198 | ``` 199 | 200 | ## C++ 便利贴： 201 | 202 | ### std::move 203 | 204 | 是 C++ 标准库中的一个函数模板，用于将对象转移（移动）到新的位置，通常用于优化资源管理和减少不必要的复制操作。它不会实际执行任何数据的拷贝，而是将对象的内部资源（如指针、文件句柄等）的所有权从一个对象转移到另一个对象。 205 | 206 | 主要用途包括： 207 | 208 | - 避免复制开销：在某些情况下，如果你知道对象即将失去对资源的所有权，并且不再需要拷贝源对象，你可以使用 std::move 来避免不必要的数据拷贝，从而提高性能。 209 | 210 | - 移动语义：C++11 引入了移动语义，允许通过转移而不是拷贝来优化资源管理。通过移动，对象可以更高效地转移资源的所有权，从而提高程序性能。 211 | 212 | - 容器中的移动操作：在使用标准容器时，例如 std::vector，移动元素比拷贝元素要高效。使用 std::move 可以将对象从一个容器转移到另一个容器，而不会触发不必要的复制构造函数。 213 | 214 | `publisher_->publish(std::move(message_))`中 215 | ，std::move 用于发布消息对象：这里的 std::move(message_) 将 message_ 对象的所有权转移到 publish 函数中，而不会进行数据的拷贝。这在发布消息时是一种常见的做法，以避免消息的复制，并将其资源有效地移交给发布函数。注意，在使用 std::move 之后，原始的 message_ 对象处于有效但未定义的状态，不能再对其进行操作。 -------------------------------------------------------------------------------- /source/docs/路径规划仿真一.md: -------------------------------------------------------------------------------- 1 | # 全局路径规划仿真一 2 | 3 | 为了完成全局路径规划的仿真，我们需要加载`navigation2`中的`planner_server`节点，并为`planner_server`编写我们自己的路径规划器插件。 4 | 5 | 基于此，在`nav_simulator/src`目录下，新建一个`planner`的package，用于路径规划相关的功能介绍。 6 | 7 | 8 | ``` 9 | cd nav_simulator/src/ 10 | ros2 pkg create --build-type ament_cmake --library-name planners --dependencies nav2_core --maintainer-name zcf --maintainer-email xxxx@162.com planner 11 | ``` 12 | 13 | 下面会介绍一下`planner server`，然后利用启动脚本将其加载到系统里， 后面一个小节还会提供一个路劲规划器的用例。 14 | 15 | ## Planner server的介绍 16 | 17 | ![planner server](./../images/planner_server.svg) 18 | 19 | `nav2_planner`是`navigation2`中负责管理全局路径规划的主要模块，帮助机器人在不同环境中选择安全和高效的全局路径。下面关于`nav2_planner`的一些介绍： 20 | 21 | - lifecycle node： `nav2_planner`设计为一个lifecycle节点，被lifecycle manager有效管理起来，可以最大程度上利用系统资源。 22 | 23 | - 路径规划： `nav2_planner`主要任务就是管理规划器插件，并根据任务需求，选择指定插件完成路径规划任务。它决定了机器人如何从其当前位置到达目标位置，同时避免碰撞障碍物。 24 | 25 | - 插件架构： `nav2_planner`的架构是可插拔的，这意味着您可以选择不同的路径规划算法和策略来满足您的需求。这种灵活性使得您可以根据不同的应用场景选择最适合的路径规划方法。 26 | 27 | - compute_path_to_pose： 常规的A\B点之间的路径规划 28 | 29 | - compute_path_through_poses： 用于多个途经点的路径规划 30 | 31 | 官方文档说明可参考：[planner wiki](https://navigation.ros.org/concepts/index.html#planners) 32 | ## planner server的使用 33 | 34 | ### 创建配置文件 35 | 36 | 在`planner`的根目录下,新建一个`params`目录用于参数文件, 并在该目录创建planner.yaml： 37 | 38 | ``` 39 | cd planner 40 | mkdir params 41 | touch planner.yaml 42 | ``` 43 | 44 | `planner.yaml`文件中存放`planner_server`相关的参数： 45 | 46 | ``` 47 | global_costmap: 48 | global_costmap: 49 | ros__parameters: 50 | update_frequency: 1.0 51 | publish_frequency: 1.0 52 | global_frame: map 53 | robot_base_frame: base_link 54 | use_sim_time: True 55 | robot_radius: 0.22 56 | resolution: 0.05 57 | track_unknown_space: true 58 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 59 | obstacle_layer: 60 | plugin: "nav2_costmap_2d::ObstacleLayer" 61 | enabled: True 62 | observation_sources: scan 63 | scan: 64 | topic: /scan 65 | max_obstacle_height: 2.0 66 | clearing: True 67 | marking: True 68 | data_type: "LaserScan" 69 | raytrace_max_range: 3.0 70 | raytrace_min_range: 0.0 71 | obstacle_max_range: 2.5 72 | obstacle_min_range: 0.0 73 | static_layer: 74 | plugin: "nav2_costmap_2d::StaticLayer" 75 | map_subscribe_transient_local: True 76 | inflation_layer: 77 | plugin: "nav2_costmap_2d::InflationLayer" 78 | cost_scaling_factor: 3.0 79 | inflation_radius: 0.55 80 | always_send_full_costmap: True 81 | global_costmap_client: 82 | ros__parameters: 83 | use_sim_time: True 84 | global_costmap_rclcpp_node: 85 | ros__parameters: 86 | use_sim_time: True 87 | 88 | planner_server: 89 | ros__parameters: 90 | expected_planner_frequency: 20.0 91 | use_sim_time: True 92 | planner_plugins: ["GridBased"] 93 | GridBased: 94 | plugin: "nav2_navfn_planner/NavfnPlanner" 95 | tolerance: 0.5 96 | use_astar: false 97 | allow_unknown: true 98 | 99 | planner_server_rclcpp_node: 100 | ros__parameters: 101 | use_sim_time: True 102 | ``` 103 | 104 | 更多planner server配置相关的详细内容可以参考：[Planner Server](https://navigation.ros.org/configuration/packages/configuring-planner-server.html) 105 | 106 | ### 创建启动脚本 107 | 108 | 在`planner`的根目录下,新建一个`launch`目录用于存放启动脚本, 并在该目录创建planner.launch.py： 109 | 110 | ``` 111 | cd planner 112 | mkdir launch 113 | touch planner.launch.py 114 | ``` 115 | 116 | 按照如下内容编辑启动脚本： 117 | 118 | ``` 119 | import os 120 | 121 | from ament_index_python.packages import get_package_share_directory 122 | 123 | from launch import LaunchDescription 124 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 125 | from launch.substitutions import LaunchConfiguration 126 | from launch_ros.actions import Node 127 | from nav2_common.launch import RewrittenYaml 128 | 129 | 130 | def generate_launch_description(): 131 | # Get the launch directory 132 | package_dir = get_package_share_directory('planner') 133 | 134 | use_sim_time = LaunchConfiguration('use_sim_time') 135 | autostart = LaunchConfiguration('autostart') 136 | params_file = LaunchConfiguration('params_file') 137 | 138 | lifecycle_nodes = ['planner_server'] 139 | 140 | 141 | # Create our own temporary YAML files that include substitutions 142 | param_substitutions = { 143 | 'use_sim_time': use_sim_time, 144 | 'autostart': autostart} 145 | 146 | configured_params = RewrittenYaml( 147 | source_file=params_file, 148 | root_key='', 149 | param_rewrites=param_substitutions, 150 | convert_types=True) 151 | 152 | return LaunchDescription([ 153 | 154 | DeclareLaunchArgument( 155 | 'use_sim_time', default_value='true', 156 | description='Use simulation (Gazebo) clock if true'), 157 | 158 | DeclareLaunchArgument( 159 | 'autostart', default_value='true', 160 | description='Automatically startup the nav2 stack'), 161 | 162 | DeclareLaunchArgument( 163 | 'params_file', 164 | default_value=os.path.join(planner, 'params', 'planner.yaml'), 165 | description='Full path to the ROS2 parameters file to use'), 166 | 167 | Node( 168 | package='nav2_planner', 169 | executable='planner_server', 170 | name='planner_server', 171 | output='screen', 172 | parameters=[configured_params], 173 | remappings=remappings), 174 | 175 | Node( 176 | package='nav2_lifecycle_manager', 177 | executable='lifecycle_manager', 178 | name='lifecycle_manager_navigation', 179 | output='screen', 180 | parameters=[{'use_sim_time': use_sim_time}, 181 | {'autostart': autostart}, 182 | {'node_names': lifecycle_nodes}]), 183 | ]) 184 | ``` 185 | 186 | ### 修改CMakeList并编译 187 | 188 | 并将上述两个文件夹，在`CMakeList.txt`进行`install`相关指令的处理，以便在启动加载时能够在安装目录中找到对应的文件。 189 | 190 | ``` 191 | install( 192 | DIRECTORY params launch 193 | DESTINATION share/${PROJECT_NAME} 194 | ) 195 | 196 | ``` 197 | 198 | 在`nav_simulator`目录下执行编译指令：`colcon build`， 待编译完成后，可以启动两个终端（注意source）： 199 | 200 | ## 功能验证 201 | 202 | 第一个终端启动路径规划仿真器： 203 | ``` 204 | ros2 launch simulator planner_simulator.launch.py 205 | ``` 206 | 207 | 另一个终端启动planner_server： 208 | ``` 209 | ros2 launch planner planner.launch.py 210 | ``` 211 | 212 | 此时在rviz界面中加入相关的显示控件：`costmap2d`, `plan`等（设置完成后保存在simulator目录下的rviz目录中）。 213 | 214 | 然后执行如下的action请求，则可以看到如下的仿真结果。 215 | 216 | ### compute path to pose 217 | 218 | - action msg 219 | ``` 220 | #goal definition 221 | geometry_msgs/PoseStamped goal 222 | geometry_msgs/PoseStamped start 223 | string planner_id 224 | bool use_start # If true, use current robot pose as path start, if false, use start above instead 225 | --- 226 | #result definition 227 | nav_msgs/Path path 228 | builtin_interfaces/Duration planning_time 229 | --- 230 | #feedback 231 | ``` 232 | - command 233 | ``` 234 | ros2 action send_goal /compute_path_to_pose nav2_msgs/action/ComputePathToPose "{use_start: true, planner_id: GridBased, start: {header: {frame_id: map}, pose: {position: {x: 0.0, y: 0.0}}}, goal: {header: {frame_id: map}, pose: {position: {x: -10.0, y: -5.0}}}}" 235 | ``` 236 | 237 | ![plan](./../images/planner.png) 238 | 239 | ### compute path through poses 240 | 241 | - action msg 242 | ``` 243 | #goal definition 244 | geometry_msgs/PoseStamped[] goals 245 | geometry_msgs/PoseStamped start 246 | string planner_id 247 | bool use_start # If true, use current robot pose as path start, if false, use start above instead 248 | --- 249 | #result definition 250 | nav_msgs/Path path 251 | builtin_interfaces/Duration planning_time 252 | --- 253 | #feedback 254 | 255 | ``` 256 | 257 | - command 258 | ``` 259 | ros2 action send_goal /compute_path_through_poses nav2_msgs/action/ComputePathThroughPoses "{use_start: true, planner_id: GridBased, start: {header: {frame_id: map}, pose: {position: {x: 0.0, y: 0.0}}}, goals: [{header: {frame_id: map}, pose: {position: {x: -10.0, y: -5.0}}}, {header: {frame_id: map}, pose: {position: {x: -3.0, y: -3.0}}},{header: {frame_id: map}, pose: {position: {x: 3.0, y: 3.0}}}]}" 260 | ``` 261 | 262 | ![through](./../images/plans.png) 263 | 264 | 当然，可以通过调整如上的goal和pose的坐标，以获得不同的路径规划结果。为了更高效的进行路径规划效果的验证，在接下来的一个小节会在`simulator`中实现一个订阅转发工具，用于使用rviz上的空间完成路径规划的任务下发。 265 | -------------------------------------------------------------------------------- /source/docs/路径规划仿真二.md: -------------------------------------------------------------------------------- 1 | # 全局路径规划仿真二 2 | 3 | 上一个小节实现了planner server启动脚本的加载，以及相关参数文件的创建，并基于此使用action client命令行的形式进行了功能验证，而日常开发需要更高效的验证方法，这一小节会在`simulator`中实现一个订阅转发工具planner_bridge，用于使用rviz上的控件来完成路径规划的任务下发。 4 | 5 | ## planner_bridge功能简介 6 | 7 | planner_bridge，作为rviz与planner server之间的桥梁，其订阅rviz控件发出的`2D Goal Pose`以及`Publish Point`的消息，然后包装成为action client的消息，向planner server发起规划请求。这样在后续的路径规划调试中，可以很方便的通过rviz控件来反复验证路径效果。 `需要注意的是，到这里为止，尚未实现机器人的重定位功能，所以路径规划的起点都是坐标原点；后面会逐步将里程计，以及机器人重定位功能开发完成之后，就可以随心所欲的进行路径效果验证了。` 8 | 9 | - `Publish Point` 用来补充`compute_plan_through_poses`的中间点。 10 | 11 | - `2D Goal Pose` 作为路径规划的触发条件，会组合机器人当前位置，`Publish Point`的点序列，以及`2D Goal Pose`的位置。根据目标个数自行选择使用`compute_plan_through_poses`或者是`compute_plan_to_pose`。 12 | 13 | ## planner_bridge.cpp 14 | 15 | 在`simulator/src/`目录下创建planner_bridge.cpp文件，并加入如下的内容： 16 | 17 | ``` 18 | #include 19 | #include 20 | #include 21 | #include "rclcpp_action/rclcpp_action.hpp" 22 | 23 | #include "nav2_msgs/action/compute_path_to_pose.hpp" 24 | #include "nav2_msgs/action/compute_path_through_poses.hpp" 25 | #include "geometry_msgs/msg/point_stamped.hpp" 26 | #include "geometry_msgs/msg/pose_stamped.hpp" 27 | 28 | using namespace std::chrono_literals; 29 | 30 | namespace simulator{ 31 | 32 | class PlannerBridge: public rclcpp::Node 33 | { 34 | public: 35 | PlannerBridge() 36 | : rclcpp::Node("planner_bridge") 37 | { 38 | // 用于订阅rviz中的2d goal pose 话题空间 39 | goal_sub_ = create_subscription( 40 | "goal_pose", 41 | rclcpp::SystemDefaultsQoS(), 42 | std::bind(&PlannerBridge::onGoalReceived, this, std::placeholders::_1)); 43 | 44 | // 用于订阅rviz中的clicked_point 话题空间 45 | point_sub_ = create_subscription( 46 | "clicked_point", 47 | rclcpp::SystemDefaultsQoS(), 48 | std::bind(&PlannerBridge::onPointReceived, this, std::placeholders::_1)); 49 | 50 | // 用于向planner server发布翻译之后的action 请求,两点间的路劲规划任务 51 | pose_client_ = rclcpp_action::create_client( 52 | this, 53 | "compute_path_to_pose"); 54 | 55 | // 用于向planner server发布翻译之后的action 请求,多个途经点间的路劲规划任务 56 | poses_client_ = rclcpp_action::create_client( 57 | this, 58 | "compute_path_through_poses"); 59 | } 60 | 61 | protected: 62 | // 翻译rviz中发来的目标信息，并组织action client的请求消息后，对planner server发出请求。 63 | void onGoalReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose) 64 | { 65 | if(poses_.size() > 0){ 66 | poses_.push_back(*pose); 67 | auto goal_msg = nav2_msgs::action::ComputePathThroughPoses::Goal(); 68 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 69 | 70 | goal_msg.goals = poses_; 71 | 72 | if (!poses_client_->wait_for_action_server()) { 73 | RCLCPP_ERROR(get_logger(), "Action server %s not available after waiting", "compute_path_through_pose"); 74 | return; 75 | } 76 | poses_client_->async_send_goal(goal_msg, send_goal_options); 77 | }else{ 78 | auto goal_msg = nav2_msgs::action::ComputePathToPose::Goal(); 79 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 80 | goal_msg.goal = *pose; 81 | if (!pose_client_->wait_for_action_server()) { 82 | RCLCPP_ERROR(get_logger(), "Action server %s not available after waiting", "compute_path_to_pose"); 83 | return; 84 | } 85 | pose_client_->async_send_goal(goal_msg, send_goal_options); 86 | } 87 | poses_.clear(); 88 | } 89 | 90 | void onPointReceived(const geometry_msgs::msg::PointStamped::SharedPtr pose) 91 | { 92 | geometry_msgs::msg::PoseStamped p; 93 | p.header = pose->header; 94 | p.pose.position.x = pose->point.x; 95 | p.pose.position.y = pose->point.y; 96 | poses_.push_back(p); 97 | } 98 | 99 | private: 100 | 101 | std::vector poses_; 102 | 103 | rclcpp::Subscription::SharedPtr goal_sub_; 104 | rclcpp::Subscription::SharedPtr point_sub_; 105 | 106 | rclcpp_action::Client::SharedPtr pose_client_; 107 | rclcpp_action::Client::SharedPtr poses_client_; 108 | }; 109 | } 110 | 111 | int main(int argc, char * argv[]) 112 | { 113 | rclcpp::init(argc, argv); 114 | auto planner_bridge = std::make_shared(); 115 | rclcpp::spin(planner_bridge->get_node_base_interface()); 116 | rclcpp::shutdown(); 117 | return 0; 118 | } 119 | 120 | ``` 121 | 122 | ## CMakeList.txt 123 | 124 | 将planner_bridge.cpp加入到编译系统中，并添加相关依赖： 125 | ``` 126 | find_package(nav2_msgs REQUIRED) 127 | find_package(rclcpp_action REQUIRED) 128 | 129 | set(dependencies 130 | rclcpp 131 | rosgraph_msgs 132 | geometry_msgs 133 | nav2_msgs 134 | rclcpp_action 135 | ) 136 | ... 137 | 138 | add_executable(planner_bridge src/planner_bridge.cpp) 139 | target_include_directories(planner_bridge PUBLIC 140 | $ 141 | $) 142 | target_compile_features(planner_bridge PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 143 | ament_target_dependencies( 144 | planner_bridge 145 | ${dependencies} 146 | ) 147 | 148 | install(TARGETS clock_node telecontrol planner_bridge 149 | DESTINATION lib/${PROJECT_NAME}) 150 | 151 | 152 | ``` 153 | 154 | ## package.xml 155 | 加入相关的依赖包 156 | 157 | ``` 158 | ... 159 | nav2_msgs 160 | rclcpp_action 161 | ... 162 | 163 | ``` 164 | 165 | ## launch.py 166 | 将planner_bridge节点加入到启动脚本里。 167 | 168 | ``` 169 | ... 170 | # rviz控件指令桥接器，用于planner server指令翻译发送 171 | Node( 172 | package='simulator', 173 | executable='planner_bridge', 174 | name='planner_bridge', 175 | parameters=[{'use_sim_time': True}, 176 | {'planner_id': "straight"}], 177 | output='screen' 178 | ), 179 | ... 180 | ``` 181 | ## 效果验证 182 | 183 | 分别启动两个终端，确保所有的环境都source过之后，分别执行如下两个启动脚本： 184 | 185 | ``` 186 | ros2 launch simulator planner_simulator.launch.py 187 | ros2 launch planner planner.launch.py 188 | ``` 189 | 190 | 通过`2D Goal Pose`以及`Publish Point`两个控件分别做路径目标的设置之后，可以看到如下的仿真效果： 191 | 192 | ![planner_bridge](./../images/planner_bridge.gif) -------------------------------------------------------------------------------- /source/docs/遥控器.md: -------------------------------------------------------------------------------- 1 | # 遥控器 2 | 3 | 遥控器的功能主要是在开发过程中，辅助做一些自验证的工作，比如验证机器人里程计的仿真效果，后面在进行到里程计小节的时候，可以用到。 4 | ![wheels](./../images/wheels.png) 5 | 6 | 同时在开发过程中，已知的轮子就尽量不要再造了，要充分利用已知的各种资源。这里展现的遥控器模块开发的整个流程，就是日常开发中搬砖的常见场景。 7 | 8 | ## 寻找轮子 9 | 遥控的功能，主要是将键盘的上下左右按键进行映射到机器人可执行的速度指令上（cmd_vel），能读到这一小节，相信一定都阅读过ros wiki中的移动小乌龟案例，如果没有读过，也没关系，可以通过如下链接进行简单阅读： 10 | [tf2 introducing](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Tf2/Introduction-To-Tf2.html) 11 | 12 | 13 | 这里用到了类似的键盘控制功能，我们需要做的，就是去将键盘控制这部分的内容摘录出来。由于考虑到ros2中的键盘操作是使用python实现的，而在导航仿真器中计划全部使用C++进行实现，所以从ROS1中的kinetic版本中进行了相关文件的拉取： 14 | ``` 15 | wget https://raw.githubusercontent.com/ros/ros_tutorials/kinetic-devel/turtlesim/tutorials/teleop_turtle_key.cpp 16 | ``` 17 | 完整仓库地址：[ros turtlesim](https://github.com/ros/ros_tutorials) 18 | 19 | ## 将轮子加入项目 20 | 首先将`teleop_turtle_key.cpp`放到我们的工程目录下:`nav_simulator/src/simulator/src`并修改文件名为`telecontrol.cpp`; 21 | 22 | 然后在`install(TARGETS clock_node`行之前加入如下编译指令,将其加入到CMakeList.txt的编译过程: 23 | ``` 24 | add_executable(telecontrol src/telecontrol.cpp) 25 | target_include_directories(telecontrol PUBLIC 26 | $ 27 | $) 28 | target_compile_features(telecontrol PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 29 | ament_target_dependencies( 30 | telecontrl 31 | "rclcpp" 32 | "rosgraph_msgs" 33 | ) 34 | ``` 35 | 并在`install(TARGETS clock_node`之后加入`telecontrol`如下： 36 | ``` 37 | install(TARGETS clock_node telecontrol 38 | ``` 39 | 此时在工作空间目录下，通过执行`colcon build`就会同时编译clock_node和telecontrol两个节点。 40 | 41 | 但是，上诉`ament_target_dependencies`处理编译依赖时，为了省事，可以将依赖的包进行统一的管理，然后以变量的形式进行声明使用。做如下的处理： 42 | 43 | - 在`find_package`后定义`dependencies` 44 | ``` 45 | set(dependencies 46 | rclcpp 47 | rosgraph_msgs 48 | ) 49 | ``` 50 | 51 | - 将`ament_target_dependencies`中的依赖关系进行替换 52 | ``` 53 | ... 54 | 55 | ament_target_dependencies( 56 | clock_node 57 | ${dependencies} 58 | ) 59 | 60 | ... 61 | 62 | ament_target_dependencies( 63 | clock_node 64 | ${dependencies} 65 | ) 66 | 67 | ... 68 | ``` 69 | 70 | ## 调整轮子 71 | ### ROS2相关头文件替换 72 | 73 | 将 74 | ``` 75 | #include 76 | #include 77 | ``` 78 | 替换为 79 | ``` 80 | #include 81 | #include 82 | #include 83 | ``` 84 | 这里用到了一个新的依赖包，`geometry_msgs`,需要将这个包添加到对应的`packa.xml`与`CMakeList.txt`中 85 | ``` 86 | # package.xml 87 | rclcpp 88 | rosgraph_msgs 89 | geometry_msgs 90 | ``` 91 | ``` 92 | #CMakeList.txt 93 | find_package(geometry_msgs REQUIRED) 94 | 95 | set(dependencies 96 | ... 97 | geometry_msgs 98 | ) 99 | 100 | ``` 101 | ### 命名空间 102 | 103 | 修改类名：`TeleopTurtle`到`TeleControl`。 104 | 105 | 保持项目命名空间一致，虽然并不会在外界声明telecontrol的对象。 106 | 107 | 将`main`函数以及`quit`函数移至文件最后，全局变量`kfd, cooked, raw`移到命名空间外。 108 | 109 | 并将`TeleControl`的声明与定义部分都包括在`namespace simulator`中 110 | 111 | ### ROS2相关类型与接口的替换 112 | 113 | 主要涉及如下的改动(`注释掉的语句为被替换的语句`)： 114 | 115 | 继承rclcpp::Node,并在构造函数中确定节点名称。并忽略参数的获取过程。 116 | ``` 117 | # class TeleControl 118 | class TeleControl : public rclcpp::Node 119 | 120 | ... 121 | 122 | # TeleControl::TeleControl(): 123 | TeleControl::TeleControl(): 124 | rclcpp::Node("telecontrol"), 125 | 126 | ... 127 | 128 | # nh_.param("scale_angular", a_scale_, a_scale_); 129 | # nh_.param("scale_linear", l_scale_, l_scale_); 130 | 131 | ... 132 | 133 | # geometry_msgs::Twist twist; 134 | geometry_msgs::msg::Twist twist; 135 | ``` 136 | 声明与定义发布器，并将所有的`ROS_DEBUG`语句删除。 137 | ``` 138 | # ros::Publisher twist_pub_; 139 | rclcpp::Publisher::SharedPtr twist_pub_; 140 | 141 | ... 142 | 143 | # twist_pub_ = nh_.advertise("turtle1/cmd_vel", 1); 144 | twist_pub_ = this->create_publisher("cmd_vel", 1); 145 | 146 | ... 147 | 148 | # ROS_DEBUG("value: 0x%02X\n", c);... 149 | 150 | # twist_pub_.publish(twist); 151 | twist_pub_->publish(std::move(twist)); 152 | ``` 153 | `quit`函数中的退出ROS接口 154 | ``` 155 | # ros::shutdown(); 156 | rclcpp::shutdown(); 157 | ``` 158 | `main`函数中的节点初始化过程 159 | ``` 160 | # ros::init(argc, argv, "teleop_turtle"); 161 | rclcpp::init(argc, argv); 162 | ``` 163 | 至此，遥控器功能移植完成，如果只需要单机器人仿真效果，可以直接跳到`编译验证`小节进行效果验证了。 164 | ### 加入新需求 165 | 166 | 考虑到后期，会有多机器人仿真的需求，所以在这里通过不同的ROS命名空间，对不同的机器人进行区分，进行如下的改动。 167 | 168 | `main`函数 169 | ``` 170 | ... 171 | std::string name = ""; 172 | if(argc > 2){ 173 | puts("usages: handlebar robot_name"); 174 | return 0; 175 | }else if (argc == 2){ 176 | name = std::string(argv[1]); 177 | } 178 | 179 | simulator::TeleControl teleop_turtle(name); 180 | ... 181 | ``` 182 | `TeleControl`构造函数 183 | ``` 184 | TeleControl(const std::string& name): 185 | TeleControl::TeleControl(const std::string& name): 186 | ... 187 | { 188 | 189 | twist_pub_ = this->create_publisher(name + "/cmd_vel", 1); 190 | } 191 | 192 | ``` 193 | 194 | ## 编译验证 195 | 196 | ### 不带参运行节点 197 | 198 | 通过如下命令进行编译和运行节点 199 | ``` 200 | colcon build 201 | source install/setup/bash 202 | ros2 run simulator telecontrol 203 | ``` 204 | 此时启动另一个终端，执行 205 | ``` 206 | ros2 topic echo /cmd_vel 207 | ``` 208 | 此时通过按键的上、下、左、右可以再该终端下发现cmd_vel的消息打印： 209 | ``` 210 | linear: 211 | x: -2.0 212 | y: 0.0 213 | z: 0.0 214 | angular: 215 | x: 0.0 216 | y: 0.0 217 | z: 0.0 218 | --- 219 | linear: 220 | x: -2.0 221 | y: 0.0 222 | z: 0.0 223 | angular: 224 | x: 0.0 225 | y: 0.0 226 | z: 0.0 227 | ``` 228 | 229 | ### 带参运行节点 230 | 231 | 通过如下命令进行编译和运行节点 232 | ``` 233 | ros2 run simulator telecontrol boss 234 | ``` 235 | 此时启动另一个终端，执行 236 | ``` 237 | ros2 topic echo /boss/cmd_vel 238 | ``` 239 | 此时通过按键的上、下、左、右可以再该终端下发现cmd_vel的消息打印： 240 | ``` 241 | linear: 242 | x: -2.0 243 | y: 0.0 244 | z: 0.0 245 | angular: 246 | x: 0.0 247 | y: 0.0 248 | z: 0.0 249 | --- 250 | linear: 251 | x: -2.0 252 | y: 0.0 253 | z: 0.0 254 | angular: 255 | x: 0.0 256 | y: 0.0 257 | z: 0.0 258 | ``` 259 | 260 | 后续会在里程计功能验证与激光建图功能中用到本小节遥控器的内容。 261 | -------------------------------------------------------------------------------- /source/docs/里程计一.md: -------------------------------------------------------------------------------- 1 | # 机器人的里程计概览 2 | 3 | ![framework](./../images/odom.svg) 4 | 5 | 机器人的里程计是自主导航和定位的核心技术之一，高质量的里程计数据有助于提高机器人在各种环境中的导航性能。这方面的知识对于机器人工程师和研究人员来说都是非常重要的。 6 | 7 | ## 相关概念 8 | 9 | ![odom](./../images/odom.gif) 10 | 11 | 如上图所示，从机器人出发的位置作为里程计坐标系，之后里程计的数据以此为参考点，进行累计。清晰的理解里程计坐标系与里程计数据的关系非常的重要。 12 | 在ROS系统里，里程计的数据定义如下： 13 | nav_msgs/msg/Odometry 14 | ``` 15 | std_msgs/Header header 16 | builtin_interfaces/Time stamp 17 | int32 sec 18 | uint32 nanosec 19 | string frame_id 20 | string child_frame_id 21 | 22 | geometry_msgs/PoseWithCovariance pose 23 | Pose pose 24 | Point position 25 | float64 x 26 | float64 y 27 | float64 z 28 | Quaternion orientation 29 | float64 x 0 30 | float64 y 0 31 | float64 z 0 32 | float64 w 1 33 | float64[36] covariance 34 | 35 | geometry_msgs/TwistWithCovariance twist 36 | Twist twist 37 | Vector3 linear 38 | float64 x 39 | float64 y 40 | float64 z 41 | Vector3 angular 42 | float64 x 43 | float64 y 44 | float64 z 45 | float64[36] covariance 46 | 47 | ``` 48 | 49 | 在里程计数据中，covariance（协方差）是一种用于表示不确定性或误差的统计量。协方差矩阵通常用于描述测量值之间的关系，尤其是在机器人导航和定位中，它可以用来表示机器人位置和姿态的不确定性。 50 | 51 | 具体来说，里程计数据中的协方差矩阵通常包含了以下信息： 52 | 53 | 1. **位置误差**：协方差矩阵的对角线元素表示机器人位置（x、y、z坐标）的不确定性。较大的值表示更大的不确定性或误差，较小的值表示更小的不确定性。 54 | 55 | 2. **姿态误差**：协方差矩阵的非对角线元素通常表示机器人姿态（例如，方向或角度）之间的关联。这些元素告诉我们，一个方向的误差是否会影响其他方向。 56 | 57 | 3. **线性关系**：协方差矩阵还可以显示不同状态变量之间的线性关系。例如，如果x和y之间有正的协方差，这表示机器人在x方向上的误差可能会影响y方向上的误差，反之亦然。 58 | 59 | 4. **非线性关系**：在某些情况下，协方差矩阵还可以显示状态变量之间的非线性关系，尤其是在非线性系统中。 60 | 61 | 总之，协方差矩阵提供了关于里程计测量误差和不确定性的重要信息。这些信息对于机器人定位和导航非常关键，因为它们可以用来估计导航解决方案的可靠性，并帮助机器人在不确定性的环境中做出更准确的决策。 62 | 63 | ### 里程计坐标系 64 | ![odom_tf](./../images/odom_tf.gif) 65 | 66 | 里程计坐标系是机器人中的一个关键概念，在机器人启动时，通常会将当前位置定义为起始点或原点。这个点的坐标通常被设置为(0, 0)，这是里程计坐标系的起点，机器人启动时的朝向定义为坐标系的x轴正方向，左侧为y轴正方向。该参考系的作用，就是来度量此后机器人所有的移动旋转等动作。 67 | 68 | 移动旋转等动作就是使用上面的里程计数据来描述的，里程计数据通常为定频输出，将每一个里程计数据进行累计描述，就可以实时的表述机器人从开机到当前时刻的运动轨迹。有时里程计的精度就是使用机器人按照一个闭环的场景进行运动，在机器人从原点出发后又回到出发原点时，里程计的统计误差情况来衡量的。 69 | 70 | 通常，在slam系统重，里程计坐标系会位于map与base_link之间，最后通过坐标系的变换解算来描述世界坐标系与机器人坐标系的关系。一般情况下，这个关系都是机器人导航算法所不感知的部分。 71 | 72 | ### 里程计 73 | 74 | 机器人的里程计（Odometry）是通过各种传感器测量技术，用于测量机器人在运动过程中相对于某个参考点的位置和方向变化的传感器或算法。通常，机器人里程计是通过测量轮子或其他运动部件的旋转来估计机器人的位姿（位置和方向）变化。它用于跟踪机器人在一个相对于起始位置的局部坐标系中的运动。里程计通常在机器人的局部区域内提供较为准确的位姿估计。然而，里程计存在误差和累积问题，因此在长时间运动后，位置估计可能会变得不准确。这个误差，一般包括轮子滑动、传感器精度、积分过程等都会造成累积。 75 | 76 | 常见的里程计基本都是和传感器绑定到一起的，从命名上就有很直观的体现：轮式里程计，腿式里程计，惯性里程计，视觉里程计，GPS里程计，激光里程计等等。 77 | 78 | 79 | ### 里程计融合 80 | 为了提高机器人的定位精度和鲁棒性，通常会将多种里程计数据进行融合，这个过程被称为里程计融合（Odometry Fusion）。里程计融合的目标是结合多个传感器源的信息，以获取更准确和可靠的机器人位姿估计。 81 | 82 | 融合算法负责将不同传感器源的信息融合在一起，以生成一组更准确的位姿估计。常见的融合算法包括卡尔曼滤波、扩展卡尔曼滤波（EKF）、粒子滤波等。 83 | 84 | 在融合过程中，需要建立每个传感器的误差模型，以便根据其相对可靠性对位姿估计进行加权。通过里程计融合，机器人可以在复杂环境中更准确地定位自己，从而支持自主导航、避障和任务执行。 85 | 86 | 机器人的里程计融合是提高机器人定位精度和鲁棒性的关键技术，它涉及传感器数据的融合和算法的优化，以确保机器人能够在复杂环境中准确导航和执行任务。 87 | 88 | ### 重定位 89 | ![odom_reloc](./../images/odom_reloc.gif) 90 | 91 | slam或者location在做定位矫正的时候，实际就是调整的odom与map之间的关系。而里程计由于其累计的特性，它的数据是不会变化的。 92 | 93 | 通过机器人运行过程中，对环境特征的提取对比来估计机器人的位姿变化，通过摄像头/雷达等传感器捕捉环境特征，并与先验的建图信息进行比对，当发现当前特征与建图特征有出入的时候，会调整里程计坐标系与世界坐标系的关系，来保证机器人人坐标系base_link的正确。 94 | 95 | 举个例子来讲这个过程： 96 | ![smile](./../images/smile.svg) 97 | 98 | 如上图所示， 99 | - 数轴5米处有一个笑脸为已知建模，小明记得自己从前测量过自己的步长为1米，所以他从数轴0米处出发向前行走。 100 | - 当他走了4步之后抵达了笑脸位置，这个时候他就很纳闷，明明才走了4步，怎么就到笑脸了呐？ 101 | - 冥思苦想之后，突然开窍，原来我不是从0米处出发的，我一定是从1米处出发的，所有走了4步就到了笑脸处了。 102 | - 于是它把起始位置假象到1米处，，之后仍然按照步长1米进行统计。 103 | 104 | 假如后续还有很多个间隔5米的笑脸，那么，小明只需要不断地调整出发位置就可以同时满足自己的自定位情况和自我认知的需求了。 105 | -------------------------------------------------------------------------------- /source/docs/里程计三.md: -------------------------------------------------------------------------------- 1 | # 里程计仿真的设计和实现 2 | 3 | ## 功能设计 4 | 上一个小节是表述的坐标在坐标系内的变换关系，下面两个图片分别映射了坐标系与坐标系之间的变换关系。 5 | 6 | ![move](../images/move.svg) | ![move](../images/move_rotate.svg) 7 | --- | --- 8 | 9 | 下图是里程计仿真模块的基本结构图，通过transform的处理，将输入与输出进行关联。 10 | 11 | ![move](../images/transform.svg) 12 | 13 | ### 重新设计坐标系统 14 | 15 | 里程计仿真所输出的tf关系如下所示： 16 | 17 | ![tf_odom](./../images/tf_odom.png) 18 | 19 | 其中： 20 | 21 | - map ---> odom 用来重定位 22 | - odom ---> base_link 用来推演定位 23 | 24 | ### tf2_ros 25 | `tf2_ros` 是 ROS 中用于处理变换（Transform）的库，它建立在 `tf2` 库之上，提供了 ROS 特定的功能。以下是 `tf2_ros` 主要接口和功能的罗列： 26 | 27 | 1. **`tf2_ros::Buffer` 类：** 28 | - **功能：** `Buffer` 类是 `tf2` 中的中心数据结构，用于存储和查询变换信息。 29 | - **主要接口：** 30 | - `lookupTransform(target_frame, source_frame, time)`：查询从 `source_frame` 到 `target_frame` 的变换。 31 | - `transformPoint(target_frame, stamped_in, target_frame, stamped_out)`：对一个点进行坐标变换。 32 | - `canTransform(target_frame, source_frame, time)`：检查是否能够查询给定时间点的变换。 33 | - `waitForTransform(target_frame, source_frame, time, timeout, polling_sleep_duration)`：等待直到能够查询到指定时间点的变换。 34 | 35 | 2. **`tf2_ros::TransformBroadcaster` 类：** 36 | - **功能：** `TransformBroadcaster` 类用于发布变换信息到 ROS 中，使得其他节点可以获取到这些变换。 37 | - **主要接口：** 38 | - `sendTransform`：发布一个变换到 ROS。 39 | 40 | 3. **`tf2_ros::MessageFilter` 类：** 41 | - **功能：** `MessageFilter` 类用于在接收到消息时，根据消息的时间戳和指定的变换关系进行筛选和处理。 42 | - **主要接口：** 43 | - `registerCallback`：注册一个回调函数，处理接收到的消息。 44 | - `setTargetFrame`：设置目标坐标系。 45 | - `setTolerance`：设置时间戳的容忍度。 46 | 47 | 4. **`tf2_ros::StaticTransformBroadcaster` 类：** 48 | - **功能：** `StaticTransformBroadcaster` 类用于发布静态变换信息到 ROS。 49 | - **主要接口：** 50 | - `sendTransform`：发布一个静态变换到 ROS。 51 | 52 | 5. **`tf2_ros::BufferClient` 类：** 53 | - **功能：** `BufferClient` 类是 `tf2` 的一个衍生类，用于在 ROS 服务中提供变换查询功能。 54 | - **主要接口：** 55 | - `transform`：在 ROS 服务中提供变换查询。 56 | 57 | 这些类和接口提供了一个强大的变换系统，使得在 ROS 中进行坐标变换非常方便。`tf2_ros` 库的设计允许 ROS 节点发布、订阅和查询坐标变换，使得机器人系统中的不同组件能够协同工作。 58 | 59 | 60 | 61 | 具体的用例可以参考wiki中相关的例子进行更细致了解 62 | 63 | [tf broadcaster](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html) 64 | 65 | [tf listener](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.html) 66 | 67 | ## 功能实现 68 | 略(参考github代码即可) 69 | 70 | ## 效果验证 71 | 72 | ``` 73 | git clone git@github.com:cf-zhang/nav_simulator.git 74 | cd nav_simulator 75 | source /opt/ros/galactic/setup.bash 76 | colcon build 77 | source install/setup.bash 78 | ros2 launch simulator simulator.launch.py 79 | ``` 80 | 81 | 到这里，基于rviz的仿真功能就启动成功了，可以看到一个rviz界面带有地图的显示。 82 | 83 | ### 重定位 84 | 85 | 使用rviz中设置机器人位置的控件，可以变换机器人的定位位置。如下图所示： 86 | 87 | ![reloc](./../images/reloc.gif) 88 | 89 | ### 遥控器 90 | 91 | ``` 92 | cd nav_simulator 93 | source /opt/ros/galactic/setup.bash 94 | source install/setup.bash 95 | ros2 run simulator telecontrol 96 | ``` 97 | 98 | 使用之前实现过的遥控器，在终端上使用上下左右四个箭头按键，可以实现机器人本体的移动，如下图所示： 99 | 100 | ![reloc](./../images/tele_move.gif) 101 | 102 | ### 导航全流程 103 | 104 | 整个功能开发到这里，基本就告一段落了，自此导航的功能，出了传感器避障之外，都可以在仿真器里面进行实际运行和验证。具体方式如下： 105 | 重复一次上述流程，启动仿真器： 106 | 107 | ``` 108 | git clone git@github.com:cf-zhang/nav_simulator.git 109 | cd nav_simulator 110 | source /opt/ros/galactic/setup.bash 111 | colcon build 112 | source install/setup.bash 113 | ros2 launch simulator simulator.launch.py 114 | ``` 115 | 116 | 再打开一个终端，启动导航的功能节点： 117 | 118 | ``` 119 | ros2 launch nav2_bringup navigation_launch.py 120 | ``` 121 | 122 | 然后使用`2D Goal Pose`控件设置目标点，可以看到机器人的导航移动效果，具体如下图所示： 123 | 124 | ![navigation](./../images/navigation.gif) 125 | -------------------------------------------------------------------------------- /source/docs/里程计二.md: -------------------------------------------------------------------------------- 1 | # 里程计仿真的理论基础 2 | 3 | 里程计的实现过程与机器人系统中的tf tree强绑定在了一起，而tf tree的内容又是依赖了ROS中比较成熟的坐标变换库[geometry2](https://github.com/ros2/geometry2)中tf2相关的内容。 4 | 5 | tf2的实际上就是对机器人系统里各种坐标系之间的关系提供了管理和查询的功能，这个功能开发离不开坐标变换相关的理论基础。 6 | 7 | 我们在使用时，虽然可以不关心内部具体的实现，但是能大概知道其原理，对于这种工具库的使用也会变得如虎添翼般丝滑。 8 | 9 | ![计算机图形学](./../images/cp.png) 10 | 11 | `本小节内容，参考上图的《计算机图形学》一书的第四章第一小节。更多内容感兴趣可以自行购买或加入 导航与仿真技术讨论群 获取电子资料临时参阅` 12 | 13 | tf的实现是3D空间中的复杂操作，我们仿真器的里程计部分只涉及其中的2D空间相关的内容。 14 | 15 | ## 坐标变换基础 16 | 17 | 图形学中，实现图形变换时通常采用齐次坐标系来表示坐标值，这样可以方便地用变换矩阵实现对图形的变换。 18 | 19 | 所谓的齐次坐标表示法就是将一个原本是n维的向量用一个n+1维向量表示。例如，二维坐标点P(x,y)的齐次坐标为(H·x, H·y, H),其中H是任意不为0的比例系数。 20 | 21 | 引入齐次坐标之前，图形变换的运算形式既有矩阵加法，又有矩阵乘法。引入齐次坐标之后，图形变换的运算形式统一成表示图形的点集矩阵与某一变换矩阵进行矩阵相乘的单一形式。 22 | 23 | 2D导航中，里程计层面上只考虑二维平面中的内容，所以这这一小节中以二维几何变换进行展开。几何变换的形式包括五中基本形式：平移、比例、旋转、对称、错切。 24 | 25 | 这里提到的五种基本变换，变换矩阵都可以用如下的3 x 3矩阵来描述 26 | 27 | $$ 28 | \left[\begin{array}{cc:c} 29 | a & b & c\\ 30 | d & e & f\\ 31 | \hdashline g & h & i 32 | \end{array}\right] 33 | $$ 34 | 35 | 根据不同的功能，可以将这个3 x 3的变换矩阵分成4个字块： 36 | 37 | - 左上角的 2 x 2子块可实现比例、旋转、对称、错切四种基本变换 38 | - 左下角的 1 x 2子块可实现平移变换 39 | - 右上角的 2 x 1子块可实现投影变换 40 | - 右下角的 1 x 1子块可实现整体比例变换 41 | 42 | 这里只对其中的平移与旋转叠加的复合变换内容进行展开，更多内容可以参阅上面提到的书目。 43 | 44 | 45 | 46 | ## 平移变换 47 | 平移是一种不产生形变而移动物体的刚体变换，如下图所示： 48 | 49 | ![平移变换](./../images/move.png) 50 | 51 | 假定从点P平移到点P'，点P沿x方向的平移量为m，沿y方向的平移量为n，则变换后P'点的坐标值分别为： 52 | 53 | $$ 54 | x' = x + m \\ 55 | y' = y + n 56 | $$ 57 | 58 | 构造平移矩阵T: 59 | 60 | $$ 61 | \left[\begin{array}{c} 62 | 1 & 0 & 0 \\ 63 | 0 & 1 & 0 \\ 64 | m & n & 1 \\ 65 | \end{array}\right] 66 | $$ 67 | 68 | 得到平移变换的矩阵运算表示为： 69 | 70 | $$ 71 | \left[\begin{array}{c} 72 | x' & y' & 1 73 | \end{array}\right] = \left[\begin{array}{c} 74 | x + m & y + n & 1 75 | \end{array}\right] = \left[\begin{array}{c} 76 | x & y & 1 77 | \end{array}\right] \cdot \left[\begin{array}{c} 78 | 1 & 0 & 0 \\ 79 | 0 & 1 & 0 \\ 80 | m & n & 1 \\ 81 | \end{array}\right] 82 | $$ 83 | 84 | 简写为: $P'=P·T$ 85 | 86 | 显而易见，正是由于采用了齐次坐标表示法，才使得平移变换的处理由原本的加法变为了矩阵乘法，从而与其余四种几何变换的运算方式相统一。 87 | 88 | ## 旋转变换 89 | 基本的旋转变换是指将图形围绕坐标原点逆时针转动一个 $\theta$ 角度的变化，如下图所示： 90 | 91 | ![旋转](../images/rotate.png) 92 | 93 | 假定 $P$ 点离原点的距离为 $\rho$ ， $P$ 点与x轴夹角 $\alpha$ ，如上面右图所示，则 $P$ 的坐标值为： 94 | 95 | $$ 96 | x = \rho \cdot cos\alpha \\ 97 | y = \rho \cdot sin\alpha 98 | $$ 99 | 100 | 从 $P$ 点绕原点逆时针旋转 $\theta$ 角到 $P'$ 点，则变换后 $P'$ 点的坐标值为： 101 | 102 | $$ 103 | x' = \rho \cdot cos(\alpha + \theta) = \rho \cdot cos\alpha cos\theta - \rho \cdot sin\alpha sin\theta \\ 104 | y' = \rho \cdot sin(\alpha + \theta) = \rho \cdot sin\alpha cos\theta + \rho \cdot cos\alpha sin\theta 105 | $$ 106 | 107 | 从而得到 108 | 109 | $$ 110 | x' = x \cdot cos\theta - y \cdot sin\theta \\ 111 | y' = x \cdot sin\theta + y \cdot cos\theta 112 | $$ 113 | 114 | 构造旋转矩阵 $T$ : 115 | 116 | $$ 117 | T = 118 | \left[\begin{array}{c} 119 | cos\theta & sin\theta & 0 \\ 120 | -sin\theta & cos\theta & 0 \\ 121 | 0 & 0 & 1 122 | \end{array}\right] 123 | $$ 124 | 125 | 得到旋转变换的矩阵运算表示为： 126 | 127 | $$ 128 | \left[\begin{array}{c} 129 | x' & y' & 1 130 | \end{array}\right] = 131 | \left[\begin{array}{c} 132 | x \cdot cos\theta - y\cdot sin\theta & x \cdot sin\theta + y \cdot cos\theta & 1 133 | \end{array}\right] = 134 | \left[\begin{array}{c} 135 | x & y & 1 136 | \end{array}\right] \cdot 137 | \left[\begin{array}{c} 138 | cos\theta & sin\theta & 0 \\ 139 | -sin\theta & cos\theta & 0 \\ 140 | 0 & 0 & 1 141 | \end{array}\right] 142 | $$ 143 | 144 | 简写为 $P' = P \cdot T$ 145 | 146 | ## 复合变换 147 | 148 | 任何一个比较复杂的变换，都可以转换成若干个连续进行的基本变换。这些基本几何变换的组合成为复合变换，也称为级联变换。 149 | 150 | 设图形经过n次基本几何变换，其变换矩阵分别为T1, T2 151 | 152 | 则，经过T1变换后： 153 | 154 | $$ 155 | \left[\begin{array}{c} 156 | x' & y' & 1 157 | \end{array}\right] = 158 | \left[\begin{array}{c} 159 | x & y & 1 160 | \end{array}\right] \cdot T1 161 | $$ 162 | 163 | 经过T2变换后： 164 | 165 | $$ 166 | \left[\begin{array}{c} 167 | x'' & y'' & 1 168 | \end{array}\right] = 169 | \left[\begin{array}{c} 170 | x' & y' & 1 171 | \end{array}\right] \cdot T2 = \left[\begin{array}{c} 172 | x & y & 1 173 | \end{array}\right] \cdot T1 \cdot T2 = 174 | \left[\begin{array}{c} 175 | x & y & 1 176 | \end{array}\right] \cdot T 177 | $$ 178 | 179 | 称 180 | 181 | $$ 182 | T=T1 \cdot T2 183 | $$ 184 | 185 | 为复合变换矩阵。 186 | 该操作用在机器人运动时调整机器人在odom坐标系的位置。 187 | 188 | ## 逆变换与逆矩阵 189 | 所谓逆变换，就是变换过程相反的变换。 190 | 191 | 逆变换:设 $\rho$ 是一个线性变换，如果存在一个线性变换 $\sigma$ ，使得 $\rho \sigma = \sigma \rho = I$ ，(I是恒等变换)，则称 $\rho$ 变换可逆，其中 $\sigma$ 是 $\rho$ 的逆变换.若变换 $\rho$ 和变换 192 | $\sigma$ 对应的矩阵分别为 $A, B$ ，则有 $BA=AB=E_{2}$ 193 | 194 | 逆矩阵:设 $A$ 是一个二阶矩阵，如果存在二阶矩阵 $B$ ，使得 $BA=AB=E_{2}$ ,则称矩阵 $A$ 可逆，其中 $B$ 为 $A$ 的逆矩阵. 195 | 196 | 一般地，设 $A$ 是一个二阶可逆矩阵，对应的线性变换为 $\rho$ ，由矩阵与线性变换的对应关系可以看出， $A$ 的逆矩阵就是 $\rho$ 的逆变换所对应的矩阵. 197 | 198 | 该操作用在机器人重定位时调整map与odom两个坐标系之间的关系。 199 | -------------------------------------------------------------------------------- /source/docs/障碍物.md: -------------------------------------------------------------------------------- 1 | # 障碍物 2 | 3 | - 障碍物 4 | 5 | 用来做导航算法的避障效果测试，可以分为永久障碍与临时障碍，临时障碍又分为静态障碍和动态障碍物。在仿真器的实现过程中，重点是如何将障碍物与地图关系进行映射，并将映射后的数据交给传感器仿真器进行数据呈现。 6 | 7 | -------------------------------------------------------------------------------- /source/images/costmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/costmap.png -------------------------------------------------------------------------------- /source/images/cp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/cp.png -------------------------------------------------------------------------------- /source/images/dwb_local_planner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/dwb_local_planner.png -------------------------------------------------------------------------------- /source/images/fork.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/fork.png -------------------------------------------------------------------------------- /source/images/fork1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/fork1.png -------------------------------------------------------------------------------- /source/images/galactic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/galactic.png -------------------------------------------------------------------------------- /source/images/gbpl.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/gbpl.gif -------------------------------------------------------------------------------- /source/images/life_cycle_sm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/life_cycle_sm.png -------------------------------------------------------------------------------- /source/images/listener.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/listener.png -------------------------------------------------------------------------------- /source/images/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/map.png -------------------------------------------------------------------------------- /source/images/move.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/move.png -------------------------------------------------------------------------------- /source/images/move.svg: -------------------------------------------------------------------------------- 1 |
map
odom
x
y
x
y
x
y
odom'
x
y
y
x
 -------------------------------------------------------------------------------- /source/images/move_rotate.svg: -------------------------------------------------------------------------------- 1 |
map
odom
x
y
y
x
y
x
odom'
x
y
y
x
 -------------------------------------------------------------------------------- /source/images/multi_car.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/multi_car.gif -------------------------------------------------------------------------------- /source/images/multi_topic.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/multi_topic.gif -------------------------------------------------------------------------------- /source/images/nav2_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/nav2_architecture.png -------------------------------------------------------------------------------- /source/images/navigation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/navigation.gif -------------------------------------------------------------------------------- /source/images/navigation_with_recovery_behaviours.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/navigation_with_recovery_behaviours.gif -------------------------------------------------------------------------------- /source/images/odom.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/odom.gif -------------------------------------------------------------------------------- /source/images/odom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/odom.png -------------------------------------------------------------------------------- /source/images/odom.svg: -------------------------------------------------------------------------------- 1 |
电机/编码器
轮 / 足
控制器
传感器(雷达，摄像头，IMU等等)
导航 / 规划
里程计
里程计/融合
感知/定位
 -------------------------------------------------------------------------------- /source/images/odom_reloc.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/odom_reloc.gif -------------------------------------------------------------------------------- /source/images/odom_tf.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/odom_tf.gif -------------------------------------------------------------------------------- /source/images/planner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/planner.png -------------------------------------------------------------------------------- /source/images/planner_bridge.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/planner_bridge.gif -------------------------------------------------------------------------------- /source/images/planner_server.svg: -------------------------------------------------------------------------------- 1 |
Planner Server(lifecycle node)
Costmap2D
interface(action server)
planner1
planner1
planner... n
compute_path_to_pose
compute_path_through_poses
 -------------------------------------------------------------------------------- /source/images/plans.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/plans.png -------------------------------------------------------------------------------- /source/images/pull.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/pull.png -------------------------------------------------------------------------------- /source/images/pull1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/pull1.png -------------------------------------------------------------------------------- /source/images/pull2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/pull2.png -------------------------------------------------------------------------------- /source/images/reloc.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/reloc.gif -------------------------------------------------------------------------------- /source/images/robot.svg: -------------------------------------------------------------------------------- 1 |
电机/编码器
轮 / 足
驱动器
控制器
电源
IMU
传感器
系统驱动 / 总线协议
感知 / 定位
导航 / 规划
任务管理系统
 -------------------------------------------------------------------------------- /source/images/rotate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/rotate.png -------------------------------------------------------------------------------- /source/images/rviz.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/rviz.gif -------------------------------------------------------------------------------- /source/images/rviz_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/rviz_map.png -------------------------------------------------------------------------------- /source/images/sim_car.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/sim_car.gif -------------------------------------------------------------------------------- /source/images/simulator.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/simulator.png -------------------------------------------------------------------------------- /source/images/single_topic.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/single_topic.gif -------------------------------------------------------------------------------- /source/images/straight.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/straight.gif -------------------------------------------------------------------------------- /source/images/straightline.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/straightline.gif -------------------------------------------------------------------------------- /source/images/talker.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/talker.png -------------------------------------------------------------------------------- /source/images/tele_move.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/tele_move.gif -------------------------------------------------------------------------------- /source/images/teleop.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/teleop.gif -------------------------------------------------------------------------------- /source/images/tf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/tf.png -------------------------------------------------------------------------------- /source/images/tf_odom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/tf_odom.png -------------------------------------------------------------------------------- /source/images/tf_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/tf_rviz.png -------------------------------------------------------------------------------- /source/images/tf_simple.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/tf_simple.png -------------------------------------------------------------------------------- /source/images/tracking.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/tracking.gif -------------------------------------------------------------------------------- /source/images/transform.svg: -------------------------------------------------------------------------------- 1 |
transform
cmd_vel
odom
tf
initial_pose
sub/pub
tf_broadcaster
timer
 -------------------------------------------------------------------------------- /source/images/vx.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/vx.jpg -------------------------------------------------------------------------------- /source/images/wheels.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/source/images/wheels.png -------------------------------------------------------------------------------- /source/index.rst: -------------------------------------------------------------------------------- 1 | 2 | 一起动手写ROS仿真器 3 | ================================= 4 | .. toctree:: 5 | :maxdepth: 1 6 | 7 | docs/ROS_VS_ROS2.md 8 | docs/写在前面.md 9 | docs/环境安装.md 10 | docs/工作空间.md 11 | docs/仿真器概述.md 12 | docs/虚拟时钟.md 13 | docs/遥控器.md 14 | docs/地图.md 15 | docs/坐标系.md 16 | docs/第一个启动脚本.md 17 | docs/路径规划仿真一.md 18 | docs/路径规划仿真二.md 19 | docs/第一个全局路径规划器.md 20 | docs/里程计一.md 21 | docs/里程计二.md 22 | docs/里程计三.md 23 | 24 | .. README 25 | .. ================================= 26 | .. .. toctree:: 27 | .. :maxdepth: 2 28 | 29 | .. docs/README 30 | -------------------------------------------------------------------------------- /source/uml/controller.puml: -------------------------------------------------------------------------------- 1 | @startuml 2 | 'https://plantuml.com/class-diagram 3 | class Controller{ 4 | } 5 | 6 | class MapBaseQueue{ 7 | } 8 | 9 | class CellData{ 10 | } 11 | 12 | class CostmapQueue{ 13 | } 14 | 15 | class LimitedCostmapQueue{ 16 | } 17 | 18 | 19 | MapBaseQueue <-- CostmapQueue 20 | CostmapQueue o-- CellData 21 | CostmapQueue *-- Costmap2D 22 | CostmapQueue <-- LimitedCostmapQueue 23 | 24 | 25 | class KinematicParameters 26 | { 27 | } 28 | 29 | class KinematicsHandler 30 | { 31 | } 32 | 33 | class OneDVelocityIterator{ 34 | } 35 | 36 | class VelocityIterator{ 37 | } 38 | 39 | class XYThetaIterator{ 40 | } 41 | 42 | class StandardTrajectoryGenerator{ 43 | } 44 | class LimitedAccelGenerator{ 45 | } 46 | 47 | KinematicsHandler *-- KinematicParameters 48 | VelocityIterator <-- XYThetaIterator 49 | XYThetaIterator *-- KinematicsHandler 50 | XYThetaIterator *-- OneDVelocityIterator 51 | StandardTrajectoryGenerator *-- KinematicsHandler 52 | StandardTrajectoryGenerator *-- VelocityIterator 53 | StandardTrajectoryGenerator <-- LimitedAccelGenerator 54 | 55 | 56 | 57 | 58 | class DWBLocalPlanner{ 59 | } 60 | 61 | class DWBPublisher 62 | { 63 | } 64 | 65 | class TrajectoryCritic 66 | { 67 | } 68 | 69 | class TrajectoryGenerator{ 70 | } 71 | 72 | class traj_utils 73 | { 74 | } 75 | TrajectoryCritic *-- Costmap2D 76 | DWBLocalPlanner *-- Costmap2D 77 | DWBLocalPlanner *-- DWBPublisher 78 | DWBLocalPlanner *-- TrajectoryCritic 79 | DWBLocalPlanner *-- TrajectoryGenerator 80 | 81 | class LineIterator{ 82 | } 83 | class alignment_util{ 84 | } 85 | 86 | class TwirlingCritic{ 87 | } 88 | 89 | class RotateToGoalCritic{ 90 | } 91 | 92 | class PreferForwardCritic{ 93 | } 94 | 95 | class BaseObstacleCritic{ 96 | } 97 | 98 | class ObstacleFootprintCritic{ 99 | } 100 | 101 | class MapGridQueue{ 102 | } 103 | enum ScoreAggregationType{ 104 | Last, 105 | Sum, 106 | Product 107 | } 108 | 109 | class MapGridCritic{ 110 | } 111 | 112 | class GoalDistCritic{ 113 | } 114 | class GoalAlignCritic{ 115 | } 116 | 117 | class PathDistCritic{ 118 | } 119 | 120 | class PathAlignCritic{ 121 | } 122 | 123 | class CommandTrend{ 124 | } 125 | 126 | class OscillationCritic{ 127 | } 128 | 129 | OscillationCritic *-- CommandTrend 130 | BaseObstacleCritic <-- ObstacleFootprintCritic 131 | BaseObstacleCritic *-- Costmap2D 132 | MapGridCritic *-- Costmap2D 133 | MapGridCritic *-- ScoreAggregationType 134 | MapGridCritic *-- MapGridQueue 135 | MapGridQueue *-- MapGridCritic 136 | MapGridCritic <-- GoalDistCritic 137 | GoalDistCritic <-- GoalAlignCritic 138 | MapGridCritic <-- PathDistCritic 139 | PathDistCritic <-- PathAlignCritic 140 | 141 | 142 | TrajectoryCritic <-- OscillationCritic 143 | CostmapQueue <-- MapGridQueue 144 | TrajectoryCritic <-- MapGridCritic 145 | TrajectoryCritic <-- PreferForwardCritic 146 | TrajectoryCritic <-- RotateToGoalCritic 147 | TrajectoryCritic <-- TwirlingCritic 148 | TrajectoryCritic <-- BaseObstacleCritic 149 | 150 | TrajectoryGenerator <-- StandardTrajectoryGenerator 151 | Controller <-- DWBLocalPlanner 152 | 153 | 154 | 155 | 156 | @enduml -------------------------------------------------------------------------------- /src/planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(planner) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_ros REQUIRED) 11 | find_package(nav2_core REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(rclcpp_action REQUIRED) 14 | find_package(rclcpp_lifecycle REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | find_package(visualization_msgs REQUIRED) 17 | find_package(nav2_util REQUIRED) 18 | find_package(nav2_msgs REQUIRED) 19 | find_package(nav_msgs REQUIRED) 20 | find_package(geometry_msgs REQUIRED) 21 | find_package(builtin_interfaces REQUIRED) 22 | find_package(tf2_ros REQUIRED) 23 | find_package(nav2_costmap_2d REQUIRED) 24 | find_package(pluginlib REQUIRED) 25 | 26 | set(dependencies 27 | rclcpp 28 | rclcpp_action 29 | rclcpp_lifecycle 30 | std_msgs 31 | visualization_msgs 32 | nav2_util 33 | nav2_msgs 34 | nav_msgs 35 | geometry_msgs 36 | builtin_interfaces 37 | tf2_ros 38 | nav2_costmap_2d 39 | nav2_core 40 | pluginlib 41 | ) 42 | 43 | add_library(planners src/straight_line.cpp) 44 | target_compile_features(planners PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 45 | target_include_directories(planners PUBLIC 46 | $ 47 | $) 48 | ament_target_dependencies( 49 | planners 50 | ${dependencies} 51 | ) 52 | 53 | # Causes the visibility macros to use dllexport rather than dllimport, 54 | # which is appropriate when building the dll but not consuming it. 55 | target_compile_definitions(planners PRIVATE "PLANNER_BUILDING_LIBRARY") 56 | 57 | pluginlib_export_plugin_description_file(nav2_core planner_plugins.xml) 58 | 59 | install( 60 | DIRECTORY include/ 61 | DESTINATION include 62 | ) 63 | install( 64 | TARGETS planners 65 | EXPORT export_${PROJECT_NAME} 66 | ARCHIVE DESTINATION lib 67 | LIBRARY DESTINATION lib 68 | RUNTIME DESTINATION bin 69 | ) 70 | 71 | install(FILES planner_plugins.xml 72 | DESTINATION share/${PROJECT_NAME} 73 | ) 74 | 75 | install( 76 | DIRECTORY params launch 77 | DESTINATION share/${PROJECT_NAME} 78 | ) 79 | 80 | if(BUILD_TESTING) 81 | find_package(ament_lint_auto REQUIRED) 82 | # the following line skips the linter which checks for copyrights 83 | # uncomment the line when a copyright and license is not present in all source files 84 | #set(ament_cmake_copyright_FOUND TRUE) 85 | # the following line skips cpplint (only works in a git repo) 86 | # uncomment the line when this package is not in a git repo 87 | #set(ament_cmake_cpplint_FOUND TRUE) 88 | ament_lint_auto_find_test_dependencies() 89 | endif() 90 | 91 | ament_export_include_directories( 92 | include 93 | ) 94 | ament_export_libraries( 95 | planners 96 | ) 97 | ament_export_targets( 98 | export_${PROJECT_NAME} 99 | ) 100 | 101 | ament_package() 102 | -------------------------------------------------------------------------------- /src/planner/include/planner/straight_line.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2020 Shivang Patel 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Shivang Patel 36 | * 37 | * Reference tutorial: 38 | * https://navigation.ros.org/tutorials/docs/writing_new_nav2planner_plugin.html 39 | *********************************************************************/ 40 | 41 | #ifndef PLANNER__STRAIGHT_LINE_HPP_ 42 | #define PLANNER__STRAIGHT_LINE_HPP_ 43 | 44 | #include 45 | #include 46 | 47 | #include "rclcpp/rclcpp.hpp" 48 | #include "geometry_msgs/msg/point.hpp" 49 | #include "geometry_msgs/msg/pose_stamped.hpp" 50 | 51 | #include "nav2_core/global_planner.hpp" 52 | #include "nav_msgs/msg/path.hpp" 53 | #include "nav2_util/robot_utils.hpp" 54 | #include "nav2_util/lifecycle_node.hpp" 55 | #include "nav2_costmap_2d/costmap_2d_ros.hpp" 56 | 57 | namespace planner 58 | { 59 | 60 | class StraightLine : public nav2_core::GlobalPlanner 61 | { 62 | public: 63 | StraightLine() = default; 64 | ~StraightLine() = default; 65 | 66 | // plugin configure 67 | void configure( 68 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 69 | std::string name, std::shared_ptr tf, 70 | std::shared_ptr costmap_ros) override; 71 | 72 | // plugin cleanup 73 | void cleanup() override; 74 | 75 | // plugin activate 76 | void activate() override; 77 | 78 | // plugin deactivate 79 | void deactivate() override; 80 | 81 | // This method creates path for given start and goal pose. 82 | nav_msgs::msg::Path createPlan( 83 | const geometry_msgs::msg::PoseStamped & start, 84 | const geometry_msgs::msg::PoseStamped & goal) override; 85 | 86 | private: 87 | // TF buffer 88 | std::shared_ptr tf_; 89 | 90 | // node ptr 91 | nav2_util::LifecycleNode::SharedPtr node_; 92 | 93 | // Global Costmap 94 | nav2_costmap_2d::Costmap2D * costmap_; 95 | 96 | // The global frame of the costmap 97 | std::string global_frame_, name_; 98 | 99 | double interpolation_resolution_; 100 | }; 101 | 102 | } // namespace planner 103 | 104 | #endif // PLANNER__STRAIGHT_LINE_HPP_ 105 | -------------------------------------------------------------------------------- /src/planner/launch/planner.launch.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | 4 | from ament_index_python.packages import get_package_share_directory 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | from nav2_common.launch import RewrittenYaml 11 | 12 | 13 | def generate_launch_description(): 14 | # Get the launch directory 15 | package_dir = get_package_share_directory('planner') 16 | 17 | use_sim_time = LaunchConfiguration('use_sim_time') 18 | autostart = LaunchConfiguration('autostart') 19 | params_file = LaunchConfiguration('params_file') 20 | 21 | lifecycle_nodes = ['planner_server'] 22 | 23 | 24 | # Create our own temporary YAML files that include substitutions 25 | param_substitutions = { 26 | 'use_sim_time': use_sim_time, 27 | 'autostart': autostart} 28 | 29 | configured_params = RewrittenYaml( 30 | source_file=params_file, 31 | root_key='', 32 | param_rewrites=param_substitutions, 33 | convert_types=True) 34 | 35 | return LaunchDescription([ 36 | 37 | DeclareLaunchArgument( 38 | 'use_sim_time', default_value='true', 39 | description='Use simulation (Gazebo) clock if true'), 40 | 41 | DeclareLaunchArgument( 42 | 'autostart', default_value='true', 43 | description='Automatically startup the nav2 stack'), 44 | 45 | DeclareLaunchArgument( 46 | 'params_file', 47 | default_value=os.path.join(package_dir, 'params', 'planner.yaml'), 48 | description='Full path to the ROS2 parameters file to use'), 49 | 50 | 51 | Node( 52 | package='nav2_planner', 53 | executable='planner_server', 54 | name='planner_server', 55 | output='screen', 56 | parameters=[configured_params]), 57 | 58 | Node( 59 | package='nav2_lifecycle_manager', 60 | executable='lifecycle_manager', 61 | name='lifecycle_manager_navigation', 62 | output='screen', 63 | parameters=[{'use_sim_time': use_sim_time}, 64 | {'autostart': autostart}, 65 | {'node_names': lifecycle_nodes}]), 66 | 67 | ]) 68 | -------------------------------------------------------------------------------- /src/planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | planner 5 | 0.0.0 6 | TODO: Package description 7 | zcf 8 | TODO: License declaration 9 | 10 | ament_cmake_ros 11 | 12 | nav2_core 13 | rclcpp 14 | rclcpp_action 15 | rclcpp_lifecycle 16 | std_msgs 17 | visualization_msgs 18 | nav2_util 19 | nav2_msgs 20 | nav_msgs 21 | geometry_msgs 22 | builtin_interfaces 23 | tf2_ros 24 | nav2_costmap_2d 25 | pluginlib 26 | 27 | ament_lint_auto 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/planner/params/planner.yaml: -------------------------------------------------------------------------------- 1 | 2 | global_costmap: 3 | global_costmap: 4 | ros__parameters: 5 | update_frequency: 1.0 6 | publish_frequency: 1.0 7 | global_frame: map 8 | robot_base_frame: base_link 9 | use_sim_time: True 10 | robot_radius: 0.22 11 | resolution: 0.05 12 | track_unknown_space: true 13 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 14 | obstacle_layer: 15 | plugin: "nav2_costmap_2d::ObstacleLayer" 16 | enabled: True 17 | observation_sources: scan 18 | scan: 19 | topic: /scan 20 | max_obstacle_height: 2.0 21 | clearing: True 22 | marking: True 23 | data_type: "LaserScan" 24 | raytrace_max_range: 3.0 25 | raytrace_min_range: 0.0 26 | obstacle_max_range: 2.5 27 | obstacle_min_range: 0.0 28 | static_layer: 29 | plugin: "nav2_costmap_2d::StaticLayer" 30 | map_subscribe_transient_local: True 31 | inflation_layer: 32 | plugin: "nav2_costmap_2d::InflationLayer" 33 | cost_scaling_factor: 3.0 34 | inflation_radius: 0.55 35 | always_send_full_costmap: True 36 | global_costmap_client: 37 | ros__parameters: 38 | use_sim_time: True 39 | global_costmap_rclcpp_node: 40 | ros__parameters: 41 | use_sim_time: True 42 | 43 | planner_server: 44 | ros__parameters: 45 | expected_planner_frequency: 20.0 46 | use_sim_time: True 47 | planner_plugins: ["GridBased", straight] 48 | GridBased: 49 | plugin: "nav2_navfn_planner/NavfnPlanner" 50 | tolerance: 0.5 51 | use_astar: false 52 | allow_unknown: true 53 | 54 | straight: 55 | plugin: "planner/StraightLine" 56 | interpolation_resolution: 0.1 57 | 58 | planner_server_rclcpp_node: 59 | ros__parameters: 60 | use_sim_time: True 61 | 62 | -------------------------------------------------------------------------------- /src/planner/planner_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is an example plugin which produces straight path. 4 | 5 | -------------------------------------------------------------------------------- /src/planner/src/straight_line.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2020 Shivang Patel 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Shivang Patel 36 | * 37 | * Reference tutorial: 38 | * https://navigation.ros.org/tutorials/docs/writing_new_nav2planner_plugin.html 39 | *********************************************************************/ 40 | 41 | #include 42 | #include 43 | #include 44 | #include "nav2_util/node_utils.hpp" 45 | 46 | #include "planner/straight_line.hpp" 47 | 48 | namespace planner 49 | { 50 | 51 | void StraightLine::configure( 52 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 53 | std::string name, std::shared_ptr tf, 54 | std::shared_ptr costmap_ros) 55 | { 56 | node_ = parent.lock(); 57 | name_ = name; 58 | tf_ = tf; 59 | costmap_ = costmap_ros->getCostmap(); 60 | global_frame_ = costmap_ros->getGlobalFrameID(); 61 | 62 | // Parameter initialization 63 | nav2_util::declare_parameter_if_not_declared( 64 | node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue( 65 | 0.1)); 66 | node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_); 67 | } 68 | 69 | void StraightLine::cleanup() 70 | { 71 | RCLCPP_INFO( 72 | node_->get_logger(), "CleaningUp plugin %s of type NavfnPlanner", 73 | name_.c_str()); 74 | } 75 | 76 | void StraightLine::activate() 77 | { 78 | RCLCPP_INFO( 79 | node_->get_logger(), "Activating plugin %s of type NavfnPlanner", 80 | name_.c_str()); 81 | } 82 | 83 | void StraightLine::deactivate() 84 | { 85 | RCLCPP_INFO( 86 | node_->get_logger(), "Deactivating plugin %s of type NavfnPlanner", 87 | name_.c_str()); 88 | } 89 | 90 | nav_msgs::msg::Path StraightLine::createPlan( 91 | const geometry_msgs::msg::PoseStamped & start, 92 | const geometry_msgs::msg::PoseStamped & goal) 93 | { 94 | nav_msgs::msg::Path global_path; 95 | 96 | // Checking if the goal and start state is in the global frame 97 | if (start.header.frame_id != global_frame_) { 98 | RCLCPP_ERROR( 99 | node_->get_logger(), "Planner will only except start position from %s frame", 100 | global_frame_.c_str()); 101 | return global_path; 102 | } 103 | 104 | if (goal.header.frame_id != global_frame_) { 105 | RCLCPP_INFO( 106 | node_->get_logger(), "Planner will only except goal position from %s frame", 107 | global_frame_.c_str()); 108 | return global_path; 109 | } 110 | 111 | global_path.poses.clear(); 112 | global_path.header.stamp = node_->now(); 113 | global_path.header.frame_id = global_frame_; 114 | // calculating the number of loops for current value of interpolation_resolution_ 115 | int total_number_of_loop = std::hypot( 116 | goal.pose.position.x - start.pose.position.x, 117 | goal.pose.position.y - start.pose.position.y) / 118 | interpolation_resolution_; 119 | double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop; 120 | double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop; 121 | 122 | for (int i = 0; i < total_number_of_loop; ++i) { 123 | geometry_msgs::msg::PoseStamped pose; 124 | pose.pose.position.x = start.pose.position.x + x_increment * i; 125 | pose.pose.position.y = start.pose.position.y + y_increment * i; 126 | pose.pose.position.z = 0.0; 127 | pose.pose.orientation.x = 0.0; 128 | pose.pose.orientation.y = 0.0; 129 | pose.pose.orientation.z = 0.0; 130 | pose.pose.orientation.w = 1.0; 131 | pose.header.stamp = node_->now(); 132 | pose.header.frame_id = global_frame_; 133 | global_path.poses.push_back(pose); 134 | } 135 | 136 | geometry_msgs::msg::PoseStamped goal_pose = goal; 137 | goal_pose.header.stamp = node_->now(); 138 | goal_pose.header.frame_id = global_frame_; 139 | global_path.poses.push_back(goal_pose); 140 | 141 | return global_path; 142 | } 143 | 144 | } // namespace planner 145 | 146 | #include "pluginlib/class_list_macros.hpp" 147 | PLUGINLIB_EXPORT_CLASS(planner::StraightLine, nav2_core::GlobalPlanner) 148 | -------------------------------------------------------------------------------- /src/simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(simulator) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(rosgraph_msgs REQUIRED) 12 | find_package(geometry_msgs REQUIRED) 13 | find_package(nav2_msgs REQUIRED) 14 | find_package(rclcpp_action REQUIRED) 15 | find_package(tf2_ros REQUIRED) 16 | find_package(tf2_geometry_msgs REQUIRED) 17 | 18 | set(dependencies 19 | rclcpp 20 | rosgraph_msgs 21 | geometry_msgs 22 | nav2_msgs 23 | rclcpp_action 24 | tf2_ros 25 | tf2_geometry_msgs 26 | ) 27 | 28 | add_executable(clock_node src/clock_node.cpp) 29 | target_include_directories(clock_node PUBLIC 30 | $ 31 | $) 32 | target_compile_features(clock_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 33 | ament_target_dependencies( 34 | clock_node 35 | ${dependencies} 36 | ) 37 | 38 | add_executable(telecontrol src/telecontrol.cpp) 39 | target_include_directories(telecontrol PUBLIC 40 | $ 41 | $) 42 | target_compile_features(telecontrol PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 43 | ament_target_dependencies( 44 | telecontrol 45 | ${dependencies} 46 | ) 47 | 48 | add_executable(planner_bridge src/planner_bridge.cpp) 49 | target_include_directories(planner_bridge PUBLIC 50 | $ 51 | $) 52 | target_compile_features(planner_bridge PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 53 | ament_target_dependencies( 54 | planner_bridge 55 | ${dependencies} 56 | ) 57 | 58 | add_executable(transform src/transform.cpp) 59 | target_include_directories(transform PUBLIC 60 | $ 61 | $) 62 | target_compile_features(transform PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 63 | ament_target_dependencies( 64 | transform 65 | ${dependencies} 66 | ) 67 | 68 | install(TARGETS clock_node telecontrol planner_bridge transform 69 | DESTINATION lib/${PROJECT_NAME}) 70 | 71 | install( 72 | DIRECTORY maps launch rviz 73 | DESTINATION share/${PROJECT_NAME} 74 | ) 75 | 76 | if(BUILD_TESTING) 77 | find_package(ament_lint_auto REQUIRED) 78 | # the following line skips the linter which checks for copyrights 79 | # uncomment the line when a copyright and license is not present in all source files 80 | #set(ament_cmake_copyright_FOUND TRUE) 81 | # the following line skips cpplint (only works in a git repo) 82 | # uncomment the line when this package is not in a git repo 83 | #set(ament_cmake_cpplint_FOUND TRUE) 84 | ament_lint_auto_find_test_dependencies() 85 | endif() 86 | 87 | ament_package() 88 | -------------------------------------------------------------------------------- /src/simulator/include/simulator/transform.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SIMULATOR_TRANSFORM_HPP__ 2 | #define __SIMULATOR_TRANSFORM_HPP__ 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include 6 | #include "rclcpp/timer.hpp" 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "tf2_ros/transform_listener.h" 17 | #include "tf2_ros/message_filter.h" 18 | #include "tf2/utils.h" 19 | #include "tf2/convert.h" 20 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 21 | #include "tf2/LinearMath/Transform.h" 22 | #include "tf2_ros/buffer.h" 23 | #include "tf2/buffer_core.h" 24 | #include "tf2_ros/message_filter.h" 25 | #include "tf2_ros/transform_broadcaster.h" 26 | #include "tf2_ros/transform_listener.h" 27 | #include "tf2_ros/create_timer_ros.h" 28 | 29 | namespace simulator { 30 | 31 | using std::placeholders::_1; 32 | using namespace std::literals::chrono_literals; 33 | class Transform : public rclcpp::Node 34 | { 35 | public: 36 | Transform(/* args */); 37 | ~Transform(); 38 | 39 | protected: 40 | void updateLoop(); 41 | void updateOdomFromVel(geometry_msgs::msg::Twist vel, rclcpp::Duration time_diff); 42 | void getTfFromOdom(nav_msgs::msg::Odometry odom); 43 | void velCallback(const geometry_msgs::msg::Twist::SharedPtr msg); 44 | void initPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); 45 | 46 | private: 47 | nav_msgs::msg::Odometry odom_; 48 | geometry_msgs::msg::TransformStamped odom_baselink_trans_; 49 | geometry_msgs::msg::TransformStamped map_odom_trans_; 50 | std::unique_ptr tf_buffer_; 51 | 52 | rclcpp::Time last_vel_moment_, latest_vel_moment_; 53 | rclcpp::Time last_update_moment_; 54 | rclcpp::Time measure_time_; 55 | bool message_received_ = false; 56 | 57 | // ROS interfaces 58 | rclcpp::Publisher::SharedPtr odom_pub_; 59 | rclcpp::Subscription::SharedPtr vel_sub_; 60 | rclcpp::Subscription::SharedPtr init_pose_sub_; 61 | std::unique_ptr tf_broadcaster_; 62 | 63 | std::string velocity_topic_; 64 | std::string odometry_topic_; 65 | std::string robot_frame_id_, odom_frame_id_, world_frame_id_; 66 | 67 | rclcpp::TimerBase::SharedPtr loop_timer_; 68 | double th_ = 0.0; 69 | std::shared_ptr initial_pose_; 70 | }; 71 | 72 | } // namespace simulator 73 | 74 | #endif // __SIMULATOR_TRANSFORM_HPP__ 75 | -------------------------------------------------------------------------------- /src/simulator/launch/planner_simulator.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | from ament_index_python.packages import get_package_share_directory 5 | 6 | def generate_launch_description(): 7 | # 获取当前package的目录，并以此得到地图文件的文件名 8 | package_dir = get_package_share_directory('simulator') 9 | yaml_filename = os.path.join(package_dir, 'maps', 'b.yaml') 10 | # 设置rviz的加载配置文件位置 11 | rviz_config = os.path.join(package_dir, 'rviz', 'planner.rviz') 12 | # 设置需要被lifecycle manager管理的lifecycle node 13 | lifecycle_nodes = ['map_server'] 14 | return LaunchDescription([ 15 | # 启动仿真时间节点，在/clock话题中发布时钟 16 | Node( 17 | package='simulator', 18 | executable='clock_node', 19 | name='clock', 20 | output='screen' 21 | ), 22 | # rviz控件指令桥接器，用于planner server指令翻译发送 23 | Node( 24 | package='simulator', 25 | executable='planner_bridge', 26 | name='planner_bridge', 27 | parameters=[{'use_sim_time': True}, 28 | {'planner_id': "GridBased"}], 29 | output='screen' 30 | ), 31 | # 发布一个静态的坐标变换关系，用于描述map->base_link 32 | Node( 33 | package='tf2_ros', 34 | executable='static_transform_publisher', 35 | parameters=[{'use_sim_time': True}], 36 | arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link']), 37 | # 打开一个用于显示的rviz2终端 38 | Node( 39 | package='rviz2', 40 | executable='rviz2', 41 | name='rviz2', 42 | arguments=['-d', rviz_config], 43 | parameters=[{'use_sim_time': True}], 44 | ), 45 | # 启动map_server并加载上面变量中的map文件 46 | Node( 47 | package='nav2_map_server', 48 | executable='map_server', 49 | name='map_server', 50 | output='screen', 51 | parameters=[{'yaml_filename': yaml_filename}, 52 | {'use_sim_time': True}]), 53 | # 启动lifecycle manager节点，用于管理map_server的生命周期 54 | Node( 55 | package='nav2_lifecycle_manager', 56 | executable='lifecycle_manager', 57 | name='lifecycle_manager_simulator', 58 | output='screen', 59 | parameters=[{'use_sim_time': True}, 60 | {'autostart': True}, 61 | {'node_names': lifecycle_nodes}]) 62 | ]) 63 | -------------------------------------------------------------------------------- /src/simulator/launch/simulator.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | from ament_index_python.packages import get_package_share_directory 5 | 6 | def generate_launch_description(): 7 | # 获取当前package的目录，并以此得到地图文件的文件名 8 | package_dir = get_package_share_directory('simulator') 9 | yaml_filename = os.path.join(package_dir, 'maps', 'b.yaml') 10 | # 设置rviz的加载配置文件位置 11 | rviz_config = os.path.join(package_dir, 'rviz', 'planner.rviz') 12 | # 设置需要被lifecycle manager管理的lifecycle node 13 | lifecycle_nodes = ['map_server'] 14 | return LaunchDescription([ 15 | # 启动仿真时间节点，在/clock话题中发布时钟 16 | Node( 17 | package='simulator', 18 | executable='clock_node', 19 | name='clock', 20 | output='screen' 21 | ), 22 | # rviz控件指令桥接器，用于planner server指令翻译发送 23 | Node( 24 | package='simulator', 25 | executable='planner_bridge', 26 | name='planner_bridge', 27 | parameters=[{'use_sim_time': True}, 28 | {'planner_id': "GridBased"}], 29 | output='screen' 30 | ), 31 | # 发布一个静态的坐标变换关系，用于描述map->base_link 32 | Node( 33 | package='simulator', 34 | executable='transform', 35 | parameters=[{'use_sim_time': True}, 36 | {'pose_x': 0.0},{'pose_y': 0.0}, {'pose_yaw': 0.0}]), 37 | # 打开一个用于显示的rviz2终端 38 | Node( 39 | package='rviz2', 40 | executable='rviz2', 41 | name='rviz2', 42 | arguments=['-d', rviz_config], 43 | parameters=[{'use_sim_time': True}], 44 | ), 45 | # 启动map_server并加载上面变量中的map文件 46 | Node( 47 | package='nav2_map_server', 48 | executable='map_server', 49 | name='map_server', 50 | output='screen', 51 | parameters=[{'yaml_filename': yaml_filename}, 52 | {'use_sim_time': True}]), 53 | # 启动lifecycle manager节点，用于管理map_server的生命周期 54 | Node( 55 | package='nav2_lifecycle_manager', 56 | executable='lifecycle_manager', 57 | name='lifecycle_manager_simulator', 58 | output='screen', 59 | parameters=[{'use_sim_time': True}, 60 | {'autostart': True}, 61 | {'node_names': lifecycle_nodes}]) 62 | ]) 63 | -------------------------------------------------------------------------------- /src/simulator/maps/a.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/src/simulator/maps/a.pgm -------------------------------------------------------------------------------- /src/simulator/maps/a.yaml: -------------------------------------------------------------------------------- 1 | image: a.pgm 2 | mode: trinary 3 | resolution: 0.04 4 | origin: [-15.5, -9.28, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /src/simulator/maps/b.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cf-zhang/nav_simulator/738522689c31a37b6695a75f3285c6d6d9b0af34/src/simulator/maps/b.pgm -------------------------------------------------------------------------------- /src/simulator/maps/b.yaml: -------------------------------------------------------------------------------- 1 | image: b.pgm 2 | mode: trinary 3 | resolution: 0.04 4 | origin: [-15.5, -9.28, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /src/simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | simulator 5 | 0.0.0 6 | a simple simulator for navgation 7 | zhangchuanfa 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rosgraph_msgs 14 | geometry_msgs 15 | tf2_geometry_msgs 16 | nav_msgs 17 | nav2_msgs 18 | rclcpp_action 19 | tf2_ros 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/simulator/rviz/planner.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1/Topic1 10 | - /Map2/Topic1 11 | Splitter Ratio: 0.5 12 | Tree Height: 746 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.699999988079071 35 | Class: rviz_default_plugins/Map 36 | Color Scheme: map 37 | Draw Behind: false 38 | Enabled: true 39 | Name: Map 40 | Topic: 41 | Depth: 5 42 | Durability Policy: Transient Local 43 | Filter size: 10 44 | History Policy: Keep Last 45 | Reliability Policy: Reliable 46 | Value: /map 47 | Update Topic: 48 | Depth: 5 49 | Durability Policy: Volatile 50 | History Policy: Keep Last 51 | Reliability Policy: Reliable 52 | Value: /map_updates 53 | Use Timestamp: false 54 | Value: true 55 | - Class: rviz_default_plugins/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: true 60 | base_link: 61 | Value: true 62 | map: 63 | Value: true 64 | odom: 65 | Value: true 66 | Marker Scale: 1 67 | Name: TF 68 | Show Arrows: true 69 | Show Axes: true 70 | Show Names: false 71 | Tree: 72 | map: 73 | odom: 74 | base_link: 75 | {} 76 | Update Interval: 0 77 | Value: true 78 | - Alpha: 0.699999988079071 79 | Class: rviz_default_plugins/Map 80 | Color Scheme: map 81 | Draw Behind: false 82 | Enabled: true 83 | Name: Map 84 | Topic: 85 | Depth: 5 86 | Durability Policy: Volatile 87 | Filter size: 10 88 | History Policy: Keep Last 89 | Reliability Policy: Reliable 90 | Value: /global_costmap/costmap 91 | Update Topic: 92 | Depth: 5 93 | Durability Policy: Volatile 94 | History Policy: Keep Last 95 | Reliability Policy: Reliable 96 | Value: /global_costmap/costmap_updates 97 | Use Timestamp: false 98 | Value: true 99 | - Alpha: 1 100 | Buffer Length: 1 101 | Class: rviz_default_plugins/Path 102 | Color: 25; 255; 0 103 | Enabled: true 104 | Head Diameter: 0.30000001192092896 105 | Head Length: 0.20000000298023224 106 | Length: 0.30000001192092896 107 | Line Style: Lines 108 | Line Width: 0.029999999329447746 109 | Name: Path 110 | Offset: 111 | X: 0 112 | Y: 0 113 | Z: 0 114 | Pose Color: 255; 85; 255 115 | Pose Style: None 116 | Radius: 0.029999999329447746 117 | Shaft Diameter: 0.10000000149011612 118 | Shaft Length: 0.10000000149011612 119 | Topic: 120 | Depth: 5 121 | Durability Policy: Volatile 122 | Filter size: 10 123 | History Policy: Keep Last 124 | Reliability Policy: Reliable 125 | Value: /plan 126 | Value: true 127 | - Alpha: 1 128 | Class: rviz_default_plugins/Polygon 129 | Color: 25; 255; 0 130 | Enabled: true 131 | Name: Polygon 132 | Topic: 133 | Depth: 5 134 | Durability Policy: Volatile 135 | Filter size: 10 136 | History Policy: Keep Last 137 | Reliability Policy: Reliable 138 | Value: /global_costmap/published_footprint 139 | Value: true 140 | Enabled: true 141 | Global Options: 142 | Background Color: 48; 48; 48 143 | Fixed Frame: map 144 | Frame Rate: 30 145 | Name: root 146 | Tools: 147 | - Class: rviz_default_plugins/Interact 148 | Hide Inactive Objects: true 149 | - Class: rviz_default_plugins/MoveCamera 150 | - Class: rviz_default_plugins/Select 151 | - Class: rviz_default_plugins/FocusCamera 152 | - Class: rviz_default_plugins/Measure 153 | Line color: 128; 128; 0 154 | - Class: rviz_default_plugins/SetInitialPose 155 | Covariance x: 0.25 156 | Covariance y: 0.25 157 | Covariance yaw: 0.06853891909122467 158 | Topic: 159 | Depth: 5 160 | Durability Policy: Volatile 161 | History Policy: Keep Last 162 | Reliability Policy: Reliable 163 | Value: /initialpose 164 | - Class: rviz_default_plugins/SetGoal 165 | Topic: 166 | Depth: 5 167 | Durability Policy: Volatile 168 | History Policy: Keep Last 169 | Reliability Policy: Reliable 170 | Value: /goal_pose 171 | - Class: rviz_default_plugins/PublishPoint 172 | Single click: true 173 | Topic: 174 | Depth: 5 175 | Durability Policy: Volatile 176 | History Policy: Keep Last 177 | Reliability Policy: Reliable 178 | Value: /clicked_point 179 | Transformation: 180 | Current: 181 | Class: rviz_default_plugins/TF 182 | Value: true 183 | Views: 184 | Current: 185 | Class: rviz_default_plugins/Orbit 186 | Distance: 14.594829559326172 187 | Enable Stereo Rendering: 188 | Stereo Eye Separation: 0.05999999865889549 189 | Stereo Focal Distance: 1 190 | Swap Stereo Eyes: false 191 | Value: false 192 | Focal Point: 193 | X: 0 194 | Y: 0 195 | Z: 0 196 | Focal Shape Fixed Size: true 197 | Focal Shape Size: 0.05000000074505806 198 | Invert Z Axis: false 199 | Name: Current View 200 | Near Clip Distance: 0.009999999776482582 201 | Pitch: 1.5697963237762451 202 | Target Frame: 203 | Value: Orbit (rviz) 204 | Yaw: 4.700395107269287 205 | Saved: ~ 206 | Window Geometry: 207 | Displays: 208 | collapsed: false 209 | Height: 1043 210 | Hide Left Dock: false 211 | Hide Right Dock: false 212 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 213 | Selection: 214 | collapsed: false 215 | Time: 216 | collapsed: false 217 | Tool Properties: 218 | collapsed: false 219 | Views: 220 | collapsed: false 221 | Width: 1848 222 | X: 1080 223 | Y: 329 224 | -------------------------------------------------------------------------------- /src/simulator/src/clock_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std::chrono_literals; 7 | 8 | namespace simulator{ 9 | 10 | class ClockPublisher : public rclcpp::Node 11 | { 12 | public: 13 | ClockPublisher() 14 | : rclcpp::Node("clock_publisher"),message_(rosgraph_msgs::msg::Clock()) 15 | { 16 | 17 | publisher_ = this->create_publisher("/clock", 10); 18 | timer_ = this->create_wall_timer( 19 | 10ms, std::bind(&ClockPublisher::timer_callback, this)); 20 | } 21 | 22 | private: 23 | 24 | void timer_callback() 25 | { 26 | message_.clock.nanosec += 10000000; 27 | if(message_.clock.nanosec >= 1000000000){ 28 | message_.clock.sec += 1; 29 | message_.clock.nanosec = 0; 30 | } 31 | publisher_->publish(std::move(message_)); 32 | } 33 | rclcpp::TimerBase::SharedPtr timer_; 34 | rosgraph_msgs::msg::Clock message_; 35 | rclcpp::Publisher::SharedPtr publisher_; 36 | }; 37 | } 38 | int main(int argc, char * argv[]) 39 | { 40 | rclcpp::init(argc, argv); 41 | auto clock_node = std::make_shared(); 42 | rclcpp::spin(clock_node->get_node_base_interface()); 43 | rclcpp::shutdown(); 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /src/simulator/src/planner_bridge.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "rclcpp_action/rclcpp_action.hpp" 5 | 6 | #include "nav2_msgs/action/compute_path_to_pose.hpp" 7 | #include "nav2_msgs/action/compute_path_through_poses.hpp" 8 | #include "geometry_msgs/msg/point_stamped.hpp" 9 | #include "geometry_msgs/msg/pose_stamped.hpp" 10 | 11 | using namespace std::chrono_literals; 12 | 13 | namespace simulator{ 14 | 15 | class PlannerBridge: public rclcpp::Node 16 | { 17 | public: 18 | PlannerBridge() 19 | : rclcpp::Node("planner_bridge") 20 | { 21 | // Parameter initialization 22 | declare_parameter("planner_id", rclcpp::ParameterValue("GridBased")); 23 | get_parameter("planner_id", planner_id_); 24 | 25 | goal_sub_ = create_subscription( 26 | "goal_pose", 27 | rclcpp::SystemDefaultsQoS(), 28 | std::bind(&PlannerBridge::onGoalReceived, this, std::placeholders::_1)); 29 | 30 | point_sub_ = create_subscription( 31 | "clicked_point", 32 | rclcpp::SystemDefaultsQoS(), 33 | std::bind(&PlannerBridge::onPointReceived, this, std::placeholders::_1)); 34 | 35 | pose_client_ = rclcpp_action::create_client( 36 | this, 37 | "compute_path_to_pose"); 38 | 39 | poses_client_ = rclcpp_action::create_client( 40 | this, 41 | "compute_path_through_poses"); 42 | } 43 | 44 | protected: 45 | 46 | void onGoalReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose) 47 | { 48 | std::cout<<"123 "< 0){ 50 | poses_.push_back(*pose); 51 | auto goal_msg = nav2_msgs::action::ComputePathThroughPoses::Goal(); 52 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 53 | 54 | goal_msg.goals = poses_; 55 | goal_msg.planner_id = planner_id_; 56 | 57 | if (!poses_client_->wait_for_action_server()) { 58 | RCLCPP_ERROR(get_logger(), "Action server %s not available after waiting", "compute_path_through_pose"); 59 | return; 60 | } 61 | poses_client_->async_send_goal(goal_msg, send_goal_options); 62 | }else{ 63 | auto goal_msg = nav2_msgs::action::ComputePathToPose::Goal(); 64 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 65 | goal_msg.goal = *pose; 66 | goal_msg.planner_id = planner_id_; 67 | if (!pose_client_->wait_for_action_server()) { 68 | RCLCPP_ERROR(get_logger(), "Action server %s not available after waiting", "compute_path_to_pose"); 69 | return; 70 | } 71 | pose_client_->async_send_goal(goal_msg, send_goal_options); 72 | } 73 | poses_.clear(); 74 | } 75 | 76 | void onPointReceived(const geometry_msgs::msg::PointStamped::SharedPtr pose) 77 | { 78 | geometry_msgs::msg::PoseStamped p; 79 | p.header = pose->header; 80 | p.pose.position.x = pose->point.x; 81 | p.pose.position.y = pose->point.y; 82 | poses_.push_back(p); 83 | } 84 | 85 | private: 86 | 87 | std::string planner_id_; 88 | std::vector poses_; 89 | 90 | rclcpp::Subscription::SharedPtr goal_sub_; 91 | rclcpp::Subscription::SharedPtr point_sub_; 92 | 93 | rclcpp_action::Client::SharedPtr pose_client_; 94 | rclcpp_action::Client::SharedPtr poses_client_; 95 | }; 96 | } 97 | 98 | int main(int argc, char * argv[]) 99 | { 100 | rclcpp::init(argc, argv); 101 | auto planner_bridge = std::make_shared(); 102 | rclcpp::spin(planner_bridge->get_node_base_interface()); 103 | rclcpp::shutdown(); 104 | return 0; 105 | } 106 | -------------------------------------------------------------------------------- /src/simulator/src/telecontrol.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define KEYCODE_R 0x43 9 | #define KEYCODE_L 0x44 10 | #define KEYCODE_U 0x41 11 | #define KEYCODE_D 0x42 12 | #define KEYCODE_Q 0x71 13 | 14 | int kfd = 0; 15 | struct termios cooked, raw; 16 | 17 | namespace simulator{ 18 | 19 | class TeleControl : public rclcpp::Node 20 | { 21 | public: 22 | TeleControl(const std::string& name); 23 | void keyLoop(); 24 | 25 | private: 26 | double linear_, angular_, l_scale_, a_scale_; 27 | rclcpp::Publisher::SharedPtr twist_pub_; 28 | }; 29 | 30 | TeleControl::TeleControl(const std::string& name): 31 | rclcpp::Node("telecontrol"), 32 | linear_(0), 33 | angular_(0), 34 | l_scale_(2.0), 35 | a_scale_(2.0) 36 | { 37 | 38 | twist_pub_ = this->create_publisher(name + "/cmd_vel", 1); 39 | } 40 | 41 | 42 | void TeleControl::keyLoop() 43 | { 44 | char c; 45 | bool dirty=false; 46 | 47 | 48 | // get the console in raw mode 49 | tcgetattr(kfd, &cooked); 50 | memcpy(&raw, &cooked, sizeof(struct termios)); 51 | raw.c_lflag &=~ (ICANON | ECHO); 52 | // Setting a new line, then end of file 53 | raw.c_cc[VEOL] = 1; 54 | raw.c_cc[VEOF] = 2; 55 | tcsetattr(kfd, TCSANOW, &raw); 56 | 57 | puts("Reading from keyboard"); 58 | puts("---------------------------"); 59 | puts("Use arrow keys to move the turtle."); 60 | 61 | 62 | for(;;) 63 | { 64 | // get the next event from the keyboard 65 | if(read(kfd, &c, 1) < 0) 66 | { 67 | perror("read():"); 68 | exit(-1); 69 | } 70 | 71 | linear_=angular_=0; 72 | 73 | switch(c) 74 | { 75 | case KEYCODE_L: 76 | angular_ = 1.0; 77 | dirty = true; 78 | break; 79 | case KEYCODE_R: 80 | angular_ = -1.0; 81 | dirty = true; 82 | break; 83 | case KEYCODE_U: 84 | linear_ = 1.0; 85 | dirty = true; 86 | break; 87 | case KEYCODE_D: 88 | linear_ = -1.0; 89 | dirty = true; 90 | break; 91 | } 92 | 93 | geometry_msgs::msg::Twist twist; 94 | twist.angular.z = a_scale_*angular_; 95 | twist.linear.x = l_scale_*linear_; 96 | if(dirty ==true) 97 | { 98 | twist_pub_->publish(std::move(twist)); 99 | dirty=false; 100 | } 101 | } 102 | return; 103 | } 104 | } 105 | 106 | void quit(int sig) 107 | { 108 | (void)sig; 109 | tcsetattr(kfd, TCSANOW, &cooked); 110 | rclcpp::shutdown(); 111 | exit(0); 112 | } 113 | 114 | int main(int argc, char** argv) 115 | { 116 | rclcpp::init(argc, argv); 117 | std::string name = ""; 118 | if(argc > 2){ 119 | puts("usages: handlebar robot_name"); 120 | return 0; 121 | }else if (argc == 2){ 122 | name = std::string(argv[1]); 123 | } 124 | 125 | simulator::TeleControl teleop_turtle(name); 126 | signal(SIGINT,quit); 127 | 128 | teleop_turtle.keyLoop(); 129 | 130 | return(0); 131 | } 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /src/simulator/src/transform.cpp: -------------------------------------------------------------------------------- 1 | #include "simulator/transform.hpp" 2 | 3 | namespace simulator 4 | { 5 | Transform::Transform() : rclcpp::Node("robot"){ 6 | declare_parameter("velocity_topic", "cmd_vel"); 7 | declare_parameter("odometry_topic", "odom"); 8 | declare_parameter("robot_frame_id", "base_link"); 9 | declare_parameter("odom_frame_id", "odom"); 10 | declare_parameter("world_frame_id", "map"); 11 | declare_parameter("pose_x", 0.0); 12 | declare_parameter("pose_y", 0.0); 13 | declare_parameter("pose_yaw", 0.0); 14 | 15 | double yaw; 16 | initial_pose_ = std::make_shared(); 17 | get_parameter("velocity_topic", velocity_topic_); 18 | get_parameter("odometry_topic", odometry_topic_); 19 | get_parameter("robot_frame_id", robot_frame_id_); 20 | get_parameter("odom_frame_id", odom_frame_id_); 21 | get_parameter("world_frame_id", world_frame_id_); 22 | get_parameter("pose_x", initial_pose_->pose.pose.position.x); 23 | get_parameter("pose_y", initial_pose_->pose.pose.position.y); 24 | get_parameter("pose_yaw", yaw); 25 | tf2::Quaternion q; 26 | q.setRPY(0, 0, yaw); 27 | initial_pose_->pose.pose.orientation = tf2::toMsg(q); 28 | initial_pose_->header.frame_id = world_frame_id_; 29 | initial_pose_->header.stamp = now(); 30 | 31 | odom_pub_ = this->create_publisher(odometry_topic_, 20); 32 | vel_sub_ = this->create_subscription(velocity_topic_, 5, 33 | std::bind(&Transform::velCallback, this, _1)); 34 | 35 | last_update_moment_ = now(); 36 | last_vel_moment_ = last_update_moment_ - rclcpp::Duration::from_seconds(0.1); 37 | tf_broadcaster_ = std::make_unique(*this); 38 | tf_buffer_ = std::make_unique(get_clock()); 39 | odom_.header.stamp = last_update_moment_; 40 | 41 | init_pose_sub_ = this->create_subscription( 42 | "initialpose", 5, std::bind(&Transform::initPoseCallback, this, _1)); 43 | map_odom_trans_.header.frame_id = world_frame_id_; 44 | map_odom_trans_.header.stamp = last_update_moment_; 45 | map_odom_trans_.child_frame_id = odom_frame_id_; 46 | map_odom_trans_.transform.rotation.w = 1.0; 47 | 48 | loop_timer_ = this->create_wall_timer(20ms, std::bind(&Transform::updateLoop, this)); 49 | 50 | initPoseCallback(initial_pose_); 51 | } 52 | 53 | Transform::~Transform(){} 54 | 55 | void Transform::updateLoop(){ 56 | last_update_moment_ = now(); 57 | if (!message_received_) 58 | { 59 | odom_.header.stamp = last_update_moment_; 60 | odom_baselink_trans_.header.stamp = last_update_moment_; 61 | } 62 | 63 | odom_pub_->publish(odom_); 64 | getTfFromOdom(odom_); 65 | tf_broadcaster_->sendTransform(odom_baselink_trans_); 66 | message_received_ = false; 67 | map_odom_trans_.header.stamp = last_update_moment_; 68 | tf_broadcaster_->sendTransform(map_odom_trans_); 69 | } 70 | 71 | void Transform::updateOdomFromVel(geometry_msgs::msg::Twist vel, rclcpp::Duration time_diff){ 72 | double dt = time_diff.seconds(); 73 | if (dt > 1.0) 74 | { 75 | return; 76 | } 77 | double delta_x = (vel.linear.x * cos(th_) - vel.linear.y * sin(th_)) * dt; 78 | double delta_y = (vel.linear.x * sin(th_) + vel.linear.y * cos(th_)) * dt; 79 | double delta_th = vel.angular.z * dt; 80 | odom_.pose.pose.position.x += delta_x; 81 | odom_.pose.pose.position.y += delta_y; 82 | th_ += delta_th; 83 | odom_.pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), th_)); 84 | odom_.twist.twist = vel; 85 | odom_.header.stamp = latest_vel_moment_; 86 | odom_.header.frame_id = odom_frame_id_; 87 | } 88 | void Transform::getTfFromOdom(nav_msgs::msg::Odometry odom){ 89 | geometry_msgs::msg::TransformStamped odom_tmp; 90 | 91 | odom_tmp.header = odom.header; 92 | odom_tmp.header.frame_id = odom_frame_id_; 93 | odom_tmp.child_frame_id = robot_frame_id_; 94 | odom_tmp.transform.translation.x = odom.pose.pose.position.x; 95 | odom_tmp.transform.translation.y = odom.pose.pose.position.y; 96 | odom_tmp.transform.translation.z = 0.0; 97 | odom_tmp.transform.rotation = odom.pose.pose.orientation; 98 | odom_baselink_trans_ = odom_tmp; 99 | } 100 | void Transform::velCallback(const geometry_msgs::msg::Twist::SharedPtr msg){ 101 | latest_vel_moment_ = now(); 102 | 103 | message_received_ = true; 104 | RCLCPP_ERROR(get_logger(), "%f, %f", last_update_moment_.seconds(),last_vel_moment_.seconds()); 105 | updateOdomFromVel(*msg, latest_vel_moment_ - last_vel_moment_); 106 | last_vel_moment_ = latest_vel_moment_; 107 | } 108 | void Transform::initPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg){ 109 | if (msg->header.frame_id.find(world_frame_id_) == msg->header.frame_id.npos) 110 | { 111 | RCLCPP_ERROR(get_logger(), "Initial pose not specified in map frame, ignoring"); 112 | return; 113 | } 114 | RCLCPP_INFO(get_logger(), "Received pose estimate of mobile base"); 115 | 116 | tf2::Transform tx_odom_tf2, pose_old; 117 | tf2::impl::Converter::convert(odom_baselink_trans_.transform, tx_odom_tf2); 118 | tf2::impl::Converter::convert(msg->pose.pose, pose_old); 119 | tf2::Transform pose_new = pose_old * tx_odom_tf2.inverse(); 120 | 121 | map_odom_trans_.header.stamp = now(); 122 | map_odom_trans_.header.frame_id = world_frame_id_; 123 | map_odom_trans_.child_frame_id = odom_frame_id_; 124 | map_odom_trans_.transform.translation.x = pose_new.getOrigin().x(); 125 | map_odom_trans_.transform.translation.y = pose_new.getOrigin().y(); 126 | map_odom_trans_.transform.translation.z = pose_new.getOrigin().z(); 127 | map_odom_trans_.transform.rotation.x = pose_new.getRotation().x(); 128 | map_odom_trans_.transform.rotation.y = pose_new.getRotation().y(); 129 | map_odom_trans_.transform.rotation.z = pose_new.getRotation().z(); 130 | map_odom_trans_.transform.rotation.w = pose_new.getRotation().w(); 131 | } 132 | } 133 | 134 | int main(int argc, char * argv[]) 135 | { 136 | rclcpp::init(argc, argv); 137 | rclcpp::spin(std::make_shared()); 138 | rclcpp::shutdown(); 139 | return 0; 140 | } 141 | --------------------------------------------------------------------------------