Merge pull request #4 from ika-rwth-aachen/improvement/runtime-optimization

Optimize runtime

Author: lreiher

include/mqtt_client/MqttClient.h

@@ -186,11 +186,11 @@ class MqttClient : public nodelet::Nodelet,
    * - serialized timestamp (optional)
    * - serialized ROS message
    *
-   * The MQTT payload is expected to carry a serialized ROS message.
-   *
-   * @param   mqtt_msg     MQTT message
+   * @param   mqtt_msg       MQTT message
+   * @param   arrival_stamp  arrival timestamp used for latency computation
    */
-  void mqtt2ros(mqtt::const_message_ptr mqtt_msg);
+  void mqtt2ros(mqtt::const_message_ptr mqtt_msg,
+                const ros::WallTime& arrival_stamp = ros::WallTime::now());
 
   /**
    * @brief Callback for when the client has successfully connected to the

src/MqttClient.cpp

@@ -153,8 +153,9 @@ void MqttClient::loadParameters() {
             }
           }
 
-          NODELET_INFO("Bridging ROS topic '%s' to MQTT topic '%s'",
-                       ros_topic.c_str(), ros2mqtt.mqtt.topic.c_str());
+          NODELET_INFO("Bridging ROS topic '%s' to MQTT topic '%s' %s",
+                       ros_topic.c_str(), ros2mqtt.mqtt.topic.c_str(),
+                       ros2mqtt.stamped ? "and measuring latency" : "");
         } else {
           NODELET_WARN(
             "Parameter 'bridge/ros2mqtt[%d]' is missing subparameter "
@@ -403,52 +404,46 @@ void MqttClient::ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg,
       mqtt_topic.c_str(), e.what());
   }
 
-  // serialize ROS message to buffer
-  uint32_t msg_length = ros::serialization::serializationLength(*ros_msg);
-  std::vector<uint8_t> msg_buffer;
-  msg_buffer.resize(msg_length);
-  ros::serialization::OStream msg_stream(msg_buffer.data(), msg_length);
-  ros::serialization::serialize(msg_stream, *ros_msg);
-
   // build 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 = ros::serialization::serializationLength(*ros_msg);
   uint32_t payload_length = 1 + msg_length;
+  uint32_t stamp_length = ros::serialization::serializationLength(ros::Time());
   uint32_t msg_offset = 1;
   std::vector<uint8_t> payload_buffer;
+  if (ros2mqtt.stamped) {
+    // allocate buffer with appropriate size to hold [1, 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]
+    payload_buffer.resize(payload_length);
+    payload_buffer[0] = 0;
+  }
+
+  // serialize ROS message to payload [0/1, -, 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) {
 
-    // serialize current timestamp
+    // 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());
-    uint32_t stamp_length = ros::serialization::serializationLength(stamp);
-    std::vector<uint8_t> stamp_buffer;
-    stamp_buffer.resize(stamp_length);
-    ros::serialization::OStream stamp_stream(stamp_buffer.data(), stamp_length);
-    ros::serialization::serialize(stamp_stream, stamp);
 
-    // inject timestamp into payload
-    payload_length += stamp_length;
-    msg_offset += stamp_length;
-    payload_buffer.resize(payload_length);
-    payload_buffer[0] = 1;
-    payload_buffer.insert(payload_buffer.begin() + 1,
-                          std::make_move_iterator(stamp_buffer.begin()),
-                          std::make_move_iterator(stamp_buffer.end()));
-
-  } else {
-
-    payload_buffer.resize(payload_length);
-    payload_buffer[0] = 0;
+    // serialize timestamp to payload [1, S, R]
+    ros::serialization::OStream stamp_stream(&payload_buffer[1], stamp_length);
+    ros::serialization::serialize(stamp_stream, stamp);
   }
-  // add ROS message to payload
-  payload_buffer.insert(payload_buffer.begin() + msg_offset,
-                        std::make_move_iterator(msg_buffer.begin()),
-                        std::make_move_iterator(msg_buffer.end()));
 
   // send ROS message to MQTT broker
   mqtt_topic = ros2mqtt.mqtt.topic;
@@ -468,7 +463,8 @@ void MqttClient::ros2mqtt(const topic_tools::ShapeShifter::ConstPtr& ros_msg,
 }
 
 
-void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg) {
+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];
@@ -486,20 +482,18 @@ void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg) {
   // if stamped, compute latency
   if (stamped) {
 
-    // copy timestamp from MQTT message to buffer
-    ros::Time stamp;
-    uint32_t stamp_length = ros::serialization::serializationLength(stamp);
-    std::vector<uint8_t> stamp_buffer;
-    stamp_buffer.resize(stamp_length);
-    std::memcpy(stamp_buffer.data(), &(payload[1]), stamp_length);
+    // create ROS message buffer on top of MQTT message payload
+    char* non_const_payload = const_cast<char*>(&payload[1]);
+    uint8_t* stamp_buffer = reinterpret_cast<uint8_t*>(non_const_payload);
 
     // deserialize stamp
-    ros::serialization::IStream stamp_stream(stamp_buffer.data(), stamp_length);
+    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::WallTime now_wall = ros::WallTime::now();
-    ros::Time now(now_wall.sec, now_wall.nsec);
+    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 "
@@ -521,17 +515,16 @@ void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg) {
     msg_offset += stamp_length;
   }
 
-  // copy ROS message from MQTT message to buffer
-  std::vector<uint8_t> msg_buffer;
-  msg_buffer.resize(msg_length);
-  std::memcpy(msg_buffer.data(), &(payload[msg_offset]), msg_length);
+  // create ROS message buffer on top of MQTT message payload
+  char* non_const_payload = const_cast<char*>(&payload[msg_offset]);
+  uint8_t* msg_buffer = reinterpret_cast<uint8_t*>(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());
-  ros::serialization::OStream msg_stream(msg_buffer.data(), msg_length);
   mqtt2ros.ros.shape_shifter.read(msg_stream);
   mqtt2ros.ros.publisher.publish(mqtt2ros.ros.shape_shifter);
 }
@@ -581,6 +574,9 @@ bool MqttClient::isConnectedService(IsConnected::Request& request,
 
 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());
   auto& payload = mqtt_msg->get_payload_ref();
@@ -590,14 +586,13 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
     mqtt_topic.find(kRosMsgTypeMqttTopicPrefix) != std::string::npos;
   if (msg_contains_ros_msg_type) {
 
-    // copy ROS message type from MQTT message to buffer
-    RosMsgType ros_msg_type;
-    std::vector<uint8_t> msg_type_buffer;
-    msg_type_buffer.resize(payload_length);
-    std::memcpy(msg_type_buffer.data(), &(payload[0]), payload_length);
+    // create ROS message buffer on top of MQTT message payload
+    char* non_const_payload = const_cast<char*>(&payload[0]);
+    uint8_t* msg_type_buffer = reinterpret_cast<uint8_t*>(non_const_payload);
 
     // deserialize ROS message type
-    ros::serialization::IStream msg_type_stream(msg_type_buffer.data(),
+    RosMsgType ros_msg_type;
+    ros::serialization::IStream msg_type_stream(msg_type_buffer,
                                                 payload_length);
     ros::serialization::deserialize(msg_type_stream, ros_msg_type);
 
@@ -632,7 +627,7 @@ void MqttClient::message_arrived(mqtt::const_message_ptr mqtt_msg) {
 
     // publish ROS message, if publisher initialized
     if (!mqtt2ros_[mqtt_topic].ros.publisher.getTopic().empty()) {
-      mqtt2ros(mqtt_msg);
+      mqtt2ros(mqtt_msg, arrival_stamp);
     } else {
       NODELET_WARN(
         "ROS publisher for data from MQTT topic '%s' is not yet initialized: "