Add support for rosauth and SSL

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
 

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'
 }
-```
+```

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

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},

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;