Add connection management methods to VectorsCollectionClient and enhance YarpRobotLoggerDevice for signal reconnections

Author: GiulioRomualdi

bindings/python/YarpUtilities/src/VectorsCollection.cpp

@@ -69,6 +69,8 @@ void CreateVectorsCollectionClient(pybind11::module& module)
             py::arg("handler"))
         .def("connect", &VectorsCollectionClient::connect)
         .def("disconnect", &VectorsCollectionClient::disconnect)
+        .def("is_connected", &VectorsCollectionClient::isConnected)
+        .def("check_connection", &VectorsCollectionClient::checkConnection)
         .def("get_metadata",
              [](VectorsCollectionClient& impl)
                  -> BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata {

devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp

@@ -1088,6 +1088,29 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::record()
         m_lookForNewLogsThread = std::thread([this] { this->lookForNewLogs(); });
     }
 
+    // Refresh all exogenous signal connections so they are re-established cleanly
+    auto refreshExogenousConnections = [](auto& signals) {
+        for (auto& [name, signal] : signals)
+        {
+            signal.disconnect();
+            signal.connected = false;
+            signal.dataArrived = false;
+        }
+    };
+    refreshExogenousConnections(m_vectorsCollectionSignals);
+    refreshExogenousConnections(m_vectorSignals);
+    refreshExogenousConnections(m_stringSignals);
+    refreshExogenousConnections(m_humanStateSignals);
+    refreshExogenousConnections(m_wearableTargetsSignals);
+    refreshExogenousConnections(m_wearableDataSignals);
+    refreshExogenousConnections(m_imageSignals);
+
+    // Also clear cached metadata for VectorsCollection signals
+    for (auto& [name, signal] : m_vectorsCollectionSignals)
+    {
+        signal.metadata.vectors.clear();
+    }
+
     // run the thread for reading the exogenous signals
     m_lookForNewExogenousSignalThread = std::thread([this] { this->lookForExogenousSignals(); });
 
@@ -1625,6 +1648,29 @@ void YarpRobotLoggerDevice::lookForExogenousSignals()
         }
     };
 
