move information about timestamp from payload to RosMsgType

Author: lreiher

README.md

@@ -366,15 +366,15 @@ Inside the callback, the generic messages received on the `ros_topic` are serial
 
 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](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.
+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. Since injection of the timestamp is optional, an extra bit of information is needed for the receiver to correctly decode the MQTT message. Therefore, the first entry in the `std::vector<uint8>` message buffer is used to indicate whether the message includes an injected timestamp. The resulting `std::vector<uint8>` payload takes on one of the following forms:
+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<uint8>` message payload takes on one of the following forms:
 
 ```txt
-[ 1 | ... serialized timestamp ... | ... serialized ROS messsage ...]
-[ 0 | ... serialized ROS messsage ...]
+[... serialized timestamp ... | ... serialized ROS messsage ...]
+[... serialized ROS messsage ...]
 ```
 
 To summarize, the dataflow is as follows:
@@ -384,12 +384,12 @@ To summarize, the dataflow is as follows:
   - ROS message type information is serialized and sent via the MQTT broker on MQTT topic `mqtt_client/ros_msg_type/<ros2mqtt_mqtt_topic>`
   - the actual ROS message is serialized
   - if `inject_timestamp`, the current timestamp is serialized and concatenated with the message
-  - an integer is added to the message's head indicating whether a timestamp was injected
   - the actual MQTT message is sent via the MQTT broker on MQTT topic `<ros2mqtt_mqtt_topic>`
 - an MQTT message containing the ROS message type information is received on MQTT topic `mqtt_client/ros_msg_type/<ros2mqtt_mqtt_topic>`
   - 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 the first element of the message, it is decoded into the serialized ROS message and the serialized timestamp
+  - 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/<mqtt2ros_ros_topic>`
   - the serialized ROS message is published using the *ShapeShifter* on ROS topic `<mqtt2ros_ros_topic>`
 

mqtt_client/include/mqtt_client/MqttClient.h

@@ -370,6 +370,7 @@ class MqttClient : public nodelet::Nodelet,
     } 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:

mqtt_client/include/mqtt_client/MqttClient.ros2.hpp

@@ -364,6 +364,7 @@ class MqttClient : public rclcpp::Node,
     } 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:

mqtt_client/src/MqttClient.cpp

@@ -490,6 +490,7 @@ void MqttClient::ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg,
   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());
@@ -534,26 +535,24 @@ void MqttClient::ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg,
         mqtt_topic.c_str(), e.what());
     }
 
-    // build MQTT payload for ROS message (R) as [1, S, R] or [0, R]
-    // where first item = 1 if timestamp (S) is included
+    // 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 = 1 + msg_length;
+    uint32_t payload_length = msg_length;
     uint32_t stamp_length =
       ros::serialization::serializationLength(ros::Time());
-    uint32_t msg_offset = 1;
+    uint32_t msg_offset = 0;
     if (ros2mqtt.stamped) {
-      // allocate buffer with appropriate size to hold [1, S, R]
+      // allocate buffer with appropriate size to hold [S, R]
       msg_offset += stamp_length;
       payload_length += stamp_length;
       payload_buffer.resize(payload_length);
-      payload_buffer[0] = 1;
     } else {
-      // allocate buffer with appropriate size to hold [0, R]
+      // allocate buffer with appropriate size to hold [R]
       payload_buffer.resize(payload_length);
-      payload_buffer[0] = 0;
     }
 
-    // serialize ROS message to payload [0/1, -, R]
+    // serialize ROS message to payload [-, R]
     ros::serialization::OStream msg_stream(&payload_buffer[msg_offset],
                                            msg_length);
     ros::serialization::serialize(msg_stream, *ros_msg);
@@ -570,8 +569,8 @@ void MqttClient::ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg,
           "running?",
           ros2mqtt.mqtt.topic.c_str());
 
-      // serialize timestamp to payload [1, S, R]
-      ros::serialization::OStream stamp_stream(&payload_buffer[1],
+      // serialize timestamp to payload [S, R]
+      ros::serialization::OStream stamp_stream(&payload_buffer[0],
                                                stamp_length);
       ros::serialization::serialize(stamp_stream, stamp);
     }
