├── .clang-format ├── .github └── workflows │ ├── docker-ros.yml │ └── industrial_ci.yml ├── .gitignore ├── .gitlab-ci.yml ├── LICENSE ├── README.md ├── mqtt_client ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── params.aws.yaml │ ├── params.ros2.aws.yaml │ ├── params.ros2.fixed-type-and-qos.yaml │ ├── params.ros2.primitive-fixed-type-and-qos.yaml │ ├── params.ros2.primitive.yaml │ ├── params.ros2.yaml │ └── params.yaml ├── doc │ └── mainpage.dox ├── include │ └── mqtt_client │ │ ├── MqttClient.h │ │ └── MqttClient.ros2.hpp ├── launch │ ├── standalone.launch │ └── standalone.launch.ros2.xml ├── nodelet_plugins.xml ├── package.xml └── src │ ├── MqttClient.cpp │ └── MqttClient.ros2.cpp └── mqtt_client_interfaces ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg └── RosMsgType.msg ├── package.xml └── srv ├── IsConnected.srv └── ros2 ├── NewMqtt2RosBridge.srv └── NewRos2MqttBridge.srv /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Google 3 | AlignEscapedNewlines: Right 4 | AllowShortFunctionsOnASingleLine: Empty 5 | BraceWrapping: 6 | AfterCaseLabel: true 7 | AfterClass: true 8 | AfterControlStatement: true 9 | AfterEnum: true 10 | AfterFunction: true 11 | AfterNamespace: true 12 | AfterObjCDeclaration: true 13 | AfterStruct: true 14 | AfterUnion: true 15 | AfterExternBlock: true 16 | BeforeCatch: true 17 | BeforeElse: true 18 | ConstructorInitializerIndentWidth: 2 19 | ContinuationIndentWidth: 2 20 | IncludeCategories: 21 | - Regex: '^<[^\/]*\.h>' 22 | Priority: 1 23 | - Regex: '^<[^\/]*>' 24 | Priority: 2 25 | - Regex: '^<.*>' 26 | Priority: 3 27 | - Regex: '.*' 28 | Priority: 4 29 | IndentWidth: 2 30 | IndentWrappedFunctionNames: true 31 | KeepEmptyLinesAtTheStartOfBlocks: true 32 | MaxEmptyLinesToKeep: 2 33 | SpacesInContainerLiterals: false -------------------------------------------------------------------------------- /.github/workflows/docker-ros.yml: -------------------------------------------------------------------------------- 1 | name: docker-ros 2 | 3 | on: push 4 | 5 | jobs: 6 | 7 | ros: 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: ika-rwth-aachen/docker-ros@main 11 | with: 12 | platform: amd64,arm64 13 | target: run 14 | image-tag: ros 15 | base-image: rwthika/ros:latest 16 | command: roslaunch mqtt_client standalone.launch 17 | enable-industrial-ci: 'true' 18 | 19 | ros2: 20 | runs-on: ubuntu-latest 21 | steps: 22 | - uses: ika-rwth-aachen/docker-ros@main 23 | with: 24 | platform: amd64,arm64 25 | target: run 26 | image-tag: ros2 27 | enable-push-as-latest: 'true' 28 | base-image: rwthika/ros2:latest 29 | command: ros2 launch mqtt_client standalone.launch.ros2.xml 30 | enable-industrial-ci: 'true' 31 | -------------------------------------------------------------------------------- /.github/workflows/industrial_ci.yml: -------------------------------------------------------------------------------- 1 | name: industrial_ci 2 | 3 | on: pull_request 4 | 5 | jobs: 6 | industrial_ci: 7 | name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) 8 | runs-on: ubuntu-latest 9 | strategy: 10 | matrix: 11 | ROS_DISTRO: 12 | - noetic 13 | - humble 14 | - iron 15 | - jazzy 16 | - rolling 17 | ROS_REPO: 18 | - testing 19 | - main 20 | steps: 21 | - uses: actions/checkout@v3 22 | - uses: ros-industrial/industrial_ci@master 23 | with: 24 | config: ${{ toJSON(matrix) }} 25 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.toptal.com/developers/gitignore/api/ros,ros2,c++,linux,macos,python,pycharm,visualstudiocode 2 | # Edit at https://www.toptal.com/developers/gitignore?templates=ros,ros2,c++,linux,macos,python,pycharm,visualstudiocode 3 | 4 | ### C++ ### 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | 38 | ### Linux ### 39 | *~ 40 | 41 | # temporary files which can be created if a process still has a handle open of a deleted file 42 | .fuse_hidden* 43 | 44 | # KDE directory preferences 45 | .directory 46 | 47 | # Linux trash folder which might appear on any partition or disk 48 | .Trash-* 49 | 50 | # .nfs files are created when an open file is removed but is still being accessed 51 | .nfs* 52 | 53 | ### macOS ### 54 | # General 55 | .DS_Store 56 | .AppleDouble 57 | .LSOverride 58 | 59 | # Icon must end with two \r 60 | Icon 61 | 62 | 63 | # Thumbnails 64 | ._* 65 | 66 | # Files that might appear in the root of a volume 67 | .DocumentRevisions-V100 68 | .fseventsd 69 | .Spotlight-V100 70 | .TemporaryItems 71 | .Trashes 72 | .VolumeIcon.icns 73 | .com.apple.timemachine.donotpresent 74 | 75 | # Directories potentially created on remote AFP share 76 | .AppleDB 77 | .AppleDesktop 78 | Network Trash Folder 79 | Temporary Items 80 | .apdisk 81 | 82 | ### macOS Patch ### 83 | # iCloud generated files 84 | *.icloud 85 | 86 | ### PyCharm ### 87 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider 88 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 89 | 90 | # User-specific stuff 91 | .idea/**/workspace.xml 92 | .idea/**/tasks.xml 93 | .idea/**/usage.statistics.xml 94 | .idea/**/dictionaries 95 | .idea/**/shelf 96 | 97 | # AWS User-specific 98 | .idea/**/aws.xml 99 | 100 | # Generated files 101 | .idea/**/contentModel.xml 102 | 103 | # Sensitive or high-churn files 104 | .idea/**/dataSources/ 105 | .idea/**/dataSources.ids 106 | .idea/**/dataSources.local.xml 107 | .idea/**/sqlDataSources.xml 108 | .idea/**/dynamic.xml 109 | .idea/**/uiDesigner.xml 110 | .idea/**/dbnavigator.xml 111 | 112 | # Gradle 113 | .idea/**/gradle.xml 114 | .idea/**/libraries 115 | 116 | # Gradle and Maven with auto-import 117 | # When using Gradle or Maven with auto-import, you should exclude module files, 118 | # since they will be recreated, and may cause churn. Uncomment if using 119 | # auto-import. 120 | # .idea/artifacts 121 | # .idea/compiler.xml 122 | # .idea/jarRepositories.xml 123 | # .idea/modules.xml 124 | # .idea/*.iml 125 | # .idea/modules 126 | # *.iml 127 | # *.ipr 128 | 129 | # CMake 130 | cmake-build-*/ 131 | 132 | # Mongo Explorer plugin 133 | .idea/**/mongoSettings.xml 134 | 135 | # File-based project format 136 | *.iws 137 | 138 | # IntelliJ 139 | out/ 140 | 141 | # mpeltonen/sbt-idea plugin 142 | .idea_modules/ 143 | 144 | # JIRA plugin 145 | atlassian-ide-plugin.xml 146 | 147 | # Cursive Clojure plugin 148 | .idea/replstate.xml 149 | 150 | # SonarLint plugin 151 | .idea/sonarlint/ 152 | 153 | # Crashlytics plugin (for Android Studio and IntelliJ) 154 | com_crashlytics_export_strings.xml 155 | crashlytics.properties 156 | crashlytics-build.properties 157 | fabric.properties 158 | 159 | # Editor-based Rest Client 160 | .idea/httpRequests 161 | 162 | # Android studio 3.1+ serialized cache file 163 | .idea/caches/build_file_checksums.ser 164 | 165 | ### PyCharm Patch ### 166 | # Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721 167 | 168 | # *.iml 169 | # modules.xml 170 | # .idea/misc.xml 171 | # *.ipr 172 | 173 | # Sonarlint plugin 174 | # https://plugins.jetbrains.com/plugin/7973-sonarlint 175 | .idea/**/sonarlint/ 176 | 177 | # SonarQube Plugin 178 | # https://plugins.jetbrains.com/plugin/7238-sonarqube-community-plugin 179 | .idea/**/sonarIssues.xml 180 | 181 | # Markdown Navigator plugin 182 | # https://plugins.jetbrains.com/plugin/7896-markdown-navigator-enhanced 183 | .idea/**/markdown-navigator.xml 184 | .idea/**/markdown-navigator-enh.xml 185 | .idea/**/markdown-navigator/ 186 | 187 | # Cache file creation bug 188 | # See https://youtrack.jetbrains.com/issue/JBR-2257 189 | .idea/$CACHE_FILE$ 190 | 191 | # CodeStream plugin 192 | # https://plugins.jetbrains.com/plugin/12206-codestream 193 | .idea/codestream.xml 194 | 195 | # Azure Toolkit for IntelliJ plugin 196 | # https://plugins.jetbrains.com/plugin/8053-azure-toolkit-for-intellij 197 | .idea/**/azureSettings.xml 198 | 199 | ### Python ### 200 | # Byte-compiled / optimized / DLL files 201 | __pycache__/ 202 | *.py[cod] 203 | *$py.class 204 | 205 | # C extensions 206 | 207 | # Distribution / packaging 208 | .Python 209 | build/ 210 | develop-eggs/ 211 | dist/ 212 | downloads/ 213 | eggs/ 214 | .eggs/ 215 | lib/ 216 | lib64/ 217 | parts/ 218 | sdist/ 219 | var/ 220 | wheels/ 221 | share/python-wheels/ 222 | *.egg-info/ 223 | .installed.cfg 224 | *.egg 225 | MANIFEST 226 | 227 | # PyInstaller 228 | # Usually these files are written by a python script from a template 229 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 230 | *.manifest 231 | *.spec 232 | 233 | # Installer logs 234 | pip-log.txt 235 | pip-delete-this-directory.txt 236 | 237 | # Unit test / coverage reports 238 | htmlcov/ 239 | .tox/ 240 | .nox/ 241 | .coverage 242 | .coverage.* 243 | .cache 244 | nosetests.xml 245 | coverage.xml 246 | *.cover 247 | *.py,cover 248 | .hypothesis/ 249 | .pytest_cache/ 250 | cover/ 251 | 252 | # Translations 253 | *.mo 254 | *.pot 255 | 256 | # Django stuff: 257 | *.log 258 | local_settings.py 259 | db.sqlite3 260 | db.sqlite3-journal 261 | 262 | # Flask stuff: 263 | instance/ 264 | .webassets-cache 265 | 266 | # Scrapy stuff: 267 | .scrapy 268 | 269 | # Sphinx documentation 270 | docs/_build/ 271 | 272 | # PyBuilder 273 | .pybuilder/ 274 | target/ 275 | 276 | # Jupyter Notebook 277 | .ipynb_checkpoints 278 | 279 | # IPython 280 | profile_default/ 281 | ipython_config.py 282 | 283 | # pyenv 284 | # For a library or package, you might want to ignore these files since the code is 285 | # intended to run in multiple environments; otherwise, check them in: 286 | # .python-version 287 | 288 | # pipenv 289 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 290 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 291 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 292 | # install all needed dependencies. 293 | #Pipfile.lock 294 | 295 | # poetry 296 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 297 | # This is especially recommended for binary packages to ensure reproducibility, and is more 298 | # commonly ignored for libraries. 299 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 300 | #poetry.lock 301 | 302 | # pdm 303 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 304 | #pdm.lock 305 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 306 | # in version control. 307 | # https://pdm.fming.dev/#use-with-ide 308 | .pdm.toml 309 | 310 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 311 | __pypackages__/ 312 | 313 | # Celery stuff 314 | celerybeat-schedule 315 | celerybeat.pid 316 | 317 | # SageMath parsed files 318 | *.sage.py 319 | 320 | # Environments 321 | .env 322 | .venv 323 | env/ 324 | venv/ 325 | ENV/ 326 | env.bak/ 327 | venv.bak/ 328 | 329 | # Spyder project settings 330 | .spyderproject 331 | .spyproject 332 | 333 | # Rope project settings 334 | .ropeproject 335 | 336 | # mkdocs documentation 337 | /site 338 | 339 | # mypy 340 | .mypy_cache/ 341 | .dmypy.json 342 | dmypy.json 343 | 344 | # Pyre type checker 345 | .pyre/ 346 | 347 | # pytype static type analyzer 348 | .pytype/ 349 | 350 | # Cython debug symbols 351 | cython_debug/ 352 | 353 | # PyCharm 354 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 355 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 356 | # and can be added to the global gitignore or merged into this file. For a more nuclear 357 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 358 | #.idea/ 359 | 360 | ### Python Patch ### 361 | # Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration 362 | poetry.toml 363 | 364 | # ruff 365 | .ruff_cache/ 366 | 367 | # LSP config files 368 | pyrightconfig.json 369 | 370 | ### ROS ### 371 | devel/ 372 | logs/ 373 | bin/ 374 | msg_gen/ 375 | srv_gen/ 376 | msg/*Action.msg 377 | msg/*ActionFeedback.msg 378 | msg/*ActionGoal.msg 379 | msg/*ActionResult.msg 380 | msg/*Feedback.msg 381 | msg/*Goal.msg 382 | msg/*Result.msg 383 | msg/_*.py 384 | build_isolated/ 385 | devel_isolated/ 386 | 387 | # Generated by dynamic reconfigure 388 | *.cfgc 389 | /cfg/cpp/ 390 | /cfg/*.py 391 | 392 | # Ignore generated docs 393 | *.dox 394 | *.wikidoc 395 | 396 | # eclipse stuff 397 | .project 398 | .cproject 399 | 400 | # qcreator stuff 401 | CMakeLists.txt.user 402 | 403 | srv/_*.py 404 | *.pcd 405 | *.pyc 406 | qtcreator-* 407 | *.user 408 | 409 | /planning/cfg 410 | /planning/docs 411 | /planning/src 412 | 413 | 414 | # Emacs 415 | .#* 416 | 417 | # Catkin custom files 418 | CATKIN_IGNORE 419 | 420 | ### ROS2 ### 421 | install/ 422 | log/ 423 | 424 | # Ignore generated docs 425 | 426 | # eclipse stuff 427 | 428 | # qcreator stuff 429 | 430 | 431 | 432 | # Emacs 433 | 434 | # Colcon custom files 435 | COLCON_IGNORE 436 | AMENT_IGNORE 437 | 438 | ### VisualStudioCode ### 439 | .vscode/* 440 | !.vscode/settings.json 441 | !.vscode/tasks.json 442 | !.vscode/launch.json 443 | !.vscode/extensions.json 444 | !.vscode/*.code-snippets 445 | 446 | # Local History for Visual Studio Code 447 | .history/ 448 | 449 | # Built Visual Studio Code Extensions 450 | *.vsix 451 | 452 | ### VisualStudioCode Patch ### 453 | # Ignore all local history of files 454 | .history 455 | .ionide 456 | 457 | # End of https://www.toptal.com/developers/gitignore/api/ros,ros2,c++,linux,macos,python,pycharm,visualstudiocode 458 | 459 | !mqtt_client/doc/mainpage.dox -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | ros: 2 | trigger: 3 | include: 4 | - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml 5 | strategy: depend 6 | variables: 7 | IMAGE_TAG: ros 8 | BASE_IMAGE: rwthika/ros:latest 9 | COMMAND: roslaunch mqtt_client standalone.launch 10 | PLATFORM: amd64,arm64 11 | TARGET: dev,run 12 | ENABLE_INDUSTRIAL_CI: 'true' 13 | 14 | ros2: 15 | trigger: 16 | include: 17 | - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml 18 | strategy: depend 19 | variables: 20 | IMAGE_TAG: ros2 21 | BASE_IMAGE: rwthika/ros2:latest 22 | COMMAND: ros2 launch mqtt_client standalone.launch.ros2.xml 23 | PLATFORM: amd64,arm64 24 | TARGET: dev,run 25 | ENABLE_INDUSTRIAL_CI: 'true' 26 | ENABLE_PUSH_AS_LATEST: 'true' 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Institute for Automotive Engineering of RWTH Aachen University 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mqtt_client 2 | 3 |

4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 |

