Make fixed type publishers ahead of time

- Updated the docs
- Added in "system_default" qos values
- Fixed type publishers are now made right away rather than waiting for
  an mqtt message to come in

Author: JayHerpin

README.md

@@ -275,58 +275,66 @@ client:
 
 ```yaml
 bridge:
-  ros2mqtt:            # Array specifying which ROS topics to map to which MQTT topics
+  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
+          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
+          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
+          qos:           # [0] MQTT QoS value
         ros:
-          queue_size:        # [1] ROS publisher queue size
-          latched:           # [false] whether to latch ROS message
+          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
-    ros_topics:          # Array specifying which ROS topics to bridge
+  ros2mqtt:                # Object specifying which ROS topics to map to which MQTT topics
+    ros_topics:            # Array specifying which ROS topics to bridge
       - {{ ros_topic_name }} # The ROS topic that should be bridged, corresponds to the sub-object in the YAML
     {{ ros_topic_name }}:
       mqtt_topic:          # MQTT topic on which the corresponding ROS messages are sent to the broker
       primitive:           # [false] whether to publish as primitive message
+      ros_type:            # [*empty*] If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the publisher
       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
+          queue_size:      # [1] ROS subscriber queue size
+          qos:
+            reliability:   # [auto] One of "auto", "system_default", "reliable", "best_effort". If auto, the QoS is automatically determined via the publisher
+            durability:    # [auto] One of "auto", "system_default", "volatile", "transient_local". If auto, the QoS is automatically determined via the publisher
         mqtt:
-          qos:               # [0] MQTT QoS value
-          retained:          # [false] whether to retain MQTT message
+          qos:             # [0] MQTT QoS value
+          retained:        # [false] whether to retain MQTT message
   mqtt2ros:            # Object specifying which MQTT topics to map to which ROS topics
     mqtt_topics:         # Array specifying which ROS topics to bridge
       - {{ mqtt_topic_name }} # The MQTT topic that should be bridged, corresponds to the sub-object in the YAML
     {{ mqtt_topic_name }}:
       ros_topic:           # ROS topic on which corresponding MQTT messages are published
+      ros_type:            # [*empty*] If set, the ROS msg type provided will be used. If empty, the type is automatically deduced via the MQTT message
       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
+          qos:
+            reliability: # [system_default] One of "system_default", "reliable", "best_effort". 
+            durability:  # [system_default] One of "system_default", "volatile", "transient_local". 
 ```
 
 ## Primitive Messages

mqtt_client/include/mqtt_client/MqttClient.ros2.hpp

@@ -209,10 +209,17 @@ class MqttClient : public rclcpp::Node,
      const std::string& ros_topic, const Ros2MqttInterface& ros2mqtt) const;
 
   /**
-   * @brief Checks all active ROS topics in order to set up generic subscribers.
+   * @brief Setup any subscriptions we can.
+   *
+   * These may be fixed type/QoS, or dynamically matched against active publisher
    */
   void setupSubscriptions();
 
+  /**
+   * @brief Setup any publishers that we can
+   */
+  void setupPublishers();
+
   /**
    * @brief Sets up the client connection options and initializes the client
    * object.

mqtt_client/src/MqttClient.ros2.cpp

@@ -448,7 +448,9 @@ void MqttClient::loadParameters() {
       rclcpp::Parameter durability_param;
       if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.durability", mqtt_topic), durability_param)) {
         const auto p = durability_param.as_string();
-        if (p == "volatile") {
+        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;
@@ -463,7 +465,9 @@ void MqttClient::loadParameters() {
       rclcpp::Parameter reliability_param;
       if (get_parameter(fmt::format("bridge.ros2mqtt.{}.advanced.ros.qos.reliability", mqtt_topic), reliability_param)) {
         const auto p = reliability_param.as_string();
-        if (p == "best_effort") {
+        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;
@@ -538,7 +542,9 @@ void MqttClient::loadParameters() {
       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 == "volatile") {
+        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 =
@@ -552,7 +558,9 @@ void MqttClient::loadParameters() {
       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 == "best_effort") {
+        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;
@@ -664,6 +672,8 @@ void MqttClient::setup() {
   check_subscriptions_timer_ =
     create_wall_timer(std::chrono::duration<double>(1.0),
                       std::bind(&MqttClient::setupSubscriptions, this));
+
+  setupPublishers ();
 }
 
 std::optional<rclcpp::QoS> MqttClient::getCompatibleQoS (const std::string &ros_topic, const rclcpp::TopicEndpointInfo &tei,
@@ -812,6 +822,31 @@ void MqttClient::setupSubscriptions() {
   }
 }
 
+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
@@ -1210,6 +1245,7 @@ void MqttClient::mqtt2fixed(mqtt::const_message_ptr mqtt_msg) {
       } catch (const rclcpp::exceptions::RCLError& e) {
         RCLCPP_ERROR(get_logger(), "Failed to create generic publisher: %s",
                      e.what());
+        return;
       }
     }