@@ -603,16 +602,13 @@ void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg,
   auto& payload = mqtt_msg->get_payload_ref();
   uint32_t payload_length = static_cast<uint32_t>(payload.size());
 
-  // determine whether timestamp is injected by reading first element
-  bool stamped = (static_cast<uint8_t>(payload[0]) > 0);
-
-  // read MQTT payload for ROS message (R) as [1, S, R] or [0, R]
-  // where first item = 1 if timestamp (S) is included
-  uint32_t msg_length = payload_length - 1;
-  uint32_t msg_offset = 1;
+  // 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 (stamped) {
+  if (mqtt2ros.stamped) {
 
     // create ROS message buffer on top of MQTT message payload
     char* non_const_payload = const_cast<char*>(&payload[1]);
@@ -888,6 +884,10 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
     // 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, "");
@@ -898,9 +898,6 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
         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(), ros_msg_type.name.c_str());
-
       // 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());

mqtt_client/src/MqttClient.ros2.cpp

@@ -523,6 +523,7 @@ void MqttClient::ros2mqtt(
   // 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());
@@ -570,27 +571,24 @@ void MqttClient::ros2mqtt(
         mqtt_topic.c_str(), e.what());
     }
 
-    // build MQTT payload for ROS message (R) as [1, S, R] or [0, R]
-    // where first item = 1 if timestamp (S) is included
+    // 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 = 1 + msg_length;
-
-    uint32_t msg_offset = 1;
+    uint32_t payload_length = msg_length;
+    uint32_t msg_offset = 0;
     if (ros2mqtt.stamped) {
-      // allocate buffer with appropriate size to hold [1, S, R]
+      // allocate buffer with appropriate size to hold [S, R]
       msg_offset += stamp_length_;
       payload_length += stamp_length_;
       payload_buffer.resize(payload_length);
-      payload_buffer[0] = 1;
     } else {
-      // allocate buffer with appropriate size to hold [0, R]
+      // allocate buffer with appropriate size to hold [R]
       payload_buffer.resize(payload_length);
-      payload_buffer[0] = 0;
     }
 
     // TODO: if not stamped, could create payload_buffer directly on top of
     // serialized_msg->get_rcl_serialized_message().buffer copy serialized ROS
-    // message to payload [0/1, -, R]
+    // 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);
@@ -601,13 +599,13 @@ void MqttClient::ros2mqtt(
       // take current timestamp
       builtin_interfaces::msg::Time stamp(rclcpp::Clock(RCL_SYSTEM_TIME).now());
 
-      // copy serialized timestamp to payload [1, S, R]
+      // 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() + 1);
+        payload_buffer.begin());
     }
   }
 
@@ -639,16 +637,13 @@ void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg,
   auto& payload = mqtt_msg->get_payload_ref();
   uint32_t payload_length = static_cast<uint32_t>(payload.size());
 
-  // determine whether timestamp is injected by reading first element
-  bool stamped = (static_cast<uint8_t>(payload[0]) > 0);
-
-  // read MQTT payload for ROS message (R) as [1, S, R] or [0, R]
-  // where first item = 1 if timestamp (S) is included
-  uint32_t msg_length = payload_length - 1;
-  uint32_t msg_offset = 1;
+  // 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 (stamped) {
+  if (mqtt2ros.stamped) {
 
     // copy stamp to generic message buffer
     rclcpp::SerializedMessage serialized_stamp(stamp_length_);
@@ -890,6 +885,7 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
     if (ros_msg_type.name != mqtt2ros.ros.msg_type) {
 
       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());

mqtt_client_interfaces/msg/RosMsgType.msg

@@ -2,4 +2,5 @@
 
 string md5          # MD5 sum of ROS message type
 string name         # ROS message type name (e.g. 'std_msgs/Float32')
-string definition   # ROS message type definition (e.g. 'float data')
+string definition   # ROS message type definition (e.g. 'float data')
+bool stamped        # whether timestamp is prepended for latency computation