Allowing non-primitive with fixed types

This change now allows non-primitive encoding to work with fixed types
on either side of the bridge.

Note that if the ros type specified mqtt2ros does not match the one
received from the mqtt type topic then the config file specified version
will win (eg, the type will not be allowed to change).

Note that this is necessary since ROS doesn't really allow us to change
the type of topics on the fly and since we used a fixed type, the
publisher was created on startup.

Author: JayHerpin

mqtt_client/config/params.ros2.fixed-type-and-qos.yaml

@@ -9,8 +9,7 @@
           - /ping/primitive
         /ping/primitive:
           mqtt_topic: pingpong/primitive
-          # This is implied by the explicit ros type
-          #primitive: true
+          primitive: false
           ros_type: std_msgs/msg/Bool
           advanced:
             ros:
@@ -23,8 +22,7 @@
           - pingpong/primitive
         pingpong/primitive:
           ros_topic: /pong/primitive
-          #This is implied by the explicit ros type
-          #primitive: true
+          primitive: false
           ros_type: std_msgs/msg/Bool
           advanced:
             ros:

mqtt_client/config/params.ros2.primitive-fixed-type-and-qos.yaml

@@ -0,0 +1,32 @@
+/**/*:
+  ros__parameters:
+    broker:
+      host: localhost
+      port: 1883
+    bridge:
+      ros2mqtt:
+        ros_topics:
+          - /ping/primitive
+        /ping/primitive:
+          mqtt_topic: pingpong/primitive
+          primitive: true
+          ros_type: std_msgs/msg/Bool
+          advanced:
+            ros:
+              queue_size: 10
+              qos:
+                durability: auto
+                reliability: auto
+      mqtt2ros:
+        mqtt_topics:
+          - pingpong/primitive
+        pingpong/primitive:
+          ros_topic: /pong/primitive
+          primitive: true
+          ros_type: std_msgs/msg/Bool
+          advanced:
+            ros:
+              queue_size: 10
+              qos:
+                durability: transient_local
+                reliability: reliable

mqtt_client/include/mqtt_client/MqttClient.ros2.hpp

@@ -474,7 +474,7 @@ class MqttClient : public rclcpp::Node,
       bool latched = false;  ///< whether to latch ROS message
       bool is_stale = false; ///< whether a new generic publisher/subscriber is required
     } ros;      ///< ROS-related variables
-    bool fixed_type = false;   ///< whether the published ros message type is specified explicitly
+    bool fixed_type = false; ///< whether the published ros message type is specified explicitly
     bool primitive = false;  ///< whether to publish as primitive message (if
                              ///< coming from non-ROS MQTT client)
     bool stamped = false;    ///< whether timestamp is injected

mqtt_client/src/MqttClient.ros2.cpp

@@ -433,7 +433,6 @@ void MqttClient::loadParameters() {
       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;
-        ros2mqtt.primitive = true;
         RCLCPP_DEBUG(get_logger(), "Using explicit ROS message type '%s'", ros2mqtt.ros.msg_type.c_str());
       }
 
@@ -532,7 +531,6 @@ void MqttClient::loadParameters() {
       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;
-        mqtt2ros.primitive = true;
         RCLCPP_DEBUG(get_logger(), "Using explicit ROS message type '%s' for '%s'", mqtt2ros.ros.msg_type.c_str(), ros_topic.c_str());
       }
 
@@ -1280,11 +1278,17 @@ void MqttClient::connected(const std::string& cause) {
 
   // subscribe MQTT topics
   for (const auto& [mqtt_topic, mqtt2ros] : mqtt2ros_) {
-    std::string mqtt_topic_to_subscribe = mqtt_topic;
-    if (!mqtt2ros.primitive)  // subscribe topics for ROS message types first
-      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 (!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());
+    }
   }
 }
 
@@ -1395,13 +1399,12 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
   if (mqtt2ros_.count(mqtt_topic) > 0) {
     Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic];
 
-    if (mqtt2ros.fixed_type) {
-      mqtt2fixed(mqtt_msg);
-      return;
-    }
-
     if (mqtt2ros.primitive) {
-      mqtt2primitive(mqtt_msg);
+      if (mqtt2ros.fixed_type) {
+        mqtt2fixed(mqtt_msg);
+      } else {
+        mqtt2primitive(mqtt_msg);
+      }
       return;
     }
   }
@@ -1432,6 +1435,21 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
     // 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(),