-
Notifications
You must be signed in to change notification settings - Fork 29
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add HumanWrench wrapper and remapper
- Loading branch information
ifeel
committed
Sep 28, 2023
1 parent
20451d7
commit 698a7b0
Showing
15 changed files
with
567 additions
and
24 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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}) | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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
12
remappers/HumanWrenchRemapper/conf/HumanWrenchRemapperExample.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.