diff --git a/README.md b/README.md
index 25be159..e718376 100644
--- a/README.md
+++ b/README.md
@@ -12,11 +12,16 @@ The following compressions are supported :
 - **CBOR**
 - **CBOR-RAW**
 
+### Authentication
+
+The server will handle `auth` request messages as specified in the [Rosbridge protocol](https://github.com/RobotWebTools/rosbridge_suite/blob/ros1/ROSBRIDGE_PROTOCOL.md#33-authentication-message). An instance of the `rosauth` service node must be running to actually process the authentication requests.
+
+If the `require_auth` parameter is set to `true`, the server will not process any messages (including any subscription requests) until a successful authentication has been performed, and will close sockets that have not authenticated after a timeout of 15s.
+
 ### Features of the RobotWebTools' rosbridge that are not implemented
 
 - PNG compression
 - fragmented packets
-- authentification (`rosauth`)
 - service server
 - TCP and UDP transport (only Websocket)
 - BSON mode
@@ -64,13 +69,17 @@ Will be encoded as :
 - `rosbridge_cpp_msgs`
 - `std_msgs`
 
-## Optionnal ROS parameters
+## Optional ROS parameters
 
 - `port` (*int*) : Websocket TCP port. `0` will let the OS attribute a random port (default value: `9090`)
 - `service_timeout` (*double*) : Timeout, in seconds, for service requests (default value: `5.0`)
 - `pong_timeout_s` (*double*) : Timeout, in seconds, of the websocket connection for each client (default value: `30.0`)
 - `watchdog_enabled` (*bool*) : Enable the watchdog (default value: `true`)
 - `watchdog_timeout` (*double*) : Watchdog timeout in seonds (default value: `5.0`)
+- `require_auth` (*bool*) : Require authentication (default value: `false`)
+- `require_ssl` (*bool*) : Start up the Websocket Server in secure mode (requires `ssl_cert_file` and `ssl_key_file` to be given) (default value: `false`)
+- `ssl_cert_file` (*string*) : Path to the SSL certificate file (default value: `""`)
+- `ssl_key_file` (*string*) : Path to the SSL key file (default value: `""`)
 
 ## Parameters set by the node
 
diff --git a/README_fr.md b/README_fr.md
index a7cd25a..021c723 100644
--- a/README_fr.md
+++ b/README_fr.md
@@ -9,11 +9,16 @@ Il utilise Qt pour l'implémentation Websocket et donc dépend de `libqt5websock
 
 Les "compressions" **JSON**, **CBOR** et **CBOR-RAW** sont supportés mais pas la compression **PNG**.
 
+### Authentification
+
+Le serveur gérera les messages de requête auth comme spécifié dans le [protocole Rosbridge](https://github.com/RobotWebTools/rosbridge_suite/blob/ros1/ROSBRIDGE_PROTOCOL.md#33-authentication-message). Une instance du nœud de service rosauth doit être en cours d'exécution pour traiter effectivement les requêtes d'authentification.
+
+Si le paramètre require_auth est défini sur true, le serveur ne traitera aucun message (y compris les demandes d'abonnement) tant qu'une authentification réussie n'aura pas été effectuée, et il fermera les sockets qui ne se sont pas authentifiés après un délai d'expiration de 15 secondes.
+
 ### Pas implémenté par rapport à la version Python
 
 - compression PNG
 - paquets fragmentés
-- authentification (`rosauth`)
 - service server
 - Autres transports que Websocket (TCP, UDP)
 - Mode BSON only
@@ -67,6 +72,10 @@ Dans l'autre sens, à partir du JSON, les deux formats sont supportés. Tableau
 - `pong_timeout_s` (*double*) : Durée de timeout, en secondes, avant que l'on déconnecte le client (défaut = `30.0`)
 - `watchdog_enabled` (*bool*) : Activation du watchdog (défault = `true`)
 - `watchdog_timeout` (*double*) : Timeout du watchdog (défault = `5.0`)
+- `require_auth` (*bool*) : Requiert l'authentification (défaut = `false`)
+- `require_ssl` (*bool*) : Démarrer le serveur Websocket en mode sécurisé (requiert `ssl_cert_file` et `ssl_key_file` à être donnés) (défaut = `false`)
+- `ssl_cert_file` (*string*) : Chemin vers le fichier de certificat SSL (défaut = `""`)
+- `ssl_key_file` (*string*) : Chemin vers le fichier de clé SSL (défaut = `""`)
 
 ## Paramètres set par le nœud
 
@@ -95,4 +104,4 @@ dir('tmp')
     sh 'git clone --depth=1 https://github.com/ros/ros'
     sh 'cp -r ros/tools/rosunit ../workspace/src'
 }
-```
\ No newline at end of file
+```
diff --git a/p_rosbridge_server_cpp/launch/rosconsole.config b/p_rosbridge_server_cpp/launch/rosconsole.config
index 5aad5b2..835717c 100644
--- a/p_rosbridge_server_cpp/launch/rosconsole.config
+++ b/p_rosbridge_server_cpp/launch/rosconsole.config
@@ -1,4 +1,5 @@
 log4j.logger.ros.p_rosbridge_server_cpp=INFO
+log4j.logger.ros.p_rosbridge_server_cpp.security=INFO
 log4j.logger.ros.p_rosbridge_server_cpp.json=INFO
 log4j.logger.ros.p_rosbridge_server_cpp.topic=INFO
 log4j.logger.ros.p_rosbridge_server_cpp.websocket=INFO
diff --git a/p_rosbridge_server_cpp/src/ROSNode.cpp b/p_rosbridge_server_cpp/src/ROSNode.cpp
index 49ce7ee..2ba8359 100644
--- a/p_rosbridge_server_cpp/src/ROSNode.cpp
+++ b/p_rosbridge_server_cpp/src/ROSNode.cpp
@@ -6,10 +6,12 @@
 #include <stdexcept>
 
 #include <QElapsedTimer>
-#include <QMetaObject>
 #include <QWebSocket>
+#include <QFile>
+#include <QSslKey>
 
 #include <rosbridge_cpp_msgs/WebSocketConnectedClients.h>
+#include <rosauth/Authentication.h>
 #include <std_msgs/Int32.h>
 #include <string>
 
@@ -40,23 +42,66 @@ ROSNode::ROSNode(QObject* parent)
           {"call_service", [this](WSClient* c, const auto& j,
                                   const auto& id) { callServiceHandler(c, j, id); }},
           {"set_level", [this](WSClient* c, const auto& j, const auto& id) {
-               setLevelHandler(c, j, id);
-           }}}
+              setLevelHandler(c, j, id); }},
+          {"auth", [this](WSClient* c, const auto& j, const auto& id) {
+              handleAuthRequest(c, j, id); }}
+      }
 {
     // Parameters
     m_nhPrivate.getParam("port", m_wsPort);
     m_nhPrivate.getParam("service_timeout", m_serviceTimeout);
     m_nhPrivate.getParam("max_wsocket_buffer_size_mbytes", m_maxWebSocketBufferSize_MB);
     m_nhPrivate.getParam("pong_timeout_s", m_pongTimeout_s);
+    m_nhPrivate.getParam("require_authentication", m_requireAuth);
+    m_nhPrivate.getParam("require_ssl", m_requireSsl);
+    m_nhPrivate.getParam("ssl_cert_file", m_sslCertFile);
+    m_nhPrivate.getParam("ssl_key_file", m_sslKeyFile);
 
     m_clientsCountPub = m_nhNs.advertise<std_msgs::Int32>("client_count", 10, true);
     m_connectedClientsPub =
         m_nhNs.advertise<rosbridge_cpp_msgs::WebSocketConnectedClients>(
             "connected_clients", 10, true);
 
-    connect(&m_wsServer, &QWebSocketServer::newConnection, this,
+    if (m_requireAuth)
+    {
+        m_authServiceClient = m_nhPrivate.serviceClient<rosauth::AuthenticationRequest>("/authenticate");
+    }
+
+    m_wsServer = new QWebSocketServer(QStringLiteral("rosbridge_server"),
+        (m_requireSsl ? QWebSocketServer::SecureMode : QWebSocketServer::NonSecureMode));
+    connect(m_wsServer, &QWebSocketServer::newConnection, this,
             &ROSNode::onNewWSConnection);
-    connect(&m_wsServer, &QWebSocketServer::serverError, this, &ROSNode::onWSServerError);
+    connect(m_wsServer, &QWebSocketServer::serverError, this, &ROSNode::onWSServerError);
+
+    if (m_requireSsl)
+    {
+        ROS_DEBUG_STREAM_NAMED("security", "Setting up ssl with certFile " << m_sslCertFile << " and keyFile " << m_sslKeyFile);
+        try
+        {
+            QSslConfiguration sslConfiguration;
+            QFile certFile(QString::fromStdString(m_sslCertFile));
+            QFile keyFile(QString::fromStdString(m_sslKeyFile));
+            certFile.open(QIODevice::ReadOnly);
+            keyFile.open(QIODevice::ReadOnly);
+            const QSslCertificate certificate(&certFile, QSsl::Pem);
+            const QSslKey sslKey(&keyFile, QSsl::Rsa, QSsl::Pem);
+            certFile.close();
+            keyFile.close();
+            sslConfiguration.setPeerVerifyMode(QSslSocket::VerifyNone);
+            sslConfiguration.setLocalCertificate(certificate);
+            sslConfiguration.setPrivateKey(sslKey);
+            m_wsServer->setSslConfiguration(sslConfiguration);
+        }
+        catch (std::exception& e)
+        {
+            ROS_ERROR_STREAM_NAMED("security", "Failed to set up SSL configuration! " << e.what());
+            throw std::runtime_error("Failed to set up SSL configuration!");
+        }
+    }
+    else
+    {
+        ROS_DEBUG_STREAM_NAMED("security", "SSL not required");
+    }
 
     m_fish = std::make_shared<ros_babel_fish::BabelFish>();
 
@@ -71,15 +116,15 @@ ROSNode::ROSNode(QObject* parent)
 
 void ROSNode::start()
 {
-    if(!m_wsServer.listen(QHostAddress::Any, static_cast<uint16_t>(m_wsPort)))
+    if(!m_wsServer->listen(QHostAddress::Any, static_cast<uint16_t>(m_wsPort)))
     {
         ROS_FATAL_STREAM("Failed to start WS server on port "
-                         << m_wsPort << ": " << m_wsServer.errorString().toStdString());
+                         << m_wsPort << ": " << m_wsServer->errorString().toStdString());
         QCoreApplication::exit(1);
     }
 
-    ROS_INFO_STREAM("Start WS on port: " << m_wsServer.serverPort());
-    m_nhPrivate.setParam("actual_port", m_wsServer.serverPort());
+    ROS_INFO_STREAM("Start WS on port: " << m_wsServer->serverPort());
+    m_nhPrivate.setParam("actual_port", m_wsServer->serverPort());
 
     publishStats();
 }
@@ -282,7 +327,7 @@ void ROSNode::publish(WSClient* client, const rbp::PublishArgs& args)
     }
 }
 
