Merge pull request #35 from mvccogo/main

Author: lreiher

README.md

@@ -383,7 +383,7 @@ Enables connected ROS-based devices or robots to exchange ROS messages via an MQ
 
 ###### Services
 
-- `is_connected` ([`mqtt_client/srv/IsConnected`](mqtt_client_interfaces/srv/IsConnected.srv))
+- `~is_connected` ([`mqtt_client/srv/IsConnected`](mqtt_client_interfaces/srv/IsConnected.srv))
   Returns whether the client is connected to the MQTT broker.
 
 ###### Parameters
@@ -412,9 +412,17 @@ Enables connected ROS-based devices or robots to exchange ROS messages via an MQ
 
 ###### Services
 
-- `is_connected` ([`mqtt_client/srv/IsConnected`](mqtt_client_interfaces/srv/IsConnected.srv))
+- `~/is_connected` ([`mqtt_client/srv/IsConnected`](mqtt_client_interfaces/srv/IsConnected.srv))
   Returns whether the client is connected to the MQTT broker.
 
+- `~/new_ros2mqtt_bridge` ([`mqtt_client/srv/NewRos2MqttBridge`](mqtt_client_interfaces/srv/NewRos2MqttBridge.srv))
+  Returns whether a new ROS -> MQTT bridge was created.
+
+- `~/new_mqtt2ros_bridge` ([`mqtt_client/srv/NewMqtt2RosBridge`](mqtt_client_interfaces/srv/NewMqtt2RosBridge.srv))
+  Returns whether a new MQTT -> ROS bridge was created.
+
+
+
 ###### Parameters
 
 See [Configuration](#configuration).

mqtt_client/include/mqtt_client/MqttClient.ros2.hpp

@@ -35,6 +35,8 @@ SOFTWARE.
 #define FMT_HEADER_ONLY
 #include <fmt/format.h>
 #include <mqtt_client_interfaces/srv/is_connected.hpp>
+#include <mqtt_client_interfaces/srv/new_mqtt2_ros_bridge.hpp>
+#include <mqtt_client_interfaces/srv/new_ros2_mqtt_bridge.hpp>
 #include <mqtt/async_client.h>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/serialization.hpp>
@@ -277,6 +279,26 @@ class MqttClient : public rclcpp::Node,
     mqtt_client_interfaces::srv::IsConnected::Request::SharedPtr request,
     mqtt_client_interfaces::srv::IsConnected::Response::SharedPtr response);
 
+  /**
+   * @brief ROS service that dynamically creates a ROS -> MQTT mapping.
+   *
+   * @param request  service request
+   * @param response service response
+   */
+  void newRos2MqttBridge(
+    mqtt_client_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request,
+    mqtt_client_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response);
+
+  /**
+   * @brief ROS service that dynamically creates an MQTT -> ROS mapping.
+   *
+   * @param request  service request
+   * @param response service response
+   */
+  void newMqtt2RosBridge(
+    mqtt_client_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request,
+    mqtt_client_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response);
+
   /**
    * @brief Callback for when the client receives a MQTT message from the
    * broker.
@@ -374,6 +396,7 @@ class MqttClient : public rclcpp::Node,
         subscriber;          ///< generic ROS subscriber
       std::string msg_type;  ///< message type of subscriber
       int queue_size = 1;    ///< ROS subscriber queue size
+      bool is_stale = false; ///< whether a new generic publisher/subscriber is required
     } ros;                   ///< ROS-related variables
     struct {
       std::string topic;      ///< MQTT topic
@@ -399,6 +422,7 @@ class MqttClient : public rclcpp::Node,
         latency_publisher;   ///< ROS publisher for latency
       int queue_size = 1;    ///< ROS publisher queue size
       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 primitive = false;  ///< whether to publish as primitive message (if
                              ///< coming from non-ROS MQTT client)
@@ -432,6 +456,18 @@ class MqttClient : public rclcpp::Node,
   rclcpp::Service<mqtt_client_interfaces::srv::IsConnected>::SharedPtr
     is_connected_service_;
 
+  /**
+   * @brief ROS Service server for providing dynamic ROS to MQTT mappings.
+   */
+  rclcpp::Service<mqtt_client_interfaces::srv::NewRos2MqttBridge>::SharedPtr
+    new_ros2mqtt_bridge_service_;
+
+  /**
+   * @brief ROS Service server for providing dynamic MQTT to ROS mappings.
+   */
+  rclcpp::Service<mqtt_client_interfaces::srv::NewMqtt2RosBridge>::SharedPtr
+    new_mqtt2ros_bridge_service_;
+
   /**
    * @brief Status variable keeping track of connection status to broker
    */