12 | 13 | The *mqtt_client* package provides a ROS nodelet or ROS 2 component node that enables connected ROS-based devices or robots to exchange ROS messages via an MQTT broker using the [MQTT](http://mqtt.org) protocol. This works generically for arbitrary ROS message types. The *mqtt_client* can also exchange primitive messages with MQTT clients running on devices not based on ROS. 14 | 15 | > [!IMPORTANT] 16 | > This repository is open-sourced and maintained by the [**Institute for Automotive Engineering (ika) at RWTH Aachen University**](https://www.ika.rwth-aachen.de/). 17 | > **V2X Communication** is one of many research topics within our [*Vehicle Intelligence & Automated Driving*](https://www.ika.rwth-aachen.de/en/competences/fields-of-research/vehicle-intelligence-automated-driving.html) domain. 18 | > If you would like to learn more about how we can support your automated driving or robotics efforts, feel free to reach out to us! 19 | > :email: ***opensource@ika.rwth-aachen.de*** 20 | 21 | - [Installation](#installation) 22 | - [docker-ros](#docker-ros) 23 | - [Usage](#usage) 24 | - [Quick Start](#quick-start) 25 | - [Launch](#launch) 26 | - [Configuration](#configuration) 27 | - [Primitive Messages](#primitive-messages) 28 | - [Latency Computation](#latency-computation) 29 | - [Package Summary](#package-summary) 30 | - [How It Works](#how-it-works) 31 | - [Acknowledgements](#acknowledgements) 32 | 33 | ## Installation 34 | 35 | The *mqtt_client* package is released as an official ROS / ROS 2 package and can easily be installed via a package manager. 36 | 37 | ```bash 38 | sudo apt update 39 | sudo apt install ros-$ROS_DISTRO-mqtt-client 40 | ``` 41 | 42 | If you would like to install *mqtt_client* from source, simply clone this repository into your ROS workspace. All dependencies that are listed in the ROS [`package.xml`](package.xml) can be installed using [*rosdep*](http://wiki.ros.org/rosdep). 43 | 44 | ```bash 45 | # mqtt_client$ 46 | rosdep install -r --ignore-src --from-paths . 47 | 48 | # ROS 49 | # workspace$ 50 | catkin build -DCMAKE_BUILD_TYPE=Release mqtt_client 51 | 52 | # ROS 2 53 | # workspace$ 54 | colcon build --packages-up-to mqtt_client --cmake-args -DCMAKE_BUILD_TYPE=Release 55 | ``` 56 | 57 | ### docker-ros 58 | 59 | *mqtt_client* is also available as a Docker image, containerized through [*docker-ros*](https://github.com/ika-rwth-aachen/docker-ros). 60 | 61 | ```bash 62 | # ROS 63 | docker run --rm ghcr.io/ika-rwth-aachen/mqtt_client:ros 64 | 65 | # ROS 2 66 | docker run --rm ghcr.io/ika-rwth-aachen/mqtt_client:ros2 67 | ``` 68 | 69 | 70 | ## Usage 71 | 72 | The *mqtt_client* can be easily integrated into an existing ROS-based system. Below, you first find a quick start guide to test the *mqtt_client* on a single machine. Then, more details are presented on how to launch and configure it in more complex applications. 73 | 74 | ### Quick Start 75 | 76 | Follow these steps to quickly launch a working *mqtt_client* that is sending ROS messages via an MQTT broker to itself. 77 | 78 | #### Demo Broker 79 | 80 | It is assumed that an MQTT broker (such as [*Mosquitto*](https://mosquitto.org/)) is running on `localhost:1883`. 81 | 82 | For this demo, you may easily launch *Mosquitto* with its default configuration using *Docker*. 83 | 84 | ```bash 85 | docker run --rm --network host --name mosquitto eclipse-mosquitto 86 | ``` 87 | 88 | For a more advanced setup of your own broker, check out our instructions for running an MQTT broker in Docker with enabled authentication and encryption [here](https://github.com/ika-rwth-aachen/mqtt-in-docker). 89 | 90 | #### Demo Configuration 91 | 92 | The *mqtt_client* is best configured with a ROS parameter *yaml* file. The configuration shown below (also see [`params.yaml`](mqtt_client/config/params.yaml) / [`params.ros2.yaml`](mqtt_client/config/params.ros2.yaml)) allows an exchange of messages as follows: 93 | 94 | - ROS messages received locally on ROS topic `/ping/ros` are sent to the broker on MQTT topic `pingpong/ros`; 95 | - MQTT messages received from the broker on MQTT topic `pingpong/ros` are published locally on ROS topic `/pong/ros`; 96 | - primitive ROS messages received locally on ROS topic `/ping/primitive` are sent as primitive (string) messages to the broker on MQTT topic `pingpong/primitive`; 97 | - MQTT messages received from the broker on MQTT topic `pingpong/primitive` are published locally as primitive ROS messages on ROS topic `/pong/primitive`. 98 | 99 | ```yaml 100 | broker: 101 | host: localhost 102 | port: 1883 103 | bridge: 104 | ros2mqtt: 105 | - ros_topic: /ping/ros 106 | mqtt_topic: pingpong/ros 107 | - ros_topic: /ping/primitive 108 | mqtt_topic: pingpong/primitive 109 | primitive: true 110 | mqtt2ros: 111 | - mqtt_topic: pingpong/ros 112 | ros_topic: /pong/ros 113 | - mqtt_topic: pingpong/primitive 114 | ros_topic: /pong/primitive 115 | primitive: true 116 | ``` 117 | 118 | #### Demo Client Launch 119 | 120 | After building your ROS workspace, launch the *mqtt_client* node with the pre-configured demo parameters using *roslaunch*, which should yield the following output. 121 | 122 | ```bash 123 | # ROS 124 | roslaunch mqtt_client standalone.launch 125 | 126 | # ROS 2 127 | ros2 launch mqtt_client standalone.launch.ros2.xml 128 | ``` 129 | 130 | ```txt 131 | [ WARN] [1665575657.358869079]: Parameter 'broker/tls/enabled' not set, defaulting to '0' 132 | [ WARN] [1665575657.359798329]: Parameter 'client/id' not set, defaulting to '' 133 | [ WARN] [1665575657.359810889]: Client buffer can not be enabled when client ID is empty 134 | [ WARN] [1665575657.360300703]: Parameter 'client/clean_session' not set, defaulting to '1' 135 | [ WARN] [1665575657.360576344]: Parameter 'client/keep_alive_interval' not set, defaulting to '60.000000' 136 | [ WARN] [1665575657.360847295]: Parameter 'client/max_inflight' not set, defaulting to '65535' 137 | [ INFO] [1665575657.361281461]: Bridging ROS topic '/ping/ros' to MQTT topic 'pingpong/ros' 138 | [ INFO] [1665575657.361303380]: Bridging primitive ROS topic '/ping/primitive' to MQTT topic 'pingpong/primitive' 139 | [ INFO] [1665575657.361352809]: Bridging MQTT topic 'pingpong/ros' to ROS topic '/pong/ros' 140 | [ INFO] [1665575657.361370558]: Bridging MQTT topic 'pingpong/primitive' to primitive ROS topic '/pong/primitive' 141 | [ INFO] [1665575657.362153083]: Connecting to broker at 'tcp://localhost:1883' ... 142 | [ INFO] [1665575657.462622065]: Connected to broker at 'tcp://localhost:1883' 143 | ``` 144 | 145 | Note that the *mqtt_client* successfully connected to the broker and also echoed which ROS/MQTT topics are being bridged. For testing the communication between *mqtt_client*, itself, and other MQTT clients, open five new terminals. 146 | 147 | In order to test the communication among *mqtt_clients*, publish any ROS message on ROS topic `/ping/ros` and wait for a response on ROS topic `/pong/ros`. 148 | 149 | ```bash 150 | # 1st terminal: publish ROS message to /ping 151 | 152 | # ROS 153 | rostopic pub -r 1 /ping/ros std_msgs/String "Hello MQTT" 154 | 155 | # ROS 2 156 | ros2 topic pub /ping/ros std_msgs/msg/String "{data: \"Hello MQTT\"}" 157 | ``` 158 | 159 | ```bash 160 | # 2nd terminal: listen for ROS messages on /pong 161 | 162 | # ROS 163 | rostopic echo /pong/ros 164 | 165 | # ROS 2 166 | ros2 topic echo /pong/ros 167 | ``` 168 | 169 | In order to test the communication between *mqtt_client* and other MQTT clients, publish a primitive ROS message on ROS topic `/ping/primitive`, directly publish a primitive MQTT message on MQTT topic `pingpong/primitive` and wait for responses on ROS topic `/pong/primitive`. Note that you need to restart the ROS 2 *mqtt_client* with a different config file. 170 | 171 | ```bash 172 | # ROS 2 173 | # mqtt_client$ 174 | ros2 launch mqtt_client standalone.launch.ros2.xml params_file:=$(ros2 pkg prefix mqtt_client)/share/mqtt_client/config/params.ros2.primitive.yaml 175 | ``` 176 | 177 | ```bash 178 | # 3rd terminal: publish primitive ROS message to /ping/primitive 179 | 180 | # ROS 181 | rostopic pub -r 1 /ping/primitive std_msgs/Int32 42 182 | 183 | # ROS2 184 | ros2 topic pub /ping/primitive std_msgs/msg/Int32 "{data: 42}" 185 | ``` 186 | 187 | ```bash 188 | # 4th terminal: listen for primitive ROS messages on /pong/primitive 189 | 190 | # ROS 191 | rostopic echo /pong/primitive 192 | 193 | # ROS2 194 | ros2 topic echo /pong/primitive 195 | ``` 196 | 197 | ```bash 198 | # 5th terminal: publish primitive MQTT message to pingpong/primitive directly using mosquitto_pub 199 | docker run --rm --network host eclipse-mosquitto mosquitto_pub -h localhost -t "pingpong/primitive" --repeat 20 --repeat-delay 1 -m 69 200 | ``` 201 | 202 | If everything works as expected, the second terminal should print a message at 1Hz, while the fourth terminal should print two different messages at 1Hz. 203 | 204 | ### Launch 205 | 206 | You can start the *mqtt_client* node with: 207 | 208 | ```bash 209 | # ROS 210 | roslaunch mqtt_client standalone.launch 211 | 212 | # ROS 2 213 | ros2 launch mqtt_client standalone.launch.ros2.xml 214 | ``` 215 | 216 | This will automatically load the provided demo [`params.yaml`](mqtt_client/config/params.yaml) / [`params.ros2.yaml`](mqtt_client/config/params.ros2.yaml). If you wish to load your custom configuration file, simply pass `params_file`. 217 | 218 | ```bash 219 | # ROS 220 | roslaunch mqtt_client standalone.launch params_file:="" 221 | 222 | # ROS 2 223 | ros2 launch mqtt_client standalone.launch.ros2.xml params_file:="" 224 | ``` 225 | 226 | In order to exploit the benefits of *mqtt_client* being a ROS nodelet / ROS 2 component, load the nodelet / component to your own nodelet manager / component container. 227 | 228 | ### Configuration 229 | 230 | All available ROS parameters supported by the *mqtt_client* and their default values (in `[]`) are listed in the following. 231 | 232 | #### Broker Parameters 233 | 234 | ```yaml 235 | broker: 236 | host: # [localhost] IP address or hostname of the machine running the MQTT broker 237 | port: # [1883] port the MQTT broker is listening on 238 | user: # username used for authenticating to the broker (if empty, will try to connect anonymously) 239 | pass: # password used for authenticating to the broker 240 | tls: 241 | enabled: # [false] whether to connect via SSL/TLS 242 | ca_certificate: # [/etc/ssl/certs/ca-certificates.crt] CA certificate file trusted by client (relative to ROS_HOME) 243 | ``` 244 | 245 | #### Client Parameters 246 | 247 | ```yaml 248 | client: 249 | id: # unique ID string used to identify the client (broker may allow empty ID and automatically generate one) 250 | buffer: 251 | size: # [0] maximum number of messages buffered by the bridge when not connected to broker (only available if client ID is not empty) 252 | directory: # [buffer] directory used to buffer messages when not connected to broker (relative to ROS_HOME) 253 | last_will: 254 | topic: # topic used for this client's last-will message (no last will, if not specified) 255 | message: # [offline] last-will message 256 | qos: # [0] QoS value for last-will message 257 | retained: # [false] whether to retain last-will message 258 | clean_session: # [true] whether to use a clean session for this client 259 | keep_alive_interval: # [60.0] keep-alive interval in seconds 260 | max_inflight: # [65535] maximum number of inflight messages 261 | tls: 262 | certificate: # client certificate file (only needed if broker requires client certificates; relative to ROS_HOME) 263 | key: # client private key file (relative to ROS_HOME) 264 | password: # client private key password 265 | version: # TLS version (https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305) 266 | verify: # verify the client should conduct post-connect checks. 267 | alpn_protos: # list of ALPN protocols (https://www.openssl.org/docs/man1.1.1/man3/SSL_CTX_set_alpn_protos.html) 268 | ``` 269 | 270 | #### Bridge Parameters 271 | 272 | ##### ROS 273 | 274 | ```yaml 275 | bridge: 276 | ros2mqtt: # Array specifying which ROS topics to map to which MQTT topics 277 | - ros_topic: # ROS topic whose messages are transformed to MQTT messages 278 | mqtt_topic: # MQTT topic on which the corresponding ROS messages are sent to the broker 279 | primitive: # [false] whether to publish as primitive message 280 | inject_timestamp: # [false] whether to attach a timestamp to a ROS2MQTT payload (for latency computation on receiver side) 281 | advanced: 282 | ros: 283 | queue_size: # [1] ROS subscriber queue size 284 | mqtt: 285 | qos: # [0] MQTT QoS value 286 | retained: # [false] whether to retain MQTT message 287 | mqtt2ros: # Array specifying which MQTT topics to map to which ROS topics 288 | - mqtt_topic: # MQTT topic on which messages are received from the broker 289 | ros_topic: # ROS topic on which corresponding MQTT messages are published 290 | primitive: # [false] whether to publish as primitive message (if coming from non-ROS MQTT client) 291 | advanced: 292 | mqtt: 293 | qos: # [0] MQTT QoS value 294 | ros: 295 | queue_size: # [1] ROS publisher queue size 296 | latched: # [false] whether to latch ROS message 297 | ``` 298 | 299 | ##### ROS 2 300 | 301 | ```yaml 302 | bridge: 303 | ros2mqtt: # Object specifying which ROS topics to map to which MQTT topics 304 | ros_topics: # Array specifying which ROS topics to bridge 305 | - {{ ros_topic_name }} # The ROS topic that should be bridged, corresponds to the sub-object in the YAML 306 | {{ ros_topic_name }}: 307 | mqtt_topic: # MQTT topic on which the corresponding ROS messages are sent to the broker 308 | primitive: # [false] whether to publish as primitive message 309 | ros_type: # [*empty*] If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the publisher 310 | inject_timestamp: # [false] whether to attach a timestamp to a ROS2MQTT payload (for latency computation on receiver side) 311 | advanced: 312 | ros: 313 | queue_size: # [1] ROS subscriber queue size 314 | qos: 315 | reliability: # [auto] One of "auto", "system_default", "reliable", "best_effort". If auto, the QoS is automatically determined via the publisher 316 | durability: # [auto] One of "auto", "system_default", "volatile", "transient_local". If auto, the QoS is automatically determined via the publisher 317 | mqtt: 318 | qos: # [0] MQTT QoS value 319 | retained: # [false] whether to retain MQTT message 320 | mqtt2ros: # Object specifying which MQTT topics to map to which ROS topics 321 | mqtt_topics: # Array specifying which ROS topics to bridge 322 | - {{ mqtt_topic_name }} # The MQTT topic that should be bridged, corresponds to the sub-object in the YAML 323 | {{ mqtt_topic_name }}: 324 | ros_topic: # ROS topic on which corresponding MQTT messages are published 325 | ros_type: # [*empty*] If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the MQTT message 326 | primitive: # [false] whether to publish as primitive message (if coming from non-ROS MQTT client) 327 | advanced: 328 | mqtt: 329 | qos: # [0] MQTT QoS value 330 | ros: 331 | queue_size: # [1] ROS publisher queue size 332 | latched: # [false] whether to latch ROS message 333 | qos: 334 | reliability: # [system_default] One of "system_default", "reliable", "best_effort". 335 | durability: # [system_default] One of "system_default", "volatile", "transient_local". 336 | ``` 337 | 338 | ## Primitive Messages 339 | 340 | As seen in the [Quick Start](#quick-start), the *mqtt_client* can not only exchange arbitrary ROS messages with other *mqtt_clients*, but it can also exchange primitive message data with other non-*mqtt_client* MQTT clients. This allows ROS-based devices to exchange primitive messages with devices not based on ROS. The `primitive` parameter can be set for both ROS-to-MQTT (`bridge/ros2mqtt`) and for MQTT-to-ROS (`bridge/mqtt2ros`) transmissions. 341 | 342 | If a ROS-to-MQTT transmission is configured as `primitive` and the ROS message type is one of the supported primitive ROS message types, the raw data is published as a string. The supported primitive ROS message types are [`std_msgs/String`](http://docs.ros.org/en/api/std_msgs/html/msg/String.html), [`std_msgs/Bool`](http://docs.ros.org/en/api/std_msgs/html/msg/Bool.html), [`std_msgs/Char`](http://docs.ros.org/en/api/std_msgs/html/msg/Char.html), [`std_msgs/UInt8`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt8.html), [`std_msgs/UInt16`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt16.html), [`std_msgs/UInt32`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt32.html), [`std_msgs/UInt64`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt16.html), [`std_msgs/Int8`](http://docs.ros.org/en/api/std_msgs/html/msg/Int8.html), [`std_msgs/Int16`](http://docs.ros.org/en/api/std_msgs/html/msg/Int16.html), [`std_msgs/Int32`](http://docs.ros.org/en/api/std_msgs/html/msg/Int32.html), [`std_msgs/Int64`](http://docs.ros.org/en/api/std_msgs/html/msg/Int64.html), [`std_msgs/Float32`](http://docs.ros.org/en/api/std_msgs/html/msg/Float32.html), [`std_msgs/Float32`](http://docs.ros.org/en/api/std_msgs/html/msg/Float64.html). 343 | 344 | If an MQTT-to-ROS transmission is configured as `primitive`, the MQTT message is interpreted and published as a primitive data type, if possible. The message is probed in the following order: `bool` ([`std_msgs/Bool`](http://docs.ros.org/en/api/std_msgs/html/msg/Bool.html)), `int` ([`std_msgs/Int32`](http://docs.ros.org/en/api/std_msgs/html/msg/Int32.html)), `float` ([`std_msgs/Float32`](http://docs.ros.org/en/api/std_msgs/html/msg/Float32.html)), `string` ([`std_msgs/String`](http://docs.ros.org/en/api/std_msgs/html/msg/String.html)). 345 | 346 | 347 | ## Latency Computation 348 | 349 | The *mqtt_client* provides built-in functionality to measure the latency of transferring a ROS message via an MQTT broker back to ROS. Note that this functionality is only available for non-primitive messages (see [Primitive Messages](#primitive-messages)). To this end, the sending client injects the current timestamp into the MQTT message. The receiving client can then compute the latency between message reception time and the injected timestamp. **Naturally, this is only accurate to the level of synchronization between clocks on sending and receiving machine.** 350 | 351 | In order to inject the current timestamp into outgoing MQTT messages, the parameter `inject_timestamp` has to be set for the corresponding `bridge/ros2mqtt` entry. The receiving *mqtt_client* will then automatically publish the measured latency in seconds as a ROS `std_msgs/Float64` message on topic `//latencies/`. 352 | 353 | These latencies can be printed easily with *rostopic echo* 354 | 355 | ```bash 356 | # ROS 357 | rostopic echo --clear //latencies//data 358 | 359 | # ROS 2 360 | ros2 topic echo //latencies//data 361 | ``` 362 | 363 | or plotted with [rqt_plot](http://wiki.ros.org/rqt_plot): 364 | 365 | ```bash 366 | # ROS 367 | rosrun rqt_plot rqt_plot //latencies//data 368 | 369 | # ROS 2 370 | ros2 run rqt_plot rqt_plot //latencies//data 371 | ``` 372 | 373 | 374 | ## Package Summary 375 | 376 | This short package summary documents the package in line with the [ROS Wiki Style Guide](http://wiki.ros.org/StyleGuide). 377 | 378 | ### ROS 379 | 380 | #### Nodelets 381 | 382 | ##### `mqtt_client/MqttClient` 383 | 384 | Enables connected ROS-based devices or robots to exchange ROS messages via an MQTT broker using the [MQTT](http://mqtt.org) protocol. 385 | 386 | ###### Subscribed Topics 387 | 388 | - `` ([`topic_tools/ShapeShifter`](http://wiki.ros.org/topic_tools)) 389 | ROS topic whose messages are transformed to MQTT messages and sent to the MQTT broker. May have arbitrary ROS message type. 390 | 391 | ###### Published Topics 392 | 393 | - `` ([`topic_tools/ShapeShifter`](http://wiki.ros.org/topic_tools)) 394 | ROS topic on which MQTT messages received from the MQTT broker are published. May have arbitrary ROS message type. 395 | - `~/latencies/` ([`std_msgs/Float64`](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html)) 396 | Latencies measured on the message transfer to `` are published here, if the received messages have a timestamp injected (see [Latency Computation](#latency-computation)). 397 | 398 | ###### Services 399 | 400 | - `~is_connected` ([`mqtt_client/srv/IsConnected`](mqtt_client_interfaces/srv/IsConnected.srv)) 401 | Returns whether the client is connected to the MQTT broker. 402 | 403 | ###### Parameters 404 | 405 | See [Configuration](#configuration). 406 | 407 | ### ROS 2 408 | 409 | #### Components 410 | 411 | ##### `mqtt_client/MqttClient` 412 | 413 | Enables connected ROS-based devices or robots to exchange ROS messages via an MQTT broker using the [MQTT](http://mqtt.org) protocol. 414 | 415 | ###### Subscribed Topics 416 | 417 | - `` ([`rclcpp::SerializedMessage`](https://docs.ros.org/en/ros2_packages/rolling/api/rclcpp/generated/classrclcpp_1_1GenericSubscription.html)) 418 | ROS topic whose messages are transformed to MQTT messages and sent to the MQTT broker. May have arbitrary ROS message type. 419 | 420 | ###### Published Topics 421 | 422 | - `` ([`rclcpp::SerializedMessage`](https://docs.ros.org/en/ros2_packages/rolling/api/rclcpp/generated/classrclcpp_1_1GenericPublisher.html)) 423 | ROS topic on which MQTT messages received from the MQTT broker are published. May have arbitrary ROS message type. 424 | - `~/latencies/` ([`std_msgs/Float64`](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html)) 425 | Latencies measured on the message transfer to `` are published here, if the received messages have a timestamp injected (see [Latency Computation](#latency-computation)). 426 | 427 | ###### Services 428 | 429 | - `~/is_connected` ([`mqtt_client/srv/IsConnected`](mqtt_client_interfaces/srv/IsConnected.srv)) 430 | Returns whether the client is connected to the MQTT broker. 431 | 432 | - `~/new_ros2mqtt_bridge` ([`mqtt_client/srv/NewRos2MqttBridge`](mqtt_client_interfaces/srv/NewRos2MqttBridge.srv)) 433 | Returns whether a new ROS -> MQTT bridge was created. 434 | 435 | - `~/new_mqtt2ros_bridge` ([`mqtt_client/srv/NewMqtt2RosBridge`](mqtt_client_interfaces/srv/NewMqtt2RosBridge.srv)) 436 | Returns whether a new MQTT -> ROS bridge was created. 437 | 438 | 439 | 440 | ###### Parameters 441 | 442 | See [Configuration](#configuration). 443 | 444 | 445 | ## How It Works 446 | 447 | ### ROS 448 | 449 | The *mqtt_client* is able to bridge ROS messages of arbitrary message type to an MQTT broker. To this end, it needs to employ generic ROS subscribers and publishers, which only take shape at runtime. 450 | 451 | These generic ROS subscribers and publishers are realized through [topic_tools::ShapeShifter](http://docs.ros.org/diamondback/api/topic_tools/html/classtopic__tools_1_1ShapeShifter.html). For each pair of `ros_topic` and `mqtt_topic` specified under `bridge/ros2mqtt/`, a ROS subscriber is setup with the following callback signature: 452 | 453 | ```cpp 454 | void ros2mqtt(topic_tools::ShapeShifter::ConstPtr&, std::string&) 455 | ``` 456 | 457 | Inside the callback, the generic messages received on the `ros_topic` are serialized using [ros::serialization](http://wiki.ros.org/roscpp/Overview/MessagesSerializationAndAdaptingTypes). The serialized form is then ready to be sent to the MQTT broker on the specified `mqtt_topic`. 458 | 459 | Upon retrieval of an MQTT message, it is republished as a ROS message on the ROS network. To this end, [topic_tools::ShapeShifter::morph](http://docs.ros.org/indigo/api/topic_tools/html/classtopic__tools_1_1ShapeShifter.html#a2b74b522fb49dac05d48f466b6b87d2d) is used to have the ShapeShifter publisher take the shape of the specific ROS message type. 460 | 461 | The required metainformation on the ROS message type can however only be extracted in the ROS subscriber callback of the publishing *mqtt_client* with calls to [topic_tools::ShapeShifter::getMD5Sum](http://docs.ros.org/indigo/api/topic_tools/html/classtopic__tools_1_1ShapeShifter.html#af05fbf42757658e4c6a0f99ff72f7daa), [topic_tools::ShapeShifter::getDataType](http://docs.ros.org/indigo/api/topic_tools/html/classtopic__tools_1_1ShapeShifter.html#a9d57b2285213fda5e878ce7ebc42f0fb), and [topic_tools::ShapeShifter::getMessageDefinition](http://docs.ros.org/indigo/api/topic_tools/html/classtopic__tools_1_1ShapeShifter.html#a503af7234eeba0ccefca64c4d0cc1df0). These attributes are wrapped in a ROS message of custom type [mqtt_client::RosMsgType](mqtt_client_interfaces/msg/RosMsgType.msg), serialized using [ros::serialization](http://wiki.ros.org/roscpp/Overview/MessagesSerializationAndAdaptingTypes) and also shared via the MQTT broker on a special topic. 462 | 463 | When an *mqtt_client* receives such ROS message type metainformation, it configures the corresponding ROS ShapeShifter publisher using [topic_tools::ShapeShifter::morph](http://docs.ros.org/indigo/api/topic_tools/html/classtopic__tools_1_1ShapeShifter.html#a2b74b522fb49dac05d48f466b6b87d2d). 464 | 465 | The *mqtt_client* also provides functionality to measure the latency of transferring a ROS message via an MQTT broker back to ROS. To this end, the sending client injects the current timestamp into the MQTT message. The receiving client can then compute the latency between message reception time and the injected timestamp. The information about whether a timestamp is injected is also included in the custom [mqtt_client::RosMsgType](mqtt_client_interfaces/msg/RosMsgType.msg) message that is sent before. The actual `std::vector` message payload takes on one of the following forms: 466 | 467 | ```txt 468 | [... serialized timestamp ... | ... serialized ROS messsage ...] 469 | [... serialized ROS messsage ...] 470 | ``` 471 | 472 | To summarize, the dataflow is as follows: 473 | 474 | - a ROS message of arbitrary type is received on ROS topic `` and passed to the generic callback 475 | - ROS message type information is extracted and wrapped as a `RosMsgType` 476 | - ROS message type information is serialized and sent via the MQTT broker on MQTT topic `mqtt_client/ros_msg_type/` 477 | - the actual ROS message is serialized 478 | - if `inject_timestamp`, the current timestamp is serialized and concatenated with the message 479 | - the actual MQTT message is sent via the MQTT broker on MQTT topic `` 480 | - an MQTT message containing the ROS message type information is received on MQTT topic `mqtt_client/ros_msg_type/` 481 | - message type information is extracted and the ShapeShifter ROS publisher is configured 482 | - information about whether a timestamp is injected is stored for the specific topic 483 | - an MQTT message containing the actual ROS message is received 484 | - depending on whether a timestamp is injected, it is decoded into the serialized ROS message and the serialized timestamp 485 | - if the message contained a timestamp, the latency is computed and published on ROS topic `~/latencies/` 486 | - the serialized ROS message is published using the *ShapeShifter* on ROS topic `` 487 | 488 | 489 | ## Acknowledgements 490 | 491 | This research is accomplished within the projects [6GEM](https://6gem.de/) (FKZ 16KISK036K) and [UNICAR*agil*](https://www.unicaragil.de/) (FKZ 16EMO0284K). We acknowledge the financial support for the projects by the Federal Ministry of Education and Research of Germany (BMBF). 492 | -------------------------------------------------------------------------------- /mqtt_client/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mqtt_client 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.3.0 (2024-05-30) 6 | ------------------ 7 | * Merge pull request `#61 `_ from Chance-Maritime-Technologies/dev-explicitTypes 8 | Added the ability to explicitly set type names and some QoS settings 9 | * Merge remote-tracking branch 'upstream/main' into dev-explicitTypes 10 | * Merge pull request `#63 `_ from tecnalia-medical-robotics/system-fmt 11 | Use system version of libfmt instead of rosfmt vendored one on ROS 1 12 | * Merge pull request `#60 `_ from ika-rwth-aachen/feature/nodename_in_params_file 13 | Modify ROS2 node name in params files 14 | * Merge pull request `#58 `_ from ika-rwth-aachen/feature/configure_node_name 15 | Make ROS/ROS2 node name configurable via launch file 16 | * Contributors: JayHerpin, Lennart Reiher 17 | 18 | 2.2.1 (2024-03-19) 19 | ------------------ 20 | * Merge pull request #50 from babakc/main 21 | Amend AWS IoT CLI command in collect the correct endpoint 22 | * Contributors: Lennart Reiher 23 | 24 | 2.2.0 (2023-11-29) 25 | ------------------ 26 | * Merge pull request `#35 `_ from mvccogo/main 27 | Dynamic registration of topics 28 | * Merge pull request `#36 `_ from ika-rwth-aachen/fix/ros1-latencies 29 | Fix bug in ros1 latency deserialization 30 | * Contributors: Lennart Reiher, Matheus V. C. Cogo, mvccogo 31 | 32 | 2.1.0 (2023-09-18) 33 | ------------------ 34 | * Merge pull request #31 from ika-rwth-aachen/features/ros2-component 35 | ROS2 Component 36 | * Merge pull request #30 from oxin-ros/ros2-add-multiple-topics 37 | ROS 2: add multiple topics 38 | * Merge pull request #28 from oxin-ros/add-ALPN-protocol-support-for-aws 39 | Add ALPN protocol support for AWS 40 | * Contributors: David B, David Buckman, Lennart Reiher 41 | 42 | 2.0.1 (2023-06-10) 43 | ------------------ 44 | * fix unrecognized build type with catkin_make_isolated 45 | order of statements is somehow revelant; catkin_make_isolated would not detect the build type; build farm jobs were failing; https://build.ros.org/job/Ndev__mqtt_client__ubuntu_focal_amd64/10/console 46 | * Contributors: Lennart Reiher 47 | 48 | 2.0.0 (2023-06-10) 49 | ------------------ 50 | * Merge pull request #23 from ika-rwth-aachen/docker-ros 51 | Integrate docker-ros 52 | * Merge pull request #16 from ika-rwth-aachen/dev/ros2 53 | Add support for ROS2 54 | * Contributors: Lennart Reiher 55 | 56 | 1.1.0 (2022-10-13) 57 | ------------------ 58 | * Merge pull request #6 from ika-rwth-aachen/feature/primitive-msgs 59 | Support exchange of primitive messages with other MQTT clients 60 | * Contributors: Lennart Reiher 61 | 62 | 1.0.2 (2022-10-07) 63 | ------------------ 64 | * Merge pull request #4 from ika-rwth-aachen/improvement/runtime-optimization 65 | Optimize runtime 66 | * Merge pull request #5 from ika-rwth-aachen/ci 67 | Set up CI 68 | * Contributors: Lennart Reiher 69 | 70 | 1.0.1 (2022-08-11) 71 | ------------------ 72 | * Merge pull request #3 from ika-rwth-aachen/doc/code-api 73 | Improve Code API Documentation 74 | * Merge pull request #1 from ika-rwth-aachen/improvement/documentation 75 | Improve documentation 76 | * Contributors: Lennart Reiher 77 | -------------------------------------------------------------------------------- /mqtt_client/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.0 FATAL_ERROR) 2 | project(mqtt_client) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | 7 | ## Compile as C++17 8 | add_compile_options(-std=c++17) 9 | link_libraries("$<$,$,9.0>>:-lstdc++fs>") 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # === ROS2 (AMENT) ============================================================= 15 | if(${ROS_VERSION} EQUAL 2) 16 | 17 | find_package(ament_cmake REQUIRED) 18 | 19 | find_package(fmt REQUIRED) 20 | find_package(mqtt_client_interfaces REQUIRED) 21 | find_package(rclcpp REQUIRED) 22 | find_package(rclcpp_components REQUIRED) 23 | find_package(std_msgs REQUIRED) 24 | 25 | # Paho MQTT C++ apt package doesn't include CMake config 26 | # find_package(PahoMqttCpp REQUIRED) 27 | find_library(PahoMqttC_LIBRARY libpaho-mqtt3as.so.1 REQUIRED) 28 | find_library(PahoMqttCpp_LIBRARY libpaho-mqttpp3.so.1 REQUIRED) 29 | 30 | add_library(${PROJECT_NAME}_lib SHARED src/MqttClient.ros2.cpp) 31 | 32 | rclcpp_components_register_node(${PROJECT_NAME}_lib 33 | PLUGIN "mqtt_client::MqttClient" 34 | EXECUTABLE ${PROJECT_NAME} 35 | ) 36 | 37 | target_include_directories(${PROJECT_NAME}_lib PUBLIC 38 | $ 39 | $) 40 | 41 | target_link_libraries(${PROJECT_NAME}_lib 42 | ${PahoMqttC_LIBRARY} 43 | ${PahoMqttCpp_LIBRARY} 44 | ) 45 | 46 | ament_target_dependencies(${PROJECT_NAME}_lib 47 | fmt 48 | mqtt_client_interfaces 49 | rclcpp 50 | rclcpp_components 51 | std_msgs 52 | ) 53 | 54 | install(TARGETS ${PROJECT_NAME}_lib 55 | ARCHIVE DESTINATION lib 56 | LIBRARY DESTINATION lib 57 | RUNTIME DESTINATION bin 58 | ) 59 | 60 | install( 61 | DIRECTORY launch 62 | DESTINATION share/${PROJECT_NAME} 63 | FILES_MATCHING PATTERN "*ros2*" 64 | ) 65 | 66 | install( 67 | DIRECTORY config 68 | DESTINATION share/${PROJECT_NAME} 69 | FILES_MATCHING PATTERN "*ros2*" 70 | ) 71 | 72 | # if(BUILD_TESTING) 73 | # find_package(ament_lint_auto REQUIRED) 74 | # # the following line skips the linter which checks for copyrights 75 | # # comment the line when a copyright and license is added to all source files 76 | # set(ament_cmake_copyright_FOUND TRUE) 77 | # # the following line skips cpplint (only works in a git repo) 78 | # # comment the line when this package is in a git repo and when 79 | # # a copyright and license is added to all source files 80 | # set(ament_cmake_cpplint_FOUND TRUE) 81 | # ament_lint_auto_find_test_dependencies() 82 | # endif() 83 | 84 | ament_package() 85 | 86 | # === ROS1 (CATKIN) ============================================================ 87 | elseif(${ROS_VERSION} EQUAL 1) 88 | 89 | ## Find catkin macros and libraries 90 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 91 | ## is used, also find other catkin packages 92 | find_package(catkin REQUIRED COMPONENTS 93 | mqtt_client_interfaces 94 | nodelet 95 | roscpp 96 | std_msgs 97 | topic_tools 98 | ) 99 | 100 | ## System dependencies are found with CMake's conventions 101 | find_package(PahoMqttCpp REQUIRED) 102 | set(PahoMqttCpp_LIBRARIES PahoMqttCpp::paho-mqttpp3) 103 | 104 | find_package(fmt REQUIRED) 105 | set(fmt_LIBRARIES fmt::fmt) 106 | 107 | 108 | ## Uncomment this if the package has a setup.py. This macro ensures 109 | ## modules and global scripts declared therein get installed 110 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 111 | # catkin_python_setup() 112 | 113 | ################################################ 114 | ## Declare ROS messages, services and actions ## 115 | ################################################ 116 | 117 | ## To declare and build messages, services or actions from within this 118 | ## package, follow these steps: 119 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 120 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 121 | ## * In the file package.xml: 122 | ## * add a build_depend tag for "message_generation" 123 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 124 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 125 | ## but can be declared for certainty nonetheless: 126 | ## * add a exec_depend tag for "message_runtime" 127 | ## * In this file (CMakeLists.txt): 128 | ## * add "message_generation" and every package in MSG_DEP_SET to 129 | ## find_package(catkin REQUIRED COMPONENTS ...) 130 | ## * add "message_runtime" and every package in MSG_DEP_SET to 131 | ## catkin_package(CATKIN_DEPENDS ...) 132 | ## * uncomment the add_*_files sections below as needed 133 | ## and list every .msg/.srv/.action file to be processed 134 | ## * uncomment the generate_messages entry below 135 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 136 | 137 | ## Generate messages in the 'msg' folder 138 | # add_message_files( 139 | # FILES 140 | # Message1.msg 141 | # Message2.msg 142 | # ) 143 | 144 | ## Generate services in the 'srv' folder 145 | # add_service_files( 146 | # FILES 147 | # Service1.srv 148 | # Service2.srv 149 | # ) 150 | 151 | ## Generate actions in the 'action' folder 152 | # add_action_files( 153 | # FILES 154 | # Action1.action 155 | # Action2.action 156 | # ) 157 | 158 | ## Generate added messages and services with any dependencies listed here 159 | # generate_messages( 160 | # DEPENDENCIES 161 | # std_msgs 162 | # ) 163 | 164 | ################################################ 165 | ## Declare ROS dynamic reconfigure parameters ## 166 | ################################################ 167 | 168 | ## To declare and build dynamic reconfigure parameters within this 169 | ## package, follow these steps: 170 | ## * In the file package.xml: 171 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 172 | ## * In this file (CMakeLists.txt): 173 | ## * add "dynamic_reconfigure" to 174 | ## find_package(catkin REQUIRED COMPONENTS ...) 175 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 176 | ## and list every .cfg file to be processed 177 | 178 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 179 | # generate_dynamic_reconfigure_options( 180 | # cfg/params.cfg 181 | # ) 182 | 183 | ################################### 184 | ## catkin specific configuration ## 185 | ################################### 186 | ## The catkin_package macro generates cmake config files for your package 187 | ## Declare things to be passed to dependent projects 188 | ## INCLUDE_DIRS: uncomment this if your package contains header files 189 | ## LIBRARIES: libraries you create in this project that dependent projects also need 190 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 191 | ## DEPENDS: system dependencies of this project that dependent projects also need 192 | catkin_package( 193 | INCLUDE_DIRS include 194 | LIBRARIES ${PROJECT_NAME} 195 | CATKIN_DEPENDS 196 | nodelet 197 | roscpp 198 | std_msgs 199 | topic_tools 200 | DEPENDS 201 | fmt 202 | PahoMqttCpp 203 | ) 204 | 205 | ########### 206 | ## Build ## 207 | ########### 208 | 209 | ## Specify additional locations of header files 210 | ## Your package locations should be listed before other locations 211 | include_directories( 212 | include 213 | ${catkin_INCLUDE_DIRS} 214 | ) 215 | 216 | ## Declare a C++ library 217 | add_library(${PROJECT_NAME} 218 | src/MqttClient.cpp 219 | ) 220 | 221 | ## Add cmake target dependencies of the library 222 | ## as an example, code may need to be generated before libraries 223 | ## either from message generation or dynamic reconfigure 224 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 225 | 226 | ## Declare a C++ executable 227 | ## With catkin_make all packages are built within a single CMake context 228 | ## The recommended prefix ensures that target names across packages don't collide 229 | # add_executable(${PROJECT_NAME}_node src/hx_testmanager_node.cpp) 230 | 231 | ## Rename C++ executable without prefix 232 | ## The above recommended prefix causes long target names, the following renames the 233 | ## target back to the shorter version for ease of user use 234 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 235 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 236 | 237 | ## Add cmake target dependencies of the executable 238 | ## same as for the library above 239 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 240 | 241 | ## Specify libraries to link a library or executable target against 242 | target_link_libraries(${PROJECT_NAME} 243 | ${catkin_LIBRARIES} 244 | ${fmt_LIBRARIES} 245 | ${PahoMqttCpp_LIBRARIES} 246 | ) 247 | 248 | ############# 249 | ## Install ## 250 | ############# 251 | 252 | # all install targets should use catkin DESTINATION variables 253 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 254 | 255 | ## Mark executable scripts (Python etc.) for installation 256 | ## in contrast to setup.py, you can choose the destination 257 | # install(PROGRAMS 258 | # scripts/my_python_script 259 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 260 | # ) 261 | 262 | ## Mark executables and/or libraries for installation 263 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME} 264 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 265 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 266 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 267 | ) 268 | 269 | ## Mark cpp header files for installation 270 | install(DIRECTORY include/${PROJECT_NAME}/ 271 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 272 | # FILES_MATCHING PATTERN "*.h" 273 | # PATTERN ".svn" EXCLUDE 274 | ) 275 | 276 | ## Mark other files for installation (e.g. launch and bag files, etc.) 277 | install(FILES 278 | nodelet_plugins.xml 279 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 280 | ) 281 | install(DIRECTORY 282 | launch 283 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 284 | PATTERN "*ros2*" EXCLUDE 285 | ) 286 | install(DIRECTORY 287 | config 288 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 289 | PATTERN "*ros2*" EXCLUDE 290 | ) 291 | 292 | ############# 293 | ## Testing ## 294 | ############# 295 | 296 | ## Add gtest based cpp test target and link libraries 297 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_hx_testmanager.cpp) 298 | #if(CATKIN_ENABLE_TESTING) 299 | # find_package(rostest REQUIRED) 300 | # add_rostest_gtest(test_ika_dogm test/ika_dogm.test test/UnitTest.cpp) 301 | # catkin_add_gtest(test_ika_dogm test/UnitTest.cpp) 302 | # target_link_libraries(test_ika_dogm ${catkin_LIBRARIES} ${PROJECT_NAME}_dogm_creation) 303 | #endif() 304 | # if(TARGET ${PROJECT_NAME}-test) 305 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 306 | # endif() 307 | 308 | ## Add folders to be run by python nosetests 309 | # catkin_add_nosetests(test) 310 | 311 | endif() 312 | -------------------------------------------------------------------------------- /mqtt_client/config/params.aws.yaml: -------------------------------------------------------------------------------- 1 | # YAML alias for MQTT SSL version. 2 | # The supported values are defined at: 3 | # https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305 4 | tls_1_2: &tls_1_2 3 5 | 6 | broker: 7 | # YOU MUST CHANGE THIS ENDPOINT 8 | # Can be found by executing: aws iot describe-endpoint --endpoint-type iot:Data-ATS 9 | host: not-a-real-endpoint-please-use-your-own.us-west-2.amazonaws.com 10 | port: 8883 11 | tls: 12 | enabled: true 13 | # Available at https://www.amazontrust.com/repository/AmazonRootCA1.pem 14 | ca_certificate: path/to/AmazonRootCA1.pem 15 | client: 16 | id: user 17 | # Whether or not to start a clean session with each reconnect. 18 | # If True, the server will forget all subscriptions with each reconnect. 19 | # Set False to request that the server resume an existing session or start 20 | # a new session that may be resumed after a connection loss. 21 | clean_session: false 22 | # The keep alive value, in seconds, to send in CONNECT packet. 23 | keep_alive_interval: 6.0 24 | tls: 25 | # The certificate generated by the AWS IoT Core service 26 | certificate: path/to/a-certificate.pem 27 | # The private key generated by the AWS IoT Core service 28 | key: path/to/a-private-key.pem 29 | # AWS uses TLS v1.2 to encrypt all communication. 30 | version: *tls_1_2 31 | verify: true 32 | # https://www.openssl.org/docs/man1.1.1/man3/SSL_CTX_set_alpn_protos.html 33 | alpn_protos: 34 | # MQTT over AWS IOT requires an ALPN protocol negotiation. 35 | # https://docs.aws.amazon.com/iot/latest/developerguide/protocols.html 36 | - x-amzn-mqtt-ca 37 | buffer: 38 | enabled: true 39 | bridge: 40 | # NOTE: It seems that AWS IOT only supports primitive topics. Using non-primitive 41 | # types results in the error message `Connection to broker lost, will try to reconnect...` 42 | ros2mqtt: 43 | - ros_topic: /ping/primitive 44 | mqtt_topic: pingpong/primitive 45 | primitive: true 46 | mqtt2ros: 47 | - mqtt_topic: pingpong/primitive 48 | ros_topic: /pong/primitive 49 | primitive: true 50 | -------------------------------------------------------------------------------- /mqtt_client/config/params.ros2.aws.yaml: -------------------------------------------------------------------------------- 1 | /**/*: 2 | ros__parameters: 3 | broker: 4 | # YOU MUST CHANGE THIS ENDPOINT 5 | # Can be found by executing: aws iot describe-endpoint --endpoint-type iot:Data-ATS 6 | host: not-a-real-endpoint-please-use-your-own.us-west-2.amazonaws.com 7 | port: 8883 8 | tls: 9 | enabled: true 10 | # Available at https://www.amazontrust.com/repository/AmazonRootCA1.pem 11 | ca_certificate: path/to/AmazonRootCA1.pem 12 | client: 13 | id: user 14 | # Whether or not to start a clean session with each reconnect. 15 | # If True, the server will forget all subscriptions with each reconnect. 16 | # Set False to request that the server resume an existing session or start 17 | # a new session that may be resumed after a connection loss. 18 | clean_session: false 19 | # The keep alive value, in seconds, to send in CONNECT packet. 20 | keep_alive_interval: 6.0 21 | tls: 22 | # The certificate generated by the AWS IoT Core service 23 | certificate: path/to/a-certificate.pem 24 | # The private key generated by the AWS IoT Core service 25 | key: path/to/a-private-key.pem 26 | # AWS uses TLS v1.2 to encrypt all communication. 27 | # The supported values are defined at: 28 | # https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305 29 | version: 3 # TLS v1.2 30 | verify: true 31 | # https://www.openssl.org/docs/man1.1.1/man3/SSL_CTX_set_alpn_protos.html 32 | alpn_protos: 33 | # MQTT over AWS IOT requires an ALPN protocol negotiation. 34 | # https://docs.aws.amazon.com/iot/latest/developerguide/protocols.html 35 | - x-amzn-mqtt-ca 36 | buffer: 37 | enabled: true 38 | bridge: 39 | # NOTE: It seems that AWS IOT only supports primitive topics. Using non-primitive 40 | # types results in the error message `Connection to broker lost, will try to reconnect...` 41 | ros2mqtt: 42 | ros_topics: 43 | - /ping/primitive 44 | /ping/primitive: 45 | mqtt_topic: pingpong/primitive 46 | primitive: true 47 | mqtt2ros: 48 | mqtt_topics: 49 | - pingpong/primitive 50 | pingpong/primitive: 51 | ros_topic: /pong/primitive 52 | primitive: true 53 | -------------------------------------------------------------------------------- /mqtt_client/config/params.ros2.fixed-type-and-qos.yaml: -------------------------------------------------------------------------------- 1 | /**/*: 2 | ros__parameters: 3 | broker: 4 | host: localhost 5 | port: 1883 6 | bridge: 7 | ros2mqtt: 8 | ros_topics: 9 | - /ping/primitive 10 | /ping/primitive: 11 | mqtt_topic: pingpong/primitive 12 | primitive: false 13 | ros_type: std_msgs/msg/Bool 14 | advanced: 15 | ros: 16 | queue_size: 10 17 | qos: 18 | durability: auto 19 | reliability: auto 20 | mqtt2ros: 21 | mqtt_topics: 22 | - pingpong/primitive 23 | pingpong/primitive: 24 | ros_topic: /pong/primitive 25 | primitive: false 26 | ros_type: std_msgs/msg/Bool 27 | advanced: 28 | ros: 29 | queue_size: 10 30 | qos: 31 | durability: transient_local 32 | reliability: reliable 33 | -------------------------------------------------------------------------------- /mqtt_client/config/params.ros2.primitive-fixed-type-and-qos.yaml: -------------------------------------------------------------------------------- 1 | /**/*: 2 | ros__parameters: 3 | broker: 4 | host: localhost 5 | port: 1883 6 | bridge: 7 | ros2mqtt: 8 | ros_topics: 9 | - /ping/primitive 10 | /ping/primitive: 11 | mqtt_topic: pingpong/primitive 12 | primitive: true 13 | ros_type: std_msgs/msg/Bool 14 | advanced: 15 | ros: 16 | queue_size: 10 17 | qos: 18 | durability: auto 19 | reliability: auto 20 | mqtt2ros: 21 | mqtt_topics: 22 | - pingpong/primitive 23 | pingpong/primitive: 24 | ros_topic: /pong/primitive 25 | primitive: true 26 | ros_type: std_msgs/msg/Bool 27 | advanced: 28 | ros: 29 | queue_size: 10 30 | qos: 31 | durability: transient_local 32 | reliability: reliable 33 | -------------------------------------------------------------------------------- /mqtt_client/config/params.ros2.primitive.yaml: -------------------------------------------------------------------------------- 1 | /**/*: 2 | ros__parameters: 3 | broker: 4 | host: localhost 5 | port: 1883 6 | bridge: 7 | ros2mqtt: 8 | ros_topics: 9 | - /ping/primitive 10 | /ping/primitive: 11 | mqtt_topic: pingpong/primitive 12 | primitive: true 13 | mqtt2ros: 14 | mqtt_topics: 15 | - pingpong/primitive 16 | pingpong/primitive: 17 | ros_topic: /pong/primitive 18 | primitive: true 19 | -------------------------------------------------------------------------------- /mqtt_client/config/params.ros2.yaml: -------------------------------------------------------------------------------- 1 | /**/*: 2 | ros__parameters: 3 | broker: 4 | host: localhost 5 | port: 1883 6 | bridge: 7 | ros2mqtt: 8 | ros_topics: 9 | - /ping/ros 10 | /ping/ros: 11 | mqtt_topic: pingpong/ros 12 | mqtt2ros: 13 | mqtt_topics: 14 | - pingpong/ros 15 | pingpong/ros: 16 | ros_topic: /pong/ros 17 | -------------------------------------------------------------------------------- /mqtt_client/config/params.yaml: -------------------------------------------------------------------------------- 1 | broker: 2 | host: localhost 3 | port: 1883 4 | bridge: 5 | ros2mqtt: 6 | - ros_topic: /ping/ros 7 | mqtt_topic: pingpong/ros 8 | - ros_topic: /ping/primitive 9 | mqtt_topic: pingpong/primitive 10 | primitive: true 11 | mqtt2ros: 12 | - mqtt_topic: pingpong/ros 13 | ros_topic: /pong/ros 14 | - mqtt_topic: pingpong/primitive 15 | ros_topic: /pong/primitive 16 | primitive: true -------------------------------------------------------------------------------- /mqtt_client/doc/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \mainpage 4 | 5 | \htmlinclude manifest.html 6 | 7 | \b mqtt_client provides a ROS nodelet that enables connected ROS-based devices or robots to exchange ROS messages via an MQTT broker using the MQTT protocol. This works generically for arbitrary ROS message types. 8 | 9 | Please note that this is the Code API Documentation. Check out the GitHub repository for more information on how to use the package, including a Quick Start guide. 10 | 11 | 16 | 17 | */ -------------------------------------------------------------------------------- /mqtt_client/include/mqtt_client/MqttClient.h: -------------------------------------------------------------------------------- 1 | /* 2 | ============================================================================== 3 | MIT License 4 | 5 | Copyright 2022 Institute for Automotive Engineering of RWTH Aachen University. 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | ============================================================================== 25 | */ 26 | 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | 43 | /** 44 | * @brief Namespace for the mqtt_client package 45 | */ 46 | namespace mqtt_client { 47 | 48 | 49 | /** 50 | * @brief ROS Nodelet for sending and receiving ROS messages via MQTT 51 | * 52 | * The MqttClient enables connected ROS-based devices or robots to 53 | * exchange ROS messages via an MQTT broker using the MQTT protocol. 54 | * This works generically for any ROS message, i.e. there is no need 55 | * to specify the ROS message type for ROS messages you wish to 56 | * exchange via the MQTT broker. 57 | */ 58 | class MqttClient : public nodelet::Nodelet, 59 | public virtual mqtt::callback, 60 | public virtual mqtt::iaction_listener { 61 | 62 | protected: 63 | /** 64 | * @brief Initializes nodelet when nodelet is loaded. 65 | * 66 | * Overrides nodelet::Nodelet::onInit(). 67 | */ 68 | virtual void onInit() override; 69 | 70 | /** 71 | * @brief Loads ROS parameters from parameter server. 72 | */ 73 | void loadParameters(); 74 | 75 | /** 76 | * @brief Loads requested ROS parameter from parameter server. 77 | * 78 | * @param[in] key parameter name 79 | * @param[out] value variable where to store the retrieved parameter 80 | * 81 | * @return true if parameter was successfully retrieved 82 | * @return false if parameter was not found 83 | */ 84 | bool loadParameter(const std::string& key, std::string& value); 85 | 86 | /** 87 | * @brief Loads requested ROS parameter from parameter server, allows default 88 | * value. 89 | * 90 | * @param[in] key parameter name 91 | * @param[out] value variable where to store the retrieved parameter 92 | * @param[in] default_value default value 93 | * 94 | * @return true if parameter was successfully retrieved 95 | * @return false if parameter was not found or default was used 96 | */ 97 | bool loadParameter(const std::string& key, std::string& value, 98 | const std::string& default_value); 99 | 100 | /** 101 | * @brief Loads requested ROS parameter from parameter server. 102 | * 103 | * @tparam T type (one of int, double, bool) 104 | * 105 | * @param[in] key parameter name 106 | * @param[out] value variable where to store the retrieved parameter 107 | * 108 | * @return true if parameter was successfully retrieved 109 | * @return false if parameter was not found 110 | */ 111 | template 112 | bool loadParameter(const std::string& key, T& value); 113 | 114 | /** 115 | * @brief Loads requested ROS parameter from parameter server, allows default 116 | * value. 117 | * 118 | * @tparam T type (one of int, double, bool) 119 | * 120 | * @param[in] key parameter name 121 | * @param[out] value variable where to store the retrieved parameter 122 | * @param[in] default_value default value 123 | * 124 | * @return true if parameter was successfully retrieved 125 | * @return false if parameter was not found or default was used 126 | */ 127 | template 128 | bool loadParameter(const std::string& key, T& value, const T& default_value); 129 | 130 | /** 131 | * @brief Loads requested ROS parameter from parameter server. 132 | * 133 | * @tparam T type (one of int, double, bool) 134 | * 135 | * @param[in] key parameter name 136 | * @param[out] value variable where to store the retrieved parameter 137 | * 138 | * @return true if parameter was successfully retrieved 139 | * @return false if parameter was not found 140 | */ 141 | template 142 | bool loadParameter(const std::string& key, std::vector& value); 143 | 144 | /** 145 | * @brief Loads requested ROS parameter from parameter server, allows default 146 | * value. 147 | * 148 | * @tparam T type (one of int, double, bool) 149 | * 150 | * @param[in] key parameter name 151 | * @param[out] value variable where to store the retrieved parameter 152 | * @param[in] default_value default value 153 | * 154 | * @return true if parameter was successfully retrieved 155 | * @return false if parameter was not found or default was used 156 | */ 157 | template 158 | bool loadParameter(const std::string& key, std::vector& value, const std::vector& default_value); 159 | 160 | /** 161 | * @brief Converts a string to a path object resolving paths relative to 162 | * ROS_HOME. 163 | * 164 | * Resolves relative to CWD, if ROS_HOME is not set. 165 | * Returns empty path, if argument is empty. 166 | * 167 | * @param path_string (relative) path as string 168 | * 169 | * @return std::filesystem::path path variable 170 | */ 171 | std::filesystem::path resolvePath(const std::string& path_string); 172 | 173 | /** 174 | * @brief Initializes broker connection and subscriptions. 175 | */ 176 | void setup(); 177 | 178 | /** 179 | * @brief Sets up the client connection options and initializes the client 180 | * object. 181 | */ 182 | void setupClient(); 183 | 184 | /** 185 | * @brief Connects to the broker using the member client and options. 186 | */ 187 | void connect(); 188 | 189 | /** 190 | * @brief Serializes and publishes a generic ROS message to the MQTT broker. 191 | * 192 | * Before serializing the ROS message and publishing it to the MQTT broker, 193 | * metadata on the ROS message type is extracted. This type information is 194 | * also sent to the MQTT broker on a separate topic. 195 | * 196 | * The MQTT payload for the actual ROS message carries the following: 197 | * - 0 or 1 (indicating if timestamp is injected (=1)) 198 | * - serialized timestamp (optional) 199 | * - serialized ROS message 200 | * 201 | * @param ros_msg generic ROS message 202 | * @param ros_topic ROS topic where the message was published 203 | */ 204 | void ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg, 205 | const std::string& ros_topic); 206 | 207 | /** 208 | * @brief Publishes a ROS message received via MQTT to ROS. 209 | * 210 | * This utilizes the ShapeShifter stored for the MQTT topic on which the 211 | * message was received. The ShapeShifter has to be configured to the ROS 212 | * message type of the message. If the message carries an injected timestamp, 213 | * the latency is computed and published. 214 | * 215 | * The MQTT payload is expected to carry the following: 216 | * - 0 or 1 (indicating if timestamp is injected (=1)) 217 | * - serialized timestamp (optional) 218 | * - serialized ROS message 219 | * 220 | * @param mqtt_msg MQTT message 221 | * @param arrival_stamp arrival timestamp used for latency computation 222 | */ 223 | void mqtt2ros(mqtt::const_message_ptr mqtt_msg, 224 | const ros::WallTime& arrival_stamp = ros::WallTime::now()); 225 | 226 | /** 227 | * @brief Publishes a primitive message received via MQTT to ROS. 228 | * 229 | * This tries to interpret the raw MQTT message as a bool, int, or float value 230 | * in the given order before falling back to string. The message is then 231 | * published as a corresponding primitive ROS message. This utilizes the 232 | * ShapeShifter stored for the MQTT topic on which the message was received. 233 | * The ShapeShifter is dynamically configured to the appropriate ROS message 234 | * type. 235 | * 236 | * The following mappings from primitive type to ROS message type hold: 237 | * bool: std_msgs/Bool 238 | * int: std_msgs/Int32 239 | * float: std_msgs/Float32 240 | * string: std_msgs/String 241 | * 242 | * @param mqtt_msg MQTT message 243 | */ 244 | void mqtt2primitive(mqtt::const_message_ptr mqtt_msg); 245 | 246 | /** 247 | * @brief Callback for when the client has successfully connected to the 248 | * broker. 249 | * 250 | * Overrides mqtt::callback::connected(const std::string&). 251 | * 252 | * @param cause 253 | */ 254 | void connected(const std::string& cause) override; 255 | 256 | /** 257 | * @brief Callback for when the client has lost connection to the broker. 258 | * 259 | * Overrides mqtt::callback::connection_lost(const std::string&). 260 | * 261 | * @param cause 262 | */ 263 | void connection_lost(const std::string& cause) override; 264 | 265 | /** 266 | * @brief Returns whether the client is connected to the broker. 267 | * 268 | * @return true if client is connected to the broker 269 | * @return false if client is not connected to the broker 270 | */ 271 | bool isConnected(); 272 | 273 | /** 274 | * @brief ROS service returning whether the client is connected to the broker. 275 | * 276 | * @param request service request 277 | * @param response service response 278 | * 279 | * @return true always 280 | * @return false never 281 | */ 282 | bool isConnectedService( 283 | mqtt_client_interfaces::IsConnected::Request& request, 284 | mqtt_client_interfaces::IsConnected::Response& response); 285 | 286 | /** 287 | * @brief Callback for when the client receives a MQTT message from the 288 | * broker. 289 | * 290 | * Overrides mqtt::callback::message_arrived(mqtt::const_message_ptr). 291 | * If the received MQTT message contains information about a ROS message type, 292 | * the corresponding ROS publisher is configured. If the received MQTT message 293 | * is a ROS message, the mqtt2ros conversion is called. 294 | * 295 | * @param mqtt_msg MQTT message 296 | */ 297 | void message_arrived(mqtt::const_message_ptr mqtt_msg) override; 298 | 299 | /** 300 | * @brief Callback for when delivery for a MQTT message has been completed. 301 | * 302 | * Overrides mqtt::callback::delivery_complete(mqtt::delivery_token_ptr). 303 | * 304 | * @param token token tracking the message delivery 305 | */ 306 | void delivery_complete(mqtt::delivery_token_ptr token) override; 307 | 308 | /** 309 | * @brief Callback for when a MQTT action succeeds. 310 | * 311 | * Overrides mqtt::iaction_listener::on_success(const mqtt::token&). 312 | * Does nothing. 313 | * 314 | * @param token token tracking the action 315 | */ 316 | void on_success(const mqtt::token& token) override; 317 | 318 | /** 319 | * @brief Callback for when a MQTT action fails. 320 | * 321 | * Overrides mqtt::iaction_listener::on_failure(const mqtt::token&). 322 | * Logs error. 323 | * 324 | * @param token token tracking the action 325 | */ 326 | void on_failure(const mqtt::token& token) override; 327 | 328 | protected: 329 | /** 330 | * @brief Struct containing broker parameters 331 | */ 332 | struct BrokerConfig { 333 | std::string host; ///< broker host 334 | int port; ///< broker port 335 | std::string user; ///< username 336 | std::string pass; ///< password 337 | struct { 338 | bool enabled; ///< whether to connect via SSL/TLS 339 | std::filesystem::path 340 | ca_certificate; ///< public CA certificate trusted by client 341 | } tls; ///< SSL/TLS-related variables 342 | }; 343 | 344 | /** 345 | * @brief Struct containing client parameters 346 | */ 347 | struct ClientConfig { 348 | std::string id; ///< client unique ID 349 | struct { 350 | bool enabled; ///< whether client buffer is enabled 351 | int size; ///< client buffer size 352 | std::filesystem::path directory; ///< client buffer directory 353 | } buffer; ///< client buffer-related variables 354 | struct { 355 | std::string topic; ///< last-will topic 356 | std::string message; ///< last-will message 357 | int qos; ///< last-will QoS value 358 | bool retained; ///< whether last-will is retained 359 | } last_will; ///< last-will-related variables 360 | bool clean_session; ///< whether client requests clean session 361 | double keep_alive_interval; ///< keep-alive interval 362 | int max_inflight; ///< maximum number of inflight messages 363 | struct { 364 | std::filesystem::path certificate; ///< client certificate 365 | std::filesystem::path key; ///< client private keyfile 366 | std::string password; ///< decryption password for private key 367 | int version; ///< TLS version (https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305) 368 | bool verify; ///< Verify the client should conduct 369 | ///< post-connect checks 370 | std::vector alpn_protos; ///< list of ALPN protocols 371 | } tls; ///< SSL/TLS-related variables 372 | }; 373 | 374 | /** 375 | * @brief Struct containing variables related to a ROS2MQTT connection. 376 | */ 377 | struct Ros2MqttInterface { 378 | struct { 379 | ros::Subscriber subscriber; ///< generic ROS subscriber 380 | int queue_size = 1; ///< ROS subscriber queue size 381 | } ros; ///< ROS-related variables 382 | struct { 383 | std::string topic; ///< MQTT topic 384 | int qos = 0; ///< MQTT QoS value 385 | bool retained = false; ///< whether to retain MQTT message 386 | } mqtt; ///< MQTT-related variables 387 | bool primitive = false; ///< whether to publish as primitive message 388 | bool stamped = false; ///< whether to inject timestamp in MQTT message 389 | }; 390 | 391 | /** 392 | * @brief Struct containing variables related to a MQTT2ROS connection. 393 | */ 394 | struct Mqtt2RosInterface { 395 | struct { 396 | int qos = 0; ///< MQTT QoS value 397 | } mqtt; ///< MQTT-related variables 398 | struct { 399 | std::string topic; ///< ROS topic 400 | ros::Publisher publisher; ///< generic ROS publisher 401 | topic_tools::ShapeShifter shape_shifter; ///< ROS msg type ShapeShifter 402 | ros::Publisher latency_publisher; ///< ROS publisher for latency 403 | int queue_size = 1; ///< ROS publisher queue size 404 | bool latched = false; ///< whether to latch ROS message 405 | } ros; ///< ROS-related variables 406 | bool primitive = false; ///< whether to publish as primitive message (if 407 | ///< coming from non-ROS MQTT client) 408 | bool stamped = false; ///< whether timestamp is injected 409 | }; 410 | 411 | protected: 412 | /** 413 | * @brief MQTT topic prefix under which ROS message type information is 414 | * published 415 | * 416 | * Must contain trailing '/'. 417 | */ 418 | static const std::string kRosMsgTypeMqttTopicPrefix; 419 | 420 | /** 421 | * @brief ROS topic prefix under which ROS2MQTT2ROS latencies are published 422 | * 423 | * Must contain trailing '/'. 424 | */ 425 | static const std::string kLatencyRosTopicPrefix; 426 | 427 | /** 428 | * @brief ROS node handle 429 | */ 430 | ros::NodeHandle node_handle_; 431 | 432 | /** 433 | * @brief Private ROS node handle 434 | */ 435 | ros::NodeHandle private_node_handle_; 436 | 437 | /** 438 | * @brief ROS Service server for providing connection status 439 | */ 440 | ros::ServiceServer is_connected_service_; 441 | 442 | /** 443 | * @brief Status variable keeping track of connection status to broker 444 | */ 445 | bool is_connected_ = false; 446 | 447 | /** 448 | * @brief Broker parameters 449 | */ 450 | BrokerConfig broker_config_; 451 | 452 | /** 453 | * @brief Client parameters 454 | */ 455 | ClientConfig client_config_; 456 | 457 | /** 458 | * @brief MQTT client variable 459 | */ 460 | std::shared_ptr client_; 461 | 462 | /** 463 | * @brief MQTT client connection options 464 | */ 465 | mqtt::connect_options connect_options_; 466 | 467 | /** 468 | * @brief ROS2MQTT connection variables sorted by ROS topic 469 | */ 470 | std::map ros2mqtt_; 471 | 472 | /** 473 | * @brief MQTT2ROS connection variables sorted by MQTT topic 474 | */ 475 | std::map mqtt2ros_; 476 | }; 477 | 478 | 479 | template 480 | bool MqttClient::loadParameter(const std::string& key, T& value) { 481 | bool found = private_node_handle_.getParam(key, value); 482 | if (found) 483 | NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), 484 | std::to_string(value).c_str()); 485 | return found; 486 | } 487 | 488 | 489 | template 490 | bool MqttClient::loadParameter(const std::string& key, T& value, 491 | const T& default_value) { 492 | bool found = private_node_handle_.param(key, value, default_value); 493 | if (!found) { 494 | if (private_node_handle_.hasParam(key)) 495 | NODELET_ERROR("Parameter '%s' has wrong data type", key.c_str()); 496 | NODELET_WARN("Parameter '%s' not set, defaulting to '%s'", key.c_str(), 497 | std::to_string(default_value).c_str()); 498 | } 499 | if (found) 500 | NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), 501 | std::to_string(value).c_str()); 502 | return found; 503 | } 504 | 505 | 506 | template 507 | bool MqttClient::loadParameter(const std::string& key, std::vector& value) 508 | { 509 | const bool found = private_node_handle_.getParam(key, value); 510 | if (found) 511 | NODELET_DEBUG("Retrieved parameter '%s' = '[%s]'", key.c_str(), 512 | fmt::format("{}", fmt::join(value, ", ")).c_str()); 513 | return found; 514 | } 515 | 516 | 517 | template 518 | bool MqttClient::loadParameter(const std::string& key, std::vector& value, 519 | const std::vector& default_value) 520 | { 521 | const bool found = private_node_handle_.param(key, value, default_value); 522 | if (!found) 523 | NODELET_WARN("Parameter '%s' not set, defaulting to '%s'", key.c_str(), 524 | fmt::format("{}", fmt::join(value, ", ")).c_str()); 525 | if (found) 526 | NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), 527 | fmt::format("{}", fmt::join(value, ", ")).c_str()); 528 | return found; 529 | } 530 | 531 | 532 | /** 533 | * Serializes a ROS message to a buffer. 534 | * 535 | * @tparam T ROS message type 536 | * 537 | * @param[in] msg ROS message 538 | * @param[out] buffer buffer to serialize to 539 | */ 540 | template 541 | void serializeRosMessage(const T& msg, std::vector& buffer) { 542 | 543 | const uint32_t length = ros::serialization::serializationLength(msg); 544 | buffer.resize(length); 545 | ros::serialization::OStream stream(buffer.data(), length); 546 | ros::serialization::serialize(stream, msg); 547 | } 548 | 549 | } // namespace mqtt_client 550 | -------------------------------------------------------------------------------- /mqtt_client/include/mqtt_client/MqttClient.ros2.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | ============================================================================== 3 | MIT License 4 | 5 | Copyright 2022 Institute for Automotive Engineering of RWTH Aachen University. 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | ============================================================================== 25 | */ 26 | 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #define FMT_HEADER_ONLY 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | 48 | /** 49 | * @brief Namespace for the mqtt_client package 50 | */ 51 | namespace mqtt_client { 52 | 53 | 54 | /** 55 | * @brief ROS Nodelet for sending and receiving ROS messages via MQTT 56 | * 57 | * The MqttClient enables connected ROS-based devices or robots to 58 | * exchange ROS messages via an MQTT broker using the MQTT protocol. 59 | * This works generically for any ROS message, i.e. there is no need 60 | * to specify the ROS message type for ROS messages you wish to 61 | * exchange via the MQTT broker. 62 | */ 63 | class MqttClient : public rclcpp::Node, 64 | public virtual mqtt::callback, 65 | public virtual mqtt::iaction_listener { 66 | 67 | public: 68 | /** 69 | * @brief Initializes node. 70 | * 71 | * @param[in] options ROS node options 72 | */ 73 | explicit MqttClient(const rclcpp::NodeOptions& options); 74 | 75 | protected: 76 | struct Ros2MqttInterface; 77 | struct Mqtt2RosInterface; 78 | 79 | /** 80 | * @brief Loads ROS parameters from parameter server. 81 | */ 82 | void loadParameters(); 83 | 84 | /** 85 | * @brief Loads requested ROS parameter from parameter server. 86 | * 87 | * @param[in] key parameter name 88 | * @param[out] value variable where to store the retrieved parameter 89 | * 90 | * @return true if parameter was successfully retrieved 91 | * @return false if parameter was not found 92 | */ 93 | bool loadParameter(const std::string& key, std::string& value); 94 | 95 | /** 96 | * @brief Loads requested ROS parameter from parameter server, allows default 97 | * value. 98 | * 99 | * @param[in] key parameter name 100 | * @param[out] value variable where to store the retrieved parameter 101 | * @param[in] default_value default value 102 | * 103 | * @return true if parameter was successfully retrieved 104 | * @return false if parameter was not found or default was used 105 | */ 106 | bool loadParameter(const std::string& key, std::string& value, const std::string& default_value); 107 | 108 | /** 109 | * @brief Loads requested ROS parameter from parameter server. 110 | * 111 | * @tparam T type (one of int, double, bool) 112 | * 113 | * @param[in] key parameter name 114 | * @param[out] value variable where to store the retrieved parameter 115 | * 116 | * @return true if parameter was successfully retrieved 117 | * @return false if parameter was not found 118 | */ 119 | template 120 | bool loadParameter(const std::string& key, T& value); 121 | 122 | /** 123 | * @brief Loads requested ROS parameter from parameter server, allows default 124 | * value. 125 | * 126 | * @tparam T type (one of int, double, bool) 127 | * 128 | * @param[in] key parameter name 129 | * @param[out] value variable where to store the retrieved parameter 130 | * @param[in] default_value default value 131 | * 132 | * @return true if parameter was successfully retrieved 133 | * @return false if parameter was not found or default was used 134 | */ 135 | template 136 | bool loadParameter(const std::string& key, T& value, const T& default_value); 137 | 138 | /** 139 | * @brief Loads requested ROS parameter from parameter server. 140 | * 141 | * @tparam T type (one of int, double, bool) 142 | * 143 | * @param[in] key parameter name 144 | * @param[out] value variable where to store the retrieved parameter 145 | * 146 | * @return true if parameter was successfully retrieved 147 | * @return false if parameter was not found 148 | */ 149 | template 150 | bool loadParameter(const std::string& key, std::vector& value); 151 | 152 | /** 153 | * @brief Loads requested ROS parameter from parameter server, allows default 154 | * value. 155 | * 156 | * @tparam T type (one of int, double, bool) 157 | * 158 | * @param[in] key parameter name 159 | * @param[out] value variable where to store the retrieved parameter 160 | * @param[in] default_value default value 161 | * 162 | * @return true if parameter was successfully retrieved 163 | * @return false if parameter was not found or default was used 164 | */ 165 | template 166 | bool loadParameter(const std::string& key, std::vector& value, const std::vector& default_value); 167 | 168 | /** 169 | * @brief Converts a string to a path object resolving paths relative to 170 | * ROS_HOME. 171 | * 172 | * Resolves relative to CWD, if ROS_HOME is not set. 173 | * Returns empty path, if argument is empty. 174 | * 175 | * @param path_string (relative) path as string 176 | * 177 | * @return std::filesystem::path path variable 178 | */ 179 | std::filesystem::path resolvePath(const std::string& path_string); 180 | 181 | /** 182 | * @brief Initializes broker connection and subscriptions. 183 | */ 184 | void setup(); 185 | 186 | /** 187 | * @brief Get the resolved compatible QOS from the interface and the endpoint 188 | * 189 | * This uses the two endpoints to decide upon a compatible QoS, resolving any "auto" QoS settings 190 | * 191 | * @param ros_topic the ROS topic we are looking on 192 | * @param tei Topic endpoint info 193 | * @param ros2mqtt the ROS to MQTT interface spec 194 | * 195 | * @returns The compatible QoS or nullopt if no compatible combination is found 196 | */ 197 | std::optional getCompatibleQoS( 198 | const std::string& ros_topic, const rclcpp::TopicEndpointInfo& tei, 199 | const Ros2MqttInterface& ros2mqtt) const; 200 | 201 | /** 202 | * @brief Get the candiate topic endpoints for subscription matching 203 | * 204 | * @param ros2mqtt the ROS to MQTT interface spec 205 | * 206 | * @returns The compatible QoS or nullopt if no compatible combination is found 207 | */ 208 | std::vector getCandidatePublishers( 209 | const std::string& ros_topic, const Ros2MqttInterface& ros2mqtt) const; 210 | 211 | /** 212 | * @brief Setup any subscriptions we can. 213 | * 214 | * These may be fixed type/QoS, or dynamically matched against active publisher 215 | */ 216 | void setupSubscriptions(); 217 | 218 | /** 219 | * @brief Setup any publishers that we can 220 | */ 221 | void setupPublishers(); 222 | 223 | /** 224 | * @brief Sets up the client connection options and initializes the client 225 | * object. 226 | */ 227 | void setupClient(); 228 | 229 | /** 230 | * @brief Connects to the broker using the member client and options. 231 | */ 232 | void connect(); 233 | 234 | /** 235 | * @brief Publishes a generic serialized ROS message to the MQTT broker. 236 | * 237 | * Before publishing the ROS message to the MQTT broker, the ROS message type 238 | * is extracted. This type information is also sent to the MQTT broker on a 239 | * separate topic. 240 | * 241 | * The MQTT payload for the actual ROS message carries the following: 242 | * - 0 or 1 (indicating if timestamp is injected (=1)) 243 | * - serialized timestamp (optional) 244 | * - serialized ROS message 245 | * 246 | * @param serialized_msg generic serialized ROS message 247 | * @param ros_topic ROS topic where the message was published 248 | */ 249 | void ros2mqtt( 250 | const std::shared_ptr& serialized_msg, 251 | const std::string& ros_topic); 252 | 253 | /** 254 | * @brief Publishes a ROS message received via MQTT to ROS. 255 | * 256 | * This utilizes the generic publisher stored for the MQTT topic on which the 257 | * message was received. The publisher has to be configured to the ROS message 258 | * type of the message. If the message carries an injected timestamp, the 259 | * latency is computed and published. 260 | * 261 | * The MQTT payload is expected to carry the following: 262 | * - 0 or 1 (indicating if timestamp is injected (=1)) 263 | * - serialized timestamp (optional) 264 | * - serialized ROS message 265 | * 266 | * @param mqtt_msg MQTT message 267 | * @param arrival_stamp arrival timestamp used for latency computation 268 | */ 269 | void mqtt2ros(mqtt::const_message_ptr mqtt_msg, 270 | const rclcpp::Time& arrival_stamp); 271 | 272 | /** 273 | * @brief Publishes a primitive message received via MQTT to ROS. 274 | * 275 | * @param mqtt_msg MQTT message 276 | */ 277 | void mqtt2primitive(mqtt::const_message_ptr mqtt_msg); 278 | 279 | /** 280 | * @brief Publishes a primitive message received via MQTT to ROS. 281 | * 282 | * @param mqtt_msg MQTT message 283 | */ 284 | void mqtt2fixed(mqtt::const_message_ptr mqtt_msg); 285 | 286 | /** 287 | * @brief Callback for when the client has successfully connected to the 288 | * broker. 289 | * 290 | * Overrides mqtt::callback::connected(const std::string&). 291 | * 292 | * @param cause 293 | */ 294 | void connected(const std::string& cause) override; 295 | 296 | /** 297 | * @brief Callback for when the client has lost connection to the broker. 298 | * 299 | * Overrides mqtt::callback::connection_lost(const std::string&). 300 | * 301 | * @param cause 302 | */ 303 | void connection_lost(const std::string& cause) override; 304 | 305 | /** 306 | * @brief Returns whether the client is connected to the broker. 307 | * 308 | * @return true if client is connected to the broker 309 | * @return false if client is not connected to the broker 310 | */ 311 | bool isConnected(); 312 | 313 | /** 314 | * @brief ROS service returning whether the client is connected to the broker. 315 | * 316 | * @param request service request 317 | * @param response service response 318 | */ 319 | void isConnectedService( 320 | mqtt_client_interfaces::srv::IsConnected::Request::SharedPtr request, 321 | mqtt_client_interfaces::srv::IsConnected::Response::SharedPtr response); 322 | 323 | /** 324 | * @brief ROS service that dynamically creates a ROS -> MQTT mapping. 325 | * 326 | * @param request service request 327 | * @param response service response 328 | */ 329 | void newRos2MqttBridge( 330 | mqtt_client_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, 331 | mqtt_client_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response); 332 | 333 | /** 334 | * @brief ROS service that dynamically creates an MQTT -> ROS mapping. 335 | * 336 | * @param request service request 337 | * @param response service response 338 | */ 339 | void newMqtt2RosBridge( 340 | mqtt_client_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, 341 | mqtt_client_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response); 342 | 343 | /** 344 | * @brief Callback for when the client receives a MQTT message from the 345 | * broker. 346 | * 347 | * Overrides mqtt::callback::message_arrived(mqtt::const_message_ptr). 348 | * If the received MQTT message contains information about a ROS message type, 349 | * the corresponding ROS publisher is configured. If the received MQTT message 350 | * is a ROS message, the mqtt2ros conversion is called. 351 | * 352 | * @param mqtt_msg MQTT message 353 | */ 354 | void message_arrived(mqtt::const_message_ptr mqtt_msg) override; 355 | 356 | /** 357 | * @brief Callback for when delivery for a MQTT message has been completed. 358 | * 359 | * Overrides mqtt::callback::delivery_complete(mqtt::delivery_token_ptr). 360 | * 361 | * @param token token tracking the message delivery 362 | */ 363 | void delivery_complete(mqtt::delivery_token_ptr token) override; 364 | 365 | /** 366 | * @brief Callback for when a MQTT action succeeds. 367 | * 368 | * Overrides mqtt::iaction_listener::on_success(const mqtt::token&). 369 | * Does nothing. 370 | * 371 | * @param token token tracking the action 372 | */ 373 | void on_success(const mqtt::token& token) override; 374 | 375 | /** 376 | * @brief Callback for when a MQTT action fails. 377 | * 378 | * Overrides mqtt::iaction_listener::on_failure(const mqtt::token&). 379 | * Logs error. 380 | * 381 | * @param token token tracking the action 382 | */ 383 | void on_failure(const mqtt::token& token) override; 384 | 385 | protected: 386 | /** 387 | * @brief Struct containing broker parameters 388 | */ 389 | struct BrokerConfig { 390 | std::string host; ///< broker host 391 | int port; ///< broker port 392 | std::string user; ///< username 393 | std::string pass; ///< password 394 | struct { 395 | bool enabled; ///< whether to connect via SSL/TLS 396 | std::filesystem::path ca_certificate; ///< public CA certificate trusted by client 397 | } tls; ///< SSL/TLS-related variables 398 | }; 399 | 400 | /** 401 | * @brief Struct containing client parameters 402 | */ 403 | struct ClientConfig { 404 | std::string id; ///< client unique ID 405 | struct { 406 | bool enabled; ///< whether client buffer is enabled 407 | int size; ///< client buffer size 408 | std::filesystem::path directory; ///< client buffer directory 409 | } buffer; ///< client buffer-related variables 410 | struct { 411 | std::string topic; ///< last-will topic 412 | std::string message; ///< last-will message 413 | int qos; ///< last-will QoS value 414 | bool retained; ///< whether last-will is retained 415 | } last_will; ///< last-will-related variables 416 | bool clean_session; ///< whether client requests clean session 417 | double keep_alive_interval; ///< keep-alive interval 418 | int max_inflight; ///< maximum number of inflight messages 419 | struct { 420 | std::filesystem::path certificate; ///< client certificate 421 | std::filesystem::path key; ///< client private keyfile 422 | std::string password; ///< decryption password for private key 423 | int version; ///< TLS version (https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305) 424 | bool verify; ///< Verify the client should conduct 425 | ///< post-connect checks 426 | std::vector alpn_protos; ///< list of ALPN protocols 427 | } tls; ///< SSL/TLS-related variables 428 | }; 429 | 430 | /** 431 | * @brief Struct containing variables related to a ROS2MQTT connection. 432 | */ 433 | struct Ros2MqttInterface { 434 | struct { 435 | rclcpp::GenericSubscription::SharedPtr 436 | subscriber; ///< generic ROS subscriber 437 | std::string msg_type; ///< message type of subscriber 438 | int queue_size = 1; ///< ROS subscriber queue size 439 | bool is_stale = false; ///< whether a new generic publisher/subscriber is required 440 | struct { 441 | // If these are set to nullopt then that part of the QoS is determine automatically based on discovery 442 | std::optional reliability; 443 | std::optional durability; 444 | } qos; 445 | } ros; ///< ROS-related variables 446 | struct { 447 | std::string topic; ///< MQTT topic 448 | int qos = 0; ///< MQTT QoS value 449 | bool retained = false; ///< whether to retain MQTT message 450 | } mqtt; ///< MQTT-related variables 451 | bool fixed_type = false; ///< whether the published message type is specified explicitly 452 | bool primitive = false; ///< whether to publish as primitive message 453 | bool stamped = false; ///< whether to inject timestamp in MQTT message 454 | }; 455 | 456 | /** 457 | * @brief Struct containing variables related to a MQTT2ROS connection. 458 | */ 459 | struct Mqtt2RosInterface { 460 | struct { 461 | int qos = 0; ///< MQTT QoS value 462 | } mqtt; ///< MQTT-related variables 463 | struct { 464 | std::string topic; ///< ROS topic 465 | std::string msg_type; ///< message type of publisher 466 | rclcpp::GenericPublisher::SharedPtr publisher; ///< generic ROS publisher 467 | rclcpp::Publisher::SharedPtr 468 | latency_publisher; ///< ROS publisher for latency 469 | int queue_size = 1; ///< ROS publisher queue size 470 | struct { 471 | rclcpp::ReliabilityPolicy reliability = rclcpp::ReliabilityPolicy::SystemDefault; 472 | rclcpp::DurabilityPolicy durability = rclcpp::DurabilityPolicy::SystemDefault; 473 | } qos; 474 | bool latched = false; ///< whether to latch ROS message 475 | bool is_stale = false; ///< whether a new generic publisher/subscriber is required 476 | } ros; ///< ROS-related variables 477 | bool fixed_type = false; ///< whether the published ros message type is specified explicitly 478 | bool primitive = false; ///< whether to publish as primitive message (if 479 | ///< coming from non-ROS MQTT client) 480 | bool stamped = false; ///< whether timestamp is injected 481 | }; 482 | 483 | protected: 484 | /** 485 | * @brief MQTT topic prefix under which ROS message type information is 486 | * published 487 | * 488 | * Must contain trailing '/'. 489 | */ 490 | static const std::string kRosMsgTypeMqttTopicPrefix; 491 | 492 | /** 493 | * @brief ROS topic prefix under which ROS2MQTT2ROS latencies are published 494 | * 495 | * Must contain trailing '/'. 496 | */ 497 | static const std::string kLatencyRosTopicPrefix; 498 | 499 | /** 500 | * @brief Timer to repeatedly check active ROS topics for topics to subscribe 501 | */ 502 | rclcpp::TimerBase::SharedPtr check_subscriptions_timer_; 503 | 504 | /** 505 | * @brief ROS Service server for providing connection status 506 | */ 507 | rclcpp::Service::SharedPtr 508 | is_connected_service_; 509 | 510 | /** 511 | * @brief ROS Service server for providing dynamic ROS to MQTT mappings. 512 | */ 513 | rclcpp::Service::SharedPtr 514 | new_ros2mqtt_bridge_service_; 515 | 516 | /** 517 | * @brief ROS Service server for providing dynamic MQTT to ROS mappings. 518 | */ 519 | rclcpp::Service::SharedPtr 520 | new_mqtt2ros_bridge_service_; 521 | 522 | /** 523 | * @brief Status variable keeping track of connection status to broker 524 | */ 525 | bool is_connected_ = false; 526 | 527 | /** 528 | * @brief Broker parameters 529 | */ 530 | BrokerConfig broker_config_; 531 | 532 | /** 533 | * @brief Client parameters 534 | */ 535 | ClientConfig client_config_; 536 | 537 | /** 538 | * @brief MQTT client variable 539 | */ 540 | std::shared_ptr client_; 541 | 542 | /** 543 | * @brief MQTT client connection options 544 | */ 545 | mqtt::connect_options connect_options_; 546 | 547 | /** 548 | * @brief ROS2MQTT connection variables sorted by ROS topic 549 | */ 550 | std::map ros2mqtt_; 551 | 552 | /** 553 | * @brief MQTT2ROS connection variables sorted by MQTT topic 554 | */ 555 | std::map mqtt2ros_; 556 | 557 | /** 558 | * Message length of a serialized `builtin_interfaces::msg::Time` message 559 | */ 560 | uint32_t stamp_length_; 561 | }; 562 | 563 | 564 | template 565 | bool MqttClient::loadParameter(const std::string& key, T& value) { 566 | bool found = get_parameter(key, value); 567 | if (found) 568 | RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), 569 | std::to_string(value).c_str()); 570 | return found; 571 | } 572 | 573 | 574 | template 575 | bool MqttClient::loadParameter(const std::string& key, T& value, 576 | const T& default_value) { 577 | bool found = get_parameter_or(key, value, default_value); 578 | if (!found) 579 | RCLCPP_WARN(get_logger(), "Parameter '%s' not set, defaulting to '%s'", 580 | key.c_str(), std::to_string(default_value).c_str()); 581 | if (found) 582 | RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), 583 | std::to_string(value).c_str()); 584 | return found; 585 | } 586 | 587 | 588 | template 589 | bool MqttClient::loadParameter(const std::string& key, std::vector& value) { 590 | const bool found = get_parameter(key, value); 591 | if (found) 592 | RCLCPP_WARN(get_logger(), "Retrieved parameter '%s' = '[%s]'", key.c_str(), 593 | fmt::format("{}", fmt::join(value, ", ")).c_str()); 594 | return found; 595 | } 596 | 597 | 598 | template 599 | bool MqttClient::loadParameter(const std::string& key, std::vector& value, 600 | const std::vector& default_value) { 601 | const bool found = get_parameter_or(key, value, default_value); 602 | if (!found) 603 | RCLCPP_WARN(get_logger(), "Parameter '%s' not set, defaulting to '%s'", 604 | key.c_str(), fmt::format("{}", fmt::join(value, ", ")).c_str()); 605 | if (found) 606 | RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), 607 | fmt::format("{}", fmt::join(value, ", ")).c_str()); 608 | return found; 609 | } 610 | 611 | 612 | /** 613 | * Serializes a ROS message. 614 | * 615 | * @tparam T ROS message type 616 | * 617 | * @param[in] msg ROS message 618 | * @param[out] serialized_msg serialized message 619 | */ 620 | template 621 | void serializeRosMessage(const T& msg, 622 | rclcpp::SerializedMessage& serialized_msg) { 623 | 624 | rclcpp::Serialization serializer; 625 | serializer.serialize_message(&msg, &serialized_msg); 626 | } 627 | 628 | 629 | /** 630 | * Deserializes a ROS message. 631 | * 632 | * @tparam T ROS message type 633 | * 634 | * @param[in] serialized_msg serialized message 635 | * @param[out] msg ROS message 636 | */ 637 | template 638 | void deserializeRosMessage(const rclcpp::SerializedMessage& serialized_msg, 639 | T& msg) { 640 | 641 | rclcpp::Serialization serializer; 642 | serializer.deserialize_message(&serialized_msg, &msg); 643 | } 644 | 645 | } // namespace mqtt_client 646 | -------------------------------------------------------------------------------- /mqtt_client/launch/standalone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /mqtt_client/launch/standalone.launch.ros2.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /mqtt_client/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mqtt_client nodelet 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /mqtt_client/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mqtt_client 5 | 2.3.0 6 | Node that enables connected ROS-based devices or robots to exchange ROS messages via an MQTT broker using the MQTT protocol. 7 | 8 | Lennart Reiher 9 | Bastian Lampe 10 | 11 | MIT 12 | 13 | http://wiki.ros.org/mqtt_client 14 | https://github.com/ika-rwth-aachen/mqtt_client 15 | 16 | Lennart Reiher 17 | Bastian Lampe 18 | Christian Wende 19 | 20 | fmt 21 | mqtt_client_interfaces 22 | ros_environment 23 | std_msgs 24 | 25 | 26 | ament_cmake 27 | libpaho-mqtt-dev 28 | libpaho-mqttpp-dev 29 | rclcpp 30 | rclcpp_components 31 | rcpputils 32 | 33 | 34 | catkin 35 | nodelet 36 | paho-mqtt-cpp 37 | roscpp 38 | topic_tools 39 | 40 | 41 | catkin 42 | ament_cmake 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /mqtt_client/src/MqttClient.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ============================================================================== 3 | MIT License 4 | 5 | Copyright 2022 Institute for Automotive Engineering of RWTH Aachen University. 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | ============================================================================== 25 | */ 26 | 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | 54 | PLUGINLIB_EXPORT_CLASS(mqtt_client::MqttClient, nodelet::Nodelet) 55 | 56 | 57 | namespace mqtt_client { 58 | 59 | 60 | const std::string MqttClient::kRosMsgTypeMqttTopicPrefix = 61 | "mqtt_client/ros_msg_type/"; 62 | 63 | const std::string MqttClient::kLatencyRosTopicPrefix = "latencies/"; 64 | 65 | 66 | /** 67 | * @brief Extracts string of primitive data types from ROS message. 68 | * 69 | * This is helpful to extract the actual data payload of a primitive ROS 70 | * message. If e.g. an std_msgs/String is serialized to a string 71 | * representation, it also contains the field name 'data'. This function 72 | * instead returns the underlying value as string. 73 | * 74 | * The following primitive ROS message types are supported: 75 | * std_msgs/String 76 | * std_msgs/Bool 77 | * std_msgs/Char 78 | * std_msgs/UInt8 79 | * std_msgs/UInt16 80 | * std_msgs/UInt32 81 | * std_msgs/UInt64 82 | * std_msgs/Int8 83 | * std_msgs/Int16 84 | * std_msgs/Int32 85 | * std_msgs/Int64 86 | * std_msgs/Float32 87 | * std_msgs/Float64 88 | * 89 | * @param [in] msg generic ShapeShifter ROS message 90 | * @param [out] primitive string representation of primitive message data 91 | * 92 | * @return true if primitive ROS message type was found 93 | * @return false if ROS message type is not primitive 94 | */ 95 | bool primitiveRosMessageToString(const topic_tools::ShapeShifter::ConstPtr& msg, 96 | std::string& primitive) { 97 | 98 | bool found_primitive = true; 99 | const std::string& msg_type_md5 = msg->getMD5Sum(); 100 | 101 | if (msg_type_md5 == ros::message_traits::MD5Sum::value()) { 102 | primitive = msg->instantiate()->data; 103 | } else if (msg_type_md5 == 104 | ros::message_traits::MD5Sum::value()) { 105 | primitive = msg->instantiate()->data ? "true" : "false"; 106 | } else if (msg_type_md5 == 107 | ros::message_traits::MD5Sum::value()) { 108 | primitive = std::to_string(msg->instantiate()->data); 109 | } else if (msg_type_md5 == 110 | ros::message_traits::MD5Sum::value()) { 111 | primitive = std::to_string(msg->instantiate()->data); 112 | } else if (msg_type_md5 == 113 | ros::message_traits::MD5Sum::value()) { 114 | primitive = std::to_string(msg->instantiate()->data); 115 | } else if (msg_type_md5 == 116 | ros::message_traits::MD5Sum::value()) { 117 | primitive = std::to_string(msg->instantiate()->data); 118 | } else if (msg_type_md5 == 119 | ros::message_traits::MD5Sum::value()) { 120 | primitive = std::to_string(msg->instantiate()->data); 121 | } else if (msg_type_md5 == 122 | ros::message_traits::MD5Sum::value()) { 123 | primitive = std::to_string(msg->instantiate()->data); 124 | } else if (msg_type_md5 == 125 | ros::message_traits::MD5Sum::value()) { 126 | primitive = std::to_string(msg->instantiate()->data); 127 | } else if (msg_type_md5 == 128 | ros::message_traits::MD5Sum::value()) { 129 | primitive = std::to_string(msg->instantiate()->data); 130 | } else if (msg_type_md5 == 131 | ros::message_traits::MD5Sum::value()) { 132 | primitive = std::to_string(msg->instantiate()->data); 133 | } else if (msg_type_md5 == 134 | ros::message_traits::MD5Sum::value()) { 135 | primitive = std::to_string(msg->instantiate()->data); 136 | } else if (msg_type_md5 == 137 | ros::message_traits::MD5Sum::value()) { 138 | primitive = std::to_string(msg->instantiate()->data); 139 | } else { 140 | found_primitive = false; 141 | } 142 | 143 | return found_primitive; 144 | } 145 | 146 | 147 | void MqttClient::onInit() { 148 | 149 | // get nodehandles 150 | node_handle_ = this->getMTNodeHandle(); 151 | private_node_handle_ = this->getMTPrivateNodeHandle(); 152 | 153 | loadParameters(); 154 | setup(); 155 | } 156 | 157 | 158 | void MqttClient::loadParameters() { 159 | 160 | // load broker parameters from parameter server 161 | std::string broker_tls_ca_certificate; 162 | loadParameter("broker/host", broker_config_.host, "localhost"); 163 | loadParameter("broker/port", broker_config_.port, 1883); 164 | if (loadParameter("broker/user", broker_config_.user)) { 165 | loadParameter("broker/pass", broker_config_.pass, ""); 166 | } 167 | if (loadParameter("broker/tls/enabled", broker_config_.tls.enabled, false)) { 168 | loadParameter("broker/tls/ca_certificate", broker_tls_ca_certificate, 169 | "/etc/ssl/certs/ca-certificates.crt"); 170 | } 171 | 172 | // load client parameters from parameter server 173 | std::string client_buffer_directory, client_tls_certificate, client_tls_key; 174 | loadParameter("client/id", client_config_.id, ""); 175 | client_config_.buffer.enabled = !client_config_.id.empty(); 176 | if (client_config_.buffer.enabled) { 177 | loadParameter("client/buffer/size", client_config_.buffer.size, 0); 178 | loadParameter("client/buffer/directory", client_buffer_directory, "buffer"); 179 | } else { 180 | NODELET_WARN("Client buffer can not be enabled when client ID is empty"); 181 | } 182 | if (loadParameter("client/last_will/topic", client_config_.last_will.topic)) { 183 | loadParameter("client/last_will/message", client_config_.last_will.message, 184 | "offline"); 185 | loadParameter("client/last_will/qos", client_config_.last_will.qos, 0); 186 | loadParameter("client/last_will/retained", 187 | client_config_.last_will.retained, false); 188 | } 189 | loadParameter("client/clean_session", client_config_.clean_session, true); 190 | loadParameter("client/keep_alive_interval", 191 | client_config_.keep_alive_interval, 60.0); 192 | loadParameter("client/max_inflight", client_config_.max_inflight, 65535); 193 | if (broker_config_.tls.enabled) { 194 | if (loadParameter("client/tls/certificate", client_tls_certificate)) { 195 | loadParameter("client/tls/key", client_tls_key); 196 | loadParameter("client/tls/password", client_config_.tls.password); 197 | loadParameter("client/tls/version", client_config_.tls.version); 198 | loadParameter("client/tls/verify", client_config_.tls.verify); 199 | loadParameter("client/tls/alpn_protos", client_config_.tls.alpn_protos); 200 | } 201 | } 202 | 203 | // resolve filepaths 204 | broker_config_.tls.ca_certificate = resolvePath(broker_tls_ca_certificate); 205 | client_config_.buffer.directory = resolvePath(client_buffer_directory); 206 | client_config_.tls.certificate = resolvePath(client_tls_certificate); 207 | client_config_.tls.key = resolvePath(client_tls_key); 208 | 209 | // load bridge parameters from parameter server 210 | XmlRpc::XmlRpcValue bridge; 211 | if (!private_node_handle_.getParam("bridge", bridge)) { 212 | NODELET_ERROR("Parameter 'bridge' is required"); 213 | exit(EXIT_FAILURE); 214 | } 215 | 216 | // parse bridge parameters 217 | try { 218 | 219 | // ros2mqtt 220 | if (bridge.hasMember("ros2mqtt")) { 221 | 222 | // loop over array 223 | XmlRpc::XmlRpcValue& ros2mqtt_params = bridge["ros2mqtt"]; 224 | for (int k = 0; k < ros2mqtt_params.size(); k++) { 225 | 226 | if (ros2mqtt_params[k].hasMember("ros_topic") && 227 | ros2mqtt_params[k].hasMember("mqtt_topic")) { 228 | 229 | // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic 230 | std::string& ros_topic = ros2mqtt_params[k]["ros_topic"]; 231 | Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; 232 | ros2mqtt.mqtt.topic = std::string(ros2mqtt_params[k]["mqtt_topic"]); 233 | 234 | // ros2mqtt[k]/primitive 235 | if (ros2mqtt_params[k].hasMember("primitive")) 236 | ros2mqtt.primitive = ros2mqtt_params[k]["primitive"]; 237 | 238 | // ros2mqtt[k]/inject_timestamp 239 | if (ros2mqtt_params[k].hasMember("inject_timestamp")) 240 | ros2mqtt.stamped = ros2mqtt_params[k]["inject_timestamp"]; 241 | if (ros2mqtt.stamped && ros2mqtt.primitive) { 242 | NODELET_WARN( 243 | "Timestamp will not be injected into primitive messages on ROS " 244 | "topic '%s'", 245 | ros_topic.c_str()); 246 | ros2mqtt.stamped = false; 247 | } 248 | 249 | // ros2mqtt[k]/advanced 250 | if (ros2mqtt_params[k].hasMember("advanced")) { 251 | XmlRpc::XmlRpcValue& advanced_params = 252 | ros2mqtt_params[k]["advanced"]; 253 | if (advanced_params.hasMember("ros")) { 254 | if (advanced_params["ros"].hasMember("queue_size")) 255 | ros2mqtt.ros.queue_size = advanced_params["ros"]["queue_size"]; 256 | } 257 | if (advanced_params.hasMember("mqtt")) { 258 | if (advanced_params["mqtt"].hasMember("qos")) 259 | ros2mqtt.mqtt.qos = advanced_params["mqtt"]["qos"]; 260 | if (advanced_params["mqtt"].hasMember("retained")) 261 | ros2mqtt.mqtt.retained = advanced_params["mqtt"]["retained"]; 262 | } 263 | } 264 | 265 | NODELET_INFO("Bridging %sROS topic '%s' to MQTT topic '%s' %s", 266 | ros2mqtt.primitive ? "primitive " : "", 267 | ros_topic.c_str(), ros2mqtt.mqtt.topic.c_str(), 268 | ros2mqtt.stamped ? "and measuring latency" : ""); 269 | } else { 270 | NODELET_WARN( 271 | "Parameter 'bridge/ros2mqtt[%d]' is missing subparameter " 272 | "'ros_topic' or 'mqtt_topic', will be ignored", 273 | k); 274 | } 275 | } 276 | } 277 | 278 | // mqtt2ros 279 | if (bridge.hasMember("mqtt2ros")) { 280 | 281 | // loop over array 282 | XmlRpc::XmlRpcValue& mqtt2ros_params = bridge["mqtt2ros"]; 283 | for (int k = 0; k < mqtt2ros_params.size(); k++) { 284 | 285 | 286 | if (mqtt2ros_params[k].hasMember("mqtt_topic") && 287 | mqtt2ros_params[k].hasMember("ros_topic")) { 288 | 289 | // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic 290 | std::string& mqtt_topic = mqtt2ros_params[k]["mqtt_topic"]; 291 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 292 | mqtt2ros.ros.topic = std::string(mqtt2ros_params[k]["ros_topic"]); 293 | 294 | // mqtt2ros[k]/primitive 295 | if (mqtt2ros_params[k].hasMember("primitive")) 296 | mqtt2ros.primitive = mqtt2ros_params[k]["primitive"]; 297 | 298 | // mqtt2ros[k]/advanced 299 | if (mqtt2ros_params[k].hasMember("advanced")) { 300 | XmlRpc::XmlRpcValue& advanced_params = 301 | mqtt2ros_params[k]["advanced"]; 302 | if (advanced_params.hasMember("mqtt")) { 303 | if (advanced_params["mqtt"].hasMember("qos")) 304 | mqtt2ros.mqtt.qos = advanced_params["mqtt"]["qos"]; 305 | } 306 | if (advanced_params.hasMember("ros")) { 307 | if (advanced_params["ros"].hasMember("queue_size")) 308 | mqtt2ros.ros.queue_size = advanced_params["ros"]["queue_size"]; 309 | if (advanced_params["ros"].hasMember("latched")) 310 | mqtt2ros.ros.latched = advanced_params["ros"]["latched"]; 311 | } 312 | } 313 | 314 | NODELET_INFO( 315 | "Bridging MQTT topic '%s' to %sROS topic '%s'", mqtt_topic.c_str(), 316 | mqtt2ros.primitive ? "primitive " : "", mqtt2ros.ros.topic.c_str()); 317 | } else { 318 | NODELET_WARN( 319 | "Parameter 'bridge/mqtt2ros[%d]' is missing subparameter " 320 | "'mqtt_topic' or 'ros_topic', will be ignored", 321 | k); 322 | } 323 | } 324 | } 325 | 326 | if (ros2mqtt_.empty() && mqtt2ros_.empty()) { 327 | NODELET_ERROR("No valid ROS-MQTT bridge found in parameter 'bridge'"); 328 | exit(EXIT_FAILURE); 329 | } 330 | 331 | } catch (XmlRpc::XmlRpcException e) { 332 | NODELET_ERROR("Parameter 'bridge' could not be parsed (XmlRpc %d: %s)", 333 | e.getCode(), e.getMessage().c_str()); 334 | exit(EXIT_FAILURE); 335 | } 336 | } 337 | 338 | 339 | bool MqttClient::loadParameter(const std::string& key, std::string& value) { 340 | bool found = private_node_handle_.getParam(key, value); 341 | if (found) 342 | NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), 343 | value.c_str()); 344 | return found; 345 | } 346 | 347 | 348 | bool MqttClient::loadParameter(const std::string& key, std::string& value, 349 | const std::string& default_value) { 350 | bool found = 351 | private_node_handle_.param(key, value, default_value); 352 | if (!found) { 353 | if (private_node_handle_.hasParam(key)) 354 | NODELET_ERROR("Parameter '%s' has wrong data type", key.c_str()); 355 | NODELET_WARN("Parameter '%s' not set, defaulting to '%s'", key.c_str(), 356 | default_value.c_str()); 357 | } 358 | if (found) 359 | NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), 360 | value.c_str()); 361 | return found; 362 | } 363 | 364 | 365 | std::filesystem::path MqttClient::resolvePath(const std::string& path_string) { 366 | 367 | std::filesystem::path path(path_string); 368 | if (path_string.empty()) return path; 369 | if (!path.has_root_path()) { 370 | std::string ros_home; 371 | ros::get_environment_variable(ros_home, "ROS_HOME"); 372 | if (ros_home.empty()) 373 | ros_home = std::string(std::filesystem::current_path()); 374 | path = std::filesystem::path(ros_home); 375 | path.append(path_string); 376 | } 377 | if (!std::filesystem::exists(path)) 378 | NODELET_WARN("Requested path '%s' does not exist", 379 | std::string(path).c_str()); 380 | return path; 381 | } 382 | 383 | 384 | void MqttClient::setup() { 385 | 386 | // initialize MQTT client 387 | setupClient(); 388 | 389 | // connect to MQTT broker 390 | connect(); 391 | 392 | // create ROS service server 393 | is_connected_service_ = private_node_handle_.advertiseService( 394 | "is_connected", &MqttClient::isConnectedService, this); 395 | 396 | // create ROS subscribers 397 | for (auto& ros2mqtt_p : ros2mqtt_) { 398 | const std::string& ros_topic = ros2mqtt_p.first; 399 | Ros2MqttInterface& ros2mqtt = ros2mqtt_p.second; 400 | const std::string& mqtt_topic = ros2mqtt.mqtt.topic; 401 | ros2mqtt.ros.subscriber = 402 | private_node_handle_.subscribe( 403 | ros_topic, ros2mqtt.ros.queue_size, 404 | boost::bind(&MqttClient::ros2mqtt, this, _1, ros_topic)); 405 | NODELET_DEBUG("Subscribed ROS topic '%s'", ros_topic.c_str()); 406 | } 407 | } 408 | 409 | 410 | void MqttClient::setupClient() { 411 | 412 | // basic client connection options 413 | connect_options_.set_automatic_reconnect(true); 414 | connect_options_.set_clean_session(client_config_.clean_session); 415 | connect_options_.set_keep_alive_interval(client_config_.keep_alive_interval); 416 | connect_options_.set_max_inflight(client_config_.max_inflight); 417 | 418 | // user authentication 419 | if (!broker_config_.user.empty()) { 420 | connect_options_.set_user_name(broker_config_.user); 421 | connect_options_.set_password(broker_config_.pass); 422 | } 423 | 424 | // last will 425 | if (!client_config_.last_will.topic.empty()) { 426 | mqtt::will_options will( 427 | client_config_.last_will.topic, client_config_.last_will.message, 428 | client_config_.last_will.qos, client_config_.last_will.retained); 429 | connect_options_.set_will(will); 430 | } 431 | 432 | // SSL/TLS 433 | if (broker_config_.tls.enabled) { 434 | mqtt::ssl_options ssl; 435 | ssl.set_trust_store(broker_config_.tls.ca_certificate); 436 | if (!client_config_.tls.certificate.empty() && 437 | !client_config_.tls.key.empty()) { 438 | ssl.set_key_store(client_config_.tls.certificate); 439 | ssl.set_private_key(client_config_.tls.key); 440 | if (!client_config_.tls.password.empty()) 441 | ssl.set_private_key_password(client_config_.tls.password); 442 | } 443 | ssl.set_ssl_version(client_config_.tls.version); 444 | ssl.set_verify(client_config_.tls.verify); 445 | ssl.set_alpn_protos(client_config_.tls.alpn_protos); 446 | connect_options_.set_ssl(ssl); 447 | } 448 | 449 | // create MQTT client 450 | const std::string protocol = broker_config_.tls.enabled ? "ssl" : "tcp"; 451 | const std::string uri = fmt::format("{}://{}:{}", protocol, broker_config_.host, 452 | broker_config_.port); 453 | try { 454 | if (client_config_.buffer.enabled) { 455 | client_ = std::shared_ptr(new mqtt::async_client( 456 | uri, client_config_.id, client_config_.buffer.size, 457 | client_config_.buffer.directory)); 458 | } else { 459 | client_ = std::shared_ptr( 460 | new mqtt::async_client(uri, client_config_.id)); 461 | } 462 | } catch (const mqtt::exception& e) { 463 | ROS_ERROR("Client could not be initialized: %s", e.what()); 464 | exit(EXIT_FAILURE); 465 | } 466 | 467 | // setup MQTT callbacks 468 | client_->set_callback(*this); 469 | } 470 | 471 | 472 | void MqttClient::connect() { 473 | 474 | std::string as_client = 475 | client_config_.id.empty() 476 | ? "" 477 | : std::string(" as '") + client_config_.id + std::string("'"); 478 | NODELET_INFO("Connecting to broker at '%s'%s ...", 479 | client_->get_server_uri().c_str(), as_client.c_str()); 480 | 481 | try { 482 | client_->connect(connect_options_, nullptr, *this); 483 | } catch (const mqtt::exception& e) { 484 | ROS_ERROR("Connection to broker failed: %s", e.what()); 485 | exit(EXIT_FAILURE); 486 | } 487 | } 488 | 489 | 490 | void MqttClient::ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg, 491 | const std::string& ros_topic) { 492 | 493 | Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; 494 | std::string mqtt_topic = ros2mqtt.mqtt.topic; 495 | std::vector payload_buffer; 496 | 497 | // gather information on ROS message type in special ROS message 498 | mqtt_client_interfaces::RosMsgType ros_msg_type; 499 | ros_msg_type.md5 = ros_msg->getMD5Sum(); 500 | ros_msg_type.name = ros_msg->getDataType(); 501 | ros_msg_type.definition = ros_msg->getMessageDefinition(); 502 | ros_msg_type.stamped = ros2mqtt.stamped; 503 | 504 | NODELET_DEBUG("Received ROS message of type '%s' on topic '%s'", 505 | ros_msg_type.name.c_str(), ros_topic.c_str()); 506 | 507 | if (ros2mqtt.primitive) { // publish as primitive (string) message 508 | 509 | // resolve ROS messages to primitive strings if possible 510 | std::string payload; 511 | bool found_primitive = primitiveRosMessageToString(ros_msg, payload); 512 | if (found_primitive) { 513 | payload_buffer = std::vector(payload.begin(), payload.end()); 514 | } else { 515 | NODELET_WARN( 516 | "Cannot send ROS message of type '%s' as primitive message, " 517 | "check supported primitive types", 518 | ros_msg_type.name.c_str()); 519 | return; 520 | } 521 | 522 | } else { // publish as complete ROS message incl. ROS message type 523 | 524 | // serialize ROS message type to buffer 525 | uint32_t msg_type_length = 526 | ros::serialization::serializationLength(ros_msg_type); 527 | std::vector msg_type_buffer; 528 | msg_type_buffer.resize(msg_type_length); 529 | ros::serialization::OStream msg_type_stream(msg_type_buffer.data(), 530 | msg_type_length); 531 | ros::serialization::serialize(msg_type_stream, ros_msg_type); 532 | 533 | // send ROS message type information to MQTT broker 534 | mqtt_topic = kRosMsgTypeMqttTopicPrefix + ros2mqtt.mqtt.topic; 535 | try { 536 | NODELET_DEBUG("Sending ROS message type to MQTT broker on topic '%s' ...", 537 | mqtt_topic.c_str()); 538 | mqtt::message_ptr mqtt_msg = 539 | mqtt::make_message(mqtt_topic, msg_type_buffer.data(), 540 | msg_type_buffer.size(), ros2mqtt.mqtt.qos, true); 541 | client_->publish(mqtt_msg); 542 | } catch (const mqtt::exception& e) { 543 | NODELET_WARN( 544 | "Publishing ROS message type information to MQTT topic '%s' failed: %s", 545 | mqtt_topic.c_str(), e.what()); 546 | } 547 | 548 | // build MQTT payload for ROS message (R) as [R] 549 | // or [S, R] if timestamp (S) is included 550 | uint32_t msg_length = ros::serialization::serializationLength(*ros_msg); 551 | uint32_t payload_length = msg_length; 552 | uint32_t stamp_length = 553 | ros::serialization::serializationLength(ros::Time()); 554 | uint32_t msg_offset = 0; 555 | if (ros2mqtt.stamped) { 556 | // allocate buffer with appropriate size to hold [S, R] 557 | msg_offset += stamp_length; 558 | payload_length += stamp_length; 559 | payload_buffer.resize(payload_length); 560 | } else { 561 | // allocate buffer with appropriate size to hold [R] 562 | payload_buffer.resize(payload_length); 563 | } 564 | 565 | // serialize ROS message to payload [-, R] 566 | ros::serialization::OStream msg_stream(&payload_buffer[msg_offset], 567 | msg_length); 568 | ros::serialization::serialize(msg_stream, *ros_msg); 569 | 570 | // inject timestamp as final step 571 | if (ros2mqtt.stamped) { 572 | 573 | // take current timestamp 574 | ros::WallTime stamp_wall = ros::WallTime::now(); 575 | ros::Time stamp(stamp_wall.sec, stamp_wall.nsec); 576 | if (stamp.isZero()) 577 | NODELET_WARN( 578 | "Injected ROS time 0 into MQTT payload on topic %s, is a ROS clock " 579 | "running?", 580 | ros2mqtt.mqtt.topic.c_str()); 581 | 582 | // serialize timestamp to payload [S, R] 583 | ros::serialization::OStream stamp_stream(&payload_buffer[0], 584 | stamp_length); 585 | ros::serialization::serialize(stamp_stream, stamp); 586 | } 587 | } 588 | 589 | // send ROS message to MQTT broker 590 | mqtt_topic = ros2mqtt.mqtt.topic; 591 | try { 592 | NODELET_DEBUG( 593 | "Sending ROS message of type '%s' to MQTT broker on topic '%s' ...", 594 | ros_msg->getDataType().c_str(), mqtt_topic.c_str()); 595 | mqtt::message_ptr mqtt_msg = mqtt::make_message( 596 | mqtt_topic, payload_buffer.data(), payload_buffer.size(), 597 | ros2mqtt.mqtt.qos, ros2mqtt.mqtt.retained); 598 | client_->publish(mqtt_msg); 599 | } catch (const mqtt::exception& e) { 600 | NODELET_WARN( 601 | "Publishing ROS message type information to MQTT topic '%s' failed: %s", 602 | mqtt_topic.c_str(), e.what()); 603 | } 604 | } 605 | 606 | 607 | void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg, 608 | const ros::WallTime& arrival_stamp) { 609 | 610 | std::string mqtt_topic = mqtt_msg->get_topic(); 611 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 612 | auto& payload = mqtt_msg->get_payload_ref(); 613 | uint32_t payload_length = static_cast(payload.size()); 614 | 615 | // read MQTT payload for ROS message (R) as [R] 616 | // or [S, R] if timestamp (S) is included 617 | uint32_t msg_length = payload_length; 618 | uint32_t msg_offset = 0; 619 | 620 | // if stamped, compute latency 621 | if (mqtt2ros.stamped) { 622 | 623 | // create ROS message buffer on top of MQTT message payload 624 | char* non_const_payload = const_cast(&payload[msg_offset]); 625 | uint8_t* stamp_buffer = reinterpret_cast(non_const_payload); 626 | 627 | // deserialize stamp 628 | ros::Time stamp; 629 | uint32_t stamp_length = ros::serialization::serializationLength(stamp); 630 | ros::serialization::IStream stamp_stream(stamp_buffer, stamp_length); 631 | ros::serialization::deserialize(stamp_stream, stamp); 632 | 633 | // compute ROS2MQTT latency 634 | ros::Time now(arrival_stamp.sec, arrival_stamp.nsec); 635 | if (now.isZero()) 636 | NODELET_WARN( 637 | "Cannot compute latency for MQTT topic %s when ROS time is 0, is a ROS " 638 | "clock running?", 639 | mqtt_topic.c_str()); 640 | ros::Duration latency = now - stamp; 641 | std_msgs::Float64 latency_msg; 642 | latency_msg.data = latency.toSec(); 643 | 644 | // publish latency 645 | if (mqtt2ros.ros.latency_publisher.getTopic().empty()) { 646 | std::string latency_topic = kLatencyRosTopicPrefix + mqtt2ros.ros.topic; 647 | mqtt2ros.ros.latency_publisher = 648 | private_node_handle_.advertise(latency_topic, 1); 649 | } 650 | mqtt2ros.ros.latency_publisher.publish(latency_msg); 651 | 652 | msg_length -= stamp_length; 653 | msg_offset += stamp_length; 654 | } 655 | 656 | // create ROS message buffer on top of MQTT message payload 657 | char* non_const_payload = const_cast(&payload[msg_offset]); 658 | uint8_t* msg_buffer = reinterpret_cast(non_const_payload); 659 | ros::serialization::OStream msg_stream(msg_buffer, msg_length); 660 | 661 | // publish via ShapeShifter 662 | NODELET_DEBUG( 663 | "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", 664 | mqtt2ros.ros.shape_shifter.getDataType().c_str(), 665 | mqtt2ros.ros.topic.c_str()); 666 | mqtt2ros.ros.shape_shifter.read(msg_stream); 667 | mqtt2ros.ros.publisher.publish(mqtt2ros.ros.shape_shifter); 668 | } 669 | 670 | 671 | void MqttClient::mqtt2primitive(mqtt::const_message_ptr mqtt_msg) { 672 | 673 | std::string mqtt_topic = mqtt_msg->get_topic(); 674 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 675 | const std::string str_msg = mqtt_msg->to_string(); 676 | 677 | bool found_primitive = false; 678 | std::string msg_type_md5; 679 | std::string msg_type_name; 680 | std::string msg_type_definition; 681 | std::vector msg_buffer; 682 | 683 | // check for bool 684 | if (!found_primitive) { 685 | std::string bool_str = str_msg; 686 | std::transform(str_msg.cbegin(), str_msg.cend(), bool_str.begin(), 687 | ::tolower); 688 | if (bool_str == "true" || bool_str == "false") { 689 | 690 | bool bool_msg = (bool_str == "true"); 691 | 692 | // construct and serialize ROS message 693 | std_msgs::Bool msg; 694 | msg.data = bool_msg; 695 | serializeRosMessage(msg, msg_buffer); 696 | 697 | // collect ROS message type information 698 | msg_type_md5 = ros::message_traits::MD5Sum::value(); 699 | msg_type_name = ros::message_traits::DataType::value(); 700 | msg_type_definition = 701 | ros::message_traits::Definition::value(); 702 | 703 | found_primitive = true; 704 | } 705 | } 706 | 707 | // check for int 708 | if (!found_primitive) { 709 | std::size_t pos; 710 | try { 711 | const int int_msg = std::stoi(str_msg, &pos); 712 | if (pos == str_msg.size()) { 713 | 714 | // construct and serialize ROS message 715 | std_msgs::Int32 msg; 716 | msg.data = int_msg; 717 | serializeRosMessage(msg, msg_buffer); 718 | 719 | // collect ROS message type information 720 | msg_type_md5 = ros::message_traits::MD5Sum::value(); 721 | msg_type_name = ros::message_traits::DataType::value(); 722 | msg_type_definition = 723 | ros::message_traits::Definition::value(); 724 | 725 | found_primitive = true; 726 | } 727 | } catch (const std::invalid_argument& ex) { 728 | } catch (const std::out_of_range& ex) { 729 | } 730 | } 731 | 732 | // check for float 733 | if (!found_primitive) { 734 | std::size_t pos; 735 | try { 736 | const float float_msg = std::stof(str_msg, &pos); 737 | if (pos == str_msg.size()) { 738 | 739 | // construct and serialize ROS message 740 | std_msgs::Float32 msg; 741 | msg.data = float_msg; 742 | serializeRosMessage(msg, msg_buffer); 743 | 744 | // collect ROS message type information 745 | msg_type_md5 = ros::message_traits::MD5Sum::value(); 746 | msg_type_name = 747 | ros::message_traits::DataType::value(); 748 | msg_type_definition = 749 | ros::message_traits::Definition::value(); 750 | 751 | found_primitive = true; 752 | } 753 | } catch (const std::invalid_argument& ex) { 754 | } catch (const std::out_of_range& ex) { 755 | } 756 | } 757 | 758 | // fall back to string 759 | if (!found_primitive) { 760 | 761 | // construct and serialize ROS message 762 | std_msgs::String msg; 763 | msg.data = str_msg; 764 | serializeRosMessage(msg, msg_buffer); 765 | 766 | // collect ROS message type information 767 | msg_type_md5 = ros::message_traits::MD5Sum::value(); 768 | msg_type_name = ros::message_traits::DataType::value(); 769 | msg_type_definition = 770 | ros::message_traits::Definition::value(); 771 | } 772 | 773 | // if ROS message type has changed 774 | if (msg_type_md5 != mqtt2ros.ros.shape_shifter.getMD5Sum()) { 775 | 776 | // configure ShapeShifter 777 | mqtt2ros.ros.shape_shifter.morph(msg_type_md5, msg_type_name, 778 | msg_type_definition, ""); 779 | 780 | // advertise with ROS publisher 781 | mqtt2ros.ros.publisher.shutdown(); 782 | mqtt2ros.ros.publisher = mqtt2ros.ros.shape_shifter.advertise( 783 | node_handle_, mqtt2ros.ros.topic, mqtt2ros.ros.queue_size, 784 | mqtt2ros.ros.latched); 785 | 786 | NODELET_INFO("ROS publisher message type on topic '%s' set to '%s'", 787 | mqtt2ros.ros.topic.c_str(), msg_type_name.c_str()); 788 | } 789 | 790 | // publish via ShapeShifter 791 | ros::serialization::OStream msg_stream(msg_buffer.data(), msg_buffer.size()); 792 | NODELET_DEBUG( 793 | "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", 794 | mqtt2ros.ros.shape_shifter.getDataType().c_str(), 795 | mqtt2ros.ros.topic.c_str()); 796 | mqtt2ros.ros.shape_shifter.read(msg_stream); 797 | mqtt2ros.ros.publisher.publish(mqtt2ros.ros.shape_shifter); 798 | } 799 | 800 | 801 | void MqttClient::connected(const std::string& cause) { 802 | 803 | is_connected_ = true; 804 | std::string as_client = 805 | client_config_.id.empty() 806 | ? "" 807 | : std::string(" as '") + client_config_.id + std::string("'"); 808 | NODELET_INFO("Connected to broker at '%s'%s", 809 | client_->get_server_uri().c_str(), as_client.c_str()); 810 | 811 | // subscribe MQTT topics 812 | for (auto& mqtt2ros_p : mqtt2ros_) { 813 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_p.second; 814 | std::string mqtt_topic = mqtt2ros_p.first; 815 | if (!mqtt2ros.primitive) // subscribe topics for ROS message types first 816 | mqtt_topic = kRosMsgTypeMqttTopicPrefix + mqtt_topic; 817 | client_->subscribe(mqtt_topic, mqtt2ros.mqtt.qos); 818 | NODELET_DEBUG("Subscribed MQTT topic '%s'", mqtt_topic.c_str()); 819 | } 820 | } 821 | 822 | 823 | void MqttClient::connection_lost(const std::string& cause) { 824 | 825 | NODELET_ERROR("Connection to broker lost, will try to reconnect..."); 826 | is_connected_ = false; 827 | connect(); 828 | } 829 | 830 | 831 | bool MqttClient::isConnected() { 832 | 833 | return is_connected_; 834 | } 835 | 836 | 837 | bool MqttClient::isConnectedService( 838 | mqtt_client_interfaces::IsConnected::Request& request, 839 | mqtt_client_interfaces::IsConnected::Response& response) { 840 | 841 | response.connected = isConnected(); 842 | return true; 843 | } 844 | 845 | 846 | void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) { 847 | 848 | // instantly take arrival timestamp 849 | ros::WallTime arrival_stamp = ros::WallTime::now(); 850 | 851 | std::string mqtt_topic = mqtt_msg->get_topic(); 852 | NODELET_DEBUG("Received MQTT message on topic '%s'", mqtt_topic.c_str()); 853 | 854 | // publish directly if primitive 855 | if (mqtt2ros_.count(mqtt_topic) > 0) { 856 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 857 | if (mqtt2ros.primitive) { 858 | mqtt2primitive(mqtt_msg); 859 | return; 860 | } 861 | } 862 | 863 | // else first check for ROS message type 864 | bool msg_contains_ros_msg_type = 865 | mqtt_topic.find(kRosMsgTypeMqttTopicPrefix) != std::string::npos; 866 | if (msg_contains_ros_msg_type) { 867 | 868 | // create ROS message buffer on top of MQTT message payload 869 | auto& payload = mqtt_msg->get_payload_ref(); 870 | uint32_t payload_length = static_cast(payload.size()); 871 | char* non_const_payload = const_cast(&payload[0]); 872 | uint8_t* msg_type_buffer = reinterpret_cast(non_const_payload); 873 | 874 | // deserialize ROS message type 875 | mqtt_client_interfaces::RosMsgType ros_msg_type; 876 | ros::serialization::IStream msg_type_stream(msg_type_buffer, 877 | payload_length); 878 | try { 879 | ros::serialization::deserialize(msg_type_stream, ros_msg_type); 880 | } catch (ros::serialization::StreamOverrunException) { 881 | NODELET_ERROR( 882 | "Failed to deserialize ROS message type from MQTT message received on " 883 | "topic '%s', got message:\n%s", 884 | mqtt_topic.c_str(), mqtt_msg->to_string().c_str()); 885 | return; 886 | } 887 | 888 | // reconstruct corresponding MQTT data topic 889 | std::string mqtt_data_topic = mqtt_topic; 890 | mqtt_data_topic.erase(mqtt_data_topic.find(kRosMsgTypeMqttTopicPrefix), 891 | kRosMsgTypeMqttTopicPrefix.length()); 892 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_data_topic]; 893 | 894 | // if ROS message type has changed 895 | if (ros_msg_type.md5 != mqtt2ros.ros.shape_shifter.getMD5Sum()) { 896 | 897 | mqtt2ros.stamped = ros_msg_type.stamped; 898 | NODELET_INFO("ROS publisher message type on topic '%s' set to '%s'", 899 | mqtt2ros.ros.topic.c_str(), ros_msg_type.name.c_str()); 900 | 901 | // configure ShapeShifter 902 | mqtt2ros.ros.shape_shifter.morph(ros_msg_type.md5, ros_msg_type.name, 903 | ros_msg_type.definition, ""); 904 | 905 | // advertise with ROS publisher 906 | mqtt2ros.ros.publisher.shutdown(); 907 | mqtt2ros.ros.publisher = mqtt2ros.ros.shape_shifter.advertise( 908 | node_handle_, mqtt2ros.ros.topic, mqtt2ros.ros.queue_size, 909 | mqtt2ros.ros.latched); 910 | 911 | // subscribe to MQTT topic with actual ROS messages 912 | client_->subscribe(mqtt_data_topic, mqtt2ros.mqtt.qos); 913 | NODELET_DEBUG("Subscribed MQTT topic '%s'", mqtt_data_topic.c_str()); 914 | } 915 | 916 | } else { 917 | 918 | // publish ROS message, if publisher initialized 919 | if (!mqtt2ros_[mqtt_topic].ros.publisher.getTopic().empty()) { 920 | mqtt2ros(mqtt_msg, arrival_stamp); 921 | } else { 922 | NODELET_WARN( 923 | "ROS publisher for data from MQTT topic '%s' is not yet initialized: " 924 | "ROS message type not yet known", 925 | mqtt_topic.c_str()); 926 | } 927 | } 928 | } 929 | 930 | 931 | void MqttClient::delivery_complete(mqtt::delivery_token_ptr token) {} 932 | 933 | 934 | void MqttClient::on_success(const mqtt::token& token) { 935 | 936 | is_connected_ = true; 937 | } 938 | 939 | 940 | void MqttClient::on_failure(const mqtt::token& token) { 941 | 942 | ROS_ERROR( 943 | "Connection to broker failed (return code %d), will automatically " 944 | "retry...", 945 | token.get_return_code()); 946 | is_connected_ = false; 947 | } 948 | 949 | } // namespace mqtt_client 950 | -------------------------------------------------------------------------------- /mqtt_client/src/MqttClient.ros2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ============================================================================== 3 | MIT License 4 | 5 | Copyright 2022 Institute for Automotive Engineering of RWTH Aachen University. 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | ============================================================================== 25 | */ 26 | 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | RCLCPP_COMPONENTS_REGISTER_NODE(mqtt_client::MqttClient) 54 | 55 | 56 | namespace mqtt_client { 57 | 58 | 59 | const std::string MqttClient::kRosMsgTypeMqttTopicPrefix = 60 | "mqtt_client/ros_msg_type/"; 61 | 62 | const std::string MqttClient::kLatencyRosTopicPrefix = "~/latencies/"; 63 | 64 | template 65 | T mqtt2float(mqtt::const_message_ptr mqtt_msg) { 66 | auto str_msg = mqtt_msg->to_string (); 67 | std::size_t pos; 68 | const T v = std::stold(str_msg, &pos); 69 | 70 | if (pos != str_msg.size()) 71 | throw std::invalid_argument ("not all charaters processed"); 72 | 73 | return v; 74 | } 75 | 76 | template 77 | T mqtt2int(mqtt::const_message_ptr mqtt_msg) { 78 | auto str_msg = mqtt_msg->to_string (); 79 | std::size_t pos; 80 | const T v = std::stoll(str_msg, &pos); 81 | 82 | if (pos != str_msg.size()) 83 | throw std::invalid_argument ("not all charaters processed"); 84 | 85 | return v; 86 | } 87 | 88 | bool mqtt2bool(mqtt::const_message_ptr mqtt_msg) { 89 | const std::string str_msg = mqtt_msg->to_string (); 90 | std::string bool_str = mqtt_msg->to_string (); 91 | std::transform(str_msg.cbegin(), str_msg.cend(), bool_str.begin(), 92 | ::tolower); 93 | if (bool_str == "true" || bool_str == "1") return true; 94 | if (bool_str == "false" || bool_str == "0") return false; 95 | 96 | throw std::invalid_argument ("unable to decode string"); 97 | } 98 | 99 | bool fixedMqtt2PrimitiveRos(mqtt::const_message_ptr mqtt_msg, 100 | const std::string& msg_type, 101 | rclcpp::SerializedMessage &serialized_msg) { 102 | try { 103 | if (msg_type == "std_msgs/msg/String") { 104 | std_msgs::msg::String msg; 105 | msg.data = mqtt_msg->to_string(); 106 | 107 | serializeRosMessage(msg, serialized_msg); 108 | } else if (msg_type == "std_msgs/msg/Bool") { 109 | std_msgs::msg::Bool msg; 110 | msg.data = mqtt2bool(mqtt_msg); 111 | 112 | serializeRosMessage(msg, serialized_msg); 113 | } else if (msg_type == "std_msgs/msg/Char") { 114 | std_msgs::msg::Char msg; 115 | msg.data = mqtt2int(mqtt_msg); 116 | 117 | serializeRosMessage(msg, serialized_msg); 118 | } else if (msg_type == "std_msgs/msg/UInt8") { 119 | std_msgs::msg::UInt8 msg; 120 | msg.data = mqtt2int(mqtt_msg); 121 | 122 | serializeRosMessage(msg, serialized_msg); 123 | } else if (msg_type == "std_msgs/msg/UInt16") { 124 | std_msgs::msg::UInt16 msg; 125 | msg.data = mqtt2int(mqtt_msg); 126 | 127 | serializeRosMessage(msg, serialized_msg); 128 | } else if (msg_type == "std_msgs/msg/UInt32") { 129 | std_msgs::msg::UInt32 msg; 130 | msg.data = mqtt2int(mqtt_msg); 131 | 132 | serializeRosMessage(msg, serialized_msg); 133 | } else if (msg_type == "std_msgs/msg/UInt64") { 134 | std_msgs::msg::UInt64 msg; 135 | msg.data = mqtt2int(mqtt_msg); 136 | 137 | serializeRosMessage(msg, serialized_msg); 138 | } else if (msg_type == "std_msgs/msg/Int8") { 139 | std_msgs::msg::Int8 msg; 140 | msg.data = mqtt2int(mqtt_msg); 141 | 142 | serializeRosMessage(msg, serialized_msg); 143 | } else if (msg_type == "std_msgs/msg/Int16") { 144 | std_msgs::msg::Int16 msg; 145 | msg.data = mqtt2int(mqtt_msg); 146 | 147 | serializeRosMessage(msg, serialized_msg); 148 | } else if (msg_type == "std_msgs/msg/Int32") { 149 | std_msgs::msg::Int32 msg; 150 | msg.data = mqtt2int(mqtt_msg); 151 | 152 | serializeRosMessage(msg, serialized_msg); 153 | } else if (msg_type == "std_msgs/msg/Int64") { 154 | std_msgs::msg::Int32 msg; 155 | msg.data = mqtt2int(mqtt_msg); 156 | 157 | serializeRosMessage(msg, serialized_msg); 158 | } else if (msg_type == "std_msgs/msg/Float32") { 159 | std_msgs::msg::Float32 msg; 160 | msg.data = mqtt2float(mqtt_msg); 161 | 162 | serializeRosMessage(msg, serialized_msg); 163 | } else if (msg_type == "std_msgs/msg/Float64") { 164 | std_msgs::msg::Float64 msg; 165 | msg.data = mqtt2float(mqtt_msg); 166 | 167 | serializeRosMessage(msg, serialized_msg); 168 | } else { 169 | throw std::domain_error("Unhandled message type (" + msg_type + ")"); 170 | } 171 | 172 | return true; 173 | 174 | } catch (const std::exception &) { 175 | return false; 176 | } 177 | 178 | 179 | } 180 | 181 | /** 182 | * @brief Extracts string of primitive data types from ROS message. 183 | * 184 | * This is helpful to extract the actual data payload of a primitive ROS 185 | * message. If e.g. an std_msgs/msg/String is serialized to a string 186 | * representation, it also contains the field name 'data'. This function 187 | * instead returns the underlying value as string. 188 | * 189 | * @param [in] serialized_msg generic serialized ROS message 190 | * @param [in] msg_type ROS message type, e.g. `std_msgs/msg/String` 191 | * @param [out] primitive string representation of primitive message data 192 | * 193 | * @return true if primitive ROS message type was found 194 | * @return false if ROS message type is not primitive 195 | */ 196 | bool primitiveRosMessageToString( 197 | const std::shared_ptr& serialized_msg, 198 | const std::string& msg_type, std::string& primitive) { 199 | 200 | bool found_primitive = true; 201 | 202 | if (msg_type == "std_msgs/msg/String") { 203 | std_msgs::msg::String msg; 204 | deserializeRosMessage(*serialized_msg, msg); 205 | primitive = msg.data; 206 | } else if (msg_type == "std_msgs/msg/Bool") { 207 | std_msgs::msg::Bool msg; 208 | deserializeRosMessage(*serialized_msg, msg); 209 | primitive = msg.data ? "true" : "false"; 210 | } else if (msg_type == "std_msgs/msg/Char") { 211 | std_msgs::msg::Char msg; 212 | deserializeRosMessage(*serialized_msg, msg); 213 | primitive = std::to_string(msg.data); 214 | } else if (msg_type == "std_msgs/msg/UInt8") { 215 | std_msgs::msg::UInt8 msg; 216 | deserializeRosMessage(*serialized_msg, msg); 217 | primitive = std::to_string(msg.data); 218 | } else if (msg_type == "std_msgs/msg/UInt16") { 219 | std_msgs::msg::UInt16 msg; 220 | deserializeRosMessage(*serialized_msg, msg); 221 | primitive = std::to_string(msg.data); 222 | } else if (msg_type == "std_msgs/msg/UInt32") { 223 | std_msgs::msg::UInt32 msg; 224 | deserializeRosMessage(*serialized_msg, msg); 225 | primitive = std::to_string(msg.data); 226 | } else if (msg_type == "std_msgs/msg/UInt64") { 227 | std_msgs::msg::UInt64 msg; 228 | deserializeRosMessage(*serialized_msg, msg); 229 | primitive = std::to_string(msg.data); 230 | } else if (msg_type == "std_msgs/msg/Int8") { 231 | std_msgs::msg::Int8 msg; 232 | deserializeRosMessage(*serialized_msg, msg); 233 | primitive = std::to_string(msg.data); 234 | } else if (msg_type == "std_msgs/msg/Int16") { 235 | std_msgs::msg::Int16 msg; 236 | deserializeRosMessage(*serialized_msg, msg); 237 | primitive = std::to_string(msg.data); 238 | } else if (msg_type == "std_msgs/msg/Int32") { 239 | std_msgs::msg::Int32 msg; 240 | deserializeRosMessage(*serialized_msg, msg); 241 | primitive = std::to_string(msg.data); 242 | } else if (msg_type == "std_msgs/msg/Int64") { 243 | std_msgs::msg::Int64 msg; 244 | deserializeRosMessage(*serialized_msg, msg); 245 | primitive = std::to_string(msg.data); 246 | } else if (msg_type == "std_msgs/msg/Float32") { 247 | std_msgs::msg::Float32 msg; 248 | deserializeRosMessage(*serialized_msg, msg); 249 | primitive = std::to_string(msg.data); 250 | } else if (msg_type == "std_msgs/msg/Float64") { 251 | std_msgs::msg::Float64 msg; 252 | deserializeRosMessage(*serialized_msg, msg); 253 | primitive = std::to_string(msg.data); 254 | } else { 255 | found_primitive = false; 256 | } 257 | 258 | return found_primitive; 259 | } 260 | 261 | 262 | MqttClient::MqttClient(const rclcpp::NodeOptions& options) : Node("mqtt_client", options) { 263 | 264 | loadParameters(); 265 | setup(); 266 | } 267 | 268 | 269 | void MqttClient::loadParameters() { 270 | 271 | rcl_interfaces::msg::ParameterDescriptor param_desc; 272 | 273 | param_desc.description = "IP address or hostname of the machine running the MQTT broker"; 274 | declare_parameter("broker.host", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 275 | param_desc.description = "port the MQTT broker is listening on"; 276 | declare_parameter("broker.port", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); 277 | param_desc.description = "username used for authenticating to the broker (if empty, will try to connect anonymously)"; 278 | declare_parameter("broker.user", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 279 | param_desc.description = "password used for authenticating to the broker"; 280 | declare_parameter("broker.pass", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 281 | param_desc.description = "whether to connect via SSL/TLS"; 282 | declare_parameter("broker.tls.enabled", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); 283 | param_desc.description = "CA certificate file trusted by client (relative to ROS_HOME)"; 284 | declare_parameter("broker.tls.ca_certificate", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 285 | 286 | param_desc.description = "unique ID used to identify the client (broker may allow empty ID and automatically generate one)"; 287 | declare_parameter("client.id", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 288 | param_desc.description = "maximum number of messages buffered by the bridge when not connected to broker (only available if client ID is not empty)"; 289 | declare_parameter("client.buffer.size", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); 290 | param_desc.description = "directory used to buffer messages when not connected to broker (relative to ROS_HOME)"; 291 | declare_parameter("client.buffer.directory", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 292 | param_desc.description = "topic used for this client's last-will message (no last will, if not specified)"; 293 | declare_parameter("client.last_will.topic", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 294 | param_desc.description = "last-will message"; 295 | declare_parameter("client.last_will.message", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 296 | param_desc.description = "QoS value for last-will message"; 297 | declare_parameter("client.last_will.qos", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); 298 | param_desc.description = "whether to retain last-will message"; 299 | declare_parameter("client.last_will.retained", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); 300 | param_desc.description = "whether to use a clean session for this client"; 301 | declare_parameter("client.clean_session", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); 302 | param_desc.description = "keep-alive interval in seconds"; 303 | declare_parameter("client.keep_alive_interval", rclcpp::ParameterType::PARAMETER_DOUBLE, param_desc); 304 | param_desc.description = "maximum number of inflight messages"; 305 | declare_parameter("client.max_inflight", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); 306 | param_desc.description = "client certificate file (only needed if broker requires client certificates; relative to ROS_HOME)"; 307 | declare_parameter("client.tls.certificate", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 308 | param_desc.description = "client private key file (relative to ROS_HOME)"; 309 | declare_parameter("client.tls.key", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 310 | param_desc.description = "client private key password"; 311 | declare_parameter("client.tls.password", rclcpp::ParameterType::PARAMETER_STRING, param_desc); 312 | 313 | param_desc.description = "The list of topics to bridge from ROS to MQTT"; 314 | const auto ros2mqtt_ros_topics = declare_parameter>("bridge.ros2mqtt.ros_topics", std::vector(), param_desc); 315 | for (const auto& ros_topic : ros2mqtt_ros_topics) 316 | { 317 | param_desc.description = "MQTT topic on which the corresponding ROS messages are sent to the broker"; 318 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.mqtt_topic", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); 319 | param_desc.description = "whether to publish as primitive message"; 320 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.primitive", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); 321 | param_desc.description = "If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the publisher"; 322 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.ros_type", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); 323 | param_desc.description = "whether to attach a timestamp to a ROS2MQTT payload (for latency computation on receiver side)"; 324 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.inject_timestamp", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); 325 | param_desc.description = "ROS subscriber queue size"; 326 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.queue_size", ros_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); 327 | param_desc.description = "ROS subscriber QoS reliability"; 328 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.reliability", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); 329 | param_desc.description = "ROS subscriber QoS durability"; 330 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.durability", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); 331 | param_desc.description = "MQTT QoS value"; 332 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.qos", ros_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); 333 | param_desc.description = "whether to retain MQTT message"; 334 | declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.retained", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); 335 | } 336 | 337 | param_desc.description = "The list of topics to bridge from MQTT to ROS"; 338 | const auto mqtt2ros_mqtt_topics = declare_parameter>("bridge.mqtt2ros.mqtt_topics", std::vector(), param_desc); 339 | for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) 340 | { 341 | param_desc.description = "ROS topic on which corresponding MQTT messages are published"; 342 | declare_parameter(fmt::format("bridge.mqtt2ros.{}.ros_topic", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); 343 | param_desc.description = "whether to publish as primitive message (if coming from non-ROS MQTT client)"; 344 | declare_parameter(fmt::format("bridge.mqtt2ros.{}.primitive", mqtt_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); 345 | param_desc.description = "If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the MQTT message"; 346 | declare_parameter(fmt::format("bridge.mqtt2ros.{}.ros_type", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); 347 | param_desc.description = "MQTT QoS value"; 348 | declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); 349 | param_desc.description = "ROS publisher queue size"; 350 | declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); 351 | param_desc.description = "ROS publisher QoS durability"; 352 | declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.durability", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); 353 | param_desc.description = "ROS publisher QoS reliability"; 354 | declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.reliability", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); 355 | param_desc.description = "whether to latch ROS message"; 356 | declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.latched", mqtt_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); 357 | } 358 | 359 | // load broker parameters from parameter server 360 | std::string broker_tls_ca_certificate; 361 | loadParameter("broker.host", broker_config_.host, "localhost"); 362 | loadParameter("broker.port", broker_config_.port, 1883); 363 | if (loadParameter("broker.user", broker_config_.user)) { 364 | loadParameter("broker.pass", broker_config_.pass, ""); 365 | } 366 | if (loadParameter("broker.tls.enabled", broker_config_.tls.enabled, false)) { 367 | loadParameter("broker.tls.ca_certificate", broker_tls_ca_certificate, 368 | "/etc/ssl/certs/ca-certificates.crt"); 369 | } 370 | 371 | // load client parameters from parameter server 372 | std::string client_buffer_directory, client_tls_certificate, client_tls_key; 373 | loadParameter("client.id", client_config_.id, ""); 374 | client_config_.buffer.enabled = !client_config_.id.empty(); 375 | if (client_config_.buffer.enabled) { 376 | loadParameter("client.buffer.size", client_config_.buffer.size, 0); 377 | loadParameter("client.buffer.directory", client_buffer_directory, "buffer"); 378 | } else { 379 | RCLCPP_WARN(get_logger(), 380 | "Client buffer can not be enabled when client ID is empty"); 381 | } 382 | if (loadParameter("client.last_will.topic", client_config_.last_will.topic)) { 383 | loadParameter("client.last_will.message", client_config_.last_will.message, 384 | "offline"); 385 | loadParameter("client.last_will.qos", client_config_.last_will.qos, 0); 386 | loadParameter("client.last_will.retained", 387 | client_config_.last_will.retained, false); 388 | } 389 | loadParameter("client.clean_session", client_config_.clean_session, true); 390 | loadParameter("client.keep_alive_interval", 391 | client_config_.keep_alive_interval, 60.0); 392 | loadParameter("client.max_inflight", client_config_.max_inflight, 65535); 393 | if (broker_config_.tls.enabled) { 394 | if (loadParameter("client.tls.certificate", client_tls_certificate)) { 395 | loadParameter("client.tls.key", client_tls_key); 396 | loadParameter("client.tls.password", client_config_.tls.password); 397 | loadParameter("client.tls.version", client_config_.tls.version); 398 | loadParameter("client.tls.verify", client_config_.tls.verify); 399 | loadParameter("client.tls.alpn_protos", client_config_.tls.alpn_protos); 400 | } 401 | } 402 | 403 | // resolve filepaths 404 | broker_config_.tls.ca_certificate = resolvePath(broker_tls_ca_certificate); 405 | client_config_.buffer.directory = resolvePath(client_buffer_directory); 406 | client_config_.tls.certificate = resolvePath(client_tls_certificate); 407 | client_config_.tls.key = resolvePath(client_tls_key); 408 | 409 | // parse bridge parameters 410 | 411 | // ros2mqtt 412 | for (const auto& ros_topic : ros2mqtt_ros_topics) { 413 | 414 | rclcpp::Parameter mqtt_topic_param; 415 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.mqtt_topic", ros_topic), mqtt_topic_param)) { 416 | 417 | // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic 418 | const std::string mqtt_topic = mqtt_topic_param.as_string(); 419 | Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; 420 | ros2mqtt.mqtt.topic = mqtt_topic; 421 | 422 | // ros2mqtt[k]/primitive 423 | rclcpp::Parameter primitive_param; 424 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.primitive", ros_topic), primitive_param)) 425 | ros2mqtt.primitive = primitive_param.as_bool(); 426 | 427 | // ros2mqtt[k]/ros_type 428 | rclcpp::Parameter ros_type_param; 429 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.ros_type", ros_topic), ros_type_param)) { 430 | ros2mqtt.ros.msg_type = ros_type_param.as_string(); 431 | ros2mqtt.fixed_type = true; 432 | RCLCPP_DEBUG(get_logger(), "Using explicit ROS message type '%s'", ros2mqtt.ros.msg_type.c_str()); 433 | } 434 | 435 | // ros2mqtt[k]/inject_timestamp 436 | rclcpp::Parameter stamped_param; 437 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.inject_timestamp", ros_topic), stamped_param)) 438 | ros2mqtt.stamped = stamped_param.as_bool(); 439 | if (ros2mqtt.stamped && ros2mqtt.primitive) { 440 | RCLCPP_WARN( 441 | get_logger(), 442 | "Timestamp will not be injected into primitive messages on ROS " 443 | "topic '%s'", 444 | ros_topic.c_str()); 445 | ros2mqtt.stamped = false; 446 | } 447 | 448 | // ros2mqtt[k]/advanced/ros/queue_size 449 | rclcpp::Parameter queue_size_param; 450 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.queue_size", ros_topic), 451 | queue_size_param)) 452 | ros2mqtt.ros.queue_size = queue_size_param.as_int(); 453 | 454 | rclcpp::Parameter durability_param; 455 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.durability", ros_topic), durability_param)) { 456 | const auto p = durability_param.as_string(); 457 | if (p == "system_default") { 458 | ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::SystemDefault; 459 | } else if (p == "volatile") { 460 | ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::Volatile; 461 | } else if (p == "transient_local") { 462 | ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::TransientLocal; 463 | } else if (p == "auto") { 464 | ros2mqtt.ros.qos.durability = {}; 465 | } else { 466 | RCLCPP_ERROR(get_logger(), "Unexpected durability %s", p.c_str ()); 467 | exit(EXIT_FAILURE); 468 | } 469 | } 470 | 471 | rclcpp::Parameter reliability_param; 472 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.reliability", ros_topic), reliability_param)) { 473 | const auto p = reliability_param.as_string(); 474 | if (p == "system_default") { 475 | ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::SystemDefault; 476 | } else if (p == "best_effort") { 477 | ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::BestEffort; 478 | } else if (p == "reliable") { 479 | ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::Reliable; 480 | } else if (p == "auto") { 481 | ros2mqtt.ros.qos.reliability = {}; 482 | } else { 483 | RCLCPP_ERROR(get_logger(), "Unexpected reliability %s", p.c_str ()); 484 | exit(EXIT_FAILURE); 485 | } 486 | } 487 | 488 | // ros2mqtt[k]/advanced/mqtt/qos 489 | rclcpp::Parameter qos_param; 490 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.qos", ros_topic), qos_param)) 491 | ros2mqtt.mqtt.qos = qos_param.as_int(); 492 | 493 | // ros2mqtt[k]/advanced/mqtt/retained 494 | rclcpp::Parameter retained_param; 495 | if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.retained", ros_topic), retained_param)) 496 | ros2mqtt.mqtt.retained = retained_param.as_bool(); 497 | 498 | RCLCPP_INFO(get_logger(), "Bridging %sROS topic '%s' to MQTT topic '%s' %s", 499 | ros2mqtt.primitive ? "primitive " : "", ros_topic.c_str(), 500 | ros2mqtt.mqtt.topic.c_str(), 501 | ros2mqtt.stamped ? "and measuring latency" : ""); 502 | } else { 503 | RCLCPP_WARN(get_logger(), 504 | fmt::format("Parameter 'bridge.ros2mqtt.{}' is missing subparameter " 505 | "'mqtt_topic', will be ignored", ros_topic).c_str()); 506 | } 507 | } 508 | 509 | // mqtt2ros 510 | for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { 511 | 512 | rclcpp::Parameter ros_topic_param; 513 | if (get_parameter(fmt::format("bridge.mqtt2ros.{}.ros_topic", mqtt_topic), ros_topic_param)) { 514 | 515 | // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic 516 | const std::string ros_topic = ros_topic_param.as_string(); 517 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 518 | mqtt2ros.ros.topic = ros_topic; 519 | 520 | // mqtt2ros[k]/primitive 521 | rclcpp::Parameter primitive_param; 522 | if (get_parameter(fmt::format("bridge.mqtt2ros.{}.primitive", mqtt_topic), primitive_param)) 523 | mqtt2ros.primitive = primitive_param.as_bool(); 524 | 525 | 526 | rclcpp::Parameter ros_type_param; 527 | if (get_parameter(fmt::format("bridge.mqtt2ros.{}.ros_type", mqtt_topic), ros_type_param)) { 528 | mqtt2ros.ros.msg_type = ros_type_param.as_string(); 529 | mqtt2ros.fixed_type = true; 530 | RCLCPP_DEBUG(get_logger(), "Using explicit ROS message type '%s' for '%s'", mqtt2ros.ros.msg_type.c_str(), ros_topic.c_str()); 531 | } 532 | 533 | // mqtt2ros[k]/advanced/mqtt/qos 534 | rclcpp::Parameter qos_param; 535 | if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), qos_param)) 536 | mqtt2ros.mqtt.qos = qos_param.as_int(); 537 | 538 | // mqtt2ros[k]/advanced/ros/queue_size 539 | rclcpp::Parameter queue_size_param; 540 | if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), 541 | queue_size_param)) 542 | mqtt2ros.ros.queue_size = queue_size_param.as_int(); 543 | 544 | rclcpp::Parameter durability_param; 545 | if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.durability", mqtt_topic), durability_param)) { 546 | const auto p = durability_param.as_string(); 547 | if (p == "system_default") { 548 | mqtt2ros.ros.qos.durability = rclcpp::DurabilityPolicy::SystemDefault; 549 | } else if (p == "volatile") { 550 | mqtt2ros.ros.qos.durability = rclcpp::DurabilityPolicy::Volatile; 551 | } else if (p == "transient_local") { 552 | mqtt2ros.ros.qos.durability = 553 | rclcpp::DurabilityPolicy::TransientLocal; 554 | } else { 555 | RCLCPP_ERROR(get_logger(), "Unexpected durability %s", p.c_str ()); 556 | exit(EXIT_FAILURE); 557 | } 558 | } 559 | 560 | rclcpp::Parameter reliability_param; 561 | if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.reliability", mqtt_topic), reliability_param)) { 562 | const auto p = reliability_param.as_string(); 563 | if (p == "system_default") { 564 | mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::SystemDefault; 565 | } else if (p == "best_effort") { 566 | mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::BestEffort; 567 | } else if (p == "reliable") { 568 | mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::Reliable; 569 | } else { 570 | RCLCPP_ERROR(get_logger(), "Unexpected reliability %s", p.c_str ()); 571 | exit(EXIT_FAILURE); 572 | } 573 | } 574 | 575 | // mqtt2ros[k]/advanced/ros/latched 576 | rclcpp::Parameter latched_param; 577 | if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.latched", mqtt_topic), latched_param)) { 578 | mqtt2ros.ros.latched = latched_param.as_bool(); 579 | RCLCPP_WARN(get_logger(), 580 | fmt::format("Parameter 'bridge.mqtt2ros.{}.advanced.ros.latched' is ignored " 581 | "since ROS 2 does not easily support latched topics.", mqtt_topic).c_str()); 582 | } 583 | 584 | RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to %sROS topic '%s'", 585 | mqtt_topic.c_str(), mqtt2ros.primitive ? "primitive " : "", 586 | mqtt2ros.ros.topic.c_str()); 587 | } 588 | else { 589 | RCLCPP_WARN(get_logger(), 590 | fmt::format("Parameter 'bridge.ros2mqtt.{}' is missing subparameter " 591 | "'ros_topic', will be ignored", mqtt_topic).c_str()); 592 | } 593 | } 594 | 595 | if (ros2mqtt_.empty() && mqtt2ros_.empty()) { 596 | RCLCPP_ERROR(get_logger(), 597 | "No valid ROS-MQTT bridge found in parameter 'bridge'"); 598 | exit(EXIT_FAILURE); 599 | } 600 | } 601 | 602 | 603 | bool MqttClient::loadParameter(const std::string& key, std::string& value) { 604 | bool found = get_parameter(key, value); 605 | if (found) 606 | RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), 607 | value.c_str()); 608 | return found; 609 | } 610 | 611 | 612 | bool MqttClient::loadParameter(const std::string& key, std::string& value, 613 | const std::string& default_value) { 614 | bool found = get_parameter_or(key, value, default_value); 615 | if (!found) 616 | RCLCPP_WARN(get_logger(), "Parameter '%s' not set, defaulting to '%s'", 617 | key.c_str(), default_value.c_str()); 618 | if (found) 619 | RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), 620 | value.c_str()); 621 | return found; 622 | } 623 | 624 | 625 | std::filesystem::path MqttClient::resolvePath(const std::string& path_string) { 626 | 627 | std::filesystem::path path(path_string); 628 | if (path_string.empty()) return path; 629 | if (!path.has_root_path()) { 630 | std::string ros_home = rcpputils::get_env_var("ROS_HOME"); 631 | if (ros_home.empty()) 632 | ros_home = std::string(std::filesystem::current_path()); 633 | path = std::filesystem::path(ros_home); 634 | path.append(path_string); 635 | } 636 | if (!std::filesystem::exists(path)) 637 | RCLCPP_WARN(get_logger(), "Requested path '%s' does not exist", 638 | std::string(path).c_str()); 639 | return path; 640 | } 641 | 642 | 643 | void MqttClient::setup() { 644 | 645 | // pre-compute timestamp length 646 | builtin_interfaces::msg::Time tmp_stamp; 647 | rclcpp::SerializedMessage tmp_serialized_stamp; 648 | serializeRosMessage(tmp_stamp, tmp_serialized_stamp); 649 | stamp_length_ = tmp_serialized_stamp.size(); 650 | 651 | // initialize MQTT client 652 | setupClient(); 653 | 654 | // connect to MQTT broker 655 | connect(); 656 | 657 | // create ROS service server 658 | is_connected_service_ = 659 | create_service( 660 | "~/is_connected", std::bind(&MqttClient::isConnectedService, this, 661 | std::placeholders::_1, std::placeholders::_2)); 662 | 663 | // create dynamic mappings services 664 | new_ros2mqtt_bridge_service_ = 665 | create_service( 666 | "~/new_ros2mqtt_bridge", std::bind(&MqttClient::newRos2MqttBridge, this, 667 | std::placeholders::_1, std::placeholders::_2)); 668 | new_mqtt2ros_bridge_service_ = 669 | create_service( 670 | "~/new_mqtt2ros_bridge", std::bind(&MqttClient::newMqtt2RosBridge, this, 671 | std::placeholders::_1, std::placeholders::_2)); 672 | 673 | // setup subscribers; check for new types every second 674 | check_subscriptions_timer_ = 675 | create_wall_timer(std::chrono::duration(1.0), 676 | std::bind(&MqttClient::setupSubscriptions, this)); 677 | 678 | setupPublishers (); 679 | } 680 | 681 | std::optional MqttClient::getCompatibleQoS (const std::string &ros_topic, const rclcpp::TopicEndpointInfo &tei, 682 | const Ros2MqttInterface &ros2mqtt) const 683 | { 684 | // the basic premise here is that we take the QoS from the publisher, overwrite any parts that are explicitly set 685 | // via configuration then check if the result is compatible with the publisher 686 | auto qos = tei.qos_profile (); 687 | 688 | if (auto r = ros2mqtt.ros.qos.reliability) 689 | qos.reliability (*r); 690 | if (auto d = ros2mqtt.ros.qos.durability) 691 | qos.durability (*d); 692 | qos.keep_last (ros2mqtt.ros.queue_size); 693 | 694 | const auto qres = rclcpp::qos_check_compatible (tei.qos_profile (), qos); 695 | 696 | switch (qres.compatibility) 697 | { 698 | case rclcpp::QoSCompatibility::Ok: 699 | return qos; 700 | case rclcpp::QoSCompatibility::Warning: 701 | RCLCPP_DEBUG(get_logger(), "QoS compatibility warning on topic %s - %s", ros_topic.c_str(), qres.reason.c_str ()); 702 | // presumably this is still compatible 703 | return qos; 704 | case rclcpp::QoSCompatibility::Error: 705 | default: 706 | return {}; 707 | } 708 | 709 | } 710 | 711 | std::vector MqttClient::getCandidatePublishers( 712 | const std::string& ros_topic, const Ros2MqttInterface& ros2mqtt) const 713 | { 714 | const auto &pubs = get_publishers_info_by_topic(ros_topic); 715 | 716 | if (pubs.empty ()) 717 | return {}; 718 | 719 | std::vector ret; 720 | 721 | ret.reserve (pubs.size ()); 722 | 723 | for (const auto &p : pubs) 724 | { 725 | const std::string msg_type = p.topic_type(); 726 | 727 | // if the type isn't set, match aginst all t ypes, otherwise only match against mtching types 728 | if (ros2mqtt.ros.msg_type.empty () || msg_type == ros2mqtt.ros.msg_type) 729 | ret.push_back (p); 730 | } 731 | 732 | // If we found any matching types, return those 733 | if (! ret.empty ()) 734 | return ret; 735 | 736 | // If we didn't and we aren't fix type, then just return the full set of publishers 737 | if (! ros2mqtt.fixed_type) 738 | return pubs; 739 | 740 | // None of these publishers will work... :sad_panda: 741 | return {}; 742 | } 743 | 744 | void MqttClient::setupSubscriptions() { 745 | 746 | // For each ros2mqtt interface, check if we need to do a lazy subscription (eg, lazy subscription would be if we 747 | // are determining either the type of QoS dynamically). If we don't, then just make the fixed subscriber if its not 748 | // already made. 749 | // 750 | // If we do, we check each publisher for a potential match. The first step is to filter down the list of 751 | // publishers based on the type. If we are fixed type then that list only includes publishers with matching types. 752 | // If we aren't its a little trickier. For dynamic types, if we already have matched against a type previously, the 753 | // candidate list will include all publishers which have matching types if any exist. If none of the publishers 754 | // have matching types, then the list will include all publishers. 755 | // 756 | // Then for each candidate publisher, check if their QoS is compatible with the QoS specic in the configuration. This 757 | // is really just needed because there is the potential for someone to set half of the QoS to auto and half to 758 | // explicit. 759 | // Condition for requiring "lazy" subscription where we need to walk the 760 | const auto requires_lazy_subscription = [] (const Ros2MqttInterface &ros2mqtt) 761 | { 762 | if (! ros2mqtt.fixed_type) 763 | return true; 764 | if (ros2mqtt.ros.qos.reliability == std::nullopt || 765 | ros2mqtt.ros.qos.durability == std::nullopt) 766 | return true; 767 | return false; 768 | }; 769 | 770 | for (auto& [ros_topic, ros2mqtt] : ros2mqtt_) { 771 | 772 | std::function msg)> bound_callback_func = 773 | std::bind(&MqttClient::ros2mqtt, this, std::placeholders::_1, ros_topic); 774 | 775 | if (! requires_lazy_subscription (ros2mqtt)) 776 | { 777 | try { 778 | if (ros2mqtt.ros.subscriber) 779 | continue; 780 | 781 | auto const qos = rclcpp::QoS(ros2mqtt.ros.queue_size) 782 | .reliability(*ros2mqtt.ros.qos.reliability) 783 | .durability(*ros2mqtt.ros.qos.durability); 784 | 785 | ros2mqtt.ros.subscriber = create_generic_subscription( 786 | ros_topic, ros2mqtt.ros.msg_type, qos, bound_callback_func); 787 | ros2mqtt.ros.is_stale = false; 788 | RCLCPP_INFO(get_logger(), "Subscribed ROS topic '%s' of type '%s'", 789 | ros_topic.c_str(), ros2mqtt.ros.msg_type.c_str()); 790 | } catch (rclcpp::exceptions::RCLError& e) { 791 | RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", 792 | e.what()); 793 | continue; 794 | } 795 | } else { 796 | const auto &pubs = getCandidatePublishers (ros_topic, ros2mqtt); 797 | 798 | if (pubs.empty()) continue; 799 | 800 | for (auto endpointInfo : pubs) { 801 | try { 802 | // check if message type has changed or if mapping is stale 803 | const std::string msg_type = endpointInfo.topic_type(); 804 | 805 | if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale && ros2mqtt.ros.subscriber) 806 | continue; 807 | 808 | auto const qos = getCompatibleQoS (ros_topic, endpointInfo, ros2mqtt); 809 | 810 | if (! qos) 811 | continue; 812 | 813 | ros2mqtt.ros.is_stale = false; 814 | ros2mqtt.ros.msg_type = msg_type; 815 | 816 | ros2mqtt.ros.subscriber = create_generic_subscription( 817 | ros_topic, msg_type, *qos, bound_callback_func); 818 | 819 | RCLCPP_INFO(get_logger(), "Subscribed ROS topic '%s' of type '%s'", 820 | ros_topic.c_str(), msg_type.c_str()); 821 | } catch (rclcpp::exceptions::RCLError& e) { 822 | RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", 823 | e.what()); 824 | continue; 825 | } 826 | } 827 | } 828 | } 829 | } 830 | 831 | void MqttClient::setupPublishers() { 832 | 833 | for (auto& [mqtt_topic, mqtt2ros] : mqtt2ros_) { 834 | if (mqtt2ros.ros.publisher) 835 | continue; 836 | 837 | // If the type is not fixed, we require a mqtt message in order to deduce the type 838 | if (!mqtt2ros.fixed_type) 839 | continue; 840 | 841 | try { 842 | const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) 843 | .durability(mqtt2ros.ros.qos.durability) 844 | .reliability(mqtt2ros.ros.qos.reliability); 845 | mqtt2ros.ros.publisher = create_generic_publisher( 846 | mqtt2ros.ros.topic, mqtt2ros.ros.msg_type, qos); 847 | 848 | mqtt2ros.ros.is_stale = false; 849 | } catch (const rclcpp::exceptions::RCLError& e) { 850 | RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", 851 | e.what()); 852 | } 853 | } 854 | } 855 | 856 | void MqttClient::setupClient() { 857 | 858 | // basic client connection options 859 | connect_options_.set_automatic_reconnect(true); 860 | connect_options_.set_clean_session(client_config_.clean_session); 861 | connect_options_.set_keep_alive_interval(client_config_.keep_alive_interval); 862 | connect_options_.set_max_inflight(client_config_.max_inflight); 863 | 864 | // user authentication 865 | if (!broker_config_.user.empty()) { 866 | connect_options_.set_user_name(broker_config_.user); 867 | connect_options_.set_password(broker_config_.pass); 868 | } 869 | 870 | // last will 871 | if (!client_config_.last_will.topic.empty()) { 872 | mqtt::will_options will( 873 | client_config_.last_will.topic, client_config_.last_will.message, 874 | client_config_.last_will.qos, client_config_.last_will.retained); 875 | connect_options_.set_will(will); 876 | } 877 | 878 | // SSL/TLS 879 | if (broker_config_.tls.enabled) { 880 | mqtt::ssl_options ssl; 881 | ssl.set_trust_store(broker_config_.tls.ca_certificate); 882 | if (!client_config_.tls.certificate.empty() && 883 | !client_config_.tls.key.empty()) { 884 | ssl.set_key_store(client_config_.tls.certificate); 885 | ssl.set_private_key(client_config_.tls.key); 886 | if (!client_config_.tls.password.empty()) 887 | ssl.set_private_key_password(client_config_.tls.password); 888 | } 889 | ssl.set_ssl_version(client_config_.tls.version); 890 | ssl.set_verify(client_config_.tls.verify); 891 | ssl.set_alpn_protos(client_config_.tls.alpn_protos); 892 | connect_options_.set_ssl(ssl); 893 | } 894 | 895 | // create MQTT client 896 | const std::string protocol = broker_config_.tls.enabled ? "ssl" : "tcp"; 897 | const std::string uri = fmt::format("{}://{}:{}", protocol, broker_config_.host, 898 | broker_config_.port); 899 | try { 900 | if (client_config_.buffer.enabled) { 901 | client_ = std::shared_ptr(new mqtt::async_client( 902 | uri, client_config_.id, client_config_.buffer.size, 903 | client_config_.buffer.directory)); 904 | } else { 905 | client_ = std::shared_ptr( 906 | new mqtt::async_client(uri, client_config_.id)); 907 | } 908 | } catch (const mqtt::exception& e) { 909 | RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); 910 | exit(EXIT_FAILURE); 911 | } 912 | 913 | // setup MQTT callbacks 914 | client_->set_callback(*this); 915 | } 916 | 917 | 918 | void MqttClient::connect() { 919 | 920 | std::string as_client = 921 | client_config_.id.empty() 922 | ? "" 923 | : std::string(" as '") + client_config_.id + std::string("'"); 924 | RCLCPP_INFO(get_logger(), "Connecting to broker at '%s'%s ...", 925 | client_->get_server_uri().c_str(), as_client.c_str()); 926 | 927 | try { 928 | client_->connect(connect_options_, nullptr, *this); 929 | } catch (const mqtt::exception& e) { 930 | RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); 931 | exit(EXIT_FAILURE); 932 | } 933 | } 934 | 935 | 936 | void MqttClient::ros2mqtt( 937 | const std::shared_ptr& serialized_msg, 938 | const std::string& ros_topic) { 939 | 940 | Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; 941 | std::string mqtt_topic = ros2mqtt.mqtt.topic; 942 | std::vector payload_buffer; 943 | 944 | // gather information on ROS message type in special ROS message 945 | mqtt_client_interfaces::msg::RosMsgType ros_msg_type; 946 | ros_msg_type.name = ros2mqtt.ros.msg_type; 947 | ros_msg_type.stamped = ros2mqtt.stamped; 948 | 949 | RCLCPP_DEBUG(get_logger(), "Received ROS message of type '%s' on topic '%s'", 950 | ros_msg_type.name.c_str(), ros_topic.c_str()); 951 | 952 | if (ros2mqtt.primitive) { // publish as primitive (string) message 953 | 954 | // resolve ROS messages to primitive strings if possible 955 | std::string payload; 956 | bool found_primitive = 957 | primitiveRosMessageToString(serialized_msg, ros_msg_type.name, payload); 958 | if (found_primitive) { 959 | payload_buffer = std::vector(payload.begin(), payload.end()); 960 | } else { 961 | RCLCPP_WARN(get_logger(), 962 | "Cannot send ROS message of type '%s' as primitive message, " 963 | "check supported primitive types", 964 | ros_msg_type.name.c_str()); 965 | return; 966 | } 967 | 968 | } else { // publish as complete ROS message incl. ROS message type 969 | 970 | // serialize ROS message type 971 | rclcpp::SerializedMessage serialized_ros_msg_type; 972 | serializeRosMessage(ros_msg_type, serialized_ros_msg_type); 973 | uint32_t msg_type_length = serialized_ros_msg_type.size(); 974 | std::vector msg_type_buffer = std::vector( 975 | serialized_ros_msg_type.get_rcl_serialized_message().buffer, 976 | serialized_ros_msg_type.get_rcl_serialized_message().buffer + msg_type_length); 977 | 978 | // send ROS message type information to MQTT broker 979 | mqtt_topic = kRosMsgTypeMqttTopicPrefix + ros2mqtt.mqtt.topic; 980 | try { 981 | RCLCPP_DEBUG(get_logger(), 982 | "Sending ROS message type to MQTT broker on topic '%s' ...", 983 | mqtt_topic.c_str()); 984 | mqtt::message_ptr mqtt_msg = 985 | mqtt::make_message(mqtt_topic, msg_type_buffer.data(), 986 | msg_type_buffer.size(), ros2mqtt.mqtt.qos, true); 987 | client_->publish(mqtt_msg); 988 | } catch (const mqtt::exception& e) { 989 | RCLCPP_WARN( 990 | get_logger(), 991 | "Publishing ROS message type information to MQTT topic '%s' failed: %s", 992 | mqtt_topic.c_str(), e.what()); 993 | } 994 | 995 | // build MQTT payload for ROS message (R) as [R] 996 | // or [S, R] if timestamp (S) is included 997 | uint32_t msg_length = serialized_msg->size(); 998 | uint32_t payload_length = msg_length; 999 | uint32_t msg_offset = 0; 1000 | if (ros2mqtt.stamped) { 1001 | 1002 | // allocate buffer with appropriate size to hold [S, R] 1003 | msg_offset += stamp_length_; 1004 | payload_length += stamp_length_; 1005 | payload_buffer.resize(payload_length); 1006 | 1007 | // copy serialized ROS message to payload [-, R] 1008 | std::copy(serialized_msg->get_rcl_serialized_message().buffer, 1009 | serialized_msg->get_rcl_serialized_message().buffer + msg_length, 1010 | payload_buffer.begin() + msg_offset); 1011 | } else { 1012 | 1013 | // directly build payload buffer on top of serialized message 1014 | payload_buffer = std::vector( 1015 | serialized_msg->get_rcl_serialized_message().buffer, 1016 | serialized_msg->get_rcl_serialized_message().buffer + msg_length); 1017 | } 1018 | 1019 | // inject timestamp as final step 1020 | if (ros2mqtt.stamped) { 1021 | 1022 | // take current timestamp 1023 | builtin_interfaces::msg::Time stamp(rclcpp::Clock(RCL_SYSTEM_TIME).now()); 1024 | 1025 | // copy serialized timestamp to payload [S, R] 1026 | rclcpp::SerializedMessage serialized_stamp; 1027 | serializeRosMessage(stamp, serialized_stamp); 1028 | std::copy( 1029 | serialized_stamp.get_rcl_serialized_message().buffer, 1030 | serialized_stamp.get_rcl_serialized_message().buffer + stamp_length_, 1031 | payload_buffer.begin()); 1032 | } 1033 | } 1034 | 1035 | // send ROS message to MQTT broker 1036 | mqtt_topic = ros2mqtt.mqtt.topic; 1037 | try { 1038 | RCLCPP_DEBUG( 1039 | get_logger(), 1040 | "Sending ROS message of type '%s' to MQTT broker on topic '%s' ...", 1041 | ros_msg_type.name.c_str(), mqtt_topic.c_str()); 1042 | mqtt::message_ptr mqtt_msg = mqtt::make_message( 1043 | mqtt_topic, payload_buffer.data(), payload_buffer.size(), 1044 | ros2mqtt.mqtt.qos, ros2mqtt.mqtt.retained); 1045 | client_->publish(mqtt_msg); 1046 | } catch (const mqtt::exception& e) { 1047 | RCLCPP_WARN( 1048 | get_logger(), 1049 | "Publishing ROS message type information to MQTT topic '%s' failed: %s", 1050 | mqtt_topic.c_str(), e.what()); 1051 | } 1052 | } 1053 | 1054 | 1055 | void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg, 1056 | const rclcpp::Time& arrival_stamp) { 1057 | std::string mqtt_topic = mqtt_msg->get_topic(); 1058 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 1059 | auto& payload = mqtt_msg->get_payload_ref(); 1060 | uint32_t payload_length = static_cast(payload.size()); 1061 | 1062 | // read MQTT payload for ROS message (R) as [R] 1063 | // or [S, R] if timestamp (S) is included 1064 | uint32_t msg_length = payload_length; 1065 | uint32_t msg_offset = 0; 1066 | 1067 | // if stamped, compute latency 1068 | if (mqtt2ros.stamped) { 1069 | 1070 | // copy stamp to generic message buffer 1071 | rclcpp::SerializedMessage serialized_stamp(stamp_length_); 1072 | std::memcpy(serialized_stamp.get_rcl_serialized_message().buffer, 1073 | &(payload[msg_offset]), stamp_length_); 1074 | serialized_stamp.get_rcl_serialized_message().buffer_length = stamp_length_; 1075 | 1076 | // deserialize stamp 1077 | builtin_interfaces::msg::Time stamp; 1078 | deserializeRosMessage(serialized_stamp, stamp); 1079 | 1080 | // compute ROS2MQTT latency 1081 | rclcpp::Duration latency = arrival_stamp - stamp; 1082 | std_msgs::msg::Float64 latency_msg; 1083 | latency_msg.data = latency.seconds(); 1084 | 1085 | // publish latency 1086 | if (!mqtt2ros.ros.latency_publisher) { 1087 | std::string latency_topic = kLatencyRosTopicPrefix + mqtt2ros.ros.topic; 1088 | latency_topic.replace(latency_topic.find("//"), 2, "/"); 1089 | mqtt2ros.ros.latency_publisher = 1090 | create_publisher(latency_topic, 1); 1091 | } 1092 | mqtt2ros.ros.latency_publisher->publish(latency_msg); 1093 | 1094 | msg_length -= stamp_length_; 1095 | msg_offset += stamp_length_; 1096 | } 1097 | 1098 | // copy ROS message from MQTT message to generic message buffer 1099 | rclcpp::SerializedMessage serialized_msg(msg_length); 1100 | std::memcpy(serialized_msg.get_rcl_serialized_message().buffer, 1101 | &(payload[msg_offset]), msg_length); 1102 | serialized_msg.get_rcl_serialized_message().buffer_length = msg_length; 1103 | 1104 | // publish generic ROS message 1105 | RCLCPP_DEBUG( 1106 | get_logger(), 1107 | "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", 1108 | mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); 1109 | mqtt2ros.ros.publisher->publish(serialized_msg); 1110 | } 1111 | 1112 | 1113 | void MqttClient::mqtt2primitive(mqtt::const_message_ptr mqtt_msg) { 1114 | 1115 | std::string mqtt_topic = mqtt_msg->get_topic(); 1116 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 1117 | 1118 | const std::string str_msg = mqtt_msg->to_string(); 1119 | bool found_primitive = false; 1120 | std::string ros_msg_type = "std_msgs/msg/String"; 1121 | rclcpp::SerializedMessage serialized_msg; 1122 | 1123 | // check for bool 1124 | if (!found_primitive) { 1125 | std::string bool_str = str_msg; 1126 | std::transform(str_msg.cbegin(), str_msg.cend(), bool_str.begin(), 1127 | ::tolower); 1128 | if (bool_str == "true" || bool_str == "false") { 1129 | 1130 | bool bool_msg = (bool_str == "true"); 1131 | 1132 | // construct and serialize ROS message 1133 | std_msgs::msg::Bool msg; 1134 | msg.data = bool_msg; 1135 | serializeRosMessage(msg, serialized_msg); 1136 | 1137 | ros_msg_type = "std_msgs/msg/Bool"; 1138 | found_primitive = true; 1139 | } 1140 | } 1141 | 1142 | // check for int 1143 | if (!found_primitive) { 1144 | std::size_t pos; 1145 | try { 1146 | const int int_msg = std::stoi(str_msg, &pos); 1147 | if (pos == str_msg.size()) { 1148 | 1149 | // construct and serialize ROS message 1150 | std_msgs::msg::Int32 msg; 1151 | msg.data = int_msg; 1152 | serializeRosMessage(msg, serialized_msg); 1153 | 1154 | ros_msg_type = "std_msgs/msg/Int32"; 1155 | found_primitive = true; 1156 | } 1157 | } catch (const std::invalid_argument& ex) { 1158 | } catch (const std::out_of_range& ex) { 1159 | } 1160 | } 1161 | 1162 | // check for float 1163 | if (!found_primitive) { 1164 | std::size_t pos; 1165 | try { 1166 | const float float_msg = std::stof(str_msg, &pos); 1167 | if (pos == str_msg.size()) { 1168 | 1169 | // construct and serialize ROS message 1170 | std_msgs::msg::Float32 msg; 1171 | msg.data = float_msg; 1172 | serializeRosMessage(msg, serialized_msg); 1173 | 1174 | ros_msg_type = "std_msgs/msg/Float32"; 1175 | found_primitive = true; 1176 | } 1177 | } catch (const std::invalid_argument& ex) { 1178 | } catch (const std::out_of_range& ex) { 1179 | } 1180 | } 1181 | 1182 | // fall back to string 1183 | if (!found_primitive) { 1184 | 1185 | // construct and serialize ROS message 1186 | std_msgs::msg::String msg; 1187 | msg.data = str_msg; 1188 | serializeRosMessage(msg, serialized_msg); 1189 | } 1190 | 1191 | // if ROS message type has changed or if mapping is stale 1192 | if (ros_msg_type != mqtt2ros.ros.msg_type || mqtt2ros.ros.is_stale) { 1193 | 1194 | mqtt2ros.ros.msg_type = ros_msg_type; 1195 | RCLCPP_INFO(get_logger(), 1196 | "ROS publisher message type on topic '%s' set to '%s'", 1197 | mqtt2ros.ros.topic.c_str(), ros_msg_type.c_str()); 1198 | 1199 | // recreate generic publisher 1200 | try { 1201 | const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) 1202 | .durability(mqtt2ros.ros.qos.durability) 1203 | .reliability(mqtt2ros.ros.qos.reliability); 1204 | mqtt2ros.ros.publisher = create_generic_publisher( 1205 | mqtt2ros.ros.topic, ros_msg_type, qos); 1206 | } catch (rclcpp::exceptions::RCLError& e) { 1207 | RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", 1208 | e.what()); 1209 | return; 1210 | } 1211 | mqtt2ros.ros.is_stale = false; 1212 | } 1213 | 1214 | // publish 1215 | RCLCPP_DEBUG( 1216 | get_logger(), 1217 | "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", 1218 | mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); 1219 | mqtt2ros.ros.publisher->publish(serialized_msg); 1220 | } 1221 | 1222 | void MqttClient::mqtt2fixed(mqtt::const_message_ptr mqtt_msg) { 1223 | 1224 | std::string mqtt_topic = mqtt_msg->get_topic(); 1225 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 1226 | 1227 | rclcpp::SerializedMessage serialized_msg; 1228 | 1229 | if (!fixedMqtt2PrimitiveRos(mqtt_msg, mqtt2ros.ros.msg_type, serialized_msg)) { 1230 | RCLCPP_WARN( 1231 | get_logger(), 1232 | "Could not convert mqtt message into type %s on topic %s ...", 1233 | mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); 1234 | } else { 1235 | 1236 | if (!mqtt2ros.ros.publisher) 1237 | { 1238 | RCLCPP_INFO(get_logger(), 1239 | "ROS publisher message type on topic '%s' set to '%s'", 1240 | mqtt2ros.ros.topic.c_str(), mqtt2ros.ros.msg_type.c_str()); 1241 | 1242 | // recreate generic publisher 1243 | try { 1244 | const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) 1245 | .durability(mqtt2ros.ros.qos.durability) 1246 | .reliability(mqtt2ros.ros.qos.reliability); 1247 | mqtt2ros.ros.publisher = create_generic_publisher( 1248 | mqtt2ros.ros.topic, mqtt2ros.ros.msg_type, qos); 1249 | 1250 | mqtt2ros.ros.is_stale = false; 1251 | } catch (const rclcpp::exceptions::RCLError& e) { 1252 | RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", 1253 | e.what()); 1254 | return; 1255 | } 1256 | } 1257 | 1258 | RCLCPP_DEBUG(get_logger(), 1259 | "Sending ROS message of type '%s' from MQTT broker to ROS " 1260 | "topic '%s' ...", 1261 | mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); 1262 | mqtt2ros.ros.publisher->publish(serialized_msg); 1263 | } 1264 | } 1265 | 1266 | 1267 | void MqttClient::connected(const std::string& cause) { 1268 | 1269 | (void) cause; // Avoid compiler warning for unused parameter. 1270 | 1271 | is_connected_ = true; 1272 | std::string as_client = 1273 | client_config_.id.empty() 1274 | ? "" 1275 | : std::string(" as '") + client_config_.id + std::string("'"); 1276 | RCLCPP_INFO(get_logger(), "Connected to broker at '%s'%s", 1277 | client_->get_server_uri().c_str(), as_client.c_str()); 1278 | 1279 | // subscribe MQTT topics 1280 | for (const auto& [mqtt_topic, mqtt2ros] : mqtt2ros_) { 1281 | if (!mqtt2ros.primitive) { 1282 | std::string const mqtt_topic_to_subscribe = kRosMsgTypeMqttTopicPrefix + mqtt_topic; 1283 | client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); 1284 | RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); 1285 | } 1286 | // If not primitive and not fixed, we need the message type before we can public. In that case 1287 | // wait for the type to come in before subscribing to the data topic 1288 | if (mqtt2ros.primitive || mqtt2ros.fixed_type) { 1289 | client_->subscribe(mqtt_topic, mqtt2ros.mqtt.qos); 1290 | RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic.c_str()); 1291 | } 1292 | } 1293 | } 1294 | 1295 | 1296 | void MqttClient::connection_lost(const std::string& cause) { 1297 | 1298 | (void) cause; // Avoid compiler warning for unused parameter. 1299 | 1300 | RCLCPP_ERROR(get_logger(), 1301 | "Connection to broker lost, will try to reconnect..."); 1302 | is_connected_ = false; 1303 | connect(); 1304 | } 1305 | 1306 | 1307 | bool MqttClient::isConnected() { 1308 | 1309 | return is_connected_; 1310 | } 1311 | 1312 | 1313 | void MqttClient::isConnectedService( 1314 | mqtt_client_interfaces::srv::IsConnected::Request::SharedPtr request, 1315 | mqtt_client_interfaces::srv::IsConnected::Response::SharedPtr response) { 1316 | 1317 | (void) request; // Avoid compiler warning for unused parameter. 1318 | response->connected = isConnected(); 1319 | } 1320 | 1321 | void MqttClient::newRos2MqttBridge( 1322 | mqtt_client_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, 1323 | mqtt_client_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response){ 1324 | 1325 | // add mapping definition to ros2mqtt_ 1326 | Ros2MqttInterface& ros2mqtt = ros2mqtt_[request->ros_topic]; 1327 | ros2mqtt.ros.is_stale = true; 1328 | ros2mqtt.mqtt.topic = request->mqtt_topic; 1329 | ros2mqtt.primitive = request->primitive; 1330 | ros2mqtt.stamped = request->inject_timestamp; 1331 | ros2mqtt.ros.queue_size = request->ros_queue_size; 1332 | ros2mqtt.mqtt.qos = request->mqtt_qos; 1333 | ros2mqtt.mqtt.retained = request->mqtt_retained; 1334 | 1335 | if (ros2mqtt.stamped && ros2mqtt.primitive) { 1336 | RCLCPP_WARN( 1337 | get_logger(), 1338 | "Timestamp will not be injected into primitive messages on ROS " 1339 | "topic '%s'", 1340 | request->ros_topic.c_str()); 1341 | ros2mqtt.stamped = false; 1342 | } 1343 | 1344 | RCLCPP_INFO(get_logger(), "Bridging %sROS topic '%s' to MQTT topic '%s' %s", 1345 | ros2mqtt.primitive ? "primitive " : "", request->ros_topic.c_str(), 1346 | ros2mqtt.mqtt.topic.c_str(), 1347 | ros2mqtt.stamped ? "and measuring latency" : ""); 1348 | 1349 | // (re-)setup ROS subscriptions 1350 | setupSubscriptions(); 1351 | 1352 | response->success = true; 1353 | } 1354 | 1355 | void MqttClient::newMqtt2RosBridge( 1356 | mqtt_client_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, 1357 | mqtt_client_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response){ 1358 | 1359 | // add mapping definition to mqtt2ros_ 1360 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[request->mqtt_topic]; 1361 | mqtt2ros.ros.is_stale = true; 1362 | mqtt2ros.ros.topic = request->ros_topic; 1363 | mqtt2ros.primitive = request->primitive; 1364 | mqtt2ros.mqtt.qos = request->mqtt_qos; 1365 | mqtt2ros.ros.queue_size = request->ros_queue_size; 1366 | mqtt2ros.ros.latched = request->ros_latched; 1367 | if (mqtt2ros.ros.latched) { 1368 | RCLCPP_WARN(get_logger(), 1369 | fmt::format("Parameter 'bridge.mqtt2ros.{}.advanced.ros.latched' is ignored " 1370 | "since ROS 2 does not easily support latched topics.", request->mqtt_topic).c_str()); 1371 | 1372 | } 1373 | 1374 | RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to %sROS topic '%s'", 1375 | request->mqtt_topic.c_str(), mqtt2ros.primitive ? "primitive " : "", 1376 | mqtt2ros.ros.topic.c_str()); 1377 | 1378 | // subscribe to the MQTT topic 1379 | std::string mqtt_topic_to_subscribe = request->mqtt_topic; 1380 | if (!mqtt2ros.primitive) 1381 | mqtt_topic_to_subscribe = kRosMsgTypeMqttTopicPrefix + request->mqtt_topic; 1382 | client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); 1383 | RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); 1384 | 1385 | response->success = true; 1386 | } 1387 | 1388 | void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) { 1389 | 1390 | // instantly take arrival timestamp 1391 | rclcpp::Time arrival_stamp( 1392 | builtin_interfaces::msg::Time(rclcpp::Clock(RCL_SYSTEM_TIME).now())); 1393 | 1394 | std::string mqtt_topic = mqtt_msg->get_topic(); 1395 | RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", 1396 | mqtt_topic.c_str()); 1397 | 1398 | // publish directly if primitive 1399 | if (mqtt2ros_.count(mqtt_topic) > 0) { 1400 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; 1401 | 1402 | if (mqtt2ros.primitive) { 1403 | if (mqtt2ros.fixed_type) { 1404 | mqtt2fixed(mqtt_msg); 1405 | } else { 1406 | mqtt2primitive(mqtt_msg); 1407 | } 1408 | return; 1409 | } 1410 | } 1411 | 1412 | // else first check for ROS message type 1413 | bool msg_contains_ros_msg_type = 1414 | mqtt_topic.find(kRosMsgTypeMqttTopicPrefix) != std::string::npos; 1415 | if (msg_contains_ros_msg_type) { 1416 | 1417 | // copy message type to generic message buffer 1418 | auto& payload = mqtt_msg->get_payload_ref(); 1419 | uint32_t payload_length = static_cast(payload.size()); 1420 | rclcpp::SerializedMessage serialized_ros_msg_type(payload_length); 1421 | std::memcpy(serialized_ros_msg_type.get_rcl_serialized_message().buffer, 1422 | &(payload[0]), payload_length); 1423 | serialized_ros_msg_type.get_rcl_serialized_message().buffer_length = payload_length; 1424 | 1425 | // deserialize ROS message type 1426 | mqtt_client_interfaces::msg::RosMsgType ros_msg_type; 1427 | deserializeRosMessage(serialized_ros_msg_type, ros_msg_type); 1428 | 1429 | // reconstruct corresponding MQTT data topic 1430 | std::string mqtt_data_topic = mqtt_topic; 1431 | mqtt_data_topic.erase(mqtt_data_topic.find(kRosMsgTypeMqttTopicPrefix), 1432 | kRosMsgTypeMqttTopicPrefix.length()); 1433 | Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_data_topic]; 1434 | 1435 | // if ROS message type has changed or if mapping is stale 1436 | if (ros_msg_type.name != mqtt2ros.ros.msg_type || mqtt2ros.ros.is_stale) { 1437 | 1438 | if (mqtt2ros.fixed_type) { 1439 | // We should never be in this situation if the type has been set explicitly. As fixed_type 1440 | // is not currently supported through the service based bridge creation and the type name 1441 | // not matching means the fixed type specified in the configuration does not match the 1442 | // one we just recieved 1443 | if (ros_msg_type.name != mqtt2ros.ros.msg_type) 1444 | RCLCPP_ERROR(get_logger(), 1445 | "Unexpected type name received for topic %s (expected %s but received %s)", 1446 | mqtt2ros.ros.topic.c_str(), mqtt2ros.ros.msg_type.c_str(), ros_msg_type.name.c_str()); 1447 | if (mqtt2ros.ros.is_stale) 1448 | RCLCPP_ERROR(get_logger(), "Topic %s has been unexpectedly marked stale", 1449 | mqtt2ros.ros.topic.c_str()); 1450 | return; 1451 | } 1452 | 1453 | mqtt2ros.ros.msg_type = ros_msg_type.name; 1454 | mqtt2ros.stamped = ros_msg_type.stamped; 1455 | RCLCPP_INFO(get_logger(), 1456 | "ROS publisher message type on topic '%s' set to '%s'", 1457 | mqtt2ros.ros.topic.c_str(), ros_msg_type.name.c_str()); 1458 | 1459 | // recreate generic publisher 1460 | try { 1461 | const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) 1462 | .durability(mqtt2ros.ros.qos.durability) 1463 | .reliability(mqtt2ros.ros.qos.reliability); 1464 | mqtt2ros.ros.publisher = create_generic_publisher( 1465 | mqtt2ros.ros.topic, ros_msg_type.name, qos); 1466 | } catch (rclcpp::exceptions::RCLError& e) { 1467 | RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", 1468 | e.what()); 1469 | return; 1470 | } 1471 | mqtt2ros.ros.is_stale = false; 1472 | 1473 | // subscribe to MQTT topic with actual ROS messages 1474 | client_->subscribe(mqtt_data_topic, mqtt2ros.mqtt.qos); 1475 | RCLCPP_DEBUG(get_logger(), "Subscribed MQTT topic '%s'", 1476 | mqtt_data_topic.c_str()); 1477 | } 1478 | 1479 | } else { 1480 | 1481 | // publish ROS message, if publisher initialized 1482 | if (!mqtt2ros_[mqtt_topic].ros.msg_type.empty()) { 1483 | mqtt2ros(mqtt_msg, arrival_stamp); 1484 | } else { 1485 | RCLCPP_WARN( 1486 | get_logger(), 1487 | "ROS publisher for data from MQTT topic '%s' is not yet initialized: " 1488 | "ROS message type not yet known", 1489 | mqtt_topic.c_str()); 1490 | } 1491 | } 1492 | } 1493 | 1494 | 1495 | void MqttClient::delivery_complete(mqtt::delivery_token_ptr token) { 1496 | 1497 | (void) token; // Avoid compiler warning for unused parameter. 1498 | } 1499 | 1500 | 1501 | void MqttClient::on_success(const mqtt::token& token) { 1502 | 1503 | (void) token; // Avoid compiler warning for unused parameter. 1504 | is_connected_ = true; 1505 | } 1506 | 1507 | 1508 | void MqttClient::on_failure(const mqtt::token& token) { 1509 | 1510 | RCLCPP_ERROR( 1511 | get_logger(), 1512 | "Connection to broker failed (return code %d), will automatically " 1513 | "retry...", 1514 | token.get_return_code()); 1515 | is_connected_ = false; 1516 | } 1517 | 1518 | } // namespace mqtt_client 1519 | -------------------------------------------------------------------------------- /mqtt_client_interfaces/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mqtt_client_interfaces 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.3.0 (2024-05-30) 6 | ------------------ 7 | 8 | 2.2.1 (2024-03-19) 9 | ------------------ 10 | 11 | 2.2.0 (2023-11-29) 12 | ------------------ 13 | * Merge pull request `#35 `_ from mvccogo/main 14 | Dynamic registration of topics 15 | * Contributors: Lennart Reiher, Matheus V. C. Cogo, mvccogo 16 | 17 | 2.1.0 (2023-09-18) 18 | ------------------ 19 | 20 | 2.0.1 (2023-06-10) 21 | ------------------ 22 | * fix unrecognized build type with catkin_make_isolated 23 | order of statements is somehow revelant; catkin_make_isolated would not detect the build type; build farm jobs were failing; https://build.ros.org/job/Ndev__mqtt_client__ubuntu_focal_amd64/10/console 24 | * Contributors: Lennart Reiher 25 | 26 | 2.0.0 (2023-06-10) 27 | ------------------ 28 | * Merge pull request #23 from ika-rwth-aachen/docker-ros 29 | Integrate docker-ros 30 | * Merge pull request #16 from ika-rwth-aachen/dev/ros2 31 | Add support for ROS2 32 | * Contributors: Lennart Reiher 33 | 34 | 1.1.0 (2022-10-13) 35 | ------------------ 36 | 37 | 1.0.2 (2022-10-07) 38 | ------------------ 39 | 40 | 1.0.1 (2022-08-11) 41 | ------------------ 42 | 43 | 1.0.0 (2022-06-02) 44 | ------------------ 45 | -------------------------------------------------------------------------------- /mqtt_client_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.0 FATAL_ERROR) 2 | project(mqtt_client_interfaces) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | 7 | # === ROS2 (AMENT) ============================================================= 8 | if(${ROS_VERSION} EQUAL 2) 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(rosidl_default_generators REQUIRED) 12 | find_package(std_msgs REQUIRED) 13 | 14 | rosidl_generate_interfaces(${PROJECT_NAME} 15 | msg/RosMsgType.msg 16 | srv/IsConnected.srv 17 | srv/ros2/NewMqtt2RosBridge.srv 18 | srv/ros2/NewRos2MqttBridge.srv 19 | DEPENDENCIES std_msgs 20 | ) 21 | 22 | ament_package() 23 | 24 | # === ROS1 (CATKIN) ============================================================ 25 | elseif(${ROS_VERSION} EQUAL 1) 26 | 27 | find_package(catkin REQUIRED COMPONENTS 28 | message_generation 29 | std_msgs 30 | ) 31 | 32 | add_message_files( 33 | FILES 34 | RosMsgType.msg 35 | ) 36 | 37 | add_service_files( 38 | FILES 39 | IsConnected.srv 40 | ) 41 | 42 | generate_messages( 43 | DEPENDENCIES 44 | std_msgs 45 | ) 46 | 47 | catkin_package( 48 | CATKIN_DEPENDS 49 | message_runtime 50 | std_msgs 51 | ) 52 | 53 | endif() 54 | -------------------------------------------------------------------------------- /mqtt_client_interfaces/msg/RosMsgType.msg: -------------------------------------------------------------------------------- 1 | # This message contains information about a ROS Message Type. 2 | 3 | string md5 # MD5 sum of ROS message type 4 | string name # ROS message type name (e.g. 'std_msgs/Float32') 5 | string definition # ROS message type definition (e.g. 'float data') 6 | bool stamped # whether timestamp is prepended for latency computation -------------------------------------------------------------------------------- /mqtt_client_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mqtt_client_interfaces 5 | 2.3.0 6 | Message and service definitions for mqtt_client 7 | 8 | Lennart Reiher 9 | 10 | MIT 11 | 12 | http://wiki.ros.org/mqtt_client 13 | https://github.com/ika-rwth-aachen/mqtt_client 14 | 15 | Lennart Reiher 16 | 17 | ros_environment 18 | 19 | 20 | ament_cmake 21 | rosidl_default_generators 22 | std_msgs 23 | rosidl_default_runtime 24 | 25 | 26 | catkin 27 | message_generation 28 | std_msgs 29 | message_runtime 30 | 31 | 32 | catkin 33 | ament_cmake 34 | 35 | 36 | rosidl_interface_packages 37 | 38 | -------------------------------------------------------------------------------- /mqtt_client_interfaces/srv/IsConnected.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool connected -------------------------------------------------------------------------------- /mqtt_client_interfaces/srv/ros2/NewMqtt2RosBridge.srv: -------------------------------------------------------------------------------- 1 | # This service is used to dynamically register new MQTT -> ROS mappings. 2 | 3 | string ros_topic 4 | string mqtt_topic 5 | bool primitive false 6 | uint8 mqtt_qos 0 7 | uint32 ros_queue_size 1 8 | bool ros_latched false 9 | --- 10 | bool success 11 | -------------------------------------------------------------------------------- /mqtt_client_interfaces/srv/ros2/NewRos2MqttBridge.srv: -------------------------------------------------------------------------------- 1 | # This service is used to dynamically register new ROS -> MQTT mappings. 2 | 3 | string ros_topic 4 | string mqtt_topic 5 | bool primitive false 6 | bool inject_timestamp false 7 | uint32 ros_queue_size 1 8 | uint8 mqtt_qos 0 9 | bool mqtt_retained false 10 | --- 11 | bool success --------------------------------------------------------------------------------