-void ROSNode::udapteSubscriberClient(SubscriberClient& c, const rbp::SubscribeArgs& args)
+void ROSNode::updateSubscriberClient(SubscriberClient& c, const rbp::SubscribeArgs& args)
 {
     if(c.compression != args.compression)
     {
@@ -317,7 +362,7 @@ void ROSNode::subscribe(WSClient* client, const rbp::SubscribeArgs& args)
                [&client](const auto& clientSub) { return clientSub->client == client; });
            clientIt != it->second.clients.end())
         {
-            udapteSubscriberClient(*(*clientIt), args);
+            updateSubscriberClient(*(*clientIt), args);
             subClient = *clientIt;
         }
         else
@@ -382,7 +427,7 @@ void ROSNode::callService(WSClient* client, const rbp::CallServiceArgs& args)
     {
         ros_babel_fish::Message::Ptr req = m_fish->createServiceRequest(args.serviceType);
         auto& compound = req->as<ros_babel_fish::CompoundMessage>();
-        ros_nlohmann_converter::fillMessageFromJson(args.args, compound);
+        ros_nlohmann_converter::fillMessageFromJson(args.args, compound, ros::Time::now());
 
         // Allocated on heap, will be deleted by calling deleteLater itself later
         // Used to properly delete in the timeoutThread
@@ -495,6 +540,13 @@ void ROSNode::onWSMessage(const QString& message)
             return;
         }
 
