diff --git a/devices/wholeBodyDynamics/CMakeLists.txt b/devices/wholeBodyDynamics/CMakeLists.txt index b075062..2e506d5 100644 --- 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 index 65e361a..2460941 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -899,14 +899,10 @@ void WholeBodyDynamicsDevice::resizeBuffers() link < static_cast(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(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(netExternalWrenchesBottle, netExternalWrenchesPort); + broadcastData(netExternalWrenches, netExternalWrenchesPort); } } diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h index c3bcdbf..29f5dfc 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h @@ -29,6 +29,7 @@ #include #include +#include #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 netExternalWrenchesPort; + yarp::os::BufferedPort netExternalWrenchesPort; /** * Flag to publish the next external wrenches on each link. diff --git a/idl/CMakeLists.txt b/idl/CMakeLists.txt index d63e577..062f956 100644 --- 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 new file mode 100644 index 0000000..f3e63af --- /dev/null +++ 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 + "$" + "$") + +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 new file mode 100644 index 0000000..62bf4a1 --- /dev/null +++ b/idl/VectorsCollection/VectorsCollection.thrift @@ -0,0 +1,4 @@ +struct VectorsCollection +{ + 1: map> vectors; +}