Avoid to call BufferedPort::prepare every time VectorsCollectionServe…

…r::VectorsCollectionServer is called

bindings/python/YarpUtilities/src/VectorsCollection.cpp

@@ -43,7 +43,8 @@ void CreateVectorsCollectionServer(pybind11::module& module)
                 const std::string& key,
                 Eigen::Ref<const Eigen::VectorXd> data) -> bool {
                  return impl.populateData(key, data);
-             });
+             })
+        .def("prepare_data", &VectorsCollectionServer::prepareData);
 }
 } // namespace YarpUtilities
 } // namespace bindings

src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h

@@ -41,6 +41,8 @@ namespace YarpUtilities
  * server.populateMetadata("key2", {"metadata4", "metadata5", "metadata6"});
  * server.finalizeMetadata();
  *
+ * // prepare the data
+ * server.prepareData();
  * server.clearData(); // optional
  * server.populateData("key1", {1.0, 2.0, 3.0});
  * server.populateData("key2", {4.0, 5.0, 6.0});
@@ -94,7 +96,8 @@ class VectorsCollectionServer : public VectorsCollectionMetadataService
      * @param key key of the data.
      * @param data data.
      * @return true if the data has been set successfully, false otherwise.
-     * @note this function should be called after the metadata has been finalized.
+     * @note this function should be called after the metadata has been finalized and after the
+     * prepareData function has been called.
      */
     bool populateData(const std::string& key, const iDynTree::Span<const double>& data);
 
@@ -111,6 +114,12 @@ class VectorsCollectionServer : public VectorsCollectionMetadataService
      */
     bool areMetadataReady() override;
 
+    /**
+     * Prepare the data.
+     * @note this function should be called before the data is populated.
+     */
+    void prepareData();
+
     /**
      * Send the data filled with populateData
      * @param forceStrict If this is true, wait until any previous sends are complete. If false, the
@@ -124,8 +133,9 @@ class VectorsCollectionServer : public VectorsCollectionMetadataService
      * to reuse it without reallocating memory, you may skip calling this function. Otherwise, use
      * VectorsCollection::clearData to free the memory allocated in the internal buffer.
      * @note Note that this function only clears the data and does not affect the metadata.
+     * @return true if the data has been cleared successfully, false otherwise.
      */
-    void clearData();
+    bool clearData();
 
 private:
     struct Impl;

src/YarpUtilities/src/VectorsCollectionServer.cpp

@@ -11,6 +11,8 @@
 #include <yarp/os/BufferedPort.h>
 #include <yarp/os/Port.h>
 
+#include <functional>
+#include <optional>
 #include <unordered_set>
 
 using namespace BipedalLocomotion::YarpUtilities;
@@ -25,8 +27,21 @@ struct VectorsCollectionServer::Impl
 
     std::atomic<bool> isMetadataFinalized{false}; /**< True if the metadata has been finalized. */
     std::unordered_set<std::string> setOfKeys; /**< Set of keys. */
+    std::optional<std::reference_wrapper<VectorsCollection>> collection; /**< Reference to the
+                                                                            collection. */
+
+    /**
+     * Check if the collection is valid.
+     * @return True if the collection is valid.
+     */
+    [[nodiscard]] bool isCollectionValid() const;
 };
 
+bool VectorsCollectionServer::Impl::isCollectionValid() const
+{
+    return collection.has_value();
+}
+
 VectorsCollectionServer::VectorsCollectionServer()
 {
     m_pimpl = std::make_unique<Impl>();
@@ -129,6 +144,11 @@ bool VectorsCollectionServer::finalizeMetadata()
     return true;
 }
 
+void VectorsCollectionServer::prepareData()
+{
+    m_pimpl->collection = m_pimpl->port.prepare();
+}
+
 bool VectorsCollectionServer::populateData(const std::string& key,
                                            const iDynTree::Span<const double>& data)
 {
@@ -148,9 +168,15 @@ bool VectorsCollectionServer::populateData(const std::string& key,
         return false;
     }
 
-    // prepare the data
-    BipedalLocomotion::YarpUtilities::VectorsCollection& collection = m_pimpl->port.prepare();
-    collection.vectors[key].assign(data.begin(), data.end());
+    if (!m_pimpl->isCollectionValid())
+    {
+        log()->error("{} The data collection is not valid. Please call prepareData before "
+                     "calling this function.",
+                     logPrefix);
+        return false;
+    }
+
+    m_pimpl->collection.value().get().vectors[key].assign(data.begin(), data.end());
 
     return true;
 }
@@ -160,9 +186,18 @@ void VectorsCollectionServer::sendData(bool forceStrict /*= false */)
     m_pimpl->port.write(forceStrict);
 }
 
-void VectorsCollectionServer::clearData()
+bool VectorsCollectionServer::clearData()
 {
-    m_pimpl->port.prepare().vectors.clear();
+    // check if the reference to the collection is valid
+    if (!m_pimpl->isCollectionValid())
+    {
+        log()->error("[VectorsCollectionServer::clearData] The reference to the collection is "
+                     "invalid. Please call prepareData before calling this function.");
+        return false;
+    }
+
+    m_pimpl->collection.value().get().vectors.clear();
+    return true;
 }
 
 bool VectorsCollectionServer::areMetadataReady()