+        const auto jsonOp = json.find("op")->get<std::string>();
+        if(m_requireAuth && (jsonOp != "auth") && (!client->isAuthenticated()))
+        {
+            ROS_DEBUG_STREAM_NAMED("security", "Ignoring message from un-authenticated client");
+            return;
+        }
+
         if(const auto opIt = json.find("op"); opIt != json.end())
         {
             const auto op = opIt->get<std::string>();
@@ -512,7 +564,7 @@ void ROSNode::onWSMessage(const QString& message)
             else
             {
                 std::ostringstream ss;
-                ss << "Received unkown OP '" << op << "' ignoring: '"
+                ss << "Received unknown OP '" << op << "' ignoring: '"
                    << message.toStdString() << "'";
                 sendStatus(client, rbp::StatusLevel::Error, ss.str());
                 return;
@@ -601,23 +653,23 @@ void ROSNode::onWSClientDisconnected()
 
 void ROSNode::onNewWSConnection()
 {
-    QWebSocket* socket = m_wsServer.nextPendingConnection();
+    QWebSocket* socket = m_wsServer->nextPendingConnection();
     ROS_INFO_STREAM("New client connected! "
                     << socket->peerAddress().toString().toStdString() << ":"
                     << socket->peerPort());
 
     const int64_t max_buffer_size_bytes = m_maxWebSocketBufferSize_MB * 1000 * 1000;
     auto client =
-        std::make_shared<WSClient>(socket, max_buffer_size_bytes, 1000, m_pongTimeout_s);
+        std::make_shared<WSClient>(socket, max_buffer_size_bytes, 1000, m_pongTimeout_s, m_requireAuth);
     client->connectSignals();
     connect(client.get(), &WSClient::onWSMessage, this, &ROSNode::onWSMessage);
     connect(client.get(), &WSClient::onWSBinaryMessage, this, &ROSNode::onWSMessage);
 
     // Disconnection is handled with a QueuedConnection to be delayed later.
     // In case of an abort on the WebSocket (buffer full), the WSClient will be removed
-    // will we might be iterating on the list of clients in handleROSMessage() and
+    // while we might be iterating on the list of clients in handleROSMessage() and
     // erasing an element of a vector invalidates the iterators
-    connect(client.get(), &WSClient::disconected, this, &ROSNode::onWSClientDisconnected,
+    connect(client.get(), &WSClient::disconnected, this, &ROSNode::onWSClientDisconnected,
             Qt::QueuedConnection);
 
     m_clients.push_back(client);
@@ -629,7 +681,7 @@ void ROSNode::onWSServerError(QWebSocketProtocol::CloseCode error) const
 {
     ROS_ERROR_STREAM("Websocket server error ("
                      << static_cast<int>(error)
-                     << "): " << m_wsServer.errorString().toStdString());
+                     << "): " << m_wsServer->errorString().toStdString());
 }
 
 void ROSNode::handleROSMessage(const std::string& topic,
@@ -1080,6 +1132,36 @@ void ROSNode::setLevelHandler(WSClient* client, const nlohmann::json& json,
     }
 }
 
+void ROSNode::handleAuthRequest(WSClient* client, const nlohmann::json& json, const std::string& id)
+{
+    try
+    {
+        ROS_DEBUG_STREAM_NAMED("security", "Received auth request message");
+        rosauth::Authentication auth;
+        auth.request.mac = json.at("mac");
+        auth.request.client = json.at("client");
+        auth.request.dest = json.at("dest");
+        auth.request.rand = json.at("rand");
+        auth.request.t = ros::Time(json.at("t"));
+        auth.request.level = json.at("level");
+        auth.request.end = ros::Time(json.at("end"));
+        m_authServiceClient.call(auth);
+        client->setAuthenticated(auth.response.authenticated);
+        if (auth.response.authenticated)
+        {
+            ROS_DEBUG_STREAM_NAMED("security", "Authentication succeeded");
+        } else
+        {
+            ROS_ERROR_STREAM_NAMED("security", "Authentication failed! Closing socket");
+            client->closeIfNotAuthenticated();
+        }
+    }
+    catch(const std::runtime_error& e)
+    {
+        sendStatus(client, rbp::StatusLevel::Error, "Error handling 'auth' msg "s + e.what());
+    }
+}
+
 SubscriberClient::SubscriberClient(WSClient* wsClient, const rbp::SubscribeArgs& args)
     : client{wsClient}, queueSize{args.queueSize}, throttleRate_ms{args.throttleRate},
       fragmentSize{args.fragmentSize}, compression{args.compression},
diff --git a/p_rosbridge_server_cpp/src/ROSNode.h b/p_rosbridge_server_cpp/src/ROSNode.h
index c783d92..7a385f9 100644
--- a/p_rosbridge_server_cpp/src/ROSNode.h
+++ b/p_rosbridge_server_cpp/src/ROSNode.h
@@ -12,6 +12,7 @@
 #include <QObject>
 #include <QString>
 #include <QWebSocketServer>
+#include <QPointer>
 
 #include "nlohmann/json.hpp"
 
@@ -109,6 +110,8 @@ private slots:
                             const std::string& id);
     void setLevelHandler(WSClient* client, const nlohmann::json& json,
                          const std::string& id);
+    void handleAuthRequest(WSClient *client, const nlohmann::json &json,
+                           const std::string &id);
 
     // rosbridge protocol
     void advertise(WSClient* client, const rosbridge_protocol::AdvertiseArgs& args);
@@ -139,7 +142,7 @@ private slots:
                            const ros::Time& receivedTime);
     void addNewSubscriberClient(WSClient* client,
                                 const rosbridge_protocol::SubscribeArgs& args);
-    void udapteSubscriberClient(SubscriberClient& c,
+    void updateSubscriberClient(SubscriberClient& c,
                                 const rosbridge_protocol::SubscribeArgs& args);
 
     void publishStats() const;
@@ -150,11 +153,11 @@ private slots:
     ros::Publisher m_clientsCountPub;
     ros::Publisher m_connectedClientsPub;
     ros::Timer m_pubStatsTimer;
+    ros::ServiceClient m_authServiceClient;
     std::map<std::string, ROSBridgePublisher> m_pubs;
     std::map<std::string, ROSBridgeSubscriber> m_subs;
     std::shared_ptr<ros_babel_fish::BabelFish> m_fish;
-    QWebSocketServer m_wsServer{QStringLiteral("rosbridge server"),
-                                QWebSocketServer::NonSecureMode};
+    QPointer<QWebSocketServer> m_wsServer;
     std::vector<std::shared_ptr<WSClient>> m_clients;
     rosbridge_protocol::StatusLevel m_currentStatusLevel =
         rosbridge_protocol::StatusLevel::Error;
@@ -170,6 +173,10 @@ private slots:
     double m_serviceTimeout = 5.0;
     int m_maxWebSocketBufferSize_MB = 1000;
     double m_pongTimeout_s = 30.;
+    bool m_requireAuth = false;
+    bool m_requireSsl = false;
+    std::string m_sslCertFile;
+    std::string m_sslKeyFile;
 
     // Diags
     diagnostic_updater::Updater m_diagnostics;
diff --git a/p_rosbridge_server_cpp/src/WSClient.cpp b/p_rosbridge_server_cpp/src/WSClient.cpp
index a176668..2896677 100644
--- a/p_rosbridge_server_cpp/src/WSClient.cpp
+++ b/p_rosbridge_server_cpp/src/WSClient.cpp
@@ -6,12 +6,14 @@
 
 #include "WSClient.h"
 
-WSClient::WSClient(QWebSocket* ws, int64_t max_socket_buffer_size_bytes,
-                   int transferRateUpdatePeriod_ms, double pong_timeout_seconds)
+WSClient::WSClient(QWebSocket* ws, const int64_t max_socket_buffer_size_bytes,
+                   const int transferRateUpdatePeriod_ms, const double pong_timeout_seconds,
+                   const bool requireAuth)
     : QObject(nullptr), m_ws{ws}, m_connectionTime{ros::Time::now()},
       m_maxSocketBufferSize{max_socket_buffer_size_bytes},
       m_transferRateUpdatePeriod_ms{transferRateUpdatePeriod_ms},
-      m_pongReceiveTimeout{pong_timeout_seconds}
+      m_pongReceiveTimeout{pong_timeout_seconds},
+      m_requireAuth{requireAuth}
 {
     m_transferRateTimer.setInterval(m_transferRateUpdatePeriod_ms);
     m_transferRateTimer.setSingleShot(false);
@@ -64,6 +66,8 @@ void WSClient::connectSignals()
 
     connect(&m_pingTimer, &QTimer::timeout, this, [this]() { onPingTimer(); });
     m_pingTimer.start(2000);
+
+    QTimer::singleShot(15000, this, &WSClient::closeIfNotAuthenticated);
 }
 
 void WSClient::onPingTimer()
@@ -142,13 +146,32 @@ std::string WSClient::errorMsg() const
     return m_errorMsg;
 }
 
+bool WSClient::isAuthenticated() const
+{
+    return m_authenticated;
+}
+
+void WSClient::setAuthenticated(const bool authenticated)
+{
+    m_authenticated = authenticated;
+}
+
+void WSClient::closeIfNotAuthenticated() const
+{
+    if(!isAuthenticated())
+    {
+        ROS_WARN_STREAM_NAMED("security", "Client " << ipAddress() << " failed to authenticate, closing socket");
+        m_ws->close(QWebSocketProtocol::CloseCodeAbnormalDisconnection, "Failed to authenticate");
+    }
+}
+
 void WSClient::onWSDisconnected()
 {
     ROS_DEBUG_STREAM_NAMED("websocket", "Client Disconnected");
     auto* client = qobject_cast<QWebSocket*>(sender());
     if((client != nullptr) && client == m_ws)
     {
-        emit disconected();
+        emit disconnected();
     }
 }
 
@@ -173,7 +196,7 @@ void WSClient::abortConnection()
                      << ipAddress() << ". Connection aborted");
     // Cannot call disconnect because it requires the current buffer data to be fully sent
     // before trying to gently terminate the TCP connection. It needs to be stopped