mqtt_client/src/MqttClient.ros2.cpp

@@ -446,9 +446,18 @@ void MqttClient::setup() {
   // create ROS service server
   is_connected_service_ =
     create_service<mqtt_client_interfaces::srv::IsConnected>(
-      "is_connected", std::bind(&MqttClient::isConnectedService, this,
+      "~/is_connected", std::bind(&MqttClient::isConnectedService, this,
                                 std::placeholders::_1, std::placeholders::_2));
 
+  // create dynamic mappings services
+  new_ros2mqtt_bridge_service_ =
+  create_service<mqtt_client_interfaces::srv::NewRos2MqttBridge>(
+      "~/new_ros2mqtt_bridge", std::bind(&MqttClient::newRos2MqttBridge, this,
+                                std::placeholders::_1, std::placeholders::_2));
+  new_mqtt2ros_bridge_service_ =
+  create_service<mqtt_client_interfaces::srv::NewMqtt2RosBridge>(
+      "~/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_ =
@@ -466,9 +475,10 @@ void MqttClient::setupSubscriptions() {
   for (auto& [ros_topic, ros2mqtt] : ros2mqtt_) {
     if (all_topics_and_types.count(ros_topic)) {
 
-      // check if message type has changed
+      // check if message type has changed or if mapping is stale
       const std::string& msg_type = all_topics_and_types.at(ros_topic)[0];
-      if (msg_type == ros2mqtt.ros.msg_type) continue;
+      if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale) continue;
+      ros2mqtt.ros.is_stale = false;
       ros2mqtt.ros.msg_type = msg_type;
 
       // create new generic subscription, if message type has changed
@@ -826,8 +836,8 @@ void MqttClient::mqtt2primitive(mqtt::const_message_ptr mqtt_msg) {
     serializeRosMessage(msg, serialized_msg);
   }
 
-  // if ROS message type has changed
-  if (ros_msg_type != mqtt2ros.ros.msg_type) {
+  // 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(),
@@ -843,6 +853,7 @@ void MqttClient::mqtt2primitive(mqtt::const_message_ptr mqtt_msg) {
                    e.what());
       return;
     }
+    mqtt2ros.ros.is_stale = false;
   }
 
   // publish
@@ -902,6 +913,72 @@ void MqttClient::isConnectedService(
   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) {
 
@@ -945,8 +1022,8 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
                           kRosMsgTypeMqttTopicPrefix.length());
     Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_data_topic];
 
-    // if ROS message type has changed
-    if (ros_msg_type.name != mqtt2ros.ros.msg_type) {
+    // if ROS message type has changed or if mapping is stale
+    if (ros_msg_type.name != mqtt2ros.ros.msg_type || mqtt2ros.ros.is_stale) {
 
       mqtt2ros.ros.msg_type = ros_msg_type.name;
       mqtt2ros.stamped = ros_msg_type.stamped;
@@ -963,6 +1040,7 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
                      e.what());
         return;
       }
+      mqtt2ros.ros.is_stale = false;
 
       // subscribe to MQTT topic with actual ROS messages
       client_->subscribe(mqtt_data_topic, mqtt2ros.mqtt.qos);

mqtt_client_interfaces/CMakeLists.txt

@@ -14,6 +14,8 @@ 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
   )
 

mqtt_client_interfaces/srv/ros2/NewMqtt2RosBridge.srv

@@ -0,0 +1,10 @@
+# This service is used to dynamically register new MQTT -> ROS mappings.
+
+string ros_topic
+string mqtt_topic 
+bool primitive false 
+uint8 mqtt_qos 0
+uint32 ros_queue_size 1
+bool ros_latched false
+---
+bool success

mqtt_client_interfaces/srv/ros2/NewRos2MqttBridge.srv

@@ -0,0 +1,11 @@
+# This service is used to dynamically register new ROS -> MQTT mappings.
+
+string ros_topic
+string mqtt_topic
+bool primitive false
+bool inject_timestamp false
+uint32 ros_queue_size 1
+uint8 mqtt_qos 0
+bool mqtt_retained false
+---
+bool success