fix bug in ros1 latency deserialization

introduced as part of refactoring for ros2 version

Author: lreiher

mqtt_client/src/MqttClient.cpp

@@ -621,7 +621,7 @@ void MqttClient::mqtt2ros(mqtt::const_message_ptr mqtt_msg,
   if (mqtt2ros.stamped) {
 
     // create ROS message buffer on top of MQTT message payload
-    char* non_const_payload = const_cast<char*>(&payload[1]);
+    char* non_const_payload = const_cast<char*>(&payload[msg_offset]);
     uint8_t* stamp_buffer = reinterpret_cast<uint8_t*>(non_const_payload);
 
     // deserialize stamp