-    // immediatly to stop filling the buffer. This will trigger a disconnected signal.
+    // immediately to stop filling the buffer. This will trigger a disconnected signal.
     m_errorMsg = "Connection to client " + ipAddress() +
                  " has been aborted due to network performance issues.";
     m_ws->abort();
diff --git a/p_rosbridge_server_cpp/src/WSClient.h b/p_rosbridge_server_cpp/src/WSClient.h
index 6271f95..ceca709 100644
--- a/p_rosbridge_server_cpp/src/WSClient.h
+++ b/p_rosbridge_server_cpp/src/WSClient.h
@@ -18,8 +18,9 @@ class WSClient : public QObject
 {
     Q_OBJECT
 public:
-    explicit WSClient(QWebSocket* ws, int64_t max_socket_buffer_size_bytes,
-                      int transferRateUpdatePeriod_ms, double pong_timeout_seconds);
+    explicit WSClient(QWebSocket* ws, const int64_t max_socket_buffer_size_bytes,
+                      const int transferRateUpdatePeriod_ms, const double pong_timeout_seconds,
+                      const bool requireAuth);
     virtual ~WSClient();
     void connectSignals();
     void onPingTimer();
@@ -39,6 +40,12 @@ class WSClient : public QObject
     // Returns an error msg if something wrong happened or an empty string otherwise.
     std::string errorMsg() const;
 
+    // Auth flag to prevent processing messages from client until authentication happens
+    // don't need to filter ros -> client because no subscription messages from client will get processed until auth happens
+    bool isAuthenticated() const;
+    void setAuthenticated(const bool authenticated);
+    void closeIfNotAuthenticated() const;
+
     static constexpr int64_t DEFAULT_BUFFER_SIZE_1000MB = 1'000'000'000;
 
 public slots:
@@ -50,7 +57,7 @@ public slots:
 signals:
     void onWSMessage(const QString& msg);
     void onWSBinaryMessage(const QByteArray& msg);
-    void disconected();
+    void disconnected();
 
 private:
     void abortConnection();
@@ -58,6 +65,8 @@ public slots:
     QWebSocket* m_ws = nullptr;
     QTimer m_pingTimer;
     ros::Time m_connectionTime;
+    bool m_requireAuth = false;
+    bool m_authenticated = false;
     std::string m_name = "Not yet connected";
 
 #if QT_VERSION < QT_VERSION_CHECK(5, 12, 0)
@@ -74,7 +83,7 @@ public slots:
     int64_t m_socketBytesToWrite = 0;
 #endif
     // If the internal buffer goes over max, it means more data than a client is able to
-    // receive is being sent and the connection will aborted to avoid consumming too much
+    // receive is being sent and the connection will aborted to avoid consuming too much
     // RAM. This can be due to a disconnection / network freeze (TCP timeout will take a
     // long time to happen) or a client too slow to process the data or network congestion
     // (bandwidth too low / packet loss)
diff --git a/p_rosbridge_server_cpp/src/nlohmann_to_ros.cpp b/p_rosbridge_server_cpp/src/nlohmann_to_ros.cpp
index 259b2c9..b0a50ae 100644
--- a/p_rosbridge_server_cpp/src/nlohmann_to_ros.cpp
+++ b/p_rosbridge_server_cpp/src/nlohmann_to_ros.cpp
@@ -8,7 +8,8 @@ namespace ros_nlohmann_converter
 {
 
 void fillCompoundArray(const nlohmann::json& jsonArray,
-                       ros_babel_fish::CompoundArrayMessage& msgArray)
+                       ros_babel_fish::CompoundArrayMessage& msgArray,
+                       const ros::Time& rosTime)
 {
     if(msgArray.isFixedSize())
     {
@@ -19,20 +20,33 @@ void fillCompoundArray(const nlohmann::json& jsonArray,
         if(msgArray.isFixedSize())
         {
             fillMessageFromJson(jsonArray[i],
-                                msgArray[i].as<ros_babel_fish::CompoundMessage>());
+                                msgArray[i].as<ros_babel_fish::CompoundMessage>(),
+                                rosTime);
         }
         else
         {
             auto& newItem = msgArray.appendEmpty();
             fillMessageFromJson(jsonArray[i],
-                                newItem.as<ros_babel_fish::CompoundMessage>());
+                                newItem.as<ros_babel_fish::CompoundMessage>(),
+                                rosTime);
         }
     }
 }
 
 void fillMessageFromJson(const nlohmann::json& json,
-                         ros_babel_fish::CompoundMessage& message)
+                         ros_babel_fish::CompoundMessage& message,
+                         const ros::Time& rosTime)
 {
+    // From rosbridge protocol spec (3.4.3), if header is missing or stamp in header is
+    // missing, fill with current ROS time
+    if(message.containsKey("header"))
+    {
+        if(const auto it = json.find("header");
+           it == json.end() || !it->contains("stamp"))
+        {
+            compound["header"]["stamp"] = rosTime;
+        }
+    }
     for(const auto& m : json.items())
     {
         auto& val = message[m.key()];
@@ -91,14 +105,15 @@ void fillMessageFromJson(const nlohmann::json& json,
                 break;
             case ros_babel_fish::MessageTypes::Compound: {
                 fillCompoundArray(m.value(),
-                                  base.as<ros_babel_fish::CompoundArrayMessage>());
+                                base.as<ros_babel_fish::CompoundArrayMessage>(),
+                                rosTime);
                 break;
             }
             }
             break;
         }
         case ros_babel_fish::MessageTypes::Compound: {
-            fillMessageFromJson(m.value(), val.as<ros_babel_fish::CompoundMessage>());
+            fillMessageFromJson(m.value(), val.as<ros_babel_fish::CompoundMessage>(), rosTime);
             break;
         }
         case ros_babel_fish::MessageTypes::None: break;
