Skip to content

Commit

Permalink
Add HumanWrench wrapper and remapper
Browse files Browse the repository at this point in the history
  • Loading branch information
ifeel committed Sep 28, 2023
1 parent 20451d7 commit 698a7b0
Show file tree
Hide file tree
Showing 15 changed files with 567 additions and 24 deletions.
15 changes: 0 additions & 15 deletions devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@

#include <yarp/os/LogStream.h>
#include <yarp/os/ResourceFinder.h>
#include <yarp/dev/IAnalogSensor.h>
#include <iDynTree/yarp/YARPConversions.h>

#include <mutex>
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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){
Expand Down Expand Up @@ -1325,7 +1311,6 @@ bool HumanDynamicsEstimator::detachAll()

pImpl->iHumanState = nullptr;
pImpl->iHumanWrench = nullptr;
pImpl->iAnalogSensor = nullptr;

return true;
}
Expand Down
32 changes: 32 additions & 0 deletions msgs/yarp/thrift/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
"$<BUILD_INTERFACE:${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}>"
"$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}/hde/msgs>")

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")
9 changes: 9 additions & 0 deletions msgs/yarp/thrift/hde/msgs/HumanWrench.thrift
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
namespace yarp hde.msgs

/**
* Representation of the IHumanWrench interface
*/
struct HumanWrench {
1: list<string> wrenchSourceNames;
2: list<double> wrenches;
}
1 change: 1 addition & 0 deletions remappers/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -3,4 +3,5 @@

add_subdirectory(HumanStateRemapper)
add_subdirectory(HumanDynamicsRemapper)
add_subdirectory(HumanWrenchRemapper)
add_subdirectory(WearableTargetsRemapper)
6 changes: 3 additions & 3 deletions remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,9 @@ class HumanDynamicsRemapper::impl
std::vector<double> jointTorques;
};

// ==============
// IWEAR REMAPPER
// ==============
// =======================
// IHUMANDYNAMICS REMAPPER
// =======================

HumanDynamicsRemapper::HumanDynamicsRemapper()
: PeriodicThread(1)
Expand Down
6 changes: 3 additions & 3 deletions remappers/HumanStateRemapper/HumanStateRemapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,9 @@ class HumanStateRemapper::impl
std::array<double, 3> CoMVelocity;
};

// ==============
// IWEAR REMAPPER
// ==============
// ====================
// IHUMANSTATE REMAPPER
// ====================

HumanStateRemapper::HumanStateRemapper()
: PeriodicThread(1)
Expand Down
31 changes: 31 additions & 0 deletions remappers/HumanWrenchRemapper/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>)

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

153 changes: 153 additions & 0 deletions remappers/HumanWrenchRemapper/HumanWrenchRemapper.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,153 @@
// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
// SPDX-License-Identifier: BSD-3-Clause

#include "HumanWrenchRemapper.h"

#include <hde/msgs/HumanWrench.h>

#include <yarp/os/Network.h>
#include <yarp/os/LogStream.h>

#include <iostream>
#include <mutex>

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<hde::msgs::HumanWrench> inputPort;
bool terminationCall = false;

// Buffer HumanWrench variables
std::vector<std::string> wrenchSourceNames;
std::vector<double> 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<std::mutex> 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<std::string> HumanWrenchRemapper::getWrenchSourceNames() const
{
std::lock_guard<std::mutex> lock(pImpl->mtx);
return pImpl->wrenchSourceNames;
}

size_t HumanWrenchRemapper::getNumberOfWrenchSources() const
{
std::lock_guard<std::mutex> lock(pImpl->mtx);
return pImpl->wrenchSourceNames.size();
}

std::vector<double> HumanWrenchRemapper::getWrenches() const
{
std::lock_guard<std::mutex> lock(pImpl->mtx);
return pImpl->wrenches;
}
62 changes: 62 additions & 0 deletions remappers/HumanWrenchRemapper/HumanWrenchRemapper.h
Original file line number Diff line number Diff line change
@@ -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 <hde/interfaces/IHumanWrench.h>

#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/IPreciselyTimed.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/TypedReaderCallback.h>

#include <memory>

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<hde::msgs::HumanWrench>
// implement the periodic thread
, public yarp::os::PeriodicThread
{
private:
class impl;
std::unique_ptr<impl> 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<std::string> getWrenchSourceNames() const override;

size_t getNumberOfWrenchSources() const override;

std::vector<double> getWrenches() const override;
};

#endif // HDE_DEVICES_HUMANWRENCHREMAPPER

12 changes: 12 additions & 0 deletions remappers/HumanWrenchRemapper/conf/HumanWrenchRemapperExample.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
<robot name="Human-Wrench-Remapper" build=0 portprefix="">


<device type="human_wrench_remapper" name="HumanWrenchRemapper">
<param name="humanWrenchDataPort">/HDE/HumanWrenchWrapper/wrench:o</param>
</device>



</robot>
6 changes: 3 additions & 3 deletions remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,9 +69,9 @@ class WearableTargetsRemapper::impl
std::unordered_map<hde::TargetName, std::shared_ptr<hde::WearableSensorTarget>> wearableTargets;
};

// ==============
// IWEAR REMAPPER
// ==============
// ========================
// WEARABLETARGETS REMAPPER
// ========================

WearableTargetsRemapper::WearableTargetsRemapper()
: PeriodicThread(1)
Expand Down
Loading

0 comments on commit 698a7b0

Please sign in to comment.