+    auto checkExogeneousConnections
+        = [this](
+              std::unordered_map<std::string, VectorsCollectionSignal>& signals) -> void {
+        for (auto& [name, signal] : signals)
+        {
+            if (!signal.connected)
+            {
+                continue;
+            }
+
+            std::lock_guard<std::mutex> lock(signal.mutex);
+            if (!signal.client.checkConnection())
+            {
+                log()->warn("[YarpRobotLoggerDevice::lookForExogenousSignals] Connection lost "
+                            "for exogenous signal '{}'. Will attempt to reconnect.",
+                            name);
+                signal.connected = false;
+                signal.dataArrived = false;
+                signal.metadata.vectors.clear();
+            }
+        }
+    };
+
     while (m_lookForNewExogenousSignalIsRunning)
     {
         // detect if a clock has been reset
@@ -1647,7 +1693,8 @@ void YarpRobotLoggerDevice::lookForExogenousSignals()
         connectToExogeneous(m_wearableDataSignals);
         connectToExogeneous(m_imageSignals);
 
-        // TODO check for updated metadata from already connected signals
+        // check if already connected VectorsCollection signals are still alive
+        checkExogeneousConnections(m_vectorsCollectionSignals);
 
         // Start the logging for exogenous images
         for (auto& [name, signal] : m_imageSignals)

src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h

@@ -65,6 +65,23 @@ class VectorsCollectionClient
      */
     bool disconnect();
 
+    /**
+     * Check if the client is currently connected (cached state).
+     * @return true if the client believes it is connected, false otherwise.
+     * @note This returns the cached connection state. Use checkConnection() to actively verify
+     * YARP port connectivity.
+     */
+    bool isConnected() const;
+
+    /**
+     * Actively verify that the YARP port connections are still alive.
+     * If any connection (data or RPC) is found to be broken, the internal state is updated to
+     * disconnected.
+     * @return true if all connections are still alive, false otherwise.
+     * @note After this method returns false, call connect() to re-establish the connections.
+     */
+    bool checkConnection();
+
     /**
      * Check if new metadata is available.
      * @return true if new metadata is available, false otherwise.

src/YarpUtilities/src/VectorsCollectionClient.cpp

@@ -123,6 +123,37 @@ bool VectorsCollectionClient::disconnect()
     return true;
 }
 
+bool VectorsCollectionClient::isConnected() const
+{
+    return m_pimpl->isConnected;
+}
+
+bool VectorsCollectionClient::checkConnection()
+{
+    if (!m_pimpl->isConnected)
+    {
+        return false;
+    }
+
+    // Actively verify that both YARP connections (data and RPC) are still alive
+    const bool dataConnected
+        = yarp::os::Network::isConnected(m_pimpl->remotePortName, m_pimpl->localPortName);
+    const bool rpcConnected
+        = yarp::os::Network::isConnected(m_pimpl->localRpcPortName, m_pimpl->remoteRpcPortName);
+
+    if (!dataConnected || !rpcConnected)
+    {
+        log()->warn("[VectorsCollectionClient::checkConnection] Connection lost. Data port "
+                    "connected: {}, RPC port connected: {}.",
+                    dataConnected,
+                    rpcConnected);
+        m_pimpl->isConnected = false;
+        return false;
+    }
+
+    return true;
+}
+
 bool VectorsCollectionClient::connect()
 {
     constexpr auto rpcCarrier = "tcp";

src/YarpUtilities/tests/VectorsCollectionTest.cpp

@@ -406,13 +406,42 @@ TEST_CASE_METHOD(VectorsCollectionFixture, "VectorsCollectionClient - Connection
 
     REQUIRE(client.initialize(clientHandler));
 
+    SECTION("isConnected returns false before connect") {
+        // Test: Client should report disconnected state before connect
+        REQUIRE_FALSE(client.isConnected());
+    }
+
+    SECTION("checkConnection returns false before connect") {
+        // Test: checkConnection should return false when not connected
+        REQUIRE_FALSE(client.checkConnection());
+    }
+
     SECTION("Successful connection to available server") {
         // Test: Client should connect when server is available
         // Behavior: Establishes both data and RPC port connections
 
         // Allow server ports to fully open
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
+        REQUIRE(client.connect());
+        REQUIRE(client.isConnected());
+    }
+
+    SECTION("checkConnection returns true when connected") {
+        // Test: checkConnection should return true when both ports are alive
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        REQUIRE(client.connect());
+        REQUIRE(client.checkConnection());
+        REQUIRE(client.isConnected());
+    }
+
+    SECTION("Graceful connection failure when server unavailable") {
+        // Test: Client should connect when server is available
+        // Behavior: Establishes both data and RPC port connections
+
+        // Allow server ports to fully open
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
         REQUIRE(client.connect());
     }
 
@@ -859,3 +888,88 @@ TEST_CASE_METHOD(VectorsCollectionFixture,
     BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata meta;
     REQUIRE_FALSE(client.getMetadata(meta));
 }
+
+TEST_CASE_METHOD(VectorsCollectionFixture,
+                 "VectorsCollectionClient - checkConnection detects server shutdown")
+{
+    // Use a heap-allocated server so we can destroy it mid-test
+    auto server = std::make_unique<VectorsCollectionServer>();
+    REQUIRE(server->initialize(serverHandler));
+    server->populateMetadata("sig", {"a", "b"});
+    REQUIRE(server->finalizeMetadata());
+
+    VectorsCollectionClient client;
+    REQUIRE(client.initialize(clientHandler));
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    REQUIRE(client.connect());
+    REQUIRE(client.isConnected());
+    REQUIRE(client.checkConnection());
+
+    // Destroy the server, which closes its ports
+    server.reset();
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
+    // checkConnection should detect the broken connections
+    REQUIRE_FALSE(client.checkConnection());
+    REQUIRE_FALSE(client.isConnected());
+
+    // After detecting disconnection, connect should fail (no server)
+    REQUIRE_FALSE(client.connect());
+}
+
+TEST_CASE_METHOD(VectorsCollectionFixture,
+                 "VectorsCollectionClient - reconnect after server restart")
+{
+    // Start a server, connect, then destroy and recreate it
+    auto server = std::make_unique<VectorsCollectionServer>();
+    REQUIRE(server->initialize(serverHandler));
+    server->populateMetadata("sig", {"x"});
+    REQUIRE(server->finalizeMetadata());
+
+    VectorsCollectionClient client;
+    REQUIRE(client.initialize(clientHandler));
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    REQUIRE(client.connect());
+    REQUIRE(client.checkConnection());
+
+    // Send some data
+    server->prepareData();
+    REQUIRE(server->populateData("sig", std::vector<double>{1.0}));
+    server->sendData();
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+    auto* data = client.readData(false);
+    REQUIRE(data != nullptr);
+    REQUIRE(data->vectors.at("sig")[0] == Catch::Approx(1.0));
+
+    // Destroy the server
+    server.reset();
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+    REQUIRE_FALSE(client.checkConnection());
+
+    // Create a new server on the same ports
+    server = std::make_unique<VectorsCollectionServer>();
+    REQUIRE(server->initialize(serverHandler));
+    server->populateMetadata("sig", {"x"});
+    REQUIRE(server->finalizeMetadata());
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+    // Reconnect
+    REQUIRE(client.connect());
+    REQUIRE(client.isConnected());
+    REQUIRE(client.checkConnection());
+
+    // Verify data flows again
+    server->prepareData();
+    REQUIRE(server->populateData("sig", std::vector<double>{42.0}));
+    server->sendData();
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+    data = client.readData(false);
+    if (data != nullptr)
+    {
+        REQUIRE(data->vectors.at("sig")[0] == Catch::Approx(42.0));
+    }
+}