@@ -161,19 +176,7 @@ ros_babel_fish::BabelFishMessage::Ptr createMsg(ros_babel_fish::BabelFish& fish,
 {
     ros_babel_fish::Message::Ptr message = fish.createMessage(type);
     auto& compound = message->as<ros_babel_fish::CompoundMessage>();
-    fillMessageFromJson(json, compound);
-
-    // From rosbridge protocol spec (3.4.3), if header is missing ou stamp in header is
-    // missing, fill with current ROS time
-    if(compound.containsKey("header"))
-    {
-        if(const auto it = json.find("header");
-           it == json.end() || !it->contains("stamp"))
-        {
-            compound["header"]["stamp"] = rosTime;
-        }
-    }
-
+    fillMessageFromJson(json, compound, rosTime);
     return fish.translateMessage(message);
 }
 
diff --git a/p_rosbridge_server_cpp/src/nlohmann_to_ros.h b/p_rosbridge_server_cpp/src/nlohmann_to_ros.h
index 2b51fc8..8804e24 100644
--- a/p_rosbridge_server_cpp/src/nlohmann_to_ros.h
+++ b/p_rosbridge_server_cpp/src/nlohmann_to_ros.h
@@ -15,10 +15,12 @@ namespace ros_nlohmann_converter
 {
 
 void fillMessageFromJson(const nlohmann::json& json,
-                         ros_babel_fish::CompoundMessage& message);
+                         ros_babel_fish::CompoundMessage& message,
+                         const ros::Time& rosTime);
 
 void fillCompoundArray(const nlohmann::json& jsonArray,
-                       ros_babel_fish::CompoundArrayMessage& msgArray);
+                       ros_babel_fish::CompoundArrayMessage& msgArray,
+                       const ros::Time& rosTime);
 
 template<typename T, typename U>
 void fillArrayLoop(const U& jsonArray, ros_babel_fish::ArrayMessage<T>& msgArray,
diff --git a/p_rosbridge_server_cpp/test/test_ros_json_converter.cpp b/p_rosbridge_server_cpp/test/test_ros_json_converter.cpp
index 6844f25..d56a97b 100644
--- a/p_rosbridge_server_cpp/test/test_ros_json_converter.cpp
+++ b/p_rosbridge_server_cpp/test/test_ros_json_converter.cpp
@@ -19,6 +19,7 @@
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/SetCameraInfo.h>
+#include <tf2_msgs/TFMessage.h>
 #include <std_msgs/Duration.h>
 #include <std_msgs/Float64.h>
 #include <std_msgs/Int16MultiArray.h>
@@ -318,7 +319,7 @@ class NlohmannJSONParser : public JSONParser<nlohmann::json>
         auto json = nlohmann::json::parse(jsonStr);
         ros_babel_fish::Message::Ptr req = fish.createServiceRequest(type);
         auto& compound = req->as<ros_babel_fish::CompoundMessage>();
-        ros_nlohmann_converter::fillMessageFromJson(json, compound);
+        ros_nlohmann_converter::fillMessageFromJson(json, compound, g_rosTime);
         return fish.translateMessage(req);
     }
 
