diff --git a/.github/workflows/docker-ros.yml b/.github/workflows/docker-ros.yml index 3e32f5c..ad92316 100644 --- a/.github/workflows/docker-ros.yml +++ b/.github/workflows/docker-ros.yml @@ -4,27 +4,51 @@ on: push jobs: - ros: + ros2-humble: runs-on: ubuntu-latest steps: - uses: ika-rwth-aachen/docker-ros@main with: - platform: amd64,arm64 + platform: amd64 target: run - image-tag: ros - base-image: rwthika/ros:latest - command: roslaunch mqtt_client standalone.launch - enable-industrial-ci: 'true' + base-image: rwthika/ros2:humble + command: ros2 launch mqtt_client standalone.launch.xml + image-tag: humble + rmw-implementation: rmw_zenoh_cpp - ros2: + ros2-jazzy: runs-on: ubuntu-latest steps: - uses: ika-rwth-aachen/docker-ros@main with: - platform: amd64,arm64 + platform: amd64 target: run - image-tag: ros2 + base-image: rwthika/ros2:jazzy + command: ros2 launch mqtt_client standalone.launch.xml + image-tag: jazzy enable-push-as-latest: 'true' - base-image: rwthika/ros2:latest - command: ros2 launch mqtt_client standalone.launch.ros2.xml - enable-industrial-ci: 'true' + rmw-implementation: rmw_zenoh_cpp + + ros2-kilted: + runs-on: ubuntu-latest + steps: + - uses: ika-rwth-aachen/docker-ros@main + with: + platform: amd64 + target: run + base-image: rwthika/ros2:kilted + command: ros2 launch mqtt_client standalone.launch.xml + image-tag: kilted + rmw-implementation: rmw_zenoh_cpp + + ros2-rolling: + runs-on: ubuntu-latest + steps: + - uses: ika-rwth-aachen/docker-ros@main + with: + platform: amd64 + target: run + base-image: rwthika/ros2:rolling + command: ros2 launch mqtt_client standalone.launch.xml + image-tag: rolling + rmw-implementation: rmw_zenoh_cpp diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 25bdc5f..8d6890d 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -9,13 +9,11 @@ jobs: strategy: matrix: ROS_DISTRO: - - noetic - humble - - iron - jazzy + - kilted - rolling ROS_REPO: - - testing - main steps: - uses: actions/checkout@v3 diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 663fb66..16bf4a2 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -1,26 +1,11 @@ -ros: - trigger: - include: - - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml - strategy: depend - variables: - IMAGE_TAG: ros - BASE_IMAGE: rwthika/ros:latest - COMMAND: roslaunch mqtt_client standalone.launch - PLATFORM: amd64,arm64 - TARGET: dev,run - ENABLE_INDUSTRIAL_CI: 'true' - ros2: trigger: include: - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml strategy: depend variables: - IMAGE_TAG: ros2 - BASE_IMAGE: rwthika/ros2:latest - COMMAND: ros2 launch mqtt_client standalone.launch.ros2.xml PLATFORM: amd64,arm64 TARGET: dev,run - ENABLE_INDUSTRIAL_CI: 'true' - ENABLE_PUSH_AS_LATEST: 'true' + BASE_IMAGE: rwthika/ros2:jazzy + COMMAND: ros2 launch mqtt_client standalone.launch.xml + RMW_IMPLEMENTATION: rmw_zenoh_cpp diff --git a/README.md b/README.md index 4627672..670bed8 100644 --- a/README.md +++ b/README.md @@ -5,12 +5,11 @@ - - +

