Avoid to call BufferedPort::prepare every time VectorsCollectionServe…

…r::populateData is called (#790)

CHANGELOG.md

@@ -6,6 +6,7 @@ All notable changes to this project are documented in this file.
 
 ### Changed
 - 🤖 [`ergoCubSN001`] Update `YarpRobotLoggerDevice` configuration file to exclude the head-imu and include the arms FTs  (https://github.com/ami-iit/bipedal-locomotion-framework/pull/798)
+- Avoid to call `BufferedPort::prepare` every time `VectorsCollectionServer::populateData` is called (https://github.com/ami-iit/bipedal-locomotion-framework/pull/790)
 
 ### Fixed
 

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

devices/YarpRobotLoggerDevice/README.md

@@ -80,6 +80,7 @@ m_vectorsCollectionServer.finalizeMetadata(); // this should be called only once
 In the main loop, add the following code to prepare and populate the data:
 
 ```c++
+m_vectorsCollectionServer.prepareData(); // required to prepare the data to be sent
 m_vectorsCollectionServer.clearData(); // optional see the documentation
 
 // DCM
@@ -118,6 +119,7 @@ self.vectors_collection_server.finalize_metadata() # this should be called only
 ```
 In the main loop, add the following code to prepare and populate the data:
 ```python
+self.vectors_collection_server.prepare_data() # required to prepare the data to be sent
 self.vectors_collection_server.clear_data() # optional see the documentation
 self.vectors_collection_server.populate_data("dcm::position::measured", <signal>)
 self.vectors_collection_server.populate_data("dcm::position::desired", <signal>)

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()

utilities/balancing-position-control/script/blf-balancing-position-control.py

@@ -487,6 +487,11 @@ def main():
         com_from_desired = kindyn.getCenterOfMassPosition().toNumPy()
         com_from_measured = kindyn_with_measured.getCenterOfMassPosition().toNumPy()
 
+        if not vectors_collection_server.prepare_data():
+            raise RuntimeError("Unable to prepare the data")
+
+        vectors_collection_server.clear_data()
+
         vectors_collection_server.populate_data("zmp::desired_planner", desired_zmp)
         vectors_collection_server.populate_data(
             "zmp::measured::global::with_joint_desired", global_zmp

utilities/joints-grid-position-tracking/blf-joints-grid-position-tracking.py

@@ -203,6 +203,11 @@ def main():
         ):
             raise RuntimeError("Unable to set the references")
 
+        if not vectors_collection_server.prepare_data():
+            raise RuntimeError("Unable to prepare the data")
+
+        vectors_collection_server.clear_data()
+
         vectors_collection_server.populate_data(
             "joints::desired::position", joint_pos_des
         )