@@ -511,7 +512,7 @@ TYPED_TEST(JSONTester, CanFillPointMsgFromPartialJson)
 
 TYPED_TEST(JSONTester, CanFillPointStampedMsgFromJsonWithHeaderMissing)
 {
-    // position.z is missing
+    // header is missing
     const auto jsonData = R"({"point":{"x":5.6,"y":6.7,"z":7.8}})";
 
     ros_babel_fish::BabelFishMessage::Ptr rosMsg;
@@ -531,7 +532,7 @@ TYPED_TEST(JSONTester, CanFillPointStampedMsgFromJsonWithHeaderMissing)
 
 TYPED_TEST(JSONTester, CanFillPointStampedMsgFromJsonWithStampMissing)
 {
-    // position.z is missing
+    // header.stamp is missing
     const auto jsonData =
         R"({"header":{"frame_id":"robot","seq":2},"point":{"x":5.6,"y":6.7,"z":7.8}})";
 
@@ -550,25 +551,30 @@ TYPED_TEST(JSONTester, CanFillPointStampedMsgFromJsonWithStampMissing)
     EXPECT_EQ(compound["point"]["z"].value<double>(), 7.8);
 }
 
-TYPED_TEST(JSONTester, CanFillPointStampedMsggFromJsonWithStampMissing)
+TYPED_TEST(JSONTester, CanFillTfMsgFromJsonWithStampMissing)
 {
-    // position.z is missing
+    // header.stamp is missing for tf message inside list
     const auto jsonData =
-        R"({"header":{"frame_id":"robot","seq":2},"point":{"x":5.6,"y":6.7,"z":7.8}})";
+        R"({"transforms":[{"header":{"seq":42,"frame_id":"datum_origin"},"child_frame_id":"REMOTE_ASSIST_PALLET","transform":{"translation":{"x":19,"y":25,"z":0},"rotation":{"w":0,"x":0,"y":0,"z":1}}}]})";
 
     ros_babel_fish::BabelFishMessage::Ptr rosMsg;
     ASSERT_NO_THROW(rosMsg = this->parser.createMsgFromJson(
-                        g_fish, "geometry_msgs/PointStamped", g_rosTime, jsonData));
+                        g_fish, "tf2_msgs/TFMessage", g_rosTime, jsonData));
     ros_babel_fish::TranslatedMessage::Ptr translated = g_fish.translateMessage(rosMsg);