-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. +The *mqtt_client* package provides a 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. > [!IMPORTANT] > This repository is open-sourced and maintained by the [**Institute for Automotive Engineering (ika) at RWTH Aachen University**](https://www.ika.rwth-aachen.de/). @@ -27,29 +26,23 @@ The *mqtt_client* package provides a ROS nodelet or ROS 2 component node that en - [Primitive Messages](#primitive-messages) - [Latency Computation](#latency-computation) - [Package Summary](#package-summary) -- [How It Works](#how-it-works) - [Acknowledgements](#acknowledgements) ## Installation -The *mqtt_client* package is released as an official ROS / ROS 2 package and can easily be installed via a package manager. +The *mqtt_client* package is released as an official ROS 2 package and can easily be installed via a package manager. ```bash sudo apt update sudo apt install ros-$ROS_DISTRO-mqtt-client ``` -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). +If you would like to install *mqtt_client* from source, simply clone this repository into your ROS 2 workspace. All dependencies that are listed in the ROS [`package.xml`](package.xml) can be installed using [*rosdep*](http://wiki.ros.org/rosdep). ```bash # mqtt_client$ rosdep install -r --ignore-src --from-paths . -# ROS -# workspace$ -catkin build -DCMAKE_BUILD_TYPE=Release mqtt_client - -# ROS 2 # workspace$ colcon build --packages-up-to mqtt_client --cmake-args -DCMAKE_BUILD_TYPE=Release ``` @@ -59,11 +52,7 @@ colcon build --packages-up-to mqtt_client --cmake-args -DCMAKE_BUILD_TYPE=Releas *mqtt_client* is also available as a Docker image, containerized through [*docker-ros*](https://github.com/ika-rwth-aachen/docker-ros). ```bash -# ROS -docker run --rm ghcr.io/ika-rwth-aachen/mqtt_client:ros - -# ROS 2 -docker run --rm ghcr.io/ika-rwth-aachen/mqtt_client:ros2 +docker run --rm ghcr.io/ika-rwth-aachen/mqtt_client:latest # or distro-specific tags, e.g., :rolling ``` @@ -89,7 +78,7 @@ For a more advanced setup of your own broker, check out our instructions for run #### Demo Configuration -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: +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.yaml`](mqtt_client/config/params.yaml)) allows an exchange of messages as follows: - ROS messages received locally on ROS topic `/ping/ros` are sent to the broker on MQTT topic `pingpong/ros`; - MQTT messages received from the broker on MQTT topic `pingpong/ros` are published locally on ROS topic `/pong/ros`; @@ -117,14 +106,10 @@ bridge: #### Demo Client Launch -After building your ROS workspace, launch the *mqtt_client* node with the pre-configured demo parameters using *roslaunch*, which should yield the following output. +Launch the *mqtt_client* node with the pre-configured demo parameters: ```bash -# ROS -roslaunch mqtt_client standalone.launch - -# ROS 2 -ros2 launch mqtt_client standalone.launch.ros2.xml +ros2 launch mqtt_client standalone.launch.xml ``` ```txt @@ -148,49 +133,28 @@ In order to test the communication among *mqtt_clients*, publish any ROS message ```bash # 1st terminal: publish ROS message to /ping - -# ROS -rostopic pub -r 1 /ping/ros std_msgs/String "Hello MQTT" - -# ROS 2 ros2 topic pub /ping/ros std_msgs/msg/String "{data: \"Hello MQTT\"}" ``` ```bash # 2nd terminal: listen for ROS messages on /pong - -# ROS -rostopic echo /pong/ros - -# ROS 2 ros2 topic echo /pong/ros ``` 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. ```bash -# ROS 2 # mqtt_client$ -ros2 launch mqtt_client standalone.launch.ros2.xml params_file:=$(ros2 pkg prefix mqtt_client)/share/mqtt_client/config/params.ros2.primitive.yaml +ros2 launch mqtt_client standalone.launch.xml params_file:=$(ros2 pkg prefix mqtt_client)/share/mqtt_client/config/params.primitive.yaml ``` ```bash # 3rd terminal: publish primitive ROS message to /ping/primitive - -# ROS -rostopic pub -r 1 /ping/primitive std_msgs/Int32 42 - -# ROS2 ros2 topic pub /ping/primitive std_msgs/msg/Int32 "{data: 42}" ``` ```bash # 4th terminal: listen for primitive ROS messages on /pong/primitive - -# ROS -rostopic echo /pong/primitive - -# ROS2 ros2 topic echo /pong/primitive ``` @@ -206,24 +170,16 @@ If everything works as expected, the second terminal should print a message at 1 You can start the *mqtt_client* node with: ```bash -# ROS -roslaunch mqtt_client standalone.launch - -# ROS 2 -ros2 launch mqtt_client standalone.launch.ros2.xml +ros2 launch mqtt_client standalone.launch.xml ``` -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`. +This will automatically load the provided demo [`params.yaml`](mqtt_client/config/params.yaml) / [`params.yaml`](mqtt_client/config/params.yaml). If you wish to load your custom configuration file, simply pass `params_file`. ```bash -# ROS -roslaunch mqtt_client standalone.launch params_file:="" - -# ROS 2 -ros2 launch mqtt_client standalone.launch.ros2.xml params_file:="" +ros2 launch mqtt_client standalone.launch.xml params_file:="" ``` -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. +In order to exploit the benefits of *mqtt_client* being a ROS 2 component, load the component into your own component container. ### Configuration @@ -269,35 +225,6 @@ client: #### Bridge Parameters -##### ROS - -```yaml -bridge: - ros2mqtt: # Array specifying which ROS topics to map to which MQTT topics - - ros_topic: # ROS topic whose messages are transformed to MQTT messages - mqtt_topic: # MQTT topic on which the corresponding ROS messages are sent to the broker - primitive: # [false] whether to publish as primitive message - inject_timestamp: # [false] whether to attach a timestamp to a ROS2MQTT payload (for latency computation on receiver side) - advanced: - ros: - queue_size: # [1] ROS subscriber queue size - mqtt: - qos: # [0] MQTT QoS value - retained: # [false] whether to retain MQTT message - mqtt2ros: # Array specifying which MQTT topics to map to which ROS topics - - mqtt_topic: # MQTT topic on which messages are received from the broker - ros_topic: # ROS topic on which corresponding MQTT messages are published - primitive: # [false] whether to publish as primitive message (if coming from non-ROS MQTT client) - advanced: - mqtt: - qos: # [0] MQTT QoS value - ros: - queue_size: # [1] ROS publisher queue size - latched: # [false] whether to latch ROS message -``` - -##### ROS 2 - ```yaml bridge: ros2mqtt: # Object specifying which ROS topics to map to which MQTT topics @@ -350,23 +277,15 @@ The *mqtt_client* provides built-in functionality to measure the latency of tran 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/`. -These latencies can be printed easily with *rostopic echo* +These latencies can be printed easily with: ```bash -# ROS -rostopic echo --clear //latencies//data - -# ROS 2 ros2 topic echo //latencies//data ``` or plotted with [rqt_plot](http://wiki.ros.org/rqt_plot): ```bash -# ROS -rosrun rqt_plot rqt_plot //latencies//data - -# ROS 2 ros2 run rqt_plot rqt_plot //latencies//data ``` @@ -375,56 +294,25 @@ ros2 run rqt_plot rqt_plot //latencies//da This short package summary documents the package in line with the [ROS Wiki Style Guide](http://wiki.ros.org/StyleGuide). -### ROS - -#### Nodelets - -##### `mqtt_client/MqttClient` - -Enables connected ROS-based devices or robots to exchange ROS messages via an MQTT broker using the [MQTT](http://mqtt.org) protocol. - -###### Subscribed Topics - -- `` ([`topic_tools/ShapeShifter`](http://wiki.ros.org/topic_tools)) - ROS topic whose messages are transformed to MQTT messages and sent to the MQTT broker. May have arbitrary ROS message type. - -###### Published Topics - -- `` ([`topic_tools/ShapeShifter`](http://wiki.ros.org/topic_tools)) - ROS topic on which MQTT messages received from the MQTT broker are published. May have arbitrary ROS message type. -- `~/latencies/` ([`std_msgs/Float64`](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html)) - Latencies measured on the message transfer to `` are published here, if the received messages have a timestamp injected (see [Latency Computation](#latency-computation)). - -###### Services - -- `~is_connected` ([`mqtt_client/srv/IsConnected`](mqtt_client_interfaces/srv/IsConnected.srv)) - Returns whether the client is connected to the MQTT broker. - -###### Parameters - -See [Configuration](#configuration). +### Components -### ROS 2 - -#### Components - -##### `mqtt_client/MqttClient` +#### `mqtt_client/MqttClient` Enables connected ROS-based devices or robots to exchange ROS messages via an MQTT broker using the [MQTT](http://mqtt.org) protocol. -###### Subscribed Topics +##### Subscribed Topics - `` ([`rclcpp::SerializedMessage`](https://docs.ros.org/en/ros2_packages/rolling/api/rclcpp/generated/classrclcpp_1_1GenericSubscription.html)) ROS topic whose messages are transformed to MQTT messages and sent to the MQTT broker. May have arbitrary ROS message type. -###### Published Topics +##### Published Topics - `` ([`rclcpp::SerializedMessage`](https://docs.ros.org/en/ros2_packages/rolling/api/rclcpp/generated/classrclcpp_1_1GenericPublisher.html)) ROS topic on which MQTT messages received from the MQTT broker are published. May have arbitrary ROS message type. - `~/latencies/` ([`std_msgs/Float64`](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html)) Latencies measured on the message transfer to `` are published here, if the received messages have a timestamp injected (see [Latency Computation](#latency-computation)). -###### Services +##### Services - `~/is_connected` ([`mqtt_client/srv/IsConnected`](mqtt_client_interfaces/srv/IsConnected.srv)) Returns whether the client is connected to the MQTT broker. @@ -435,57 +323,11 @@ Enables connected ROS-based devices or robots to exchange ROS messages via an MQ - `~/new_mqtt2ros_bridge` ([`mqtt_client/srv/NewMqtt2RosBridge`](mqtt_client_interfaces/srv/NewMqtt2RosBridge.srv)) Returns whether a new MQTT -> ROS bridge was created. - - -###### Parameters +##### Parameters See [Configuration](#configuration). -## How It Works - -### ROS - -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. - -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: - -```cpp -void ros2mqtt(topic_tools::ShapeShifter::ConstPtr&, std::string&) -``` - -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`. - -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. - -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. - -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). - -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: - -```txt -[... serialized timestamp ... | ... serialized ROS messsage ...] -[... serialized ROS messsage ...] -``` - -To summarize, the dataflow is as follows: - -- a ROS message of arbitrary type is received on ROS topic `` and passed to the generic callback - - ROS message type information is extracted and wrapped as a `RosMsgType` - - ROS message type information is serialized and sent via the MQTT broker on MQTT topic `mqtt_client/ros_msg_type/` - - the actual ROS message is serialized - - if `inject_timestamp`, the current timestamp is serialized and concatenated with the message - - the actual MQTT message is sent via the MQTT broker on MQTT topic `` -- an MQTT message containing the ROS message type information is received on MQTT topic `mqtt_client/ros_msg_type/` - - message type information is extracted and the ShapeShifter ROS publisher is configured - - information about whether a timestamp is injected is stored for the specific topic -- an MQTT message containing the actual ROS message is received - - depending on whether a timestamp is injected, it is decoded into the serialized ROS message and the serialized timestamp - - if the message contained a timestamp, the latency is computed and published on ROS topic `~/latencies/` - - the serialized ROS message is published using the *ShapeShifter* on ROS topic `` - - ## Acknowledgements 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). diff --git a/mqtt_client/CMakeLists.txt b/mqtt_client/CMakeLists.txt index de5dc92..15d14e6 100644 --- a/mqtt_client/CMakeLists.txt +++ b/mqtt_client/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 3.12.0 FATAL_ERROR) project(mqtt_client) -find_package(ros_environment REQUIRED QUIET) -set(ROS_VERSION $ENV{ROS_VERSION}) - ## Compile as C++17 add_compile_options(-std=c++17) link_libraries("$<$,$,9.0>>:-lstdc++fs>") @@ -11,301 +8,56 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# === ROS2 (AMENT) ============================================================= -if(${ROS_VERSION} EQUAL 2) - - find_package(ament_cmake REQUIRED) - - find_package(fmt REQUIRED) - find_package(mqtt_client_interfaces REQUIRED) - find_package(rclcpp REQUIRED) - find_package(rclcpp_components REQUIRED) - find_package(std_msgs REQUIRED) - - # Paho MQTT C++ apt package doesn't include CMake config - # find_package(PahoMqttCpp REQUIRED) - find_library(PahoMqttC_LIBRARY libpaho-mqtt3as.so.1 REQUIRED) - find_library(PahoMqttCpp_LIBRARY libpaho-mqttpp3.so.1 REQUIRED) - - add_library(${PROJECT_NAME}_lib SHARED src/MqttClient.ros2.cpp) - - rclcpp_components_register_node(${PROJECT_NAME}_lib - PLUGIN "mqtt_client::MqttClient" - EXECUTABLE ${PROJECT_NAME} - ) - - target_include_directories(${PROJECT_NAME}_lib PUBLIC - $ - $) - - target_link_libraries(${PROJECT_NAME}_lib - ${PahoMqttC_LIBRARY} - ${PahoMqttCpp_LIBRARY} - ) - - ament_target_dependencies(${PROJECT_NAME}_lib - fmt - mqtt_client_interfaces - rclcpp - rclcpp_components - std_msgs - ) - - install(TARGETS ${PROJECT_NAME}_lib - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - ) - - install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} - FILES_MATCHING PATTERN "*ros2*" - ) - - install( - DIRECTORY config - DESTINATION share/${PROJECT_NAME} - FILES_MATCHING PATTERN "*ros2*" - ) - - # if(BUILD_TESTING) - # find_package(ament_lint_auto REQUIRED) - # # the following line skips the linter which checks for copyrights - # # comment the line when a copyright and license is added to all source files - # set(ament_cmake_copyright_FOUND TRUE) - # # the following line skips cpplint (only works in a git repo) - # # comment the line when this package is in a git repo and when - # # a copyright and license is added to all source files - # set(ament_cmake_cpplint_FOUND TRUE) - # ament_lint_auto_find_test_dependencies() - # endif() - - ament_package() - -# === ROS1 (CATKIN) ============================================================ -elseif(${ROS_VERSION} EQUAL 1) - - ## Find catkin macros and libraries - ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) - ## is used, also find other catkin packages - find_package(catkin REQUIRED COMPONENTS - mqtt_client_interfaces - nodelet - roscpp - std_msgs - topic_tools - ) - - ## System dependencies are found with CMake's conventions - find_package(PahoMqttCpp REQUIRED) - set(PahoMqttCpp_LIBRARIES PahoMqttCpp::paho-mqttpp3) - - find_package(fmt REQUIRED) - set(fmt_LIBRARIES fmt::fmt) - - - ## Uncomment this if the package has a setup.py. This macro ensures - ## modules and global scripts declared therein get installed - ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html - # catkin_python_setup() - - ################################################ - ## Declare ROS messages, services and actions ## - ################################################ - - ## To declare and build messages, services or actions from within this - ## package, follow these steps: - ## * Let MSG_DEP_SET be the set of packages whose message types you use in - ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). - ## * In the file package.xml: - ## * add a build_depend tag for "message_generation" - ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET - ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in - ## but can be declared for certainty nonetheless: - ## * add a exec_depend tag for "message_runtime" - ## * In this file (CMakeLists.txt): - ## * add "message_generation" and every package in MSG_DEP_SET to - ## find_package(catkin REQUIRED COMPONENTS ...) - ## * add "message_runtime" and every package in MSG_DEP_SET to - ## catkin_package(CATKIN_DEPENDS ...) - ## * uncomment the add_*_files sections below as needed - ## and list every .msg/.srv/.action file to be processed - ## * uncomment the generate_messages entry below - ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - - ## Generate messages in the 'msg' folder - # add_message_files( - # FILES - # Message1.msg - # Message2.msg - # ) - - ## Generate services in the 'srv' folder - # add_service_files( - # FILES - # Service1.srv - # Service2.srv - # ) +find_package(ament_cmake REQUIRED) +find_package(fmt REQUIRED) +find_package(mqtt_client_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) + +# Paho MQTT C++ apt package doesn't include CMake config +find_library(PahoMqttC_LIBRARY libpaho-mqtt3as.so.1 REQUIRED) +find_library(PahoMqttCpp_LIBRARY libpaho-mqttpp3.so.1 REQUIRED) + +add_library(${PROJECT_NAME}_lib SHARED src/MqttClient.cpp) + +rclcpp_components_register_node(${PROJECT_NAME}_lib + PLUGIN "mqtt_client::MqttClient" + EXECUTABLE ${PROJECT_NAME} +) + +target_include_directories(${PROJECT_NAME}_lib PUBLIC + $ + $) + +target_link_libraries(${PROJECT_NAME}_lib + ${PahoMqttC_LIBRARY} + ${PahoMqttCpp_LIBRARY} +) + +ament_target_dependencies(${PROJECT_NAME}_lib + fmt + mqtt_client_interfaces + rclcpp + rclcpp_components + std_msgs +) + +install(TARGETS ${PROJECT_NAME}_lib + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() - ## Generate actions in the 'action' folder - # add_action_files( - # FILES - # Action1.action - # Action2.action - # ) - - ## Generate added messages and services with any dependencies listed here - # generate_messages( - # DEPENDENCIES - # std_msgs - # ) - - ################################################ - ## Declare ROS dynamic reconfigure parameters ## - ################################################ - - ## To declare and build dynamic reconfigure parameters within this - ## package, follow these steps: - ## * In the file package.xml: - ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" - ## * In this file (CMakeLists.txt): - ## * add "dynamic_reconfigure" to - ## find_package(catkin REQUIRED COMPONENTS ...) - ## * uncomment the "generate_dynamic_reconfigure_options" section below - ## and list every .cfg file to be processed - - ## Generate dynamic reconfigure parameters in the 'cfg' folder - # generate_dynamic_reconfigure_options( - # cfg/params.cfg - # ) - - ################################### - ## catkin specific configuration ## - ################################### - ## The catkin_package macro generates cmake config files for your package - ## Declare things to be passed to dependent projects - ## INCLUDE_DIRS: uncomment this if your package contains header files - ## LIBRARIES: libraries you create in this project that dependent projects also need - ## CATKIN_DEPENDS: catkin_packages dependent projects also need - ## DEPENDS: system dependencies of this project that dependent projects also need - catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - nodelet - roscpp - std_msgs - topic_tools - DEPENDS - fmt - PahoMqttCpp - ) - - ########### - ## Build ## - ########### - - ## Specify additional locations of header files - ## Your package locations should be listed before other locations - include_directories( - include - ${catkin_INCLUDE_DIRS} - ) - - ## Declare a C++ library - add_library(${PROJECT_NAME} - src/MqttClient.cpp - ) - - ## Add cmake target dependencies of the library - ## as an example, code may need to be generated before libraries - ## either from message generation or dynamic reconfigure - # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - - ## Declare a C++ executable - ## With catkin_make all packages are built within a single CMake context - ## The recommended prefix ensures that target names across packages don't collide - # add_executable(${PROJECT_NAME}_node src/hx_testmanager_node.cpp) - - ## Rename C++ executable without prefix - ## The above recommended prefix causes long target names, the following renames the - ## target back to the shorter version for ease of user use - ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" - # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - - ## Add cmake target dependencies of the executable - ## same as for the library above - add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - - ## Specify libraries to link a library or executable target against - target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${fmt_LIBRARIES} - ${PahoMqttCpp_LIBRARIES} - ) - - ############# - ## Install ## - ############# - - # all install targets should use catkin DESTINATION variables - # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - - ## Mark executable scripts (Python etc.) for installation - ## in contrast to setup.py, you can choose the destination - # install(PROGRAMS - # scripts/my_python_script - # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - # ) - - ## Mark executables and/or libraries for installation - install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - - ## Mark cpp header files for installation - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - # FILES_MATCHING PATTERN "*.h" - # PATTERN ".svn" EXCLUDE - ) - - ## Mark other files for installation (e.g. launch and bag files, etc.) - install(FILES - nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - ) - install(DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "*ros2*" EXCLUDE - ) - install(DIRECTORY - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "*ros2*" EXCLUDE - ) - - ############# - ## Testing ## - ############# - - ## Add gtest based cpp test target and link libraries - # catkin_add_gtest(${PROJECT_NAME}-test test/test_hx_testmanager.cpp) - #if(CATKIN_ENABLE_TESTING) - # find_package(rostest REQUIRED) - # add_rostest_gtest(test_ika_dogm test/ika_dogm.test test/UnitTest.cpp) - # catkin_add_gtest(test_ika_dogm test/UnitTest.cpp) - # target_link_libraries(test_ika_dogm ${catkin_LIBRARIES} ${PROJECT_NAME}_dogm_creation) - #endif() - # if(TARGET ${PROJECT_NAME}-test) - # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) - # endif() - - ## Add folders to be run by python nosetests - # catkin_add_nosetests(test) - -endif() diff --git a/mqtt_client/config/params.aws.yaml b/mqtt_client/config/params.aws.yaml index 4114b8d..d330469 100644 --- a/mqtt_client/config/params.aws.yaml +++ b/mqtt_client/config/params.aws.yaml @@ -1,49 +1,52 @@ -# YAML alias for MQTT SSL version. -# The supported values are defined at: -# https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305 -tls_1_2: &tls_1_2 3 - -broker: - # YOU MUST CHANGE THIS ENDPOINT - # Can be found by executing: aws iot describe-endpoint --endpoint-type iot:Data-ATS - host: not-a-real-endpoint-please-use-your-own.us-west-2.amazonaws.com - port: 8883 - tls: - enabled: true - # Available at https://www.amazontrust.com/repository/AmazonRootCA1.pem - ca_certificate: path/to/AmazonRootCA1.pem -client: - id: user - # Whether or not to start a clean session with each reconnect. - # If True, the server will forget all subscriptions with each reconnect. - # Set False to request that the server resume an existing session or start - # a new session that may be resumed after a connection loss. - clean_session: false - # The keep alive value, in seconds, to send in CONNECT packet. - keep_alive_interval: 6.0 - tls: - # The certificate generated by the AWS IoT Core service - certificate: path/to/a-certificate.pem - # The private key generated by the AWS IoT Core service - key: path/to/a-private-key.pem - # AWS uses TLS v1.2 to encrypt all communication. - version: *tls_1_2 - verify: true - # https://www.openssl.org/docs/man1.1.1/man3/SSL_CTX_set_alpn_protos.html - alpn_protos: - # MQTT over AWS IOT requires an ALPN protocol negotiation. - # https://docs.aws.amazon.com/iot/latest/developerguide/protocols.html - - x-amzn-mqtt-ca - buffer: - enabled: true -bridge: - # NOTE: It seems that AWS IOT only supports primitive topics. Using non-primitive - # types results in the error message `Connection to broker lost, will try to reconnect...` - ros2mqtt: - - ros_topic: /ping/primitive - mqtt_topic: pingpong/primitive - primitive: true - mqtt2ros: - - mqtt_topic: pingpong/primitive - ros_topic: /pong/primitive - primitive: true +/**/*: + ros__parameters: + broker: + # YOU MUST CHANGE THIS ENDPOINT + # Can be found by executing: aws iot describe-endpoint --endpoint-type iot:Data-ATS + host: not-a-real-endpoint-please-use-your-own.us-west-2.amazonaws.com + port: 8883 + tls: + enabled: true + # Available at https://www.amazontrust.com/repository/AmazonRootCA1.pem + ca_certificate: path/to/AmazonRootCA1.pem + client: + id: user + # Whether or not to start a clean session with each reconnect. + # If True, the server will forget all subscriptions with each reconnect. + # Set False to request that the server resume an existing session or start + # a new session that may be resumed after a connection loss. + clean_session: false + # The keep alive value, in seconds, to send in CONNECT packet. + keep_alive_interval: 6.0 + tls: + # The certificate generated by the AWS IoT Core service + certificate: path/to/a-certificate.pem + # The private key generated by the AWS IoT Core service + key: path/to/a-private-key.pem + # AWS uses TLS v1.2 to encrypt all communication. + # The supported values are defined at: + # https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305 + version: 3 # TLS v1.2 + verify: true + # https://www.openssl.org/docs/man1.1.1/man3/SSL_CTX_set_alpn_protos.html + alpn_protos: + # MQTT over AWS IOT requires an ALPN protocol negotiation. + # https://docs.aws.amazon.com/iot/latest/developerguide/protocols.html + - x-amzn-mqtt-ca + buffer: + enabled: true + bridge: + # NOTE: It seems that AWS IOT only supports primitive topics. Using non-primitive + # types results in the error message `Connection to broker lost, will try to reconnect...` + ros2mqtt: + ros_topics: + - /ping/primitive + /ping/primitive: + mqtt_topic: pingpong/primitive + primitive: true + mqtt2ros: + mqtt_topics: + - pingpong/primitive + pingpong/primitive: + ros_topic: /pong/primitive + primitive: true diff --git a/mqtt_client/config/params.ros2.fixed-type-and-qos.yaml b/mqtt_client/config/params.fixed-type-and-qos.yaml similarity index 100% rename from mqtt_client/config/params.ros2.fixed-type-and-qos.yaml rename to mqtt_client/config/params.fixed-type-and-qos.yaml diff --git a/mqtt_client/config/params.ros2.primitive-fixed-type-and-qos.yaml b/mqtt_client/config/params.primitive-fixed-type-and-qos.yaml similarity index 100% rename from mqtt_client/config/params.ros2.primitive-fixed-type-and-qos.yaml rename to mqtt_client/config/params.primitive-fixed-type-and-qos.yaml diff --git a/mqtt_client/config/params.ros2.primitive.yaml b/mqtt_client/config/params.primitive.yaml similarity index 100% rename from mqtt_client/config/params.ros2.primitive.yaml rename to mqtt_client/config/params.primitive.yaml diff --git a/mqtt_client/config/params.ros2.aws.yaml b/mqtt_client/config/params.ros2.aws.yaml deleted file mode 100644 index d330469..0000000 --- a/mqtt_client/config/params.ros2.aws.yaml +++ /dev/null @@ -1,52 +0,0 @@ -/**/*: - ros__parameters: - broker: - # YOU MUST CHANGE THIS ENDPOINT - # Can be found by executing: aws iot describe-endpoint --endpoint-type iot:Data-ATS - host: not-a-real-endpoint-please-use-your-own.us-west-2.amazonaws.com - port: 8883 - tls: - enabled: true - # Available at https://www.amazontrust.com/repository/AmazonRootCA1.pem - ca_certificate: path/to/AmazonRootCA1.pem - client: - id: user - # Whether or not to start a clean session with each reconnect. - # If True, the server will forget all subscriptions with each reconnect. - # Set False to request that the server resume an existing session or start - # a new session that may be resumed after a connection loss. - clean_session: false - # The keep alive value, in seconds, to send in CONNECT packet. - keep_alive_interval: 6.0 - tls: - # The certificate generated by the AWS IoT Core service - certificate: path/to/a-certificate.pem - # The private key generated by the AWS IoT Core service - key: path/to/a-private-key.pem - # AWS uses TLS v1.2 to encrypt all communication. - # The supported values are defined at: - # https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305 - version: 3 # TLS v1.2 - verify: true - # https://www.openssl.org/docs/man1.1.1/man3/SSL_CTX_set_alpn_protos.html - alpn_protos: - # MQTT over AWS IOT requires an ALPN protocol negotiation. - # https://docs.aws.amazon.com/iot/latest/developerguide/protocols.html - - x-amzn-mqtt-ca - buffer: - enabled: true - bridge: - # NOTE: It seems that AWS IOT only supports primitive topics. Using non-primitive - # types results in the error message `Connection to broker lost, will try to reconnect...` - ros2mqtt: - ros_topics: - - /ping/primitive - /ping/primitive: - mqtt_topic: pingpong/primitive - primitive: true - mqtt2ros: - mqtt_topics: - - pingpong/primitive - pingpong/primitive: - ros_topic: /pong/primitive - primitive: true diff --git a/mqtt_client/config/params.ros2.yaml b/mqtt_client/config/params.ros2.yaml deleted file mode 100644 index dd6a862..0000000 --- a/mqtt_client/config/params.ros2.yaml +++ /dev/null @@ -1,16 +0,0 @@ -/**/*: - ros__parameters: - broker: - host: localhost - port: 1883 - bridge: - ros2mqtt: - ros_topics: - - /ping/ros - /ping/ros: - mqtt_topic: pingpong/ros - mqtt2ros: - mqtt_topics: - - pingpong/ros - pingpong/ros: - ros_topic: /pong/ros diff --git a/mqtt_client/config/params.yaml b/mqtt_client/config/params.yaml index bf0a2d2..dd6a862 100644 --- a/mqtt_client/config/params.yaml +++ b/mqtt_client/config/params.yaml @@ -1,16 +1,16 @@ -broker: - host: localhost - port: 1883 -bridge: - ros2mqtt: - - ros_topic: /ping/ros - mqtt_topic: pingpong/ros - - ros_topic: /ping/primitive - mqtt_topic: pingpong/primitive - primitive: true - mqtt2ros: - - mqtt_topic: pingpong/ros - ros_topic: /pong/ros - - mqtt_topic: pingpong/primitive - ros_topic: /pong/primitive - primitive: true \ No newline at end of file +/**/*: + ros__parameters: + broker: + host: localhost + port: 1883 + bridge: + ros2mqtt: + ros_topics: + - /ping/ros + /ping/ros: + mqtt_topic: pingpong/ros + mqtt2ros: + mqtt_topics: + - pingpong/ros + pingpong/ros: + ros_topic: /pong/ros diff --git a/mqtt_client/doc/mainpage.dox b/mqtt_client/doc/mainpage.dox index 3db36d0..2afe0d9 100644 --- a/mqtt_client/doc/mainpage.dox +++ b/mqtt_client/doc/mainpage.dox @@ -4,7 +4,7 @@ \htmlinclude manifest.html -\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. +\b mqtt_client provides a ROS 2 component node 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. 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. diff --git a/mqtt_client/include/mqtt_client/MqttClient.h b/mqtt_client/include/mqtt_client/MqttClient.h deleted file mode 100644 index e89e33a..0000000 --- a/mqtt_client/include/mqtt_client/MqttClient.h +++ /dev/null @@ -1,549 +0,0 @@ -/* -============================================================================== -MIT License - -Copyright 2022 Institute for Automotive Engineering of RWTH Aachen University. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -============================================================================== -*/ - - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - - -/** - * @brief Namespace for the mqtt_client package - */ -namespace mqtt_client { - - -/** - * @brief ROS Nodelet for sending and receiving ROS messages via MQTT - * - * The MqttClient enables connected ROS-based devices or robots to - * exchange ROS messages via an MQTT broker using the MQTT protocol. - * This works generically for any ROS message, i.e. there is no need - * to specify the ROS message type for ROS messages you wish to - * exchange via the MQTT broker. - */ -class MqttClient : public nodelet::Nodelet, - public virtual mqtt::callback, - public virtual mqtt::iaction_listener { - - protected: - /** - * @brief Initializes nodelet when nodelet is loaded. - * - * Overrides nodelet::Nodelet::onInit(). - */ - virtual void onInit() override; - - /** - * @brief Loads ROS parameters from parameter server. - */ - void loadParameters(); - - /** - * @brief Loads requested ROS parameter from parameter server. - * - * @param[in] key parameter name - * @param[out] value variable where to store the retrieved parameter - * - * @return true if parameter was successfully retrieved - * @return false if parameter was not found - */ - bool loadParameter(const std::string& key, std::string& value); - - /** - * @brief Loads requested ROS parameter from parameter server, allows default - * value. - * - * @param[in] key parameter name - * @param[out] value variable where to store the retrieved parameter - * @param[in] default_value default value - * - * @return true if parameter was successfully retrieved - * @return false if parameter was not found or default was used - */ - bool loadParameter(const std::string& key, std::string& value, - const std::string& default_value); - - /** - * @brief Loads requested ROS parameter from parameter server. - * - * @tparam T type (one of int, double, bool) - * - * @param[in] key parameter name - * @param[out] value variable where to store the retrieved parameter - * - * @return true if parameter was successfully retrieved - * @return false if parameter was not found - */ - template - bool loadParameter(const std::string& key, T& value); - - /** - * @brief Loads requested ROS parameter from parameter server, allows default - * value. - * - * @tparam T type (one of int, double, bool) - * - * @param[in] key parameter name - * @param[out] value variable where to store the retrieved parameter - * @param[in] default_value default value - * - * @return true if parameter was successfully retrieved - * @return false if parameter was not found or default was used - */ - template - bool loadParameter(const std::string& key, T& value, const T& default_value); - - /** - * @brief Loads requested ROS parameter from parameter server. - * - * @tparam T type (one of int, double, bool) - * - * @param[in] key parameter name - * @param[out] value variable where to store the retrieved parameter - * - * @return true if parameter was successfully retrieved - * @return false if parameter was not found - */ - template - bool loadParameter(const std::string& key, std::vector& value); - - /** - * @brief Loads requested ROS parameter from parameter server, allows default - * value. - * - * @tparam T type (one of int, double, bool) - * - * @param[in] key parameter name - * @param[out] value variable where to store the retrieved parameter - * @param[in] default_value default value - * - * @return true if parameter was successfully retrieved - * @return false if parameter was not found or default was used - */ - template - bool loadParameter(const std::string& key, std::vector& value, const std::vector& default_value); - - /** - * @brief Converts a string to a path object resolving paths relative to - * ROS_HOME. - * - * Resolves relative to CWD, if ROS_HOME is not set. - * Returns empty path, if argument is empty. - * - * @param path_string (relative) path as string - * - * @return std::filesystem::path path variable - */ - std::filesystem::path resolvePath(const std::string& path_string); - - /** - * @brief Initializes broker connection and subscriptions. - */ - void setup(); - - /** - * @brief Sets up the client connection options and initializes the client - * object. - */ - void setupClient(); - - /** - * @brief Connects to the broker using the member client and options. - */ - void connect(); - - /** - * @brief Serializes and publishes a generic ROS message to the MQTT broker. - * - * Before serializing the ROS message and publishing it to the MQTT broker, - * metadata on the ROS message type is extracted. This type information is - * also sent to the MQTT broker on a separate topic. - * - * The MQTT payload for the actual ROS message carries the following: - * - 0 or 1 (indicating if timestamp is injected (=1)) - * - serialized timestamp (optional) - * - serialized ROS message - * - * @param ros_msg generic ROS message - * @param ros_topic ROS topic where the message was published - */ - void ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg, - const std::string& ros_topic); - - /** - * @brief Publishes a ROS message received via MQTT to ROS. - * - * This utilizes the ShapeShifter stored for the MQTT topic on which the - * message was received. The ShapeShifter has to be configured to the ROS - * message type of the message. If the message carries an injected timestamp, - * the latency is computed and published. - * - * The MQTT payload is expected to carry the following: - * - 0 or 1 (indicating if timestamp is injected (=1)) - * - serialized timestamp (optional) - * - serialized ROS message - * - * @param mqtt_msg MQTT message - * @param arrival_stamp arrival timestamp used for latency computation - */ - void mqtt2ros(mqtt::const_message_ptr mqtt_msg, - const ros::WallTime& arrival_stamp = ros::WallTime::now()); - - /** - * @brief Publishes a primitive message received via MQTT to ROS. - * - * This tries to interpret the raw MQTT message as a bool, int, or float value - * in the given order before falling back to string. The message is then - * published as a corresponding primitive ROS message. This utilizes the - * ShapeShifter stored for the MQTT topic on which the message was received. - * The ShapeShifter is dynamically configured to the appropriate ROS message - * type. - * - * The following mappings from primitive type to ROS message type hold: - * bool: std_msgs/Bool - * int: std_msgs/Int32 - * float: std_msgs/Float32 - * string: std_msgs/String - * - * @param mqtt_msg MQTT message - */ - void mqtt2primitive(mqtt::const_message_ptr mqtt_msg); - - /** - * @brief Callback for when the client has successfully connected to the - * broker. - * - * Overrides mqtt::callback::connected(const std::string&). - * - * @param cause - */ - void connected(const std::string& cause) override; - - /** - * @brief Callback for when the client has lost connection to the broker. - * - * Overrides mqtt::callback::connection_lost(const std::string&). - * - * @param cause - */ - void connection_lost(const std::string& cause) override; - - /** - * @brief Returns whether the client is connected to the broker. - * - * @return true if client is connected to the broker - * @return false if client is not connected to the broker - */ - bool isConnected(); - - /** - * @brief ROS service returning whether the client is connected to the broker. - * - * @param request service request - * @param response service response - * - * @return true always - * @return false never - */ - bool isConnectedService( - mqtt_client_interfaces::IsConnected::Request& request, - mqtt_client_interfaces::IsConnected::Response& response); - - /** - * @brief Callback for when the client receives a MQTT message from the - * broker. - * - * Overrides mqtt::callback::message_arrived(mqtt::const_message_ptr). - * If the received MQTT message contains information about a ROS message type, - * the corresponding ROS publisher is configured. If the received MQTT message - * is a ROS message, the mqtt2ros conversion is called. - * - * @param mqtt_msg MQTT message - */ - void message_arrived(mqtt::const_message_ptr mqtt_msg) override; - - /** - * @brief Callback for when delivery for a MQTT message has been completed. - * - * Overrides mqtt::callback::delivery_complete(mqtt::delivery_token_ptr). - * - * @param token token tracking the message delivery - */ - void delivery_complete(mqtt::delivery_token_ptr token) override; - - /** - * @brief Callback for when a MQTT action succeeds. - * - * Overrides mqtt::iaction_listener::on_success(const mqtt::token&). - * Does nothing. - * - * @param token token tracking the action - */ - void on_success(const mqtt::token& token) override; - - /** - * @brief Callback for when a MQTT action fails. - * - * Overrides mqtt::iaction_listener::on_failure(const mqtt::token&). - * Logs error. - * - * @param token token tracking the action - */ - void on_failure(const mqtt::token& token) override; - - protected: - /** - * @brief Struct containing broker parameters - */ - struct BrokerConfig { - std::string host; ///< broker host - int port; ///< broker port - std::string user; ///< username - std::string pass; ///< password - struct { - bool enabled; ///< whether to connect via SSL/TLS - std::filesystem::path - ca_certificate; ///< public CA certificate trusted by client - } tls; ///< SSL/TLS-related variables - }; - - /** - * @brief Struct containing client parameters - */ - struct ClientConfig { - std::string id; ///< client unique ID - struct { - bool enabled; ///< whether client buffer is enabled - int size; ///< client buffer size - std::filesystem::path directory; ///< client buffer directory - } buffer; ///< client buffer-related variables - struct { - std::string topic; ///< last-will topic - std::string message; ///< last-will message - int qos; ///< last-will QoS value - bool retained; ///< whether last-will is retained - } last_will; ///< last-will-related variables - bool clean_session; ///< whether client requests clean session - double keep_alive_interval; ///< keep-alive interval - int max_inflight; ///< maximum number of inflight messages - struct { - std::filesystem::path certificate; ///< client certificate - std::filesystem::path key; ///< client private keyfile - std::string password; ///< decryption password for private key - int version; ///< TLS version (https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305) - bool verify; ///< Verify the client should conduct - ///< post-connect checks - std::vector alpn_protos; ///< list of ALPN protocols - } tls; ///< SSL/TLS-related variables - }; - - /** - * @brief Struct containing variables related to a ROS2MQTT connection. - */ - struct Ros2MqttInterface { - struct { - ros::Subscriber subscriber; ///< generic ROS subscriber - int queue_size = 1; ///< ROS subscriber queue size - } ros; ///< ROS-related variables - struct { - std::string topic; ///< MQTT topic - int qos = 0; ///< MQTT QoS value - bool retained = false; ///< whether to retain MQTT message - } mqtt; ///< MQTT-related variables - bool primitive = false; ///< whether to publish as primitive message - bool stamped = false; ///< whether to inject timestamp in MQTT message - }; - - /** - * @brief Struct containing variables related to a MQTT2ROS connection. - */ - struct Mqtt2RosInterface { - struct { - int qos = 0; ///< MQTT QoS value - } mqtt; ///< MQTT-related variables - struct { - std::string topic; ///< ROS topic - ros::Publisher publisher; ///< generic ROS publisher - topic_tools::ShapeShifter shape_shifter; ///< ROS msg type ShapeShifter - ros::Publisher latency_publisher; ///< ROS publisher for latency - int queue_size = 1; ///< ROS publisher queue size - bool latched = false; ///< whether to latch ROS message - } ros; ///< ROS-related variables - bool primitive = false; ///< whether to publish as primitive message (if - ///< coming from non-ROS MQTT client) - bool stamped = false; ///< whether timestamp is injected - }; - - protected: - /** - * @brief MQTT topic prefix under which ROS message type information is - * published - * - * Must contain trailing '/'. - */ - static const std::string kRosMsgTypeMqttTopicPrefix; - - /** - * @brief ROS topic prefix under which ROS2MQTT2ROS latencies are published - * - * Must contain trailing '/'. - */ - static const std::string kLatencyRosTopicPrefix; - - /** - * @brief ROS node handle - */ - ros::NodeHandle node_handle_; - - /** - * @brief Private ROS node handle - */ - ros::NodeHandle private_node_handle_; - - /** - * @brief ROS Service server for providing connection status - */ - ros::ServiceServer is_connected_service_; - - /** - * @brief Status variable keeping track of connection status to broker - */ - bool is_connected_ = false; - - /** - * @brief Broker parameters - */ - BrokerConfig broker_config_; - - /** - * @brief Client parameters - */ - ClientConfig client_config_; - - /** - * @brief MQTT client variable - */ - std::shared_ptr client_; - - /** - * @brief MQTT client connection options - */ - mqtt::connect_options connect_options_; - - /** - * @brief ROS2MQTT connection variables sorted by ROS topic - */ - std::map ros2mqtt_; - - /** - * @brief MQTT2ROS connection variables sorted by MQTT topic - */ - std::map mqtt2ros_; -}; - - -template -bool MqttClient::loadParameter(const std::string& key, T& value) { - bool found = private_node_handle_.getParam(key, value); - if (found) - NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), - std::to_string(value).c_str()); - return found; -} - - -template -bool MqttClient::loadParameter(const std::string& key, T& value, - const T& default_value) { - bool found = private_node_handle_.param(key, value, default_value); - if (!found) { - if (private_node_handle_.hasParam(key)) - NODELET_ERROR("Parameter '%s' has wrong data type", key.c_str()); - NODELET_WARN("Parameter '%s' not set, defaulting to '%s'", key.c_str(), - std::to_string(default_value).c_str()); - } - if (found) - NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), - std::to_string(value).c_str()); - return found; -} - - -template -bool MqttClient::loadParameter(const std::string& key, std::vector& value) -{ - const bool found = private_node_handle_.getParam(key, value); - if (found) - NODELET_DEBUG("Retrieved parameter '%s' = '[%s]'", key.c_str(), - fmt::format("{}", fmt::join(value, ", ")).c_str()); - return found; -} - - -template -bool MqttClient::loadParameter(const std::string& key, std::vector& value, - const std::vector& default_value) -{ - const bool found = private_node_handle_.param(key, value, default_value); - if (!found) - NODELET_WARN("Parameter '%s' not set, defaulting to '%s'", key.c_str(), - fmt::format("{}", fmt::join(value, ", ")).c_str()); - if (found) - NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), - fmt::format("{}", fmt::join(value, ", ")).c_str()); - return found; -} - - -/** - * Serializes a ROS message to a buffer. - * - * @tparam T ROS message type - * - * @param[in] msg ROS message - * @param[out] buffer buffer to serialize to - */ -template -void serializeRosMessage(const T& msg, std::vector& buffer) { - - const uint32_t length = ros::serialization::serializationLength(msg); - buffer.resize(length); - ros::serialization::OStream stream(buffer.data(), length); - ros::serialization::serialize(stream, msg); -} - -} // namespace mqtt_client diff --git a/mqtt_client/include/mqtt_client/MqttClient.ros2.hpp b/mqtt_client/include/mqtt_client/MqttClient.hpp similarity index 100% rename from mqtt_client/include/mqtt_client/MqttClient.ros2.hpp rename to mqtt_client/include/mqtt_client/MqttClient.hpp diff --git a/mqtt_client/launch/standalone.launch b/mqtt_client/launch/standalone.launch deleted file mode 100644 index be59d93..0000000 --- a/mqtt_client/launch/standalone.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/mqtt_client/launch/standalone.launch.ros2.xml b/mqtt_client/launch/standalone.launch.xml similarity index 92% rename from mqtt_client/launch/standalone.launch.ros2.xml rename to mqtt_client/launch/standalone.launch.xml index afbc43c..23d1e30 100644 --- a/mqtt_client/launch/standalone.launch.ros2.xml +++ b/mqtt_client/launch/standalone.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/mqtt_client/nodelet_plugins.xml b/mqtt_client/nodelet_plugins.xml deleted file mode 100644 index 53a427c..0000000 --- a/mqtt_client/nodelet_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - mqtt_client nodelet - - - diff --git a/mqtt_client/package.xml b/mqtt_client/package.xml index 757ad06..1ec4050 100644 --- a/mqtt_client/package.xml +++ b/mqtt_client/package.xml @@ -19,28 +19,17 @@ fmt mqtt_client_interfaces - ros_environment std_msgs - - ament_cmake - libpaho-mqtt-dev - libpaho-mqttpp-dev - rclcpp - rclcpp_components - rcpputils - - - catkin - nodelet - paho-mqtt-cpp - roscpp - topic_tools + ament_cmake + libpaho-mqtt-dev + libpaho-mqttpp-dev + rclcpp + rclcpp_components + rcpputils - catkin - ament_cmake - + ament_cmake diff --git a/mqtt_client/src/MqttClient.cpp b/mqtt_client/src/MqttClient.cpp index 44f9e89..cfd599d 100644 --- a/mqtt_client/src/MqttClient.cpp +++ b/mqtt_client/src/MqttClient.cpp @@ -1,949 +1,1518 @@ -/* -============================================================================== -MIT License - -Copyright 2022 Institute for Automotive Engineering of RWTH Aachen University. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -============================================================================== -*/ - - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -PLUGINLIB_EXPORT_CLASS(mqtt_client::MqttClient, nodelet::Nodelet) - - -namespace mqtt_client { - - -const std::string MqttClient::kRosMsgTypeMqttTopicPrefix = - "mqtt_client/ros_msg_type/"; - -const std::string MqttClient::kLatencyRosTopicPrefix = "latencies/"; - - -/** - * @brief Extracts string of primitive data types from ROS message. - * - * This is helpful to extract the actual data payload of a primitive ROS - * message. If e.g. an std_msgs/String is serialized to a string - * representation, it also contains the field name 'data'. This function - * instead returns the underlying value as string. - * - * The following primitive ROS message types are supported: - * std_msgs/String - * std_msgs/Bool - * std_msgs/Char - * std_msgs/UInt8 - * std_msgs/UInt16 - * std_msgs/UInt32 - * std_msgs/UInt64 - * std_msgs/Int8 - * std_msgs/Int16 - * std_msgs/Int32 - * std_msgs/Int64 - * std_msgs/Float32 - * std_msgs/Float64 - * - * @param [in] msg generic ShapeShifter ROS message - * @param [out] primitive string representation of primitive message data - * - * @return true if primitive ROS message type was found - * @return false if ROS message type is not primitive - */ -bool primitiveRosMessageToString(const topic_tools::ShapeShifter::ConstPtr& msg, - std::string& primitive) { - - bool found_primitive = true; - const std::string& msg_type_md5 = msg->getMD5Sum(); - - if (msg_type_md5 == ros::message_traits::MD5Sum::value()) { - primitive = msg->instantiate()->data; - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = msg->instantiate()->data ? "true" : "false"; - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else if (msg_type_md5 == - ros::message_traits::MD5Sum::value()) { - primitive = std::to_string(msg->instantiate()->data); - } else { - found_primitive = false; - } - - return found_primitive; -} - - -void MqttClient::onInit() { - - // get nodehandles - node_handle_ = this->getMTNodeHandle(); - private_node_handle_ = this->getMTPrivateNodeHandle(); - - loadParameters(); - setup(); -} - - -void MqttClient::loadParameters() { - - // load broker parameters from parameter server - std::string broker_tls_ca_certificate; - loadParameter("broker/host", broker_config_.host, "localhost"); - loadParameter("broker/port", broker_config_.port, 1883); - if (loadParameter("broker/user", broker_config_.user)) { - loadParameter("broker/pass", broker_config_.pass, ""); - } - if (loadParameter("broker/tls/enabled", broker_config_.tls.enabled, false)) { - loadParameter("broker/tls/ca_certificate", broker_tls_ca_certificate, - "/etc/ssl/certs/ca-certificates.crt"); - } - - // load client parameters from parameter server - std::string client_buffer_directory, client_tls_certificate, client_tls_key; - loadParameter("client/id", client_config_.id, ""); - client_config_.buffer.enabled = !client_config_.id.empty(); - if (client_config_.buffer.enabled) { - loadParameter("client/buffer/size", client_config_.buffer.size, 0); - loadParameter("client/buffer/directory", client_buffer_directory, "buffer"); - } else { - NODELET_WARN("Client buffer can not be enabled when client ID is empty"); - } - if (loadParameter("client/last_will/topic", client_config_.last_will.topic)) { - loadParameter("client/last_will/message", client_config_.last_will.message, - "offline"); - loadParameter("client/last_will/qos", client_config_.last_will.qos, 0); - loadParameter("client/last_will/retained", - client_config_.last_will.retained, false); - } - loadParameter("client/clean_session", client_config_.clean_session, true); - loadParameter("client/keep_alive_interval", - client_config_.keep_alive_interval, 60.0); - loadParameter("client/max_inflight", client_config_.max_inflight, 65535); - if (broker_config_.tls.enabled) { - if (loadParameter("client/tls/certificate", client_tls_certificate)) { - loadParameter("client/tls/key", client_tls_key); - loadParameter("client/tls/password", client_config_.tls.password); - loadParameter("client/tls/version", client_config_.tls.version); - loadParameter("client/tls/verify", client_config_.tls.verify); - loadParameter("client/tls/alpn_protos", client_config_.tls.alpn_protos); - } - } - - // resolve filepaths - broker_config_.tls.ca_certificate = resolvePath(broker_tls_ca_certificate); - client_config_.buffer.directory = resolvePath(client_buffer_directory); - client_config_.tls.certificate = resolvePath(client_tls_certificate); - client_config_.tls.key = resolvePath(client_tls_key); - - // load bridge parameters from parameter server - XmlRpc::XmlRpcValue bridge; - if (!private_node_handle_.getParam("bridge", bridge)) { - NODELET_ERROR("Parameter 'bridge' is required"); - exit(EXIT_FAILURE); - } - - // parse bridge parameters - try { - - // ros2mqtt - if (bridge.hasMember("ros2mqtt")) { - - // loop over array - XmlRpc::XmlRpcValue& ros2mqtt_params = bridge["ros2mqtt"]; - for (int k = 0; k < ros2mqtt_params.size(); k++) { - - if (ros2mqtt_params[k].hasMember("ros_topic") && - ros2mqtt_params[k].hasMember("mqtt_topic")) { - - // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic - std::string& ros_topic = ros2mqtt_params[k]["ros_topic"]; - Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; - ros2mqtt.mqtt.topic = std::string(ros2mqtt_params[k]["mqtt_topic"]); - - // ros2mqtt[k]/primitive - if (ros2mqtt_params[k].hasMember("primitive")) - ros2mqtt.primitive = ros2mqtt_params[k]["primitive"]; - - // ros2mqtt[k]/inject_timestamp - if (ros2mqtt_params[k].hasMember("inject_timestamp")) - ros2mqtt.stamped = ros2mqtt_params[k]["inject_timestamp"]; - if (ros2mqtt.stamped && ros2mqtt.primitive) { - NODELET_WARN( - "Timestamp will not be injected into primitive messages on ROS " - "topic '%s'", - ros_topic.c_str()); - ros2mqtt.stamped = false; - } - - // ros2mqtt[k]/advanced - if (ros2mqtt_params[k].hasMember("advanced")) { - XmlRpc::XmlRpcValue& advanced_params = - ros2mqtt_params[k]["advanced"]; - if (advanced_params.hasMember("ros")) { - if (advanced_params["ros"].hasMember("queue_size")) - ros2mqtt.ros.queue_size = advanced_params["ros"]["queue_size"]; - } - if (advanced_params.hasMember("mqtt")) { - if (advanced_params["mqtt"].hasMember("qos")) - ros2mqtt.mqtt.qos = advanced_params["mqtt"]["qos"]; - if (advanced_params["mqtt"].hasMember("retained")) - ros2mqtt.mqtt.retained = advanced_params["mqtt"]["retained"]; - } - } - - NODELET_INFO("Bridging %sROS topic '%s' to MQTT topic '%s' %s", - ros2mqtt.primitive ? "primitive " : "", - ros_topic.c_str(), ros2mqtt.mqtt.topic.c_str(), - ros2mqtt.stamped ? "and measuring latency" : ""); - } else { - NODELET_WARN( - "Parameter 'bridge/ros2mqtt[%d]' is missing subparameter " - "'ros_topic' or 'mqtt_topic', will be ignored", - k); - } - } - } - - // mqtt2ros - if (bridge.hasMember("mqtt2ros")) { - - // loop over array - XmlRpc::XmlRpcValue& mqtt2ros_params = bridge["mqtt2ros"]; - for (int k = 0; k < mqtt2ros_params.size(); k++) { - - - if (mqtt2ros_params[k].hasMember("mqtt_topic") && - mqtt2ros_params[k].hasMember("ros_topic")) { - - // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic - std::string& mqtt_topic = mqtt2ros_params[k]["mqtt_topic"]; - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - mqtt2ros.ros.topic = std::string(mqtt2ros_params[k]["ros_topic"]); - - // mqtt2ros[k]/primitive - if (mqtt2ros_params[k].hasMember("primitive")) - mqtt2ros.primitive = mqtt2ros_params[k]["primitive"]; - - // mqtt2ros[k]/advanced - if (mqtt2ros_params[k].hasMember("advanced")) { - XmlRpc::XmlRpcValue& advanced_params = - mqtt2ros_params[k]["advanced"]; - if (advanced_params.hasMember("mqtt")) { - if (advanced_params["mqtt"].hasMember("qos")) - mqtt2ros.mqtt.qos = advanced_params["mqtt"]["qos"]; - } - if (advanced_params.hasMember("ros")) { - if (advanced_params["ros"].hasMember("queue_size")) - mqtt2ros.ros.queue_size = advanced_params["ros"]["queue_size"]; - if (advanced_params["ros"].hasMember("latched")) - mqtt2ros.ros.latched = advanced_params["ros"]["latched"]; - } - } - - NODELET_INFO( - "Bridging MQTT topic '%s' to %sROS topic '%s'", mqtt_topic.c_str(), - mqtt2ros.primitive ? "primitive " : "", mqtt2ros.ros.topic.c_str()); - } else { - NODELET_WARN( - "Parameter 'bridge/mqtt2ros[%d]' is missing subparameter " - "'mqtt_topic' or 'ros_topic', will be ignored", - k); - } - } - } - - if (ros2mqtt_.empty() && mqtt2ros_.empty()) { - NODELET_ERROR("No valid ROS-MQTT bridge found in parameter 'bridge'"); - exit(EXIT_FAILURE); - } - - } catch (XmlRpc::XmlRpcException e) { - NODELET_ERROR("Parameter 'bridge' could not be parsed (XmlRpc %d: %s)", - e.getCode(), e.getMessage().c_str()); - exit(EXIT_FAILURE); - } -} - - -bool MqttClient::loadParameter(const std::string& key, std::string& value) { - bool found = private_node_handle_.getParam(key, value); - if (found) - NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), - value.c_str()); - return found; -} - - -bool MqttClient::loadParameter(const std::string& key, std::string& value, - const std::string& default_value) { - bool found = - private_node_handle_.param(key, value, default_value); - if (!found) { - if (private_node_handle_.hasParam(key)) - NODELET_ERROR("Parameter '%s' has wrong data type", key.c_str()); - NODELET_WARN("Parameter '%s' not set, defaulting to '%s'", key.c_str(), - default_value.c_str()); - } - if (found) - NODELET_DEBUG("Retrieved parameter '%s' = '%s'", key.c_str(), - value.c_str()); - return found; -} - - -std::filesystem::path MqttClient::resolvePath(const std::string& path_string) { - - std::filesystem::path path(path_string); - if (path_string.empty()) return path; - if (!path.has_root_path()) { - std::string ros_home; - ros::get_environment_variable(ros_home, "ROS_HOME"); - if (ros_home.empty()) - ros_home = std::string(std::filesystem::current_path()); - path = std::filesystem::path(ros_home); - path.append(path_string); - } - if (!std::filesystem::exists(path)) - NODELET_WARN("Requested path '%s' does not exist", - std::string(path).c_str()); - return path; -} - - -void MqttClient::setup() { - - // initialize MQTT client - setupClient(); - - // connect to MQTT broker - connect(); - - // create ROS service server - is_connected_service_ = private_node_handle_.advertiseService( - "is_connected", &MqttClient::isConnectedService, this); - - // create ROS subscribers - for (auto& ros2mqtt_p : ros2mqtt_) { - const std::string& ros_topic = ros2mqtt_p.first; - Ros2MqttInterface& ros2mqtt = ros2mqtt_p.second; - const std::string& mqtt_topic = ros2mqtt.mqtt.topic; - ros2mqtt.ros.subscriber = - private_node_handle_.subscribe( - ros_topic, ros2mqtt.ros.queue_size, - boost::bind(&MqttClient::ros2mqtt, this, _1, ros_topic)); - NODELET_DEBUG("Subscribed ROS topic '%s'", ros_topic.c_str()); - } -} - - -void MqttClient::setupClient() { - - // basic client connection options - connect_options_.set_automatic_reconnect(true); - connect_options_.set_clean_session(client_config_.clean_session); - connect_options_.set_keep_alive_interval(client_config_.keep_alive_interval); - connect_options_.set_max_inflight(client_config_.max_inflight); - - // user authentication - if (!broker_config_.user.empty()) { - connect_options_.set_user_name(broker_config_.user); - connect_options_.set_password(broker_config_.pass); - } - - // last will - if (!client_config_.last_will.topic.empty()) { - mqtt::will_options will( - client_config_.last_will.topic, client_config_.last_will.message, - client_config_.last_will.qos, client_config_.last_will.retained); - connect_options_.set_will(will); - } - - // SSL/TLS - if (broker_config_.tls.enabled) { - mqtt::ssl_options ssl; - ssl.set_trust_store(broker_config_.tls.ca_certificate); - if (!client_config_.tls.certificate.empty() && - !client_config_.tls.key.empty()) { - ssl.set_key_store(client_config_.tls.certificate); - ssl.set_private_key(client_config_.tls.key); - if (!client_config_.tls.password.empty()) - ssl.set_private_key_password(client_config_.tls.password); - } - ssl.set_ssl_version(client_config_.tls.version); - ssl.set_verify(client_config_.tls.verify); - ssl.set_alpn_protos(client_config_.tls.alpn_protos); - connect_options_.set_ssl(ssl); - } - - // create MQTT client - const std::string protocol = broker_config_.tls.enabled ? "ssl" : "tcp"; - const std::string uri = fmt::format("{}://{}:{}", protocol, broker_config_.host, - broker_config_.port); - try { - if (client_config_.buffer.enabled) { - client_ = std::shared_ptr(new mqtt::async_client( - uri, client_config_.id, client_config_.buffer.size, - client_config_.buffer.directory)); - } else { - client_ = std::shared_ptr( - new mqtt::async_client(uri, client_config_.id)); - } - } catch (const mqtt::exception& e) { - ROS_ERROR("Client could not be initialized: %s", e.what()); - exit(EXIT_FAILURE); - } - - // setup MQTT callbacks - client_->set_callback(*this); -} - - -void MqttClient::connect() { - - std::string as_client = - client_config_.id.empty() - ? "" - : std::string(" as '") + client_config_.id + std::string("'"); - NODELET_INFO("Connecting to broker at '%s'%s ...", - client_->get_server_uri().c_str(), as_client.c_str()); - - try { - client_->connect(connect_options_, nullptr, *this); - } catch (const mqtt::exception& e) { - ROS_ERROR("Connection to broker failed: %s", e.what()); - exit(EXIT_FAILURE); - } -} - - -void MqttClient::ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg, - const std::string& ros_topic) { - - Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; - std::string mqtt_topic = ros2mqtt.mqtt.topic; - std::vector payload_buffer; - - // gather information on ROS message type in special ROS message - mqtt_client_interfaces::RosMsgType ros_msg_type; - ros_msg_type.md5 = ros_msg->getMD5Sum(); - ros_msg_type.name = ros_msg->getDataType(); - ros_msg_type.definition = ros_msg->getMessageDefinition(); - ros_msg_type.stamped = ros2mqtt.stamped; - - NODELET_DEBUG("Received ROS message of type '%s' on topic '%s'", - ros_msg_type.name.c_str(), ros_topic.c_str()); - - if (ros2mqtt.primitive) { // publish as primitive (string) message - - // resolve ROS messages to primitive strings if possible - std::string payload; - bool found_primitive = primitiveRosMessageToString(ros_msg, payload); - if (found_primitive) { - payload_buffer = std::vector(payload.begin(), payload.end()); - } else { - NODELET_WARN( - "Cannot send ROS message of type '%s' as primitive message, " - "check supported primitive types", - ros_msg_type.name.c_str()); - return; - } - - } else { // publish as complete ROS message incl. ROS message type - - // serialize ROS message type to buffer - uint32_t msg_type_length = - ros::serialization::serializationLength(ros_msg_type); - std::vector msg_type_buffer; - msg_type_buffer.resize(msg_type_length); - ros::serialization::OStream msg_type_stream(msg_type_buffer.data(), - msg_type_length); - ros::serialization::serialize(msg_type_stream, ros_msg_type); - - // send ROS message type information to MQTT broker - mqtt_topic = kRosMsgTypeMqttTopicPrefix + ros2mqtt.mqtt.topic; - try { - NODELET_DEBUG("Sending ROS message type to MQTT broker on topic '%s' ...", - mqtt_topic.c_str()); - mqtt::message_ptr mqtt_msg = - mqtt::make_message(mqtt_topic, msg_type_buffer.data(), - msg_type_buffer.size(), ros2mqtt.mqtt.qos, true); - client_->publish(mqtt_msg); - } catch (const mqtt::exception& e) { - NODELET_WARN( - "Publishing ROS message type information to MQTT topic '%s' failed: %s", - mqtt_topic.c_str(), e.what()); - } - - // build MQTT payload for ROS message (R) as [R] - // or [S, R] if timestamp (S) is included - uint32_t msg_length = ros::serialization::serializationLength(*ros_msg); - uint32_t payload_length = msg_length; - uint32_t stamp_length = - ros::serialization::serializationLength(ros::Time()); - uint32_t msg_offset = 0; - if (ros2mqtt.stamped) { - // allocate buffer with appropriate size to hold [S, R] - msg_offset += stamp_length; - payload_length += stamp_length; - payload_buffer.resize(payload_length); - } else { - // allocate buffer with appropriate size to hold [R] - payload_buffer.resize(payload_length); - } - - // serialize ROS message to payload [-, R] - ros::serialization::OStream msg_stream(&payload_buffer[msg_offset], - msg_length); - ros::serialization::serialize(msg_stream, *ros_msg); - - // inject timestamp as final step - if (ros2mqtt.stamped) { - - // take current timestamp - ros::WallTime stamp_wall = ros::WallTime::now(); - ros::Time stamp(stamp_wall.sec, stamp_wall.nsec); - if (stamp.isZero()) - NODELET_WARN( - "Injected ROS time 0 into MQTT payload on topic %s, is a ROS clock " - "running?", - ros2mqtt.mqtt.topic.c_str()); - - // serialize timestamp to payload [S, R] - ros::serialization::OStream stamp_stream(&payload_buffer[0], - stamp_length); - ros::serialization::serialize(stamp_stream, stamp); - } - } - - // send ROS message to MQTT broker - mqtt_topic = ros2mqtt.mqtt.topic; - try { - NODELET_DEBUG( - "Sending ROS message of type '%s' to MQTT broker on topic '%s' ...", - ros_msg->getDataType().c_str(), mqtt_topic.c_str()); - mqtt::message_ptr mqtt_msg = mqtt::make_message( - mqtt_topic, payload_buffer.data(), payload_buffer.size(), - ros2mqtt.mqtt.qos, ros2mqtt.mqtt.retained); - client_->publish(mqtt_msg); - } catch (const mqtt::exception& e) { - NODELET_WARN( - "Publishing ROS message type information to MQTT topic '%s' failed: %s", - mqtt_topic.c_str(), e.what()); - } -} - - -void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg, - const ros::WallTime& arrival_stamp) { - - std::string mqtt_topic = mqtt_msg->get_topic(); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - auto& payload = mqtt_msg->get_payload_ref(); - uint32_t payload_length = static_cast(payload.size()); - - // read MQTT payload for ROS message (R) as [R] - // or [S, R] if timestamp (S) is included - uint32_t msg_length = payload_length; - uint32_t msg_offset = 0; - - // if stamped, compute latency - if (mqtt2ros.stamped) { - - // create ROS message buffer on top of MQTT message payload - char* non_const_payload = const_cast(&payload[msg_offset]); - uint8_t* stamp_buffer = reinterpret_cast(non_const_payload); - - // deserialize stamp - ros::Time stamp; - uint32_t stamp_length = ros::serialization::serializationLength(stamp); - ros::serialization::IStream stamp_stream(stamp_buffer, stamp_length); - ros::serialization::deserialize(stamp_stream, stamp); - - // compute ROS2MQTT latency - ros::Time now(arrival_stamp.sec, arrival_stamp.nsec); - if (now.isZero()) - NODELET_WARN( - "Cannot compute latency for MQTT topic %s when ROS time is 0, is a ROS " - "clock running?", - mqtt_topic.c_str()); - ros::Duration latency = now - stamp; - std_msgs::Float64 latency_msg; - latency_msg.data = latency.toSec(); - - // publish latency - if (mqtt2ros.ros.latency_publisher.getTopic().empty()) { - std::string latency_topic = kLatencyRosTopicPrefix + mqtt2ros.ros.topic; - mqtt2ros.ros.latency_publisher = - private_node_handle_.advertise(latency_topic, 1); - } - mqtt2ros.ros.latency_publisher.publish(latency_msg); - - msg_length -= stamp_length; - msg_offset += stamp_length; - } - - // create ROS message buffer on top of MQTT message payload - char* non_const_payload = const_cast(&payload[msg_offset]); - uint8_t* msg_buffer = reinterpret_cast(non_const_payload); - ros::serialization::OStream msg_stream(msg_buffer, msg_length); - - // publish via ShapeShifter - NODELET_DEBUG( - "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", - mqtt2ros.ros.shape_shifter.getDataType().c_str(), - mqtt2ros.ros.topic.c_str()); - mqtt2ros.ros.shape_shifter.read(msg_stream); - mqtt2ros.ros.publisher.publish(mqtt2ros.ros.shape_shifter); -} - - -void MqttClient::mqtt2primitive(mqtt::const_message_ptr mqtt_msg) { - - std::string mqtt_topic = mqtt_msg->get_topic(); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - const std::string str_msg = mqtt_msg->to_string(); - - bool found_primitive = false; - std::string msg_type_md5; - std::string msg_type_name; - std::string msg_type_definition; - std::vector msg_buffer; - - // check for bool - if (!found_primitive) { - std::string bool_str = str_msg; - std::transform(str_msg.cbegin(), str_msg.cend(), bool_str.begin(), - ::tolower); - if (bool_str == "true" || bool_str == "false") { - - bool bool_msg = (bool_str == "true"); - - // construct and serialize ROS message - std_msgs::Bool msg; - msg.data = bool_msg; - serializeRosMessage(msg, msg_buffer); - - // collect ROS message type information - msg_type_md5 = ros::message_traits::MD5Sum::value(); - msg_type_name = ros::message_traits::DataType::value(); - msg_type_definition = - ros::message_traits::Definition::value(); - - found_primitive = true; - } - } - - // check for int - if (!found_primitive) { - std::size_t pos; - try { - const int int_msg = std::stoi(str_msg, &pos); - if (pos == str_msg.size()) { - - // construct and serialize ROS message - std_msgs::Int32 msg; - msg.data = int_msg; - serializeRosMessage(msg, msg_buffer); - - // collect ROS message type information - msg_type_md5 = ros::message_traits::MD5Sum::value(); - msg_type_name = ros::message_traits::DataType::value(); - msg_type_definition = - ros::message_traits::Definition::value(); - - found_primitive = true; - } - } catch (const std::invalid_argument& ex) { - } catch (const std::out_of_range& ex) { - } - } - - // check for float - if (!found_primitive) { - std::size_t pos; - try { - const float float_msg = std::stof(str_msg, &pos); - if (pos == str_msg.size()) { - - // construct and serialize ROS message - std_msgs::Float32 msg; - msg.data = float_msg; - serializeRosMessage(msg, msg_buffer); - - // collect ROS message type information - msg_type_md5 = ros::message_traits::MD5Sum::value(); - msg_type_name = - ros::message_traits::DataType::value(); - msg_type_definition = - ros::message_traits::Definition::value(); - - found_primitive = true; - } - } catch (const std::invalid_argument& ex) { - } catch (const std::out_of_range& ex) { - } - } - - // fall back to string - if (!found_primitive) { - - // construct and serialize ROS message - std_msgs::String msg; - msg.data = str_msg; - serializeRosMessage(msg, msg_buffer); - - // collect ROS message type information - msg_type_md5 = ros::message_traits::MD5Sum::value(); - msg_type_name = ros::message_traits::DataType::value(); - msg_type_definition = - ros::message_traits::Definition::value(); - } - - // if ROS message type has changed - if (msg_type_md5 != mqtt2ros.ros.shape_shifter.getMD5Sum()) { - - // configure ShapeShifter - mqtt2ros.ros.shape_shifter.morph(msg_type_md5, msg_type_name, - msg_type_definition, ""); - - // advertise with ROS publisher - mqtt2ros.ros.publisher.shutdown(); - mqtt2ros.ros.publisher = mqtt2ros.ros.shape_shifter.advertise( - node_handle_, mqtt2ros.ros.topic, mqtt2ros.ros.queue_size, - mqtt2ros.ros.latched); - - NODELET_INFO("ROS publisher message type on topic '%s' set to '%s'", - mqtt2ros.ros.topic.c_str(), msg_type_name.c_str()); - } - - // publish via ShapeShifter - ros::serialization::OStream msg_stream(msg_buffer.data(), msg_buffer.size()); - NODELET_DEBUG( - "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", - mqtt2ros.ros.shape_shifter.getDataType().c_str(), - mqtt2ros.ros.topic.c_str()); - mqtt2ros.ros.shape_shifter.read(msg_stream); - mqtt2ros.ros.publisher.publish(mqtt2ros.ros.shape_shifter); -} - - -void MqttClient::connected(const std::string& cause) { - - is_connected_ = true; - std::string as_client = - client_config_.id.empty() - ? "" - : std::string(" as '") + client_config_.id + std::string("'"); - NODELET_INFO("Connected to broker at '%s'%s", - client_->get_server_uri().c_str(), as_client.c_str()); - - // subscribe MQTT topics - for (auto& mqtt2ros_p : mqtt2ros_) { - Mqtt2RosInterface& mqtt2ros = mqtt2ros_p.second; - std::string mqtt_topic = mqtt2ros_p.first; - if (!mqtt2ros.primitive) // subscribe topics for ROS message types first - mqtt_topic = kRosMsgTypeMqttTopicPrefix + mqtt_topic; - client_->subscribe(mqtt_topic, mqtt2ros.mqtt.qos); - NODELET_DEBUG("Subscribed MQTT topic '%s'", mqtt_topic.c_str()); - } -} - - -void MqttClient::connection_lost(const std::string& cause) { - - NODELET_ERROR("Connection to broker lost, will try to reconnect..."); - is_connected_ = false; - connect(); -} - - -bool MqttClient::isConnected() { - - return is_connected_; -} - - -bool MqttClient::isConnectedService( - mqtt_client_interfaces::IsConnected::Request& request, - mqtt_client_interfaces::IsConnected::Response& response) { - - response.connected = isConnected(); - return true; -} - - -void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) { - - // instantly take arrival timestamp - ros::WallTime arrival_stamp = ros::WallTime::now(); - - std::string mqtt_topic = mqtt_msg->get_topic(); - NODELET_DEBUG("Received MQTT message on topic '%s'", mqtt_topic.c_str()); - - // publish directly if primitive - if (mqtt2ros_.count(mqtt_topic) > 0) { - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - if (mqtt2ros.primitive) { - mqtt2primitive(mqtt_msg); - return; - } - } - - // else first check for ROS message type - bool msg_contains_ros_msg_type = - mqtt_topic.find(kRosMsgTypeMqttTopicPrefix) != std::string::npos; - if (msg_contains_ros_msg_type) { - - // create ROS message buffer on top of MQTT message payload - auto& payload = mqtt_msg->get_payload_ref(); - uint32_t payload_length = static_cast(payload.size()); - char* non_const_payload = const_cast(&payload[0]); - uint8_t* msg_type_buffer = reinterpret_cast(non_const_payload); - - // deserialize ROS message type - mqtt_client_interfaces::RosMsgType ros_msg_type; - ros::serialization::IStream msg_type_stream(msg_type_buffer, - payload_length); - try { - ros::serialization::deserialize(msg_type_stream, ros_msg_type); - } catch (ros::serialization::StreamOverrunException) { - NODELET_ERROR( - "Failed to deserialize ROS message type from MQTT message received on " - "topic '%s', got message:\n%s", - mqtt_topic.c_str(), mqtt_msg->to_string().c_str()); - return; - } - - // reconstruct corresponding MQTT data topic - std::string mqtt_data_topic = mqtt_topic; - mqtt_data_topic.erase(mqtt_data_topic.find(kRosMsgTypeMqttTopicPrefix), - kRosMsgTypeMqttTopicPrefix.length()); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_data_topic]; - - // if ROS message type has changed - if (ros_msg_type.md5 != mqtt2ros.ros.shape_shifter.getMD5Sum()) { - - mqtt2ros.stamped = ros_msg_type.stamped; - NODELET_INFO("ROS publisher message type on topic '%s' set to '%s'", - mqtt2ros.ros.topic.c_str(), ros_msg_type.name.c_str()); - - // configure ShapeShifter - mqtt2ros.ros.shape_shifter.morph(ros_msg_type.md5, ros_msg_type.name, - ros_msg_type.definition, ""); - - // advertise with ROS publisher - mqtt2ros.ros.publisher.shutdown(); - mqtt2ros.ros.publisher = mqtt2ros.ros.shape_shifter.advertise( - node_handle_, mqtt2ros.ros.topic, mqtt2ros.ros.queue_size, - mqtt2ros.ros.latched); - - // subscribe to MQTT topic with actual ROS messages - client_->subscribe(mqtt_data_topic, mqtt2ros.mqtt.qos); - NODELET_DEBUG("Subscribed MQTT topic '%s'", mqtt_data_topic.c_str()); - } - - } else { - - // publish ROS message, if publisher initialized - if (!mqtt2ros_[mqtt_topic].ros.publisher.getTopic().empty()) { - mqtt2ros(mqtt_msg, arrival_stamp); - } else { - NODELET_WARN( - "ROS publisher for data from MQTT topic '%s' is not yet initialized: " - "ROS message type not yet known", - mqtt_topic.c_str()); - } - } -} - - -void MqttClient::delivery_complete(mqtt::delivery_token_ptr token) {} - - -void MqttClient::on_success(const mqtt::token& token) { - - is_connected_ = true; -} - - -void MqttClient::on_failure(const mqtt::token& token) { - - ROS_ERROR( - "Connection to broker failed (return code %d), will automatically " - "retry...", - token.get_return_code()); - is_connected_ = false; -} - -} // namespace mqtt_client +/* +============================================================================== +MIT License + +Copyright 2022 Institute for Automotive Engineering of RWTH Aachen University. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================== +*/ + + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mqtt_client::MqttClient) + + +namespace mqtt_client { + + +const std::string MqttClient::kRosMsgTypeMqttTopicPrefix = + "mqtt_client/ros_msg_type/"; + +const std::string MqttClient::kLatencyRosTopicPrefix = "~/latencies/"; + +template +T mqtt2float(mqtt::const_message_ptr mqtt_msg) { + auto str_msg = mqtt_msg->to_string (); + std::size_t pos; + const T v = std::stold(str_msg, &pos); + + if (pos != str_msg.size()) + throw std::invalid_argument ("not all characters processed"); + + return v; +} + +template +T mqtt2int(mqtt::const_message_ptr mqtt_msg) { + auto str_msg = mqtt_msg->to_string (); + std::size_t pos; + const T v = std::stoll(str_msg, &pos); + + if (pos != str_msg.size()) + throw std::invalid_argument ("not all characters processed"); + + return v; +} + +bool mqtt2bool(mqtt::const_message_ptr mqtt_msg) { + const std::string str_msg = mqtt_msg->to_string (); + std::string bool_str = mqtt_msg->to_string (); + std::transform(str_msg.cbegin(), str_msg.cend(), bool_str.begin(), + ::tolower); + if (bool_str == "true" || bool_str == "1") return true; + if (bool_str == "false" || bool_str == "0") return false; + + throw std::invalid_argument ("unable to decode string"); +} + +bool fixedMqtt2PrimitiveRos(mqtt::const_message_ptr mqtt_msg, + const std::string& msg_type, + rclcpp::SerializedMessage &serialized_msg) { + try { + if (msg_type == "std_msgs/msg/String") { + std_msgs::msg::String msg; + msg.data = mqtt_msg->to_string(); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/Bool") { + std_msgs::msg::Bool msg; + msg.data = mqtt2bool(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/Char") { + std_msgs::msg::Char msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/UInt8") { + std_msgs::msg::UInt8 msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/UInt16") { + std_msgs::msg::UInt16 msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/UInt32") { + std_msgs::msg::UInt32 msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/UInt64") { + std_msgs::msg::UInt64 msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/Int8") { + std_msgs::msg::Int8 msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/Int16") { + std_msgs::msg::Int16 msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/Int32") { + std_msgs::msg::Int32 msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/Int64") { + std_msgs::msg::Int64 msg; + msg.data = mqtt2int(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/Float32") { + std_msgs::msg::Float32 msg; + msg.data = mqtt2float(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/Float64") { + std_msgs::msg::Float64 msg; + msg.data = mqtt2float(mqtt_msg); + + serializeRosMessage(msg, serialized_msg); + } else { + throw std::domain_error("Unhandled message type (" + msg_type + ")"); + } + + return true; + + } catch (const std::exception &) { + return false; + } + + +} + +/** + * @brief Extracts string of primitive data types from ROS message. + * + * This is helpful to extract the actual data payload of a primitive ROS + * message. If e.g. an std_msgs/msg/String is serialized to a string + * representation, it also contains the field name 'data'. This function + * instead returns the underlying value as string. + * + * @param [in] serialized_msg generic serialized ROS message + * @param [in] msg_type ROS message type, e.g. `std_msgs/msg/String` + * @param [out] primitive string representation of primitive message data + * + * @return true if primitive ROS message type was found + * @return false if ROS message type is not primitive + */ +bool primitiveRosMessageToString( + const std::shared_ptr& serialized_msg, + const std::string& msg_type, std::string& primitive) { + + bool found_primitive = true; + + if (msg_type == "std_msgs/msg/String") { + std_msgs::msg::String msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = msg.data; + } else if (msg_type == "std_msgs/msg/Bool") { + std_msgs::msg::Bool msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = msg.data ? "true" : "false"; + } else if (msg_type == "std_msgs/msg/Char") { + std_msgs::msg::Char msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/UInt8") { + std_msgs::msg::UInt8 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/UInt16") { + std_msgs::msg::UInt16 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/UInt32") { + std_msgs::msg::UInt32 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/UInt64") { + std_msgs::msg::UInt64 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/Int8") { + std_msgs::msg::Int8 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/Int16") { + std_msgs::msg::Int16 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/Int32") { + std_msgs::msg::Int32 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/Int64") { + std_msgs::msg::Int64 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/Float32") { + std_msgs::msg::Float32 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/Float64") { + std_msgs::msg::Float64 msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::to_string(msg.data); + } else { + found_primitive = false; + } + + return found_primitive; +} + + +MqttClient::MqttClient(const rclcpp::NodeOptions& options) : Node("mqtt_client", options) { + + loadParameters(); + setup(); +} + + +void MqttClient::loadParameters() { + + rcl_interfaces::msg::ParameterDescriptor param_desc; + + param_desc.description = "IP address or hostname of the machine running the MQTT broker"; + declare_parameter("broker.host", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "port the MQTT broker is listening on"; + declare_parameter("broker.port", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "username used for authenticating to the broker (if empty, will try to connect anonymously)"; + declare_parameter("broker.user", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "password used for authenticating to the broker"; + declare_parameter("broker.pass", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "whether to connect via SSL/TLS"; + declare_parameter("broker.tls.enabled", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); + param_desc.description = "CA certificate file trusted by client (relative to ROS_HOME)"; + declare_parameter("broker.tls.ca_certificate", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + + param_desc.description = "unique ID used to identify the client (broker may allow empty ID and automatically generate one)"; + declare_parameter("client.id", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "maximum number of messages buffered by the bridge when not connected to broker (only available if client ID is not empty)"; + declare_parameter("client.buffer.size", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "directory used to buffer messages when not connected to broker (relative to ROS_HOME)"; + declare_parameter("client.buffer.directory", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "topic used for this client's last-will message (no last will, if not specified)"; + declare_parameter("client.last_will.topic", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "last-will message"; + declare_parameter("client.last_will.message", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "QoS value for last-will message"; + declare_parameter("client.last_will.qos", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "whether to retain last-will message"; + declare_parameter("client.last_will.retained", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); + param_desc.description = "whether to use a clean session for this client"; + declare_parameter("client.clean_session", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); + param_desc.description = "keep-alive interval in seconds"; + declare_parameter("client.keep_alive_interval", rclcpp::ParameterType::PARAMETER_DOUBLE, param_desc); + param_desc.description = "maximum number of inflight messages"; + declare_parameter("client.max_inflight", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "client certificate file (only needed if broker requires client certificates; relative to ROS_HOME)"; + declare_parameter("client.tls.certificate", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "client private key file (relative to ROS_HOME)"; + declare_parameter("client.tls.key", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "client private key password"; + declare_parameter("client.tls.password", rclcpp::ParameterType::PARAMETER_STRING, param_desc); + + param_desc.description = "The list of topics to bridge from ROS to MQTT"; + const auto ros2mqtt_ros_topics = declare_parameter>("bridge.ros2mqtt.ros_topics", std::vector(), param_desc); + for (const auto& ros_topic : ros2mqtt_ros_topics) + { + param_desc.description = "MQTT topic on which the corresponding ROS messages are sent to the broker"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.mqtt_topic", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "whether to publish as primitive message"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.primitive", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); + param_desc.description = "If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the publisher"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.ros_type", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "whether to attach a timestamp to a ROS2MQTT payload (for latency computation on receiver side)"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.inject_timestamp", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); + param_desc.description = "ROS subscriber queue size"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.queue_size", ros_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "ROS subscriber QoS reliability"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.reliability", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "ROS subscriber QoS durability"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.durability", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "MQTT QoS value"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.qos", ros_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "whether to retain MQTT message"; + declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.retained", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); + } + + param_desc.description = "The list of topics to bridge from MQTT to ROS"; + const auto mqtt2ros_mqtt_topics = declare_parameter>("bridge.mqtt2ros.mqtt_topics", std::vector(), param_desc); + for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) + { + param_desc.description = "ROS topic on which corresponding MQTT messages are published"; + declare_parameter(fmt::format("bridge.mqtt2ros.{}.ros_topic", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "whether to publish as primitive message (if coming from non-ROS MQTT client)"; + declare_parameter(fmt::format("bridge.mqtt2ros.{}.primitive", mqtt_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); + param_desc.description = "If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the MQTT message"; + declare_parameter(fmt::format("bridge.mqtt2ros.{}.ros_type", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "MQTT QoS value"; + declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "ROS publisher queue size"; + declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); + param_desc.description = "ROS publisher QoS durability"; + declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.durability", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "ROS publisher QoS reliability"; + declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.reliability", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "whether to latch ROS message"; + declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.latched", mqtt_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); + } + + // load broker parameters from parameter server + std::string broker_tls_ca_certificate; + loadParameter("broker.host", broker_config_.host, "localhost"); + loadParameter("broker.port", broker_config_.port, 1883); + if (loadParameter("broker.user", broker_config_.user)) { + loadParameter("broker.pass", broker_config_.pass, ""); + } + if (loadParameter("broker.tls.enabled", broker_config_.tls.enabled, false)) { + loadParameter("broker.tls.ca_certificate", broker_tls_ca_certificate, + "/etc/ssl/certs/ca-certificates.crt"); + } + + // load client parameters from parameter server + std::string client_buffer_directory, client_tls_certificate, client_tls_key; + loadParameter("client.id", client_config_.id, ""); + client_config_.buffer.enabled = !client_config_.id.empty(); + if (client_config_.buffer.enabled) { + loadParameter("client.buffer.size", client_config_.buffer.size, 0); + loadParameter("client.buffer.directory", client_buffer_directory, "buffer"); + } else { + RCLCPP_WARN(get_logger(), + "Client buffer can not be enabled when client ID is empty"); + } + if (loadParameter("client.last_will.topic", client_config_.last_will.topic)) { + loadParameter("client.last_will.message", client_config_.last_will.message, + "offline"); + loadParameter("client.last_will.qos", client_config_.last_will.qos, 0); + loadParameter("client.last_will.retained", + client_config_.last_will.retained, false); + } + loadParameter("client.clean_session", client_config_.clean_session, true); + loadParameter("client.keep_alive_interval", + client_config_.keep_alive_interval, 60.0); + loadParameter("client.max_inflight", client_config_.max_inflight, 65535); + if (broker_config_.tls.enabled) { + if (loadParameter("client.tls.certificate", client_tls_certificate)) { + loadParameter("client.tls.key", client_tls_key); + loadParameter("client.tls.password", client_config_.tls.password); + loadParameter("client.tls.version", client_config_.tls.version); + loadParameter("client.tls.verify", client_config_.tls.verify); + loadParameter("client.tls.alpn_protos", client_config_.tls.alpn_protos); + } + } + + // resolve filepaths + broker_config_.tls.ca_certificate = resolvePath(broker_tls_ca_certificate); + client_config_.buffer.directory = resolvePath(client_buffer_directory); + client_config_.tls.certificate = resolvePath(client_tls_certificate); + client_config_.tls.key = resolvePath(client_tls_key); + + // parse bridge parameters + + // ros2mqtt + for (const auto& ros_topic : ros2mqtt_ros_topics) { + + rclcpp::Parameter mqtt_topic_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.mqtt_topic", ros_topic), mqtt_topic_param)) { + + // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic + const std::string mqtt_topic = mqtt_topic_param.as_string(); + Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; + ros2mqtt.mqtt.topic = mqtt_topic; + + // ros2mqtt[k]/primitive + rclcpp::Parameter primitive_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.primitive", ros_topic), primitive_param)) + ros2mqtt.primitive = primitive_param.as_bool(); + + // ros2mqtt[k]/ros_type + rclcpp::Parameter ros_type_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.ros_type", ros_topic), ros_type_param)) { + ros2mqtt.ros.msg_type = ros_type_param.as_string(); + ros2mqtt.fixed_type = true; + RCLCPP_DEBUG(get_logger(), "Using explicit ROS message type '%s'", ros2mqtt.ros.msg_type.c_str()); + } + + // ros2mqtt[k]/inject_timestamp + rclcpp::Parameter stamped_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.inject_timestamp", ros_topic), stamped_param)) + ros2mqtt.stamped = stamped_param.as_bool(); + if (ros2mqtt.stamped && ros2mqtt.primitive) { + RCLCPP_WARN( + get_logger(), + "Timestamp will not be injected into primitive messages on ROS " + "topic '%s'", + ros_topic.c_str()); + ros2mqtt.stamped = false; + } + + // ros2mqtt[k]/advanced/ros/queue_size + rclcpp::Parameter queue_size_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.queue_size", ros_topic), + queue_size_param)) + ros2mqtt.ros.queue_size = queue_size_param.as_int(); + + rclcpp::Parameter durability_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.durability", ros_topic), durability_param)) { + const auto p = durability_param.as_string(); + if (p == "system_default") { + ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::SystemDefault; + } else if (p == "volatile") { + ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::Volatile; + } else if (p == "transient_local") { + ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::TransientLocal; + } else if (p == "auto") { + ros2mqtt.ros.qos.durability = {}; + } else { + RCLCPP_ERROR(get_logger(), "Unexpected durability %s", p.c_str ()); + exit(EXIT_FAILURE); + } + } + + rclcpp::Parameter reliability_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.reliability", ros_topic), reliability_param)) { + const auto p = reliability_param.as_string(); + if (p == "system_default") { + ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::SystemDefault; + } else if (p == "best_effort") { + ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::BestEffort; + } else if (p == "reliable") { + ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::Reliable; + } else if (p == "auto") { + ros2mqtt.ros.qos.reliability = {}; + } else { + RCLCPP_ERROR(get_logger(), "Unexpected reliability %s", p.c_str ()); + exit(EXIT_FAILURE); + } + } + + // ros2mqtt[k]/advanced/mqtt/qos + rclcpp::Parameter qos_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.qos", ros_topic), qos_param)) + ros2mqtt.mqtt.qos = qos_param.as_int(); + + // ros2mqtt[k]/advanced/mqtt/retained + rclcpp::Parameter retained_param; + if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.retained", ros_topic), retained_param)) + ros2mqtt.mqtt.retained = retained_param.as_bool(); + + RCLCPP_INFO(get_logger(), "Bridging %sROS topic '%s' to MQTT topic '%s' %s", + ros2mqtt.primitive ? "primitive " : "", ros_topic.c_str(), + ros2mqtt.mqtt.topic.c_str(), + ros2mqtt.stamped ? "and measuring latency" : ""); + } else { + RCLCPP_WARN(get_logger(), + fmt::format("Parameter 'bridge.ros2mqtt.{}' is missing subparameter " + "'mqtt_topic', will be ignored", ros_topic).c_str()); + } + } + + // mqtt2ros + for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { + + rclcpp::Parameter ros_topic_param; + if (get_parameter(fmt::format("bridge.mqtt2ros.{}.ros_topic", mqtt_topic), ros_topic_param)) { + + // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic + const std::string ros_topic = ros_topic_param.as_string(); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + mqtt2ros.ros.topic = ros_topic; + + // mqtt2ros[k]/primitive + rclcpp::Parameter primitive_param; + if (get_parameter(fmt::format("bridge.mqtt2ros.{}.primitive", mqtt_topic), primitive_param)) + mqtt2ros.primitive = primitive_param.as_bool(); + + + rclcpp::Parameter ros_type_param; + if (get_parameter(fmt::format("bridge.mqtt2ros.{}.ros_type", mqtt_topic), ros_type_param)) { + mqtt2ros.ros.msg_type = ros_type_param.as_string(); + mqtt2ros.fixed_type = true; + RCLCPP_DEBUG(get_logger(), "Using explicit ROS message type '%s' for '%s'", mqtt2ros.ros.msg_type.c_str(), ros_topic.c_str()); + } + + // mqtt2ros[k]/advanced/mqtt/qos + rclcpp::Parameter qos_param; + if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), qos_param)) + mqtt2ros.mqtt.qos = qos_param.as_int(); + + // mqtt2ros[k]/advanced/ros/queue_size + rclcpp::Parameter queue_size_param; + if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), + queue_size_param)) + mqtt2ros.ros.queue_size = queue_size_param.as_int(); + + rclcpp::Parameter durability_param; + if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.durability", mqtt_topic), durability_param)) { + const auto p = durability_param.as_string(); + if (p == "system_default") { + mqtt2ros.ros.qos.durability = rclcpp::DurabilityPolicy::SystemDefault; + } else if (p == "volatile") { + mqtt2ros.ros.qos.durability = rclcpp::DurabilityPolicy::Volatile; + } else if (p == "transient_local") { + mqtt2ros.ros.qos.durability = + rclcpp::DurabilityPolicy::TransientLocal; + } else { + RCLCPP_ERROR(get_logger(), "Unexpected durability %s", p.c_str ()); + exit(EXIT_FAILURE); + } + } + + rclcpp::Parameter reliability_param; + if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.reliability", mqtt_topic), reliability_param)) { + const auto p = reliability_param.as_string(); + if (p == "system_default") { + mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::SystemDefault; + } else if (p == "best_effort") { + mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::BestEffort; + } else if (p == "reliable") { + mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::Reliable; + } else { + RCLCPP_ERROR(get_logger(), "Unexpected reliability %s", p.c_str ()); + exit(EXIT_FAILURE); + } + } + + // mqtt2ros[k]/advanced/ros/latched + rclcpp::Parameter latched_param; + if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.latched", mqtt_topic), latched_param)) { + mqtt2ros.ros.latched = latched_param.as_bool(); + RCLCPP_WARN(get_logger(), + fmt::format("Parameter 'bridge.mqtt2ros.{}.advanced.ros.latched' is ignored " + "since ROS 2 does not easily support latched topics.", mqtt_topic).c_str()); + } + + RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to %sROS topic '%s'", + mqtt_topic.c_str(), mqtt2ros.primitive ? "primitive " : "", + mqtt2ros.ros.topic.c_str()); + } + else { + RCLCPP_WARN(get_logger(), + fmt::format("Parameter 'bridge.ros2mqtt.{}' is missing subparameter " + "'ros_topic', will be ignored", mqtt_topic).c_str()); + } + } + + if (ros2mqtt_.empty() && mqtt2ros_.empty()) { + RCLCPP_ERROR(get_logger(), + "No valid ROS-MQTT bridge found in parameter 'bridge'"); + exit(EXIT_FAILURE); + } +} + + +bool MqttClient::loadParameter(const std::string& key, std::string& value) { + bool found = get_parameter(key, value); + if (found) + RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), + value.c_str()); + return found; +} + + +bool MqttClient::loadParameter(const std::string& key, std::string& value, + const std::string& default_value) { + bool found = get_parameter_or(key, value, default_value); + if (!found) + RCLCPP_WARN(get_logger(), "Parameter '%s' not set, defaulting to '%s'", + key.c_str(), default_value.c_str()); + if (found) + RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), + value.c_str()); + return found; +} + + +std::filesystem::path MqttClient::resolvePath(const std::string& path_string) { + + std::filesystem::path path(path_string); + if (path_string.empty()) return path; + if (!path.has_root_path()) { + std::string ros_home = rcpputils::get_env_var("ROS_HOME"); + if (ros_home.empty()) + ros_home = std::string(std::filesystem::current_path()); + path = std::filesystem::path(ros_home); + path.append(path_string); + } + if (!std::filesystem::exists(path)) + RCLCPP_WARN(get_logger(), "Requested path '%s' does not exist", + std::string(path).c_str()); + return path; +} + + +void MqttClient::setup() { + + // pre-compute timestamp length + builtin_interfaces::msg::Time tmp_stamp; + rclcpp::SerializedMessage tmp_serialized_stamp; + serializeRosMessage(tmp_stamp, tmp_serialized_stamp); + stamp_length_ = tmp_serialized_stamp.size(); + + // initialize MQTT client + setupClient(); + + // connect to MQTT broker + connect(); + + // create ROS service server + is_connected_service_ = + create_service( + "~/is_connected", std::bind(&MqttClient::isConnectedService, this, + std::placeholders::_1, std::placeholders::_2)); + + // create dynamic mappings services + new_ros2mqtt_bridge_service_ = + create_service( + "~/new_ros2mqtt_bridge", std::bind(&MqttClient::newRos2MqttBridge, this, + std::placeholders::_1, std::placeholders::_2)); + new_mqtt2ros_bridge_service_ = + create_service( + "~/new_mqtt2ros_bridge", std::bind(&MqttClient::newMqtt2RosBridge, this, + std::placeholders::_1, std::placeholders::_2)); + + // setup subscribers; check for new types every second + check_subscriptions_timer_ = + create_wall_timer(std::chrono::duration(1.0), + std::bind(&MqttClient::setupSubscriptions, this)); + + setupPublishers (); +} + +std::optional MqttClient::getCompatibleQoS (const std::string &ros_topic, const rclcpp::TopicEndpointInfo &tei, + const Ros2MqttInterface &ros2mqtt) const +{ + // the basic premise here is that we take the QoS from the publisher, overwrite any parts that are explicitly set + // via configuration then check if the result is compatible with the publisher + auto qos = tei.qos_profile (); + + if (auto r = ros2mqtt.ros.qos.reliability) + qos.reliability (*r); + if (auto d = ros2mqtt.ros.qos.durability) + qos.durability (*d); + qos.keep_last (ros2mqtt.ros.queue_size); + + const auto qres = rclcpp::qos_check_compatible (tei.qos_profile (), qos); + + switch (qres.compatibility) + { + case rclcpp::QoSCompatibility::Ok: + return qos; + case rclcpp::QoSCompatibility::Warning: + RCLCPP_DEBUG(get_logger(), "QoS compatibility warning on topic %s - %s", ros_topic.c_str(), qres.reason.c_str ()); + // presumably this is still compatible + return qos; + case rclcpp::QoSCompatibility::Error: + default: + return {}; + } + +} + +std::vector MqttClient::getCandidatePublishers( + const std::string& ros_topic, const Ros2MqttInterface& ros2mqtt) const +{ + const auto &pubs = get_publishers_info_by_topic(ros_topic); + + if (pubs.empty ()) + return {}; + + std::vector ret; + + ret.reserve (pubs.size ()); + + for (const auto &p : pubs) + { + const std::string msg_type = p.topic_type(); + + // if the type isn't set, match aginst all t ypes, otherwise only match against mtching types + if (ros2mqtt.ros.msg_type.empty () || msg_type == ros2mqtt.ros.msg_type) + ret.push_back (p); + } + + // If we found any matching types, return those + if (! ret.empty ()) + return ret; + + // If we didn't and we aren't fix type, then just return the full set of publishers + if (! ros2mqtt.fixed_type) + return pubs; + + // None of these publishers will work... :sad_panda: + return {}; +} + +void MqttClient::setupSubscriptions() { + + // For each ros2mqtt interface, check if we need to do a lazy subscription (eg, lazy subscription would be if we + // are determining either the type of QoS dynamically). If we don't, then just make the fixed subscriber if its not + // already made. + // + // If we do, we check each publisher for a potential match. The first step is to filter down the list of + // publishers based on the type. If we are fixed type then that list only includes publishers with matching types. + // If we aren't its a little trickier. For dynamic types, if we already have matched against a type previously, the + // candidate list will include all publishers which have matching types if any exist. If none of the publishers + // have matching types, then the list will include all publishers. + // + // Then for each candidate publisher, check if their QoS is compatible with the QoS specic in the configuration. This + // is really just needed because there is the potential for someone to set half of the QoS to auto and half to + // explicit. + // Condition for requiring "lazy" subscription where we need to walk the + const auto requires_lazy_subscription = [] (const Ros2MqttInterface &ros2mqtt) + { + if (! ros2mqtt.fixed_type) + return true; + if (ros2mqtt.ros.qos.reliability == std::nullopt || + ros2mqtt.ros.qos.durability == std::nullopt) + return true; + return false; + }; + + for (auto& [ros_topic, ros2mqtt] : ros2mqtt_) { + + std::function msg)> bound_callback_func = + std::bind(&MqttClient::ros2mqtt, this, std::placeholders::_1, ros_topic); + + if (! requires_lazy_subscription (ros2mqtt)) + { + try { + if (ros2mqtt.ros.subscriber) + continue; + + auto const qos = rclcpp::QoS(ros2mqtt.ros.queue_size) + .reliability(*ros2mqtt.ros.qos.reliability) + .durability(*ros2mqtt.ros.qos.durability); + + ros2mqtt.ros.subscriber = create_generic_subscription( + ros_topic, ros2mqtt.ros.msg_type, qos, bound_callback_func); + ros2mqtt.ros.is_stale = false; + RCLCPP_INFO(get_logger(), "Subscribed ROS topic '%s' of type '%s'", + ros_topic.c_str(), ros2mqtt.ros.msg_type.c_str()); + } catch (rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", + e.what()); + continue; + } + } else { + const auto &pubs = getCandidatePublishers (ros_topic, ros2mqtt); + + if (pubs.empty()) continue; + + for (auto endpointInfo : pubs) { + try { + // check if message type has changed or if mapping is stale + const std::string msg_type = endpointInfo.topic_type(); + + if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale && ros2mqtt.ros.subscriber) + continue; + + auto const qos = getCompatibleQoS (ros_topic, endpointInfo, ros2mqtt); + + if (! qos) + continue; + + ros2mqtt.ros.is_stale = false; + ros2mqtt.ros.msg_type = msg_type; + + ros2mqtt.ros.subscriber = create_generic_subscription( + ros_topic, msg_type, *qos, bound_callback_func); + + RCLCPP_INFO(get_logger(), "Subscribed ROS topic '%s' of type '%s'", + ros_topic.c_str(), msg_type.c_str()); + } catch (rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", + e.what()); + continue; + } + } + } + } +} + +void MqttClient::setupPublishers() { + + for (auto& [mqtt_topic, mqtt2ros] : mqtt2ros_) { + if (mqtt2ros.ros.publisher) + continue; + + // If the type is not fixed, we require a mqtt message in order to deduce the type + if (!mqtt2ros.fixed_type) + continue; + + try { + const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) + .durability(mqtt2ros.ros.qos.durability) + .reliability(mqtt2ros.ros.qos.reliability); + mqtt2ros.ros.publisher = create_generic_publisher( + mqtt2ros.ros.topic, mqtt2ros.ros.msg_type, qos); + + mqtt2ros.ros.is_stale = false; + } catch (const rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", + e.what()); + } + } +} + +void MqttClient::setupClient() { + + // basic client connection options + connect_options_.set_automatic_reconnect(true); + connect_options_.set_clean_session(client_config_.clean_session); + connect_options_.set_keep_alive_interval(client_config_.keep_alive_interval); + connect_options_.set_max_inflight(client_config_.max_inflight); + + // user authentication + if (!broker_config_.user.empty()) { + connect_options_.set_user_name(broker_config_.user); + connect_options_.set_password(broker_config_.pass); + } + + // last will + if (!client_config_.last_will.topic.empty()) { + mqtt::will_options will( + client_config_.last_will.topic, client_config_.last_will.message, + client_config_.last_will.qos, client_config_.last_will.retained); + connect_options_.set_will(will); + } + + // SSL/TLS + if (broker_config_.tls.enabled) { + mqtt::ssl_options ssl; + ssl.set_trust_store(broker_config_.tls.ca_certificate); + if (!client_config_.tls.certificate.empty() && + !client_config_.tls.key.empty()) { + ssl.set_key_store(client_config_.tls.certificate); + ssl.set_private_key(client_config_.tls.key); + if (!client_config_.tls.password.empty()) + ssl.set_private_key_password(client_config_.tls.password); + } + ssl.set_ssl_version(client_config_.tls.version); + ssl.set_verify(client_config_.tls.verify); + ssl.set_alpn_protos(client_config_.tls.alpn_protos); + connect_options_.set_ssl(ssl); + } + + // create MQTT client + const std::string protocol = broker_config_.tls.enabled ? "ssl" : "tcp"; + const std::string uri = fmt::format("{}://{}:{}", protocol, broker_config_.host, + broker_config_.port); + try { + if (client_config_.buffer.enabled) { + client_ = std::shared_ptr(new mqtt::async_client( + uri, client_config_.id, client_config_.buffer.size, + client_config_.buffer.directory)); + } else { + client_ = std::shared_ptr( + new mqtt::async_client(uri, client_config_.id)); + } + } catch (const mqtt::exception& e) { + RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); + exit(EXIT_FAILURE); + } + + // setup MQTT callbacks + client_->set_callback(*this); +} + + +void MqttClient::connect() { + + std::string as_client = + client_config_.id.empty() + ? "" + : std::string(" as '") + client_config_.id + std::string("'"); + RCLCPP_INFO(get_logger(), "Connecting to broker at '%s'%s ...", + client_->get_server_uri().c_str(), as_client.c_str()); + + try { + client_->connect(connect_options_, nullptr, *this); + } catch (const mqtt::exception& e) { + RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); + exit(EXIT_FAILURE); + } +} + + +void MqttClient::ros2mqtt( + const std::shared_ptr& serialized_msg, + const std::string& ros_topic) { + + Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; + std::string mqtt_topic = ros2mqtt.mqtt.topic; + std::vector payload_buffer; + + // gather information on ROS message type in special ROS message + mqtt_client_interfaces::msg::RosMsgType ros_msg_type; + ros_msg_type.name = ros2mqtt.ros.msg_type; + ros_msg_type.stamped = ros2mqtt.stamped; + + RCLCPP_DEBUG(get_logger(), "Received ROS message of type '%s' on topic '%s'", + ros_msg_type.name.c_str(), ros_topic.c_str()); + + if (ros2mqtt.primitive) { // publish as primitive (string) message + + // resolve ROS messages to primitive strings if possible + std::string payload; + bool found_primitive = + primitiveRosMessageToString(serialized_msg, ros_msg_type.name, payload); + if (found_primitive) { + payload_buffer = std::vector(payload.begin(), payload.end()); + } else { + RCLCPP_WARN(get_logger(), + "Cannot send ROS message of type '%s' as primitive message, " + "check supported primitive types", + ros_msg_type.name.c_str()); + return; + } + + } else { // publish as complete ROS message incl. ROS message type + + // serialize ROS message type + rclcpp::SerializedMessage serialized_ros_msg_type; + serializeRosMessage(ros_msg_type, serialized_ros_msg_type); + uint32_t msg_type_length = serialized_ros_msg_type.size(); + std::vector msg_type_buffer = std::vector( + serialized_ros_msg_type.get_rcl_serialized_message().buffer, + serialized_ros_msg_type.get_rcl_serialized_message().buffer + msg_type_length); + + // send ROS message type information to MQTT broker + mqtt_topic = kRosMsgTypeMqttTopicPrefix + ros2mqtt.mqtt.topic; + try { + RCLCPP_DEBUG(get_logger(), + "Sending ROS message type to MQTT broker on topic '%s' ...", + mqtt_topic.c_str()); + mqtt::message_ptr mqtt_msg = + mqtt::make_message(mqtt_topic, msg_type_buffer.data(), + msg_type_buffer.size(), ros2mqtt.mqtt.qos, true); + client_->publish(mqtt_msg); + } catch (const mqtt::exception& e) { + RCLCPP_WARN( + get_logger(), + "Publishing ROS message type information to MQTT topic '%s' failed: %s", + mqtt_topic.c_str(), e.what()); + } + + // build MQTT payload for ROS message (R) as [R] + // or [S, R] if timestamp (S) is included + uint32_t msg_length = serialized_msg->size(); + uint32_t payload_length = msg_length; + uint32_t msg_offset = 0; + if (ros2mqtt.stamped) { + + // allocate buffer with appropriate size to hold [S, R] + msg_offset += stamp_length_; + payload_length += stamp_length_; + payload_buffer.resize(payload_length); + + // copy serialized ROS message to payload [-, R] + std::copy(serialized_msg->get_rcl_serialized_message().buffer, + serialized_msg->get_rcl_serialized_message().buffer + msg_length, + payload_buffer.begin() + msg_offset); + } else { + + // directly build payload buffer on top of serialized message + payload_buffer = std::vector( + serialized_msg->get_rcl_serialized_message().buffer, + serialized_msg->get_rcl_serialized_message().buffer + msg_length); + } + + // inject timestamp as final step + if (ros2mqtt.stamped) { + + // take current timestamp + builtin_interfaces::msg::Time stamp(rclcpp::Clock(RCL_SYSTEM_TIME).now()); + + // copy serialized timestamp to payload [S, R] + rclcpp::SerializedMessage serialized_stamp; + serializeRosMessage(stamp, serialized_stamp); + std::copy( + serialized_stamp.get_rcl_serialized_message().buffer, + serialized_stamp.get_rcl_serialized_message().buffer + stamp_length_, + payload_buffer.begin()); + } + } + + // send ROS message to MQTT broker + mqtt_topic = ros2mqtt.mqtt.topic; + try { + RCLCPP_DEBUG( + get_logger(), + "Sending ROS message of type '%s' to MQTT broker on topic '%s' ...", + ros_msg_type.name.c_str(), mqtt_topic.c_str()); + mqtt::message_ptr mqtt_msg = mqtt::make_message( + mqtt_topic, payload_buffer.data(), payload_buffer.size(), + ros2mqtt.mqtt.qos, ros2mqtt.mqtt.retained); + client_->publish(mqtt_msg); + } catch (const mqtt::exception& e) { + RCLCPP_WARN( + get_logger(), + "Publishing ROS message type information to MQTT topic '%s' failed: %s", + mqtt_topic.c_str(), e.what()); + } +} + + +void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg, + const rclcpp::Time& arrival_stamp) { + std::string mqtt_topic = mqtt_msg->get_topic(); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + auto& payload = mqtt_msg->get_payload_ref(); + uint32_t payload_length = static_cast(payload.size()); + + // read MQTT payload for ROS message (R) as [R] + // or [S, R] if timestamp (S) is included + uint32_t msg_length = payload_length; + uint32_t msg_offset = 0; + + // if stamped, compute latency + if (mqtt2ros.stamped) { + + // copy stamp to generic message buffer + rclcpp::SerializedMessage serialized_stamp(stamp_length_); + std::memcpy(serialized_stamp.get_rcl_serialized_message().buffer, + &(payload[msg_offset]), stamp_length_); + serialized_stamp.get_rcl_serialized_message().buffer_length = stamp_length_; + + // deserialize stamp + builtin_interfaces::msg::Time stamp; + deserializeRosMessage(serialized_stamp, stamp); + + // compute ROS2MQTT latency + rclcpp::Duration latency = arrival_stamp - stamp; + std_msgs::msg::Float64 latency_msg; + latency_msg.data = latency.seconds(); + + // publish latency + if (!mqtt2ros.ros.latency_publisher) { + std::string latency_topic = kLatencyRosTopicPrefix + mqtt2ros.ros.topic; + latency_topic.replace(latency_topic.find("//"), 2, "/"); + mqtt2ros.ros.latency_publisher = + create_publisher(latency_topic, 1); + } + mqtt2ros.ros.latency_publisher->publish(latency_msg); + + msg_length -= stamp_length_; + msg_offset += stamp_length_; + } + + // copy ROS message from MQTT message to generic message buffer + rclcpp::SerializedMessage serialized_msg(msg_length); + std::memcpy(serialized_msg.get_rcl_serialized_message().buffer, + &(payload[msg_offset]), msg_length); + serialized_msg.get_rcl_serialized_message().buffer_length = msg_length; + + // publish generic ROS message + RCLCPP_DEBUG( + get_logger(), + "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", + mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); + mqtt2ros.ros.publisher->publish(serialized_msg); +} + + +void MqttClient::mqtt2primitive(mqtt::const_message_ptr mqtt_msg) { + + std::string mqtt_topic = mqtt_msg->get_topic(); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + + const std::string str_msg = mqtt_msg->to_string(); + bool found_primitive = false; + std::string ros_msg_type = "std_msgs/msg/String"; + rclcpp::SerializedMessage serialized_msg; + + // check for bool + if (!found_primitive) { + std::string bool_str = str_msg; + std::transform(str_msg.cbegin(), str_msg.cend(), bool_str.begin(), + ::tolower); + if (bool_str == "true" || bool_str == "false") { + + bool bool_msg = (bool_str == "true"); + + // construct and serialize ROS message + std_msgs::msg::Bool msg; + msg.data = bool_msg; + serializeRosMessage(msg, serialized_msg); + + ros_msg_type = "std_msgs/msg/Bool"; + found_primitive = true; + } + } + + // check for int + if (!found_primitive) { + std::size_t pos; + try { + const int int_msg = std::stoi(str_msg, &pos); + if (pos == str_msg.size()) { + + // construct and serialize ROS message + std_msgs::msg::Int32 msg; + msg.data = int_msg; + serializeRosMessage(msg, serialized_msg); + + ros_msg_type = "std_msgs/msg/Int32"; + found_primitive = true; + } + } catch (const std::invalid_argument& ex) { + } catch (const std::out_of_range& ex) { + } + } + + // check for float + if (!found_primitive) { + std::size_t pos; + try { + const float float_msg = std::stof(str_msg, &pos); + if (pos == str_msg.size()) { + + // construct and serialize ROS message + std_msgs::msg::Float32 msg; + msg.data = float_msg; + serializeRosMessage(msg, serialized_msg); + + ros_msg_type = "std_msgs/msg/Float32"; + found_primitive = true; + } + } catch (const std::invalid_argument& ex) { + } catch (const std::out_of_range& ex) { + } + } + + // fall back to string + if (!found_primitive) { + + // construct and serialize ROS message + std_msgs::msg::String msg; + msg.data = str_msg; + serializeRosMessage(msg, serialized_msg); + } + + // if ROS message type has changed or if mapping is stale + if (ros_msg_type != mqtt2ros.ros.msg_type || mqtt2ros.ros.is_stale) { + + mqtt2ros.ros.msg_type = ros_msg_type; + RCLCPP_INFO(get_logger(), + "ROS publisher message type on topic '%s' set to '%s'", + mqtt2ros.ros.topic.c_str(), ros_msg_type.c_str()); + + // recreate generic publisher + try { + const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) + .durability(mqtt2ros.ros.qos.durability) + .reliability(mqtt2ros.ros.qos.reliability); + mqtt2ros.ros.publisher = create_generic_publisher( + mqtt2ros.ros.topic, ros_msg_type, qos); + } catch (rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", + e.what()); + return; + } + mqtt2ros.ros.is_stale = false; + } + + // publish + RCLCPP_DEBUG( + get_logger(), + "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", + mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); + mqtt2ros.ros.publisher->publish(serialized_msg); +} + +void MqttClient::mqtt2fixed(mqtt::const_message_ptr mqtt_msg) { + + std::string mqtt_topic = mqtt_msg->get_topic(); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + + rclcpp::SerializedMessage serialized_msg; + + if (!fixedMqtt2PrimitiveRos(mqtt_msg, mqtt2ros.ros.msg_type, serialized_msg)) { + RCLCPP_WARN( + get_logger(), + "Could not convert mqtt message into type %s on topic %s ...", + mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); + } else { + + if (!mqtt2ros.ros.publisher) + { + RCLCPP_INFO(get_logger(), + "ROS publisher message type on topic '%s' set to '%s'", + mqtt2ros.ros.topic.c_str(), mqtt2ros.ros.msg_type.c_str()); + + // recreate generic publisher + try { + const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) + .durability(mqtt2ros.ros.qos.durability) + .reliability(mqtt2ros.ros.qos.reliability); + mqtt2ros.ros.publisher = create_generic_publisher( + mqtt2ros.ros.topic, mqtt2ros.ros.msg_type, qos); + + mqtt2ros.ros.is_stale = false; + } catch (const rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", + e.what()); + return; + } + } + + RCLCPP_DEBUG(get_logger(), + "Sending ROS message of type '%s' from MQTT broker to ROS " + "topic '%s' ...", + mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); + mqtt2ros.ros.publisher->publish(serialized_msg); + } +} + + +void MqttClient::connected(const std::string& cause) { + + (void) cause; // Avoid compiler warning for unused parameter. + + is_connected_ = true; + std::string as_client = + client_config_.id.empty() + ? "" + : std::string(" as '") + client_config_.id + std::string("'"); + RCLCPP_INFO(get_logger(), "Connected to broker at '%s'%s", + client_->get_server_uri().c_str(), as_client.c_str()); + + // subscribe MQTT topics + for (const auto& [mqtt_topic, mqtt2ros] : mqtt2ros_) { + if (!mqtt2ros.primitive) { + std::string const mqtt_topic_to_subscribe = kRosMsgTypeMqttTopicPrefix + mqtt_topic; + client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); + RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); + } + // If not primitive and not fixed, we need the message type before we can public. In that case + // wait for the type to come in before subscribing to the data topic + if (mqtt2ros.primitive || mqtt2ros.fixed_type) { + client_->subscribe(mqtt_topic, mqtt2ros.mqtt.qos); + RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic.c_str()); + } + } +} + + +void MqttClient::connection_lost(const std::string& cause) { + + (void) cause; // Avoid compiler warning for unused parameter. + + RCLCPP_ERROR(get_logger(), + "Connection to broker lost, will try to reconnect..."); + is_connected_ = false; + connect(); +} + + +bool MqttClient::isConnected() { + + return is_connected_; +} + + +void MqttClient::isConnectedService( + mqtt_client_interfaces::srv::IsConnected::Request::SharedPtr request, + mqtt_client_interfaces::srv::IsConnected::Response::SharedPtr response) { + + (void) request; // Avoid compiler warning for unused parameter. + response->connected = isConnected(); +} + +void MqttClient::newRos2MqttBridge( + mqtt_client_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, + mqtt_client_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response){ + + // add mapping definition to ros2mqtt_ + Ros2MqttInterface& ros2mqtt = ros2mqtt_[request->ros_topic]; + ros2mqtt.ros.is_stale = true; + ros2mqtt.mqtt.topic = request->mqtt_topic; + ros2mqtt.primitive = request->primitive; + ros2mqtt.stamped = request->inject_timestamp; + ros2mqtt.ros.queue_size = request->ros_queue_size; + ros2mqtt.mqtt.qos = request->mqtt_qos; + ros2mqtt.mqtt.retained = request->mqtt_retained; + + if (ros2mqtt.stamped && ros2mqtt.primitive) { + RCLCPP_WARN( + get_logger(), + "Timestamp will not be injected into primitive messages on ROS " + "topic '%s'", + request->ros_topic.c_str()); + ros2mqtt.stamped = false; + } + + RCLCPP_INFO(get_logger(), "Bridging %sROS topic '%s' to MQTT topic '%s' %s", + ros2mqtt.primitive ? "primitive " : "", request->ros_topic.c_str(), + ros2mqtt.mqtt.topic.c_str(), + ros2mqtt.stamped ? "and measuring latency" : ""); + + // (re-)setup ROS subscriptions + setupSubscriptions(); + + response->success = true; +} + +void MqttClient::newMqtt2RosBridge( + mqtt_client_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, + mqtt_client_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response){ + + // add mapping definition to mqtt2ros_ + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[request->mqtt_topic]; + mqtt2ros.ros.is_stale = true; + mqtt2ros.ros.topic = request->ros_topic; + mqtt2ros.primitive = request->primitive; + mqtt2ros.mqtt.qos = request->mqtt_qos; + mqtt2ros.ros.queue_size = request->ros_queue_size; + mqtt2ros.ros.latched = request->ros_latched; + if (mqtt2ros.ros.latched) { + RCLCPP_WARN(get_logger(), + fmt::format("Parameter 'bridge.mqtt2ros.{}.advanced.ros.latched' is ignored " + "since ROS 2 does not easily support latched topics.", request->mqtt_topic).c_str()); + + } + + RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to %sROS topic '%s'", + request->mqtt_topic.c_str(), mqtt2ros.primitive ? "primitive " : "", + mqtt2ros.ros.topic.c_str()); + + // subscribe to the MQTT topic + std::string mqtt_topic_to_subscribe = request->mqtt_topic; + if (!mqtt2ros.primitive) + mqtt_topic_to_subscribe = kRosMsgTypeMqttTopicPrefix + request->mqtt_topic; + client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); + RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); + + response->success = true; +} + +void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) { + + // instantly take arrival timestamp + rclcpp::Time arrival_stamp( + builtin_interfaces::msg::Time(rclcpp::Clock(RCL_SYSTEM_TIME).now())); + + std::string mqtt_topic = mqtt_msg->get_topic(); + RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", + mqtt_topic.c_str()); + + // publish directly if primitive + if (mqtt2ros_.count(mqtt_topic) > 0) { + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + + if (mqtt2ros.primitive) { + if (mqtt2ros.fixed_type) { + mqtt2fixed(mqtt_msg); + } else { + mqtt2primitive(mqtt_msg); + } + return; + } + } + + // else first check for ROS message type + bool msg_contains_ros_msg_type = + mqtt_topic.find(kRosMsgTypeMqttTopicPrefix) != std::string::npos; + if (msg_contains_ros_msg_type) { + + // copy message type to generic message buffer + auto& payload = mqtt_msg->get_payload_ref(); + uint32_t payload_length = static_cast(payload.size()); + rclcpp::SerializedMessage serialized_ros_msg_type(payload_length); + std::memcpy(serialized_ros_msg_type.get_rcl_serialized_message().buffer, + &(payload[0]), payload_length); + serialized_ros_msg_type.get_rcl_serialized_message().buffer_length = payload_length; + + // deserialize ROS message type + mqtt_client_interfaces::msg::RosMsgType ros_msg_type; + deserializeRosMessage(serialized_ros_msg_type, ros_msg_type); + + // reconstruct corresponding MQTT data topic + std::string mqtt_data_topic = mqtt_topic; + mqtt_data_topic.erase(mqtt_data_topic.find(kRosMsgTypeMqttTopicPrefix), + kRosMsgTypeMqttTopicPrefix.length()); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_data_topic]; + + // if ROS message type has changed or if mapping is stale + if (ros_msg_type.name != mqtt2ros.ros.msg_type || mqtt2ros.ros.is_stale) { + + if (mqtt2ros.fixed_type) { + // We should never be in this situation if the type has been set explicitly. As fixed_type + // is not currently supported through the service based bridge creation and the type name + // not matching means the fixed type specified in the configuration does not match the + // one we just recieved + if (ros_msg_type.name != mqtt2ros.ros.msg_type) + RCLCPP_ERROR(get_logger(), + "Unexpected type name received for topic %s (expected %s but received %s)", + mqtt2ros.ros.topic.c_str(), mqtt2ros.ros.msg_type.c_str(), ros_msg_type.name.c_str()); + if (mqtt2ros.ros.is_stale) + RCLCPP_ERROR(get_logger(), "Topic %s has been unexpectedly marked stale", + mqtt2ros.ros.topic.c_str()); + return; + } + + mqtt2ros.ros.msg_type = ros_msg_type.name; + mqtt2ros.stamped = ros_msg_type.stamped; + RCLCPP_INFO(get_logger(), + "ROS publisher message type on topic '%s' set to '%s'", + mqtt2ros.ros.topic.c_str(), ros_msg_type.name.c_str()); + + // recreate generic publisher + try { + const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) + .durability(mqtt2ros.ros.qos.durability) + .reliability(mqtt2ros.ros.qos.reliability); + mqtt2ros.ros.publisher = create_generic_publisher( + mqtt2ros.ros.topic, ros_msg_type.name, qos); + } catch (rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", + e.what()); + return; + } + mqtt2ros.ros.is_stale = false; + + // subscribe to MQTT topic with actual ROS messages + client_->subscribe(mqtt_data_topic, mqtt2ros.mqtt.qos); + RCLCPP_DEBUG(get_logger(), "Subscribed MQTT topic '%s'", + mqtt_data_topic.c_str()); + } + + } else { + + // publish ROS message, if publisher initialized + if (!mqtt2ros_[mqtt_topic].ros.msg_type.empty()) { + mqtt2ros(mqtt_msg, arrival_stamp); + } else { + RCLCPP_WARN( + get_logger(), + "ROS publisher for data from MQTT topic '%s' is not yet initialized: " + "ROS message type not yet known", + mqtt_topic.c_str()); + } + } +} + + +void MqttClient::delivery_complete(mqtt::delivery_token_ptr token) { + + (void) token; // Avoid compiler warning for unused parameter. +} + + +void MqttClient::on_success(const mqtt::token& token) { + + (void) token; // Avoid compiler warning for unused parameter. + is_connected_ = true; +} + + +void MqttClient::on_failure(const mqtt::token& token) { + + RCLCPP_ERROR( + get_logger(), + "Connection to broker failed (return code %d), will automatically " + "retry...", + token.get_return_code()); + is_connected_ = false; +} + +} // namespace mqtt_client diff --git a/mqtt_client/src/MqttClient.ros2.cpp b/mqtt_client/src/MqttClient.ros2.cpp deleted file mode 100644 index bf7f62c..0000000 --- a/mqtt_client/src/MqttClient.ros2.cpp +++ /dev/null @@ -1,1518 +0,0 @@ -/* -============================================================================== -MIT License - -Copyright 2022 Institute for Automotive Engineering of RWTH Aachen University. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -============================================================================== -*/ - - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(mqtt_client::MqttClient) - - -namespace mqtt_client { - - -const std::string MqttClient::kRosMsgTypeMqttTopicPrefix = - "mqtt_client/ros_msg_type/"; - -const std::string MqttClient::kLatencyRosTopicPrefix = "~/latencies/"; - -template -T mqtt2float(mqtt::const_message_ptr mqtt_msg) { - auto str_msg = mqtt_msg->to_string (); - std::size_t pos; - const T v = std::stold(str_msg, &pos); - - if (pos != str_msg.size()) - throw std::invalid_argument ("not all characters processed"); - - return v; -} - -template -T mqtt2int(mqtt::const_message_ptr mqtt_msg) { - auto str_msg = mqtt_msg->to_string (); - std::size_t pos; - const T v = std::stoll(str_msg, &pos); - - if (pos != str_msg.size()) - throw std::invalid_argument ("not all characters processed"); - - return v; -} - -bool mqtt2bool(mqtt::const_message_ptr mqtt_msg) { - const std::string str_msg = mqtt_msg->to_string (); - std::string bool_str = mqtt_msg->to_string (); - std::transform(str_msg.cbegin(), str_msg.cend(), bool_str.begin(), - ::tolower); - if (bool_str == "true" || bool_str == "1") return true; - if (bool_str == "false" || bool_str == "0") return false; - - throw std::invalid_argument ("unable to decode string"); -} - -bool fixedMqtt2PrimitiveRos(mqtt::const_message_ptr mqtt_msg, - const std::string& msg_type, - rclcpp::SerializedMessage &serialized_msg) { - try { - if (msg_type == "std_msgs/msg/String") { - std_msgs::msg::String msg; - msg.data = mqtt_msg->to_string(); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/Bool") { - std_msgs::msg::Bool msg; - msg.data = mqtt2bool(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/Char") { - std_msgs::msg::Char msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/UInt8") { - std_msgs::msg::UInt8 msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/UInt16") { - std_msgs::msg::UInt16 msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/UInt32") { - std_msgs::msg::UInt32 msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/UInt64") { - std_msgs::msg::UInt64 msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/Int8") { - std_msgs::msg::Int8 msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/Int16") { - std_msgs::msg::Int16 msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/Int32") { - std_msgs::msg::Int32 msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/Int64") { - std_msgs::msg::Int64 msg; - msg.data = mqtt2int(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/Float32") { - std_msgs::msg::Float32 msg; - msg.data = mqtt2float(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else if (msg_type == "std_msgs/msg/Float64") { - std_msgs::msg::Float64 msg; - msg.data = mqtt2float(mqtt_msg); - - serializeRosMessage(msg, serialized_msg); - } else { - throw std::domain_error("Unhandled message type (" + msg_type + ")"); - } - - return true; - - } catch (const std::exception &) { - return false; - } - - -} - -/** - * @brief Extracts string of primitive data types from ROS message. - * - * This is helpful to extract the actual data payload of a primitive ROS - * message. If e.g. an std_msgs/msg/String is serialized to a string - * representation, it also contains the field name 'data'. This function - * instead returns the underlying value as string. - * - * @param [in] serialized_msg generic serialized ROS message - * @param [in] msg_type ROS message type, e.g. `std_msgs/msg/String` - * @param [out] primitive string representation of primitive message data - * - * @return true if primitive ROS message type was found - * @return false if ROS message type is not primitive - */ -bool primitiveRosMessageToString( - const std::shared_ptr& serialized_msg, - const std::string& msg_type, std::string& primitive) { - - bool found_primitive = true; - - if (msg_type == "std_msgs/msg/String") { - std_msgs::msg::String msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = msg.data; - } else if (msg_type == "std_msgs/msg/Bool") { - std_msgs::msg::Bool msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = msg.data ? "true" : "false"; - } else if (msg_type == "std_msgs/msg/Char") { - std_msgs::msg::Char msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/UInt8") { - std_msgs::msg::UInt8 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/UInt16") { - std_msgs::msg::UInt16 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/UInt32") { - std_msgs::msg::UInt32 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/UInt64") { - std_msgs::msg::UInt64 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/Int8") { - std_msgs::msg::Int8 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/Int16") { - std_msgs::msg::Int16 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/Int32") { - std_msgs::msg::Int32 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/Int64") { - std_msgs::msg::Int64 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/Float32") { - std_msgs::msg::Float32 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else if (msg_type == "std_msgs/msg/Float64") { - std_msgs::msg::Float64 msg; - deserializeRosMessage(*serialized_msg, msg); - primitive = std::to_string(msg.data); - } else { - found_primitive = false; - } - - return found_primitive; -} - - -MqttClient::MqttClient(const rclcpp::NodeOptions& options) : Node("mqtt_client", options) { - - loadParameters(); - setup(); -} - - -void MqttClient::loadParameters() { - - rcl_interfaces::msg::ParameterDescriptor param_desc; - - param_desc.description = "IP address or hostname of the machine running the MQTT broker"; - declare_parameter("broker.host", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "port the MQTT broker is listening on"; - declare_parameter("broker.port", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "username used for authenticating to the broker (if empty, will try to connect anonymously)"; - declare_parameter("broker.user", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "password used for authenticating to the broker"; - declare_parameter("broker.pass", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "whether to connect via SSL/TLS"; - declare_parameter("broker.tls.enabled", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); - param_desc.description = "CA certificate file trusted by client (relative to ROS_HOME)"; - declare_parameter("broker.tls.ca_certificate", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - - param_desc.description = "unique ID used to identify the client (broker may allow empty ID and automatically generate one)"; - declare_parameter("client.id", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "maximum number of messages buffered by the bridge when not connected to broker (only available if client ID is not empty)"; - declare_parameter("client.buffer.size", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "directory used to buffer messages when not connected to broker (relative to ROS_HOME)"; - declare_parameter("client.buffer.directory", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "topic used for this client's last-will message (no last will, if not specified)"; - declare_parameter("client.last_will.topic", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "last-will message"; - declare_parameter("client.last_will.message", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "QoS value for last-will message"; - declare_parameter("client.last_will.qos", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "whether to retain last-will message"; - declare_parameter("client.last_will.retained", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); - param_desc.description = "whether to use a clean session for this client"; - declare_parameter("client.clean_session", rclcpp::ParameterType::PARAMETER_BOOL, param_desc); - param_desc.description = "keep-alive interval in seconds"; - declare_parameter("client.keep_alive_interval", rclcpp::ParameterType::PARAMETER_DOUBLE, param_desc); - param_desc.description = "maximum number of inflight messages"; - declare_parameter("client.max_inflight", rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "client certificate file (only needed if broker requires client certificates; relative to ROS_HOME)"; - declare_parameter("client.tls.certificate", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "client private key file (relative to ROS_HOME)"; - declare_parameter("client.tls.key", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "client private key password"; - declare_parameter("client.tls.password", rclcpp::ParameterType::PARAMETER_STRING, param_desc); - - param_desc.description = "The list of topics to bridge from ROS to MQTT"; - const auto ros2mqtt_ros_topics = declare_parameter>("bridge.ros2mqtt.ros_topics", std::vector(), param_desc); - for (const auto& ros_topic : ros2mqtt_ros_topics) - { - param_desc.description = "MQTT topic on which the corresponding ROS messages are sent to the broker"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.mqtt_topic", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "whether to publish as primitive message"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.primitive", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); - param_desc.description = "If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the publisher"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.ros_type", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "whether to attach a timestamp to a ROS2MQTT payload (for latency computation on receiver side)"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.inject_timestamp", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); - param_desc.description = "ROS subscriber queue size"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.queue_size", ros_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "ROS subscriber QoS reliability"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.reliability", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "ROS subscriber QoS durability"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.durability", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "MQTT QoS value"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.qos", ros_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "whether to retain MQTT message"; - declare_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.retained", ros_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); - } - - param_desc.description = "The list of topics to bridge from MQTT to ROS"; - const auto mqtt2ros_mqtt_topics = declare_parameter>("bridge.mqtt2ros.mqtt_topics", std::vector(), param_desc); - for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) - { - param_desc.description = "ROS topic on which corresponding MQTT messages are published"; - declare_parameter(fmt::format("bridge.mqtt2ros.{}.ros_topic", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "whether to publish as primitive message (if coming from non-ROS MQTT client)"; - declare_parameter(fmt::format("bridge.mqtt2ros.{}.primitive", mqtt_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); - param_desc.description = "If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the MQTT message"; - declare_parameter(fmt::format("bridge.mqtt2ros.{}.ros_type", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "MQTT QoS value"; - declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "ROS publisher queue size"; - declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "ROS publisher QoS durability"; - declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.durability", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "ROS publisher QoS reliability"; - declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.reliability", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "whether to latch ROS message"; - declare_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.latched", mqtt_topic), rclcpp::ParameterType::PARAMETER_BOOL, param_desc); - } - - // load broker parameters from parameter server - std::string broker_tls_ca_certificate; - loadParameter("broker.host", broker_config_.host, "localhost"); - loadParameter("broker.port", broker_config_.port, 1883); - if (loadParameter("broker.user", broker_config_.user)) { - loadParameter("broker.pass", broker_config_.pass, ""); - } - if (loadParameter("broker.tls.enabled", broker_config_.tls.enabled, false)) { - loadParameter("broker.tls.ca_certificate", broker_tls_ca_certificate, - "/etc/ssl/certs/ca-certificates.crt"); - } - - // load client parameters from parameter server - std::string client_buffer_directory, client_tls_certificate, client_tls_key; - loadParameter("client.id", client_config_.id, ""); - client_config_.buffer.enabled = !client_config_.id.empty(); - if (client_config_.buffer.enabled) { - loadParameter("client.buffer.size", client_config_.buffer.size, 0); - loadParameter("client.buffer.directory", client_buffer_directory, "buffer"); - } else { - RCLCPP_WARN(get_logger(), - "Client buffer can not be enabled when client ID is empty"); - } - if (loadParameter("client.last_will.topic", client_config_.last_will.topic)) { - loadParameter("client.last_will.message", client_config_.last_will.message, - "offline"); - loadParameter("client.last_will.qos", client_config_.last_will.qos, 0); - loadParameter("client.last_will.retained", - client_config_.last_will.retained, false); - } - loadParameter("client.clean_session", client_config_.clean_session, true); - loadParameter("client.keep_alive_interval", - client_config_.keep_alive_interval, 60.0); - loadParameter("client.max_inflight", client_config_.max_inflight, 65535); - if (broker_config_.tls.enabled) { - if (loadParameter("client.tls.certificate", client_tls_certificate)) { - loadParameter("client.tls.key", client_tls_key); - loadParameter("client.tls.password", client_config_.tls.password); - loadParameter("client.tls.version", client_config_.tls.version); - loadParameter("client.tls.verify", client_config_.tls.verify); - loadParameter("client.tls.alpn_protos", client_config_.tls.alpn_protos); - } - } - - // resolve filepaths - broker_config_.tls.ca_certificate = resolvePath(broker_tls_ca_certificate); - client_config_.buffer.directory = resolvePath(client_buffer_directory); - client_config_.tls.certificate = resolvePath(client_tls_certificate); - client_config_.tls.key = resolvePath(client_tls_key); - - // parse bridge parameters - - // ros2mqtt - for (const auto& ros_topic : ros2mqtt_ros_topics) { - - rclcpp::Parameter mqtt_topic_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.mqtt_topic", ros_topic), mqtt_topic_param)) { - - // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic - const std::string mqtt_topic = mqtt_topic_param.as_string(); - Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; - ros2mqtt.mqtt.topic = mqtt_topic; - - // ros2mqtt[k]/primitive - rclcpp::Parameter primitive_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.primitive", ros_topic), primitive_param)) - ros2mqtt.primitive = primitive_param.as_bool(); - - // ros2mqtt[k]/ros_type - rclcpp::Parameter ros_type_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.ros_type", ros_topic), ros_type_param)) { - ros2mqtt.ros.msg_type = ros_type_param.as_string(); - ros2mqtt.fixed_type = true; - RCLCPP_DEBUG(get_logger(), "Using explicit ROS message type '%s'", ros2mqtt.ros.msg_type.c_str()); - } - - // ros2mqtt[k]/inject_timestamp - rclcpp::Parameter stamped_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.inject_timestamp", ros_topic), stamped_param)) - ros2mqtt.stamped = stamped_param.as_bool(); - if (ros2mqtt.stamped && ros2mqtt.primitive) { - RCLCPP_WARN( - get_logger(), - "Timestamp will not be injected into primitive messages on ROS " - "topic '%s'", - ros_topic.c_str()); - ros2mqtt.stamped = false; - } - - // ros2mqtt[k]/advanced/ros/queue_size - rclcpp::Parameter queue_size_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.queue_size", ros_topic), - queue_size_param)) - ros2mqtt.ros.queue_size = queue_size_param.as_int(); - - rclcpp::Parameter durability_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.durability", ros_topic), durability_param)) { - const auto p = durability_param.as_string(); - if (p == "system_default") { - ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::SystemDefault; - } else if (p == "volatile") { - ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::Volatile; - } else if (p == "transient_local") { - ros2mqtt.ros.qos.durability = rclcpp::DurabilityPolicy::TransientLocal; - } else if (p == "auto") { - ros2mqtt.ros.qos.durability = {}; - } else { - RCLCPP_ERROR(get_logger(), "Unexpected durability %s", p.c_str ()); - exit(EXIT_FAILURE); - } - } - - rclcpp::Parameter reliability_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.reliability", ros_topic), reliability_param)) { - const auto p = reliability_param.as_string(); - if (p == "system_default") { - ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::SystemDefault; - } else if (p == "best_effort") { - ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::BestEffort; - } else if (p == "reliable") { - ros2mqtt.ros.qos.reliability = rclcpp::ReliabilityPolicy::Reliable; - } else if (p == "auto") { - ros2mqtt.ros.qos.reliability = {}; - } else { - RCLCPP_ERROR(get_logger(), "Unexpected reliability %s", p.c_str ()); - exit(EXIT_FAILURE); - } - } - - // ros2mqtt[k]/advanced/mqtt/qos - rclcpp::Parameter qos_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.qos", ros_topic), qos_param)) - ros2mqtt.mqtt.qos = qos_param.as_int(); - - // ros2mqtt[k]/advanced/mqtt/retained - rclcpp::Parameter retained_param; - if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.mqtt.retained", ros_topic), retained_param)) - ros2mqtt.mqtt.retained = retained_param.as_bool(); - - RCLCPP_INFO(get_logger(), "Bridging %sROS topic '%s' to MQTT topic '%s' %s", - ros2mqtt.primitive ? "primitive " : "", ros_topic.c_str(), - ros2mqtt.mqtt.topic.c_str(), - ros2mqtt.stamped ? "and measuring latency" : ""); - } else { - RCLCPP_WARN(get_logger(), - fmt::format("Parameter 'bridge.ros2mqtt.{}' is missing subparameter " - "'mqtt_topic', will be ignored", ros_topic).c_str()); - } - } - - // mqtt2ros - for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { - - rclcpp::Parameter ros_topic_param; - if (get_parameter(fmt::format("bridge.mqtt2ros.{}.ros_topic", mqtt_topic), ros_topic_param)) { - - // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic - const std::string ros_topic = ros_topic_param.as_string(); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - mqtt2ros.ros.topic = ros_topic; - - // mqtt2ros[k]/primitive - rclcpp::Parameter primitive_param; - if (get_parameter(fmt::format("bridge.mqtt2ros.{}.primitive", mqtt_topic), primitive_param)) - mqtt2ros.primitive = primitive_param.as_bool(); - - - rclcpp::Parameter ros_type_param; - if (get_parameter(fmt::format("bridge.mqtt2ros.{}.ros_type", mqtt_topic), ros_type_param)) { - mqtt2ros.ros.msg_type = ros_type_param.as_string(); - mqtt2ros.fixed_type = true; - RCLCPP_DEBUG(get_logger(), "Using explicit ROS message type '%s' for '%s'", mqtt2ros.ros.msg_type.c_str(), ros_topic.c_str()); - } - - // mqtt2ros[k]/advanced/mqtt/qos - rclcpp::Parameter qos_param; - if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), qos_param)) - mqtt2ros.mqtt.qos = qos_param.as_int(); - - // mqtt2ros[k]/advanced/ros/queue_size - rclcpp::Parameter queue_size_param; - if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), - queue_size_param)) - mqtt2ros.ros.queue_size = queue_size_param.as_int(); - - rclcpp::Parameter durability_param; - if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.durability", mqtt_topic), durability_param)) { - const auto p = durability_param.as_string(); - if (p == "system_default") { - mqtt2ros.ros.qos.durability = rclcpp::DurabilityPolicy::SystemDefault; - } else if (p == "volatile") { - mqtt2ros.ros.qos.durability = rclcpp::DurabilityPolicy::Volatile; - } else if (p == "transient_local") { - mqtt2ros.ros.qos.durability = - rclcpp::DurabilityPolicy::TransientLocal; - } else { - RCLCPP_ERROR(get_logger(), "Unexpected durability %s", p.c_str ()); - exit(EXIT_FAILURE); - } - } - - rclcpp::Parameter reliability_param; - if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.qos.reliability", mqtt_topic), reliability_param)) { - const auto p = reliability_param.as_string(); - if (p == "system_default") { - mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::SystemDefault; - } else if (p == "best_effort") { - mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::BestEffort; - } else if (p == "reliable") { - mqtt2ros.ros.qos.reliability = rclcpp::ReliabilityPolicy::Reliable; - } else { - RCLCPP_ERROR(get_logger(), "Unexpected reliability %s", p.c_str ()); - exit(EXIT_FAILURE); - } - } - - // mqtt2ros[k]/advanced/ros/latched - rclcpp::Parameter latched_param; - if (get_parameter(fmt::format("bridge.mqtt2ros.{}.advanced.ros.latched", mqtt_topic), latched_param)) { - mqtt2ros.ros.latched = latched_param.as_bool(); - RCLCPP_WARN(get_logger(), - fmt::format("Parameter 'bridge.mqtt2ros.{}.advanced.ros.latched' is ignored " - "since ROS 2 does not easily support latched topics.", mqtt_topic).c_str()); - } - - RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to %sROS topic '%s'", - mqtt_topic.c_str(), mqtt2ros.primitive ? "primitive " : "", - mqtt2ros.ros.topic.c_str()); - } - else { - RCLCPP_WARN(get_logger(), - fmt::format("Parameter 'bridge.ros2mqtt.{}' is missing subparameter " - "'ros_topic', will be ignored", mqtt_topic).c_str()); - } - } - - if (ros2mqtt_.empty() && mqtt2ros_.empty()) { - RCLCPP_ERROR(get_logger(), - "No valid ROS-MQTT bridge found in parameter 'bridge'"); - exit(EXIT_FAILURE); - } -} - - -bool MqttClient::loadParameter(const std::string& key, std::string& value) { - bool found = get_parameter(key, value); - if (found) - RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), - value.c_str()); - return found; -} - - -bool MqttClient::loadParameter(const std::string& key, std::string& value, - const std::string& default_value) { - bool found = get_parameter_or(key, value, default_value); - if (!found) - RCLCPP_WARN(get_logger(), "Parameter '%s' not set, defaulting to '%s'", - key.c_str(), default_value.c_str()); - if (found) - RCLCPP_DEBUG(get_logger(), "Retrieved parameter '%s' = '%s'", key.c_str(), - value.c_str()); - return found; -} - - -std::filesystem::path MqttClient::resolvePath(const std::string& path_string) { - - std::filesystem::path path(path_string); - if (path_string.empty()) return path; - if (!path.has_root_path()) { - std::string ros_home = rcpputils::get_env_var("ROS_HOME"); - if (ros_home.empty()) - ros_home = std::string(std::filesystem::current_path()); - path = std::filesystem::path(ros_home); - path.append(path_string); - } - if (!std::filesystem::exists(path)) - RCLCPP_WARN(get_logger(), "Requested path '%s' does not exist", - std::string(path).c_str()); - return path; -} - - -void MqttClient::setup() { - - // pre-compute timestamp length - builtin_interfaces::msg::Time tmp_stamp; - rclcpp::SerializedMessage tmp_serialized_stamp; - serializeRosMessage(tmp_stamp, tmp_serialized_stamp); - stamp_length_ = tmp_serialized_stamp.size(); - - // initialize MQTT client - setupClient(); - - // connect to MQTT broker - connect(); - - // create ROS service server - is_connected_service_ = - create_service( - "~/is_connected", std::bind(&MqttClient::isConnectedService, this, - std::placeholders::_1, std::placeholders::_2)); - - // create dynamic mappings services - new_ros2mqtt_bridge_service_ = - create_service( - "~/new_ros2mqtt_bridge", std::bind(&MqttClient::newRos2MqttBridge, this, - std::placeholders::_1, std::placeholders::_2)); - new_mqtt2ros_bridge_service_ = - create_service( - "~/new_mqtt2ros_bridge", std::bind(&MqttClient::newMqtt2RosBridge, this, - std::placeholders::_1, std::placeholders::_2)); - - // setup subscribers; check for new types every second - check_subscriptions_timer_ = - create_wall_timer(std::chrono::duration(1.0), - std::bind(&MqttClient::setupSubscriptions, this)); - - setupPublishers (); -} - -std::optional MqttClient::getCompatibleQoS (const std::string &ros_topic, const rclcpp::TopicEndpointInfo &tei, - const Ros2MqttInterface &ros2mqtt) const -{ - // the basic premise here is that we take the QoS from the publisher, overwrite any parts that are explicitly set - // via configuration then check if the result is compatible with the publisher - auto qos = tei.qos_profile (); - - if (auto r = ros2mqtt.ros.qos.reliability) - qos.reliability (*r); - if (auto d = ros2mqtt.ros.qos.durability) - qos.durability (*d); - qos.keep_last (ros2mqtt.ros.queue_size); - - const auto qres = rclcpp::qos_check_compatible (tei.qos_profile (), qos); - - switch (qres.compatibility) - { - case rclcpp::QoSCompatibility::Ok: - return qos; - case rclcpp::QoSCompatibility::Warning: - RCLCPP_DEBUG(get_logger(), "QoS compatibility warning on topic %s - %s", ros_topic.c_str(), qres.reason.c_str ()); - // presumably this is still compatible - return qos; - case rclcpp::QoSCompatibility::Error: - default: - return {}; - } - -} - -std::vector MqttClient::getCandidatePublishers( - const std::string& ros_topic, const Ros2MqttInterface& ros2mqtt) const -{ - const auto &pubs = get_publishers_info_by_topic(ros_topic); - - if (pubs.empty ()) - return {}; - - std::vector ret; - - ret.reserve (pubs.size ()); - - for (const auto &p : pubs) - { - const std::string msg_type = p.topic_type(); - - // if the type isn't set, match aginst all t ypes, otherwise only match against mtching types - if (ros2mqtt.ros.msg_type.empty () || msg_type == ros2mqtt.ros.msg_type) - ret.push_back (p); - } - - // If we found any matching types, return those - if (! ret.empty ()) - return ret; - - // If we didn't and we aren't fix type, then just return the full set of publishers - if (! ros2mqtt.fixed_type) - return pubs; - - // None of these publishers will work... :sad_panda: - return {}; -} - -void MqttClient::setupSubscriptions() { - - // For each ros2mqtt interface, check if we need to do a lazy subscription (eg, lazy subscription would be if we - // are determining either the type of QoS dynamically). If we don't, then just make the fixed subscriber if its not - // already made. - // - // If we do, we check each publisher for a potential match. The first step is to filter down the list of - // publishers based on the type. If we are fixed type then that list only includes publishers with matching types. - // If we aren't its a little trickier. For dynamic types, if we already have matched against a type previously, the - // candidate list will include all publishers which have matching types if any exist. If none of the publishers - // have matching types, then the list will include all publishers. - // - // Then for each candidate publisher, check if their QoS is compatible with the QoS specic in the configuration. This - // is really just needed because there is the potential for someone to set half of the QoS to auto and half to - // explicit. - // Condition for requiring "lazy" subscription where we need to walk the - const auto requires_lazy_subscription = [] (const Ros2MqttInterface &ros2mqtt) - { - if (! ros2mqtt.fixed_type) - return true; - if (ros2mqtt.ros.qos.reliability == std::nullopt || - ros2mqtt.ros.qos.durability == std::nullopt) - return true; - return false; - }; - - for (auto& [ros_topic, ros2mqtt] : ros2mqtt_) { - - std::function msg)> bound_callback_func = - std::bind(&MqttClient::ros2mqtt, this, std::placeholders::_1, ros_topic); - - if (! requires_lazy_subscription (ros2mqtt)) - { - try { - if (ros2mqtt.ros.subscriber) - continue; - - auto const qos = rclcpp::QoS(ros2mqtt.ros.queue_size) - .reliability(*ros2mqtt.ros.qos.reliability) - .durability(*ros2mqtt.ros.qos.durability); - - ros2mqtt.ros.subscriber = create_generic_subscription( - ros_topic, ros2mqtt.ros.msg_type, qos, bound_callback_func); - ros2mqtt.ros.is_stale = false; - RCLCPP_INFO(get_logger(), "Subscribed ROS topic '%s' of type '%s'", - ros_topic.c_str(), ros2mqtt.ros.msg_type.c_str()); - } catch (rclcpp::exceptions::RCLError& e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", - e.what()); - continue; - } - } else { - const auto &pubs = getCandidatePublishers (ros_topic, ros2mqtt); - - if (pubs.empty()) continue; - - for (auto endpointInfo : pubs) { - try { - // check if message type has changed or if mapping is stale - const std::string msg_type = endpointInfo.topic_type(); - - if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale && ros2mqtt.ros.subscriber) - continue; - - auto const qos = getCompatibleQoS (ros_topic, endpointInfo, ros2mqtt); - - if (! qos) - continue; - - ros2mqtt.ros.is_stale = false; - ros2mqtt.ros.msg_type = msg_type; - - ros2mqtt.ros.subscriber = create_generic_subscription( - ros_topic, msg_type, *qos, bound_callback_func); - - RCLCPP_INFO(get_logger(), "Subscribed ROS topic '%s' of type '%s'", - ros_topic.c_str(), msg_type.c_str()); - } catch (rclcpp::exceptions::RCLError& e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", - e.what()); - continue; - } - } - } - } -} - -void MqttClient::setupPublishers() { - - for (auto& [mqtt_topic, mqtt2ros] : mqtt2ros_) { - if (mqtt2ros.ros.publisher) - continue; - - // If the type is not fixed, we require a mqtt message in order to deduce the type - if (!mqtt2ros.fixed_type) - continue; - - try { - const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) - .durability(mqtt2ros.ros.qos.durability) - .reliability(mqtt2ros.ros.qos.reliability); - mqtt2ros.ros.publisher = create_generic_publisher( - mqtt2ros.ros.topic, mqtt2ros.ros.msg_type, qos); - - mqtt2ros.ros.is_stale = false; - } catch (const rclcpp::exceptions::RCLError& e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", - e.what()); - } - } -} - -void MqttClient::setupClient() { - - // basic client connection options - connect_options_.set_automatic_reconnect(true); - connect_options_.set_clean_session(client_config_.clean_session); - connect_options_.set_keep_alive_interval(client_config_.keep_alive_interval); - connect_options_.set_max_inflight(client_config_.max_inflight); - - // user authentication - if (!broker_config_.user.empty()) { - connect_options_.set_user_name(broker_config_.user); - connect_options_.set_password(broker_config_.pass); - } - - // last will - if (!client_config_.last_will.topic.empty()) { - mqtt::will_options will( - client_config_.last_will.topic, client_config_.last_will.message, - client_config_.last_will.qos, client_config_.last_will.retained); - connect_options_.set_will(will); - } - - // SSL/TLS - if (broker_config_.tls.enabled) { - mqtt::ssl_options ssl; - ssl.set_trust_store(broker_config_.tls.ca_certificate); - if (!client_config_.tls.certificate.empty() && - !client_config_.tls.key.empty()) { - ssl.set_key_store(client_config_.tls.certificate); - ssl.set_private_key(client_config_.tls.key); - if (!client_config_.tls.password.empty()) - ssl.set_private_key_password(client_config_.tls.password); - } - ssl.set_ssl_version(client_config_.tls.version); - ssl.set_verify(client_config_.tls.verify); - ssl.set_alpn_protos(client_config_.tls.alpn_protos); - connect_options_.set_ssl(ssl); - } - - // create MQTT client - const std::string protocol = broker_config_.tls.enabled ? "ssl" : "tcp"; - const std::string uri = fmt::format("{}://{}:{}", protocol, broker_config_.host, - broker_config_.port); - try { - if (client_config_.buffer.enabled) { - client_ = std::shared_ptr(new mqtt::async_client( - uri, client_config_.id, client_config_.buffer.size, - client_config_.buffer.directory)); - } else { - client_ = std::shared_ptr( - new mqtt::async_client(uri, client_config_.id)); - } - } catch (const mqtt::exception& e) { - RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); - exit(EXIT_FAILURE); - } - - // setup MQTT callbacks - client_->set_callback(*this); -} - - -void MqttClient::connect() { - - std::string as_client = - client_config_.id.empty() - ? "" - : std::string(" as '") + client_config_.id + std::string("'"); - RCLCPP_INFO(get_logger(), "Connecting to broker at '%s'%s ...", - client_->get_server_uri().c_str(), as_client.c_str()); - - try { - client_->connect(connect_options_, nullptr, *this); - } catch (const mqtt::exception& e) { - RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); - exit(EXIT_FAILURE); - } -} - - -void MqttClient::ros2mqtt( - const std::shared_ptr& serialized_msg, - const std::string& ros_topic) { - - Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; - std::string mqtt_topic = ros2mqtt.mqtt.topic; - std::vector payload_buffer; - - // gather information on ROS message type in special ROS message - mqtt_client_interfaces::msg::RosMsgType ros_msg_type; - ros_msg_type.name = ros2mqtt.ros.msg_type; - ros_msg_type.stamped = ros2mqtt.stamped; - - RCLCPP_DEBUG(get_logger(), "Received ROS message of type '%s' on topic '%s'", - ros_msg_type.name.c_str(), ros_topic.c_str()); - - if (ros2mqtt.primitive) { // publish as primitive (string) message - - // resolve ROS messages to primitive strings if possible - std::string payload; - bool found_primitive = - primitiveRosMessageToString(serialized_msg, ros_msg_type.name, payload); - if (found_primitive) { - payload_buffer = std::vector(payload.begin(), payload.end()); - } else { - RCLCPP_WARN(get_logger(), - "Cannot send ROS message of type '%s' as primitive message, " - "check supported primitive types", - ros_msg_type.name.c_str()); - return; - } - - } else { // publish as complete ROS message incl. ROS message type - - // serialize ROS message type - rclcpp::SerializedMessage serialized_ros_msg_type; - serializeRosMessage(ros_msg_type, serialized_ros_msg_type); - uint32_t msg_type_length = serialized_ros_msg_type.size(); - std::vector msg_type_buffer = std::vector( - serialized_ros_msg_type.get_rcl_serialized_message().buffer, - serialized_ros_msg_type.get_rcl_serialized_message().buffer + msg_type_length); - - // send ROS message type information to MQTT broker - mqtt_topic = kRosMsgTypeMqttTopicPrefix + ros2mqtt.mqtt.topic; - try { - RCLCPP_DEBUG(get_logger(), - "Sending ROS message type to MQTT broker on topic '%s' ...", - mqtt_topic.c_str()); - mqtt::message_ptr mqtt_msg = - mqtt::make_message(mqtt_topic, msg_type_buffer.data(), - msg_type_buffer.size(), ros2mqtt.mqtt.qos, true); - client_->publish(mqtt_msg); - } catch (const mqtt::exception& e) { - RCLCPP_WARN( - get_logger(), - "Publishing ROS message type information to MQTT topic '%s' failed: %s", - mqtt_topic.c_str(), e.what()); - } - - // build MQTT payload for ROS message (R) as [R] - // or [S, R] if timestamp (S) is included - uint32_t msg_length = serialized_msg->size(); - uint32_t payload_length = msg_length; - uint32_t msg_offset = 0; - if (ros2mqtt.stamped) { - - // allocate buffer with appropriate size to hold [S, R] - msg_offset += stamp_length_; - payload_length += stamp_length_; - payload_buffer.resize(payload_length); - - // copy serialized ROS message to payload [-, R] - std::copy(serialized_msg->get_rcl_serialized_message().buffer, - serialized_msg->get_rcl_serialized_message().buffer + msg_length, - payload_buffer.begin() + msg_offset); - } else { - - // directly build payload buffer on top of serialized message - payload_buffer = std::vector( - serialized_msg->get_rcl_serialized_message().buffer, - serialized_msg->get_rcl_serialized_message().buffer + msg_length); - } - - // inject timestamp as final step - if (ros2mqtt.stamped) { - - // take current timestamp - builtin_interfaces::msg::Time stamp(rclcpp::Clock(RCL_SYSTEM_TIME).now()); - - // copy serialized timestamp to payload [S, R] - rclcpp::SerializedMessage serialized_stamp; - serializeRosMessage(stamp, serialized_stamp); - std::copy( - serialized_stamp.get_rcl_serialized_message().buffer, - serialized_stamp.get_rcl_serialized_message().buffer + stamp_length_, - payload_buffer.begin()); - } - } - - // send ROS message to MQTT broker - mqtt_topic = ros2mqtt.mqtt.topic; - try { - RCLCPP_DEBUG( - get_logger(), - "Sending ROS message of type '%s' to MQTT broker on topic '%s' ...", - ros_msg_type.name.c_str(), mqtt_topic.c_str()); - mqtt::message_ptr mqtt_msg = mqtt::make_message( - mqtt_topic, payload_buffer.data(), payload_buffer.size(), - ros2mqtt.mqtt.qos, ros2mqtt.mqtt.retained); - client_->publish(mqtt_msg); - } catch (const mqtt::exception& e) { - RCLCPP_WARN( - get_logger(), - "Publishing ROS message type information to MQTT topic '%s' failed: %s", - mqtt_topic.c_str(), e.what()); - } -} - - -void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg, - const rclcpp::Time& arrival_stamp) { - std::string mqtt_topic = mqtt_msg->get_topic(); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - auto& payload = mqtt_msg->get_payload_ref(); - uint32_t payload_length = static_cast(payload.size()); - - // read MQTT payload for ROS message (R) as [R] - // or [S, R] if timestamp (S) is included - uint32_t msg_length = payload_length; - uint32_t msg_offset = 0; - - // if stamped, compute latency - if (mqtt2ros.stamped) { - - // copy stamp to generic message buffer - rclcpp::SerializedMessage serialized_stamp(stamp_length_); - std::memcpy(serialized_stamp.get_rcl_serialized_message().buffer, - &(payload[msg_offset]), stamp_length_); - serialized_stamp.get_rcl_serialized_message().buffer_length = stamp_length_; - - // deserialize stamp - builtin_interfaces::msg::Time stamp; - deserializeRosMessage(serialized_stamp, stamp); - - // compute ROS2MQTT latency - rclcpp::Duration latency = arrival_stamp - stamp; - std_msgs::msg::Float64 latency_msg; - latency_msg.data = latency.seconds(); - - // publish latency - if (!mqtt2ros.ros.latency_publisher) { - std::string latency_topic = kLatencyRosTopicPrefix + mqtt2ros.ros.topic; - latency_topic.replace(latency_topic.find("//"), 2, "/"); - mqtt2ros.ros.latency_publisher = - create_publisher(latency_topic, 1); - } - mqtt2ros.ros.latency_publisher->publish(latency_msg); - - msg_length -= stamp_length_; - msg_offset += stamp_length_; - } - - // copy ROS message from MQTT message to generic message buffer - rclcpp::SerializedMessage serialized_msg(msg_length); - std::memcpy(serialized_msg.get_rcl_serialized_message().buffer, - &(payload[msg_offset]), msg_length); - serialized_msg.get_rcl_serialized_message().buffer_length = msg_length; - - // publish generic ROS message - RCLCPP_DEBUG( - get_logger(), - "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", - mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); - mqtt2ros.ros.publisher->publish(serialized_msg); -} - - -void MqttClient::mqtt2primitive(mqtt::const_message_ptr mqtt_msg) { - - std::string mqtt_topic = mqtt_msg->get_topic(); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - - const std::string str_msg = mqtt_msg->to_string(); - bool found_primitive = false; - std::string ros_msg_type = "std_msgs/msg/String"; - rclcpp::SerializedMessage serialized_msg; - - // check for bool - if (!found_primitive) { - std::string bool_str = str_msg; - std::transform(str_msg.cbegin(), str_msg.cend(), bool_str.begin(), - ::tolower); - if (bool_str == "true" || bool_str == "false") { - - bool bool_msg = (bool_str == "true"); - - // construct and serialize ROS message - std_msgs::msg::Bool msg; - msg.data = bool_msg; - serializeRosMessage(msg, serialized_msg); - - ros_msg_type = "std_msgs/msg/Bool"; - found_primitive = true; - } - } - - // check for int - if (!found_primitive) { - std::size_t pos; - try { - const int int_msg = std::stoi(str_msg, &pos); - if (pos == str_msg.size()) { - - // construct and serialize ROS message - std_msgs::msg::Int32 msg; - msg.data = int_msg; - serializeRosMessage(msg, serialized_msg); - - ros_msg_type = "std_msgs/msg/Int32"; - found_primitive = true; - } - } catch (const std::invalid_argument& ex) { - } catch (const std::out_of_range& ex) { - } - } - - // check for float - if (!found_primitive) { - std::size_t pos; - try { - const float float_msg = std::stof(str_msg, &pos); - if (pos == str_msg.size()) { - - // construct and serialize ROS message - std_msgs::msg::Float32 msg; - msg.data = float_msg; - serializeRosMessage(msg, serialized_msg); - - ros_msg_type = "std_msgs/msg/Float32"; - found_primitive = true; - } - } catch (const std::invalid_argument& ex) { - } catch (const std::out_of_range& ex) { - } - } - - // fall back to string - if (!found_primitive) { - - // construct and serialize ROS message - std_msgs::msg::String msg; - msg.data = str_msg; - serializeRosMessage(msg, serialized_msg); - } - - // if ROS message type has changed or if mapping is stale - if (ros_msg_type != mqtt2ros.ros.msg_type || mqtt2ros.ros.is_stale) { - - mqtt2ros.ros.msg_type = ros_msg_type; - RCLCPP_INFO(get_logger(), - "ROS publisher message type on topic '%s' set to '%s'", - mqtt2ros.ros.topic.c_str(), ros_msg_type.c_str()); - - // recreate generic publisher - try { - const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) - .durability(mqtt2ros.ros.qos.durability) - .reliability(mqtt2ros.ros.qos.reliability); - mqtt2ros.ros.publisher = create_generic_publisher( - mqtt2ros.ros.topic, ros_msg_type, qos); - } catch (rclcpp::exceptions::RCLError& e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", - e.what()); - return; - } - mqtt2ros.ros.is_stale = false; - } - - // publish - RCLCPP_DEBUG( - get_logger(), - "Sending ROS message of type '%s' from MQTT broker to ROS topic '%s' ...", - mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); - mqtt2ros.ros.publisher->publish(serialized_msg); -} - -void MqttClient::mqtt2fixed(mqtt::const_message_ptr mqtt_msg) { - - std::string mqtt_topic = mqtt_msg->get_topic(); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - - rclcpp::SerializedMessage serialized_msg; - - if (!fixedMqtt2PrimitiveRos(mqtt_msg, mqtt2ros.ros.msg_type, serialized_msg)) { - RCLCPP_WARN( - get_logger(), - "Could not convert mqtt message into type %s on topic %s ...", - mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); - } else { - - if (!mqtt2ros.ros.publisher) - { - RCLCPP_INFO(get_logger(), - "ROS publisher message type on topic '%s' set to '%s'", - mqtt2ros.ros.topic.c_str(), mqtt2ros.ros.msg_type.c_str()); - - // recreate generic publisher - try { - const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) - .durability(mqtt2ros.ros.qos.durability) - .reliability(mqtt2ros.ros.qos.reliability); - mqtt2ros.ros.publisher = create_generic_publisher( - mqtt2ros.ros.topic, mqtt2ros.ros.msg_type, qos); - - mqtt2ros.ros.is_stale = false; - } catch (const rclcpp::exceptions::RCLError& e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", - e.what()); - return; - } - } - - RCLCPP_DEBUG(get_logger(), - "Sending ROS message of type '%s' from MQTT broker to ROS " - "topic '%s' ...", - mqtt2ros.ros.msg_type.c_str(), mqtt2ros.ros.topic.c_str()); - mqtt2ros.ros.publisher->publish(serialized_msg); - } -} - - -void MqttClient::connected(const std::string& cause) { - - (void) cause; // Avoid compiler warning for unused parameter. - - is_connected_ = true; - std::string as_client = - client_config_.id.empty() - ? "" - : std::string(" as '") + client_config_.id + std::string("'"); - RCLCPP_INFO(get_logger(), "Connected to broker at '%s'%s", - client_->get_server_uri().c_str(), as_client.c_str()); - - // subscribe MQTT topics - for (const auto& [mqtt_topic, mqtt2ros] : mqtt2ros_) { - if (!mqtt2ros.primitive) { - std::string const mqtt_topic_to_subscribe = kRosMsgTypeMqttTopicPrefix + mqtt_topic; - client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); - RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); - } - // If not primitive and not fixed, we need the message type before we can public. In that case - // wait for the type to come in before subscribing to the data topic - if (mqtt2ros.primitive || mqtt2ros.fixed_type) { - client_->subscribe(mqtt_topic, mqtt2ros.mqtt.qos); - RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic.c_str()); - } - } -} - - -void MqttClient::connection_lost(const std::string& cause) { - - (void) cause; // Avoid compiler warning for unused parameter. - - RCLCPP_ERROR(get_logger(), - "Connection to broker lost, will try to reconnect..."); - is_connected_ = false; - connect(); -} - - -bool MqttClient::isConnected() { - - return is_connected_; -} - - -void MqttClient::isConnectedService( - mqtt_client_interfaces::srv::IsConnected::Request::SharedPtr request, - mqtt_client_interfaces::srv::IsConnected::Response::SharedPtr response) { - - (void) request; // Avoid compiler warning for unused parameter. - response->connected = isConnected(); -} - -void MqttClient::newRos2MqttBridge( - mqtt_client_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, - mqtt_client_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response){ - - // add mapping definition to ros2mqtt_ - Ros2MqttInterface& ros2mqtt = ros2mqtt_[request->ros_topic]; - ros2mqtt.ros.is_stale = true; - ros2mqtt.mqtt.topic = request->mqtt_topic; - ros2mqtt.primitive = request->primitive; - ros2mqtt.stamped = request->inject_timestamp; - ros2mqtt.ros.queue_size = request->ros_queue_size; - ros2mqtt.mqtt.qos = request->mqtt_qos; - ros2mqtt.mqtt.retained = request->mqtt_retained; - - if (ros2mqtt.stamped && ros2mqtt.primitive) { - RCLCPP_WARN( - get_logger(), - "Timestamp will not be injected into primitive messages on ROS " - "topic '%s'", - request->ros_topic.c_str()); - ros2mqtt.stamped = false; - } - - RCLCPP_INFO(get_logger(), "Bridging %sROS topic '%s' to MQTT topic '%s' %s", - ros2mqtt.primitive ? "primitive " : "", request->ros_topic.c_str(), - ros2mqtt.mqtt.topic.c_str(), - ros2mqtt.stamped ? "and measuring latency" : ""); - - // (re-)setup ROS subscriptions - setupSubscriptions(); - - response->success = true; -} - -void MqttClient::newMqtt2RosBridge( - mqtt_client_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, - mqtt_client_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response){ - - // add mapping definition to mqtt2ros_ - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[request->mqtt_topic]; - mqtt2ros.ros.is_stale = true; - mqtt2ros.ros.topic = request->ros_topic; - mqtt2ros.primitive = request->primitive; - mqtt2ros.mqtt.qos = request->mqtt_qos; - mqtt2ros.ros.queue_size = request->ros_queue_size; - mqtt2ros.ros.latched = request->ros_latched; - if (mqtt2ros.ros.latched) { - RCLCPP_WARN(get_logger(), - fmt::format("Parameter 'bridge.mqtt2ros.{}.advanced.ros.latched' is ignored " - "since ROS 2 does not easily support latched topics.", request->mqtt_topic).c_str()); - - } - - RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to %sROS topic '%s'", - request->mqtt_topic.c_str(), mqtt2ros.primitive ? "primitive " : "", - mqtt2ros.ros.topic.c_str()); - - // subscribe to the MQTT topic - std::string mqtt_topic_to_subscribe = request->mqtt_topic; - if (!mqtt2ros.primitive) - mqtt_topic_to_subscribe = kRosMsgTypeMqttTopicPrefix + request->mqtt_topic; - client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); - RCLCPP_INFO(get_logger(), "Subscribed MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); - - response->success = true; -} - -void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) { - - // instantly take arrival timestamp - rclcpp::Time arrival_stamp( - builtin_interfaces::msg::Time(rclcpp::Clock(RCL_SYSTEM_TIME).now())); - - std::string mqtt_topic = mqtt_msg->get_topic(); - RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", - mqtt_topic.c_str()); - - // publish directly if primitive - if (mqtt2ros_.count(mqtt_topic) > 0) { - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; - - if (mqtt2ros.primitive) { - if (mqtt2ros.fixed_type) { - mqtt2fixed(mqtt_msg); - } else { - mqtt2primitive(mqtt_msg); - } - return; - } - } - - // else first check for ROS message type - bool msg_contains_ros_msg_type = - mqtt_topic.find(kRosMsgTypeMqttTopicPrefix) != std::string::npos; - if (msg_contains_ros_msg_type) { - - // copy message type to generic message buffer - auto& payload = mqtt_msg->get_payload_ref(); - uint32_t payload_length = static_cast(payload.size()); - rclcpp::SerializedMessage serialized_ros_msg_type(payload_length); - std::memcpy(serialized_ros_msg_type.get_rcl_serialized_message().buffer, - &(payload[0]), payload_length); - serialized_ros_msg_type.get_rcl_serialized_message().buffer_length = payload_length; - - // deserialize ROS message type - mqtt_client_interfaces::msg::RosMsgType ros_msg_type; - deserializeRosMessage(serialized_ros_msg_type, ros_msg_type); - - // reconstruct corresponding MQTT data topic - std::string mqtt_data_topic = mqtt_topic; - mqtt_data_topic.erase(mqtt_data_topic.find(kRosMsgTypeMqttTopicPrefix), - kRosMsgTypeMqttTopicPrefix.length()); - Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_data_topic]; - - // if ROS message type has changed or if mapping is stale - if (ros_msg_type.name != mqtt2ros.ros.msg_type || mqtt2ros.ros.is_stale) { - - if (mqtt2ros.fixed_type) { - // We should never be in this situation if the type has been set explicitly. As fixed_type - // is not currently supported through the service based bridge creation and the type name - // not matching means the fixed type specified in the configuration does not match the - // one we just recieved - if (ros_msg_type.name != mqtt2ros.ros.msg_type) - RCLCPP_ERROR(get_logger(), - "Unexpected type name received for topic %s (expected %s but received %s)", - mqtt2ros.ros.topic.c_str(), mqtt2ros.ros.msg_type.c_str(), ros_msg_type.name.c_str()); - if (mqtt2ros.ros.is_stale) - RCLCPP_ERROR(get_logger(), "Topic %s has been unexpectedly marked stale", - mqtt2ros.ros.topic.c_str()); - return; - } - - mqtt2ros.ros.msg_type = ros_msg_type.name; - mqtt2ros.stamped = ros_msg_type.stamped; - RCLCPP_INFO(get_logger(), - "ROS publisher message type on topic '%s' set to '%s'", - mqtt2ros.ros.topic.c_str(), ros_msg_type.name.c_str()); - - // recreate generic publisher - try { - const auto qos = rclcpp::QoS(mqtt2ros.ros.queue_size) - .durability(mqtt2ros.ros.qos.durability) - .reliability(mqtt2ros.ros.qos.reliability); - mqtt2ros.ros.publisher = create_generic_publisher( - mqtt2ros.ros.topic, ros_msg_type.name, qos); - } catch (rclcpp::exceptions::RCLError& e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s", - e.what()); - return; - } - mqtt2ros.ros.is_stale = false; - - // subscribe to MQTT topic with actual ROS messages - client_->subscribe(mqtt_data_topic, mqtt2ros.mqtt.qos); - RCLCPP_DEBUG(get_logger(), "Subscribed MQTT topic '%s'", - mqtt_data_topic.c_str()); - } - - } else { - - // publish ROS message, if publisher initialized - if (!mqtt2ros_[mqtt_topic].ros.msg_type.empty()) { - mqtt2ros(mqtt_msg, arrival_stamp); - } else { - RCLCPP_WARN( - get_logger(), - "ROS publisher for data from MQTT topic '%s' is not yet initialized: " - "ROS message type not yet known", - mqtt_topic.c_str()); - } - } -} - - -void MqttClient::delivery_complete(mqtt::delivery_token_ptr token) { - - (void) token; // Avoid compiler warning for unused parameter. -} - - -void MqttClient::on_success(const mqtt::token& token) { - - (void) token; // Avoid compiler warning for unused parameter. - is_connected_ = true; -} - - -void MqttClient::on_failure(const mqtt::token& token) { - - RCLCPP_ERROR( - get_logger(), - "Connection to broker failed (return code %d), will automatically " - "retry...", - token.get_return_code()); - is_connected_ = false; -} - -} // namespace mqtt_client diff --git a/mqtt_client_interfaces/CMakeLists.txt b/mqtt_client_interfaces/CMakeLists.txt index 848364c..83a979b 100644 --- a/mqtt_client_interfaces/CMakeLists.txt +++ b/mqtt_client_interfaces/CMakeLists.txt @@ -1,53 +1,17 @@ cmake_minimum_required(VERSION 3.12.0 FATAL_ERROR) project(mqtt_client_interfaces) -find_package(ros_environment REQUIRED QUIET) -set(ROS_VERSION $ENV{ROS_VERSION}) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) -# === ROS2 (AMENT) ============================================================= -if(${ROS_VERSION} EQUAL 2) +rosidl_generate_interfaces(${PROJECT_NAME} + msg/RosMsgType.msg + srv/IsConnected.srv + srv/ros2/NewMqtt2RosBridge.srv + srv/ros2/NewRos2MqttBridge.srv + DEPENDENCIES std_msgs +) - find_package(ament_cmake REQUIRED) - find_package(rosidl_default_generators REQUIRED) - find_package(std_msgs REQUIRED) +ament_package() - rosidl_generate_interfaces(${PROJECT_NAME} - msg/RosMsgType.msg - srv/IsConnected.srv - srv/ros2/NewMqtt2RosBridge.srv - srv/ros2/NewRos2MqttBridge.srv - DEPENDENCIES std_msgs - ) - - ament_package() - -# === ROS1 (CATKIN) ============================================================ -elseif(${ROS_VERSION} EQUAL 1) - - find_package(catkin REQUIRED COMPONENTS - message_generation - std_msgs - ) - - add_message_files( - FILES - RosMsgType.msg - ) - - add_service_files( - FILES - IsConnected.srv - ) - - generate_messages( - DEPENDENCIES - std_msgs - ) - - catkin_package( - CATKIN_DEPENDS - message_runtime - std_msgs - ) - -endif() diff --git a/mqtt_client_interfaces/package.xml b/mqtt_client_interfaces/package.xml index cee0328..0a5d9d2 100644 --- a/mqtt_client_interfaces/package.xml +++ b/mqtt_client_interfaces/package.xml @@ -14,25 +14,15 @@ Lennart Reiher - ros_environment - - - ament_cmake - rosidl_default_generators - std_msgs - rosidl_default_runtime - - - catkin - message_generation - std_msgs - message_runtime + ament_cmake + rosidl_default_generators + std_msgs + rosidl_default_runtime - catkin - ament_cmake + ament_cmake - rosidl_interface_packages + rosidl_interface_packages \ No newline at end of file