diff --git a/conf/xml/Human.xml b/conf/xml/Human.xml index 5e7983f65..29b5e3cdd 100644 --- a/conf/xml/Human.xml +++ b/conf/xml/Human.xml @@ -182,15 +182,15 @@ - - /HDE/HumanWrenchWrapper/wrench:o - 10 + + 0.1 + /HDE/HumanWrenchWrapper/wrench:o - HumanWrenchProvider + HumanWrenchProvider - + diff --git a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp index 76f29f7a9..57b62835a 100644 --- a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp +++ b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp @@ -26,7 +26,6 @@ #include #include -#include #include #include @@ -761,7 +760,6 @@ class HumanDynamicsEstimator::Impl // Attached interfaces hde::interfaces::IHumanState* iHumanState = nullptr; hde::interfaces::IHumanWrench* iHumanWrench = nullptr; - yarp::dev::IAnalogSensor* iAnalogSensor = nullptr; mutable std::mutex mutex; iDynTree::Vector3 gravity; @@ -1258,18 +1256,6 @@ bool HumanDynamicsEstimator::Impl::attach(yarp::dev::PolyDriver* poly) yInfo() << LogPrefix << "Device" << deviceName << "attached successfully as IHumanWrench"; iHumanWrench = tmpIHumanWrench; - // Try to attach IAnalogSensor - if(!poly->view(iAnalogSensor)){ - yError() << LogPrefix << "Device" << deviceName << "must implement also the IAnalog interface!"; - return false; - } - - // Check the interface - if (iAnalogSensor->getChannels() != 6 * numberOfWrenchSources) { - yError() << LogPrefix << "The IAnalogSensor interface might not be ready"; - return false; - } - } if(!tmpIHumanState && !tmpIHumanWrench){ @@ -1325,7 +1311,6 @@ bool HumanDynamicsEstimator::detachAll() pImpl->iHumanState = nullptr; pImpl->iHumanWrench = nullptr; - pImpl->iAnalogSensor = nullptr; return true; } diff --git a/modules/HumanStateVisualizer/CMakeLists.txt b/modules/HumanStateVisualizer/CMakeLists.txt index 2b93fcb6a..c7e084952 100644 --- a/modules/HumanStateVisualizer/CMakeLists.txt +++ b/modules/HumanStateVisualizer/CMakeLists.txt @@ -39,6 +39,7 @@ target_link_libraries(${EXE_TARGET_NAME} LINK_PUBLIC iDynTree::idyntree-modelio iDynTree::idyntree-visualization IHumanState - IWearableTargets) + IWearableTargets + IHumanWrench) install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) \ No newline at end of file diff --git a/modules/HumanStateVisualizer/src/main.cpp b/modules/HumanStateVisualizer/src/main.cpp index 91dfe5a70..a35f23866 100644 --- a/modules/HumanStateVisualizer/src/main.cpp +++ b/modules/HumanStateVisualizer/src/main.cpp @@ -2,6 +2,7 @@ // SPDX-License-Identifier: BSD-3-Clause #include +#include #include #include @@ -379,19 +380,19 @@ int main(int argc, char* argv[]) // initialize iHumanState interface from remapper - yarp::dev::PolyDriver remapperDevice; + yarp::dev::PolyDriver humanStateRemapperDevice; hde::interfaces::IHumanState* iHumanState{nullptr}; yarp::os::Property remapperOptions; remapperOptions.put("device", "human_state_remapper"); remapperOptions.put("humanStateDataPort", humanStateDataPortName); - if(!remapperDevice.open(remapperOptions)) + if(!humanStateRemapperDevice.open(remapperOptions)) { yError() << LogPrefix << "Failed to connect remapper device"; return EXIT_FAILURE; } - if(!remapperDevice.view(iHumanState) || !iHumanState ) + if(!humanStateRemapperDevice.view(iHumanState) || !iHumanState ) { yError() << LogPrefix << "Failed to view iHumanState interface"; return EXIT_FAILURE; @@ -485,32 +486,37 @@ int main(int argc, char* argv[]) } } - // initialize wrench port - yarp::os::BufferedPort wrenchPort; - yarp::sig::Vector* wrenchMeasuresVector; + // initialize iHumanWrench + yarp::dev::PolyDriver humanWrenchRemapperDevice; + hde::interfaces::IHumanWrench* iHumanWrench{nullptr}; if (visualizeWrenches) { - wrenchPort.open("/HumanStateVisualizer" + humanWrenchWrapperPortName); - if (wrenchPort.isClosed()) + yarp::os::Property humanWrenchRemapperOptions; + humanWrenchRemapperOptions.put("device", "human_wrench_remapper"); + humanWrenchRemapperOptions.put("humanWrenchDataPort", humanWrenchWrapperPortName); + + if(!humanWrenchRemapperDevice.open(humanWrenchRemapperOptions)) { - yError() << LogPrefix << "failed to open the port /HumanStateVisualizer" << humanWrenchWrapperPortName; + yError() << LogPrefix << "Failed to connect remapper device"; return EXIT_FAILURE; } - if (!yarp.connect(humanWrenchWrapperPortName, wrenchPort.getName())) + if(!humanWrenchRemapperDevice.view(iHumanWrench) || !iHumanWrench ) { - yError() << LogPrefix << "failed to connect to the port" << humanWrenchWrapperPortName; + yError() << LogPrefix << "Failed to view iHumanWrench interface"; return EXIT_FAILURE; } - wrenchMeasuresVector = wrenchPort.read(true); - if (wrenchMeasuresVector == nullptr) + + // wait for the iHumanWrench to be initialized + while (iHumanWrench->getWrenchSourceNames().empty()) { - yError() << LogPrefix << "no data coming from the port " << humanWrenchWrapperPortName; - return EXIT_FAILURE; + std::this_thread::sleep_for(std::chrono::seconds(1)); + yInfo() << LogPrefix << "Waiting for data from HumanWrenchRemapper"; } - if(wrenchMeasuresVector->size() != (numberOfWrenchElements) ) + + if(iHumanWrench->getNumberOfWrenchSources() != wrenchSourceLinks.size() ) { - yError() << LogPrefix << "expected " << numberOfWrenchElements << " elements in port " << humanWrenchWrapperPortName - << ", received " << wrenchMeasuresVector->size(); + yError() << LogPrefix << "expected " << wrenchSourceLinks.size() << " wrench sources in port " << humanWrenchWrapperPortName + << ", received " << iHumanWrench->getNumberOfWrenchSources(); return EXIT_FAILURE; } } @@ -556,7 +562,7 @@ int main(int argc, char* argv[]) linkTransform = viz.modelViz("human").getWorldLinkTransform(wrenchSourceLinkIndices.at(vectorIndex)); for (size_t i = 0; i < 3; i++) { - force.setVal(i, forceScalingFactor * wrenchMeasuresVector->data()[6 * vectorIndex + i]); + force.setVal(i, forceScalingFactor * iHumanWrench->getWrenches()[6 * vectorIndex + i]); } force = linkTransform.getRotation() * force; viz.vectors().addVector(linkTransform.getPosition(), force); @@ -649,13 +655,12 @@ int main(int argc, char* argv[]) size_t vectorsIterator = 0; if (visualizeWrenches) { - wrenchMeasuresVector = wrenchPort.read(true); for (size_t vectorIndex = 0; vectorIndex < wrenchSourceLinks.size(); vectorIndex++) { linkTransform = viz.modelViz("human").getWorldLinkTransform(wrenchSourceLinkIndices.at(vectorIndex)); for (size_t i = 0; i < 3; i++) { - force.setVal(i, forceScalingFactor * wrenchMeasuresVector->data()[6 * vectorIndex + i]); + force.setVal(i, forceScalingFactor * iHumanWrench->getWrenches()[6 * vectorIndex + i]); } force = linkTransform.getRotation() * force; viz.vectors().updateVector(vectorIndex, linkTransform.getPosition(), force); @@ -735,9 +740,9 @@ int main(int argc, char* argv[]) } viz.close(); - remapperDevice.close(); + humanStateRemapperDevice.close(); wearableTargetsRemapperDevice.close(); - wrenchPort.close(); + humanWrenchRemapperDevice.close(); return 0; } diff --git a/msgs/yarp/thrift/CMakeLists.txt b/msgs/yarp/thrift/CMakeLists.txt index f10a5bc24..0b4630507 100644 --- a/msgs/yarp/thrift/CMakeLists.txt +++ b/msgs/yarp/thrift/CMakeLists.txt @@ -100,3 +100,35 @@ install(TARGETS ${LIBRARY_TARGET_NAME} RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/hde/msgs") + +set(LIBRARY_TARGET_NAME HumanWrenchMsg) + +yarp_idl_to_dir(hde/msgs/HumanWrench.thrift + OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/autogenerated + SOURCES_VAR ${LIBRARY_TARGET_NAME}_GEN_SRC + HEADERS_VAR ${LIBRARY_TARGET_NAME}_GEN_HDR + INCLUDE_DIRS_VAR ${LIBRARY_TARGET_NAME}_INCLUDE_DIRS + CMAKE_SCRIPTS_VAR ${LIBRARY_TARGET_NAME}_CMAKE_SCRIPTS) +foreach(_script ${${LIBRARY_TARGET_NAME}_CMAKE_SCRIPTS}) + include("${_script}") +endforeach() + +add_library(${LIBRARY_TARGET_NAME} ${${LIBRARY_TARGET_NAME}_GEN_SRC} ${${LIBRARY_TARGET_NAME}_GEN_HDR}) +target_link_libraries(${LIBRARY_TARGET_NAME} YARP::YARP_OS YARP::YARP_init) + +set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES + PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_GEN_HDR}") + +target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC + "$" + "$") + +set_property(TARGET ${LIBRARY_TARGET_NAME} PROPERTY PUBLIC_HEADER ${${LIBRARY_TARGET_NAME}_GEN_HDR}) + +add_library(${PROJECT_NAME}::${LIBRARY_TARGET_NAME} ALIAS ${LIBRARY_TARGET_NAME}) + +install(TARGETS ${LIBRARY_TARGET_NAME} + EXPORT ${PROJECT_NAME} + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" + LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" + PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/hde/msgs") diff --git a/msgs/yarp/thrift/hde/msgs/HumanWrench.thrift b/msgs/yarp/thrift/hde/msgs/HumanWrench.thrift new file mode 100644 index 000000000..35cd7d4e3 --- /dev/null +++ b/msgs/yarp/thrift/hde/msgs/HumanWrench.thrift @@ -0,0 +1,9 @@ +namespace yarp hde.msgs + +/** + * Representation of the IHumanWrench interface + */ +struct HumanWrench { + 1: list wrenchSourceNames; + 2: list wrenches; +} diff --git a/remappers/CMakeLists.txt b/remappers/CMakeLists.txt index 6c0fa0798..ebb845780 100644 --- a/remappers/CMakeLists.txt +++ b/remappers/CMakeLists.txt @@ -3,4 +3,5 @@ add_subdirectory(HumanStateRemapper) add_subdirectory(HumanDynamicsRemapper) +add_subdirectory(HumanWrenchRemapper) add_subdirectory(WearableTargetsRemapper) diff --git a/remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp b/remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp index 7d188ef02..d2da39b7e 100644 --- a/remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp +++ b/remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp @@ -33,9 +33,9 @@ class HumanDynamicsRemapper::impl std::vector jointTorques; }; -// ============== -// IWEAR REMAPPER -// ============== +// ======================= +// IHUMANDYNAMICS REMAPPER +// ======================= HumanDynamicsRemapper::HumanDynamicsRemapper() : PeriodicThread(1) diff --git a/remappers/HumanStateRemapper/HumanStateRemapper.cpp b/remappers/HumanStateRemapper/HumanStateRemapper.cpp index 9a5cc3c29..0ff9a3909 100644 --- a/remappers/HumanStateRemapper/HumanStateRemapper.cpp +++ b/remappers/HumanStateRemapper/HumanStateRemapper.cpp @@ -43,9 +43,9 @@ class HumanStateRemapper::impl std::array CoMVelocity; }; -// ============== -// IWEAR REMAPPER -// ============== +// ==================== +// IHUMANSTATE REMAPPER +// ==================== HumanStateRemapper::HumanStateRemapper() : PeriodicThread(1) diff --git a/remappers/HumanWrenchRemapper/CMakeLists.txt b/remappers/HumanWrenchRemapper/CMakeLists.txt new file mode 100644 index 000000000..3a02fc0c3 --- /dev/null +++ b/remappers/HumanWrenchRemapper/CMakeLists.txt @@ -0,0 +1,31 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +yarp_prepare_plugin(human_wrench_remapper + TYPE hde::devices::HumanWrenchRemapper + INCLUDE HumanWrenchRemapper.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(HumanWrenchRemapper + HumanWrenchRemapper.cpp + HumanWrenchRemapper.h) + +target_include_directories(HumanWrenchRemapper PUBLIC + $) + +target_link_libraries(HumanWrenchRemapper PUBLIC + IHumanWrench + HumanWrenchMsg + YARP::YARP_OS + YARP::YARP_dev + YARP::YARP_init) + +yarp_install( + TARGETS HumanWrenchRemapper + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) + diff --git a/remappers/HumanWrenchRemapper/HumanWrenchRemapper.cpp b/remappers/HumanWrenchRemapper/HumanWrenchRemapper.cpp new file mode 100644 index 000000000..cab6296f0 --- /dev/null +++ b/remappers/HumanWrenchRemapper/HumanWrenchRemapper.cpp @@ -0,0 +1,153 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "HumanWrenchRemapper.h" + +#include + +#include +#include + +#include +#include + +const std::string RemapperName = "HumanWrenchRemapper"; +const std::string LogPrefix = RemapperName + " :"; + +using namespace hde::devices; + +// ============== +// IMPL AND UTILS +// ============== + +class HumanWrenchRemapper::impl +{ +public: + std::mutex mtx; + yarp::os::Network network; + yarp::os::BufferedPort inputPort; + bool terminationCall = false; + + // Buffer HumanWrench variables + std::vector wrenchSourceNames; + std::vector wrenches; +}; + +// ==================== +// HUMANWRENCH REMAPPER +// ==================== + +HumanWrenchRemapper::HumanWrenchRemapper() + : PeriodicThread(1) + , pImpl{new impl()} +{} + +HumanWrenchRemapper::~HumanWrenchRemapper() = default; + +// parsing the configuration file and connect ports +bool HumanWrenchRemapper::open(yarp::os::Searchable& config) +{ + // =============================== + // CHECK THE CONFIGURATION OPTIONS + // =============================== + + // Data ports + // TODO: where to check this port? + if (!(config.check("humanWrenchDataPort") && config.find("humanWrenchDataPort").isString())) { + yError() << LogPrefix << "humanWrenchData option does not exist or it is not a list"; + return false; + } + + // =============================== + // PARSE THE CONFIGURATION OPTIONS + // =============================== + + std::string humanWrenchDataPortName = config.find("humanWrenchDataPort").asString(); + + // Initialize the network + // TODO: is this required in every DeviceDriver? + pImpl->network = yarp::os::Network(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + yError() << LogPrefix << "YARP server wasn't found active."; + return false; + } + + // ========================== + // CONFIGURE INPUT DATA PORTS + // ========================== + yDebug() << LogPrefix << "Configuring input data ports"; + + pImpl->inputPort.useCallback(*this); + if (!pImpl->inputPort.open("...")) { + yError() << LogPrefix << "Failed to open port" << humanWrenchDataPortName; + return false; + } + + // ================ + // OPEN INPUT PORTS + // ================ + yDebug() << LogPrefix << "Opening input ports"; + + + if (!yarp::os::Network::connect(humanWrenchDataPortName, + pImpl->inputPort.getName())) { + yError() << LogPrefix << "Failed to connect " << humanWrenchDataPortName + << " with " << pImpl->inputPort.getName(); + return false; + } + + // We use callbacks on the input ports, the loop is a no-op + start(); + + yDebug() << LogPrefix << "Opened correctly"; + return true; +} + +void HumanWrenchRemapper::threadRelease() +{} + +bool HumanWrenchRemapper::close() +{ + pImpl->terminationCall = true; + + while(isRunning()) { + stop(); + } + + return true; +} + +void HumanWrenchRemapper::run() +{ + return; +} + +// data are read from the port and saved in buffer variables +void HumanWrenchRemapper::onRead(hde::msgs::HumanWrench& humanWrenchData) +{ + std::lock_guard lock(pImpl->mtx); + if(!pImpl->terminationCall) { + pImpl->wrenchSourceNames = humanWrenchData.wrenchSourceNames; + + pImpl->wrenches = humanWrenchData.wrenches; + } +} + +// method of IHumanWrench interface expose the buffer variables data +std::vector HumanWrenchRemapper::getWrenchSourceNames() const +{ + std::lock_guard lock(pImpl->mtx); + return pImpl->wrenchSourceNames; +} + +size_t HumanWrenchRemapper::getNumberOfWrenchSources() const +{ + std::lock_guard lock(pImpl->mtx); + return pImpl->wrenchSourceNames.size(); +} + +std::vector HumanWrenchRemapper::getWrenches() const +{ + std::lock_guard lock(pImpl->mtx); + return pImpl->wrenches; +} diff --git a/remappers/HumanWrenchRemapper/HumanWrenchRemapper.h b/remappers/HumanWrenchRemapper/HumanWrenchRemapper.h new file mode 100644 index 000000000..800db9722 --- /dev/null +++ b/remappers/HumanWrenchRemapper/HumanWrenchRemapper.h @@ -0,0 +1,62 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef HDE_DEVICES_HUMANWRENCHREMAPPER +#define HDE_DEVICES_HUMANWRENCHREMAPPER + +#include + +#include +#include +#include +#include + +#include + +namespace hde::msgs { + class HumanWrench; +} // namespace hde::msgs +namespace hde::devices { + class HumanWrenchRemapper; +} // namespace hde::devices + +class hde::devices::HumanWrenchRemapper final + : public yarp::dev::DeviceDriver + // inherite from the interface to be exposed + , public hde::interfaces::IHumanWrench + // implement the callback to read the thrifted message + , public yarp::os::TypedReaderCallback + // implement the periodic thread + , public yarp::os::PeriodicThread +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + HumanWrenchRemapper(); + ~HumanWrenchRemapper() override; + + + + // DeviceDriver interface + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // PeriodicThread + void run() override; + void threadRelease() override; + + // TypedReaderCallback + void onRead(hde::msgs::HumanWrench& humanWrench) override; + + // IHumanWrench interface + std::vector getWrenchSourceNames() const override; + + size_t getNumberOfWrenchSources() const override; + + std::vector getWrenches() const override; +}; + +#endif // HDE_DEVICES_HUMANWRENCHREMAPPER + diff --git a/remappers/HumanWrenchRemapper/conf/HumanWrenchRemapperExample.xml b/remappers/HumanWrenchRemapper/conf/HumanWrenchRemapperExample.xml new file mode 100644 index 000000000..06276defc --- /dev/null +++ b/remappers/HumanWrenchRemapper/conf/HumanWrenchRemapperExample.xml @@ -0,0 +1,12 @@ + + + + + + + /HDE/HumanWrenchWrapper/wrench:o + + + + + diff --git a/remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp b/remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp index 400043430..3e0f19e26 100644 --- a/remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp +++ b/remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp @@ -69,9 +69,9 @@ class WearableTargetsRemapper::impl std::unordered_map> wearableTargets; }; -// ============== -// IWEAR REMAPPER -// ============== +// ======================== +// WEARABLETARGETS REMAPPER +// ======================== WearableTargetsRemapper::WearableTargetsRemapper() : PeriodicThread(1) diff --git a/wrappers/CMakeLists.txt b/wrappers/CMakeLists.txt index 4cd3444b7..aacaaff32 100644 --- a/wrappers/CMakeLists.txt +++ b/wrappers/CMakeLists.txt @@ -3,4 +3,5 @@ add_subdirectory(HumanStateWrapper) add_subdirectory(HumanDynamicsWrapper) +add_subdirectory(HumanWrenchWrapper) add_subdirectory(WearableTargetsWrapper) diff --git a/wrappers/HumanWrenchWrapper/CMakeLists.txt b/wrappers/HumanWrenchWrapper/CMakeLists.txt new file mode 100644 index 000000000..aab79168f --- /dev/null +++ b/wrappers/HumanWrenchWrapper/CMakeLists.txt @@ -0,0 +1,30 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +yarp_prepare_plugin(human_wrench_wrapper + TYPE hde::wrappers::HumanWrenchWrapper + INCLUDE HumanWrenchWrapper.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(HumanWrenchWrapper + HumanWrenchWrapper.cpp + HumanWrenchWrapper.h) + +target_include_directories(HumanWrenchWrapper PUBLIC + $) + +target_link_libraries(HumanWrenchWrapper PUBLIC + IHumanWrench + HumanWrenchMsg + YARP::YARP_OS + YARP::YARP_dev + YARP::YARP_init) + +yarp_install( + TARGETS HumanWrenchWrapper + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) diff --git a/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.cpp b/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.cpp new file mode 100644 index 000000000..03e01a285 --- /dev/null +++ b/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.cpp @@ -0,0 +1,175 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "HumanWrenchWrapper.h" +#include +#include + +#include +#include +#include + +#include + +const std::string DeviceName = "HumanWrenchWrapper"; +const std::string LogPrefix = DeviceName + " :"; +constexpr double DefaultPeriod = 0.01; + +using namespace hde::wrappers; + +class HumanWrenchWrapper::impl +{ +public: + mutable std::mutex mutex; + hde::interfaces::IHumanWrench* humanWrench = nullptr; + yarp::os::BufferedPort outputPort; +}; + +HumanWrenchWrapper::HumanWrenchWrapper() + : PeriodicThread(DefaultPeriod) + , pImpl{new impl()} +{} + +HumanWrenchWrapper::~HumanWrenchWrapper() {} + +bool HumanWrenchWrapper::open(yarp::os::Searchable& config) +{ + // =============================== + // CHECK THE CONFIGURATION OPTIONS + // =============================== + + if (!(config.check("period") && config.find("period").isFloat64())) { + yInfo() << LogPrefix << "Using default period:" << DefaultPeriod << "s"; + } + + if (!(config.check("outputPort") && config.find("outputPort").isString())) { + yError() << LogPrefix << "outputPort option not found or not valid"; + return false; + } + + // =============================== + // PARSE THE CONFIGURATION OPTIONS + // =============================== + + double period = config.check("period", yarp::os::Value(DefaultPeriod)).asFloat64(); + std::string outputPortName = config.find("outputPort").asString(); + + // ============= + // OPEN THE PORT + // ============= + + if (!pImpl->outputPort.open(outputPortName)) { + yError() << LogPrefix << "Failed to open port" << outputPortName; + return false; + } + + // ================ + // SETUP THE THREAD + // ================ + + setPeriod(period); + + return true; +} + +bool HumanWrenchWrapper::close() +{ + pImpl->outputPort.close(); + return true; +} + +void HumanWrenchWrapper::run() +{ + // Get data from the interface + std::vector wrenchSourceNames = pImpl->humanWrench->getWrenchSourceNames(); + std::vector wrenches = pImpl->humanWrench->getWrenches(); + + // Prepare the message + hde::msgs::HumanWrench& humanWrenchData = pImpl->outputPort.prepare(); + + + // Convert the wrench siurce names + humanWrenchData.wrenchSourceNames.resize(wrenchSourceNames.size()); + for (unsigned i = 0; i < wrenchSourceNames.size(); ++i) { + humanWrenchData.wrenchSourceNames[i] = wrenchSourceNames[i]; + } + + // Convert the joint torques + humanWrenchData.wrenches.resize(wrenches.size()); + for (unsigned i = 0; i < wrenches.size(); ++i) { + humanWrenchData.wrenches[i] = wrenches[i]; + } + + // Send the data + pImpl->outputPort.write(); +} + +bool HumanWrenchWrapper::attach(yarp::dev::PolyDriver* poly) +{ + if (!poly) { + yError() << LogPrefix << "Passed PolyDriver is nullptr"; + return false; + } + + if (pImpl->humanWrench || !poly->view(pImpl->humanWrench) || !pImpl->humanWrench) { + yError() << LogPrefix << "Failed to view the IHumanWrench interface from the PolyDriver"; + return false; + } + + // =================== + // CHECK THE INTERFACE + // =================== + + auto numberOfWrenchSources = pImpl->humanWrench->getNumberOfWrenchSources(); + while ( numberOfWrenchSources == 0 || + numberOfWrenchSources != pImpl->humanWrench->getWrenchSourceNames().size()) { + yInfo() << LogPrefix << "IHumanWrench interface waiting for first data. Waiting..."; + yarp::os::Time::delay(5); + } + + // ==== + // MISC + // ==== + + // Start the PeriodicThread loop + if (!start()) { + yError() << LogPrefix << "Failed to start the loop"; + return false; + } + + return true; +} + +void HumanWrenchWrapper::threadRelease() {} + +bool HumanWrenchWrapper::detach() +{ + while (isRunning()) { + stop(); + } + + pImpl->humanWrench = nullptr; + return true; +} + +bool HumanWrenchWrapper::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + if (driverList.size() > 1) { + yError() << LogPrefix << "This wrapper accepts only one attached PolyDriver"; + return false; + } + + const yarp::dev::PolyDriverDescriptor* driver = driverList[0]; + + if (!driver) { + yError() << LogPrefix << "Passed PolyDriverDescriptor is nullptr"; + return false; + } + + return attach(driver->poly); +} + +bool HumanWrenchWrapper::detachAll() +{ + return detach(); +} diff --git a/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.h b/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.h new file mode 100644 index 000000000..f3bf3a291 --- /dev/null +++ b/wrappers/HumanWrenchWrapper/HumanWrenchWrapper.h @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef HDE_DEVICES_HUMANWRENCHWRAPPER +#define HDE_DEVICES_HUMANWRENCHWRAPPER + +#include +#include +#include +#include + +#include + +namespace hde { + namespace wrappers { + class HumanWrenchWrapper; + } // namespace wrappers +} // namespace hde + +class hde::wrappers::HumanWrenchWrapper final + : public yarp::dev::DeviceDriver + , public yarp::dev::IWrapper + , public yarp::dev::IMultipleWrapper + , public yarp::os::PeriodicThread +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + HumanWrenchWrapper(); + ~HumanWrenchWrapper() override; + + // DeviceDriver interface + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // PeriodicThread + void run() override; + void threadRelease() override; + + // IWrapper interface + bool attach(yarp::dev::PolyDriver* poly) override; + bool detach() override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() override; + +}; + +#endif // HDE_DEVICES_HUMANWRENCHWRAPPER