-    auto& compound =
-        translated->translated_message->as<ros_babel_fish::CompoundMessage>();
-    EXPECT_EQ(compound["header"]["frame_id"].value<std::string>(), "robot");
-    EXPECT_EQ(compound["header"]["seq"].value<uint32_t>(), 2u);
-    EXPECT_EQ(compound["header"]["stamp"].value<ros::Time>().sec, 34325437u);
-    EXPECT_EQ(compound["header"]["stamp"].value<ros::Time>().nsec, 432427u);
-    EXPECT_EQ(compound["point"]["x"].value<double>(), 5.6);
-    EXPECT_EQ(compound["point"]["y"].value<double>(), 6.7);
-    EXPECT_EQ(compound["point"]["z"].value<double>(), 7.8);
+    auto& msg = translated->translated_message->as<ros_babel_fish::CompoundMessage>();
+    auto& tf = msg["transforms"].as<ros_babel_fish::CompoundArrayMessage>()[0];
+    EXPECT_EQ(tf["header"]["frame_id"].value<std::string>(), "datum_origin");
+    EXPECT_EQ(tf["header"]["seq"].value<uint32_t>(), 42u);
+    EXPECT_EQ(tf["header"]["stamp"].value<ros::Time>().sec, 34325437u);
+    EXPECT_EQ(tf["header"]["stamp"].value<ros::Time>().nsec, 432427u);
+    EXPECT_EQ(tf["child_frame_id"].value<std::string>(), "REMOTE_ASSIST_PALLET");
+    EXPECT_EQ(tf["transform"]["translation"]["x"].value<double>(), 19);
+    EXPECT_EQ(tf["transform"]["translation"]["y"].value<double>(), 25);
+    EXPECT_EQ(tf["transform"]["translation"]["z"].value<double>(), 0);
+    EXPECT_EQ(tf["transform"]["rotation"]["w"].value<double>(), 0);
+    EXPECT_EQ(tf["transform"]["rotation"]["x"].value<double>(), 0);
+    EXPECT_EQ(tf["transform"]["rotation"]["y"].value<double>(), 0);
+    EXPECT_EQ(tf["transform"]["rotation"]["z"].value<double>(), 1);
 }
 
 TYPED_TEST(JSONTester, CanFillNavSatFixFromJSON)