Merge pull request #155 from robotology/vectors_collection

Implement vectors commection and use it for streaming netExternalWrenches
robotology · Aug 30, 2022 · 30b1d95 · 30b1d95
2 parents 63785f1 + 0d767a4
commit 30b1d95
Show file tree

Hide file tree

Showing 6 changed files with 47 additions and 15 deletions.
diff --git a/devices/wholeBodyDynamics/CMakeLists.txt b/devices/wholeBodyDynamics/CMakeLists.txt
@@ -26,6 +26,7 @@ if(ENABLE_wholebodydynamics)
 
     target_link_libraries(wholeBodyDynamicsDevice   wholeBodyDynamicsSettings
                                                     wholeBodyDynamics_IDLServer
+                                                    VectorsCollection
                                                     ctrlLibRT
                                                     ${YARP_LIBRARIES}
                                                     skinDynLib

diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
@@ -899,14 +899,10 @@ void WholeBodyDynamicsDevice::resizeBuffers()
          link < static_cast<iDynTree::LinkIndex>(netExternalWrenchesExertedByTheEnviroment.getNrOfLinks());
          ++link)
     {
-        yarp::os::Bottle& wrenchBottle = netExternalWrenchesBottle.addList();
-        wrenchBottle.addString(estimator.model().getLinkName(link));
-        yarp::os::Bottle& wrenchValues = wrenchBottle.addList();
-
-        for (size_t i = 0; i < 6; ++i)
-        {
-            wrenchValues.addFloat64(0.0);
-        }
+        netExternalWrenches.vectors[estimator.model().getLinkName(link)].resize(6);
+        std::fill(netExternalWrenches.vectors[estimator.model().getLinkName(link)].begin(),
+                  netExternalWrenches.vectors[estimator.model().getLinkName(link)].end(),
+                  0.0);
     }
 }
 
@@ -2839,14 +2835,13 @@ void WholeBodyDynamicsDevice::publishExternalWrenches()
              link < static_cast<iDynTree::LinkIndex>(netExternalWrenchesExertedByTheEnviroment.getNrOfLinks());
              ++link)
         {
-            yarp::os::Bottle* wrenchPair = netExternalWrenchesBottle.get(link).asList();
-            yarp::os::Bottle* linkWrenchBottle = wrenchPair->get(1).asList(); //The first value is the name, the second the wrench
+            auto& wrench = netExternalWrenches.vectors[estimator.model().getLinkName(link)];
             for (size_t i = 0; i < 6; ++i)
             {
-                linkWrenchBottle->get(i) = yarp::os::Value(netExternalWrenchesExertedByTheEnviroment(link)(i));
+                wrench[i] = netExternalWrenchesExertedByTheEnviroment(link)(i);
             }
         }
-        broadcastData<yarp::os::Bottle>(netExternalWrenchesBottle, netExternalWrenchesPort);
+        broadcastData(netExternalWrenches, netExternalWrenchesPort);
     }
 
 }

diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
@@ -29,6 +29,7 @@
 
 #include <wholeBodyDynamicsSettings.h>
 #include <wholeBodyDynamics_IDLServer.h>
+#include <VectorsCollection.h>
 #include "SixAxisForceTorqueMeasureHelpers.h"
 #include "GravityCompensationHelpers.h"
 
@@ -271,7 +272,6 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     /**
      * Open-related methods
      */
-
     bool openSettingsPort();
     bool openRPCPort();
     bool openRemapperControlBoard(os::Searchable& config);
@@ -731,12 +731,12 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
      * in the link frame. The bottle is a list of pairs. The first element is
      * the link name, the second is the wrench.
      */
-    yarp::os::Bottle netExternalWrenchesBottle;
+    VectorsCollection netExternalWrenches;
 
     /**
      * Port for streaming the netWrenchesBottle;
      */
-    yarp::os::BufferedPort<yarp::os::Bottle> netExternalWrenchesPort;
+    yarp::os::BufferedPort<VectorsCollection> netExternalWrenchesPort;
 
     /**
      * Flag to publish the next external wrenches on each link.

diff --git a/idl/CMakeLists.txt b/idl/CMakeLists.txt
@@ -1,3 +1,4 @@
 add_subdirectory(wholeBodyDynamicsSettings)
 add_subdirectory(wholeBodyDynamics_IDLServer)
 add_subdirectory(floatingBaseEstimatorRPC)
+add_subdirectory(VectorsCollection)
diff --git a/idl/VectorsCollection/CMakeLists.txt b/idl/VectorsCollection/CMakeLists.txt
@@ -0,0 +1,31 @@
+# Copyright (C) 2016 Istituto Italiano di Tecnologia  iCub Facility
+# Authors: Silvio Traversaro
+# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+
+cmake_minimum_required(VERSION 3.5)
+
+project(VectorsCollection)
+
+yarp_idl_to_dir(INPUT_FILES VectorsCollection.thrift
+                    OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/autogenerated
+                    SOURCES_VAR WBDSETTINGS_SRC
+                    HEADERS_VAR WBDSETTINGS_HEADERS
+                    INCLUDE_DIRS_VAR WBDSETTINGS_INCLUDES)
+
+add_library(VectorsCollection STATIC ${WBDSETTINGS_SRC} ${WBDSETTINGS_HEADERS})
+target_include_directories(VectorsCollection PUBLIC ${WBDSETTINGS_INCLUDES})
+set_property(TARGET VectorsCollection PROPERTY POSITION_INDEPENDENT_CODE ON)
+target_link_libraries(VectorsCollection ${YARP_LIBRARIES})
+
+target_include_directories(VectorsCollection PUBLIC
+                            "$<BUILD_INTERFACE:${WBDSETTINGS_INCLUDES}>"
+                            "$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}/wholeBodyDynamics>")
+
+set_property(TARGET VectorsCollection PROPERTY PUBLIC_HEADER ${WBDSETTINGS_HEADERS})
+
+install(TARGETS VectorsCollection
+        RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin
+        LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib
+        ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib
+        PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wholeBodyDynamics)
+
diff --git a/idl/VectorsCollection/VectorsCollection.thrift b/idl/VectorsCollection/VectorsCollection.thrift
@@ -0,0 +1,4 @@
+struct VectorsCollection
+{
+    1: map<string, list<double>> vectors;
+}