Skip to content

Commit 3804cf7

Browse files
lrapettilrapetti
authored
Allow IWearRemapper to attach and remap IWear interfaces (#170)
* Enable the remapper to read interfaces * make wearableDataPorts parameter optional in IWearRemapper * remove unused variable in IWearRemapper * Add waitForAttachAll flag * Optimize configuration file parsing * Update CHANGELOG.md
1 parent fc6c7cc commit 3804cf7

File tree

3 files changed

+234
-67
lines changed

3 files changed

+234
-67
lines changed

‎CHANGELOG.md

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
66

77
## [Unreleased]
88

9+
### Changed
10+
- The `IWearRemapper` can be attached also to IWear interfaces (https://github.com/robotology/wearables/pull/170)
11+
912
### Removed
1013
- Remove unused definition in actuators thrift messages(https://github.com/robotology/wearables/pull/171).
1114

‎devices/IWearRemapper/include/IWearRemapper.h

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
#include <yarp/dev/DeviceDriver.h>
1515
#include <yarp/dev/PreciselyTimed.h>
1616
#include <yarp/os/PeriodicThread.h>
17+
#include <yarp/dev/Wrapper.h>
1718
#include <yarp/os/TypedReaderCallback.h>
1819

1920
#include <memory>
@@ -32,6 +33,7 @@ class wearable::devices::IWearRemapper
3233
, public wearable::IWear
3334
, public yarp::os::TypedReaderCallback<msg::WearableData>
3435
, public yarp::os::PeriodicThread
36+
, public yarp::dev::IMultipleWrapper
3537
, public yarp::dev::IPreciselyTimed
3638
{
3739
private:
@@ -127,6 +129,10 @@ class wearable::devices::IWearRemapper
127129

128130
inline ElementPtr<const actuator::IHeater>
129131
getHeaterActuator(const actuator::ActuatorName) const override;
132+
133+
// IMultipleWrapper interface
134+
bool attachAll(const yarp::dev::PolyDriverList& driverList) override;
135+
bool detachAll() override;
130136
};
131137

132138
inline wearable::ElementPtr<const wearable::actuator::IActuator>

‎devices/IWearRemapper/src/IWearRemapper.cpp

Lines changed: 225 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -39,14 +39,16 @@ class IWearRemapper::impl
3939
TimeStamp timestamp;
4040
bool firstRun = true;
4141
bool terminationCall = false;
42+
bool inputDataPorts = false;
43+
44+
// Flag to wait for first data received
45+
bool waitForAttachAll = false;
4246

4347
mutable std::recursive_mutex mutex;
4448

4549
msg::WearableData wearableData;
4650
std::vector<std::unique_ptr<yarp::os::BufferedPort<msg::WearableData>>> inputPortsWearData;
4751

48-
std::map<wearable::sensor::SensorName, size_t> sensorNameToIndex;
49-
5052
// Sensors stored for exposing wearable::IWear
5153
std::map<std::string, std::shared_ptr<sensor::impl::Accelerometer>> accelerometers;
5254
std::map<std::string, std::shared_ptr<sensor::impl::EmgSensor>> emgSensors;
@@ -89,86 +91,113 @@ IWearRemapper::~IWearRemapper() = default;
8991

9092
bool IWearRemapper::open(yarp::os::Searchable& config)
9193
{
92-
// ===============================
93-
// CHECK THE CONFIGURATION OPTIONS
94-
// ===============================
95-
96-
// Data ports
97-
if (!(config.check("wearableDataPorts") && config.find("wearableDataPorts").isList())) {
98-
yError() << logPrefix << "wearableDataPorts option does not exist or it is not a list";
99-
return false;
100-
}
101-
yarp::os::Bottle* inputDataPortsNamesList = config.find("wearableDataPorts").asList();
102-
for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) {
103-
if (!inputDataPortsNamesList->get(i).isString()) {
104-
yError() << logPrefix << "ith entry of wearableDataPorts list is not a string";
94+
// =====================
95+
// CHECK THE INPUT PORTS
96+
// =====================
97+
98+
// wait for attachAll
99+
if (config.check("waitForAttachAll")) {
100+
if (!config.find("waitForAttachAll").isBool()) {
101+
yError() << logPrefix << "waitForAttachAll option is not a bool";
105102
return false;
106103
}
104+
pImpl->waitForAttachAll = config.find("waitForAttachAll").asBool();
107105
}
108106

109-
// ===============================
110-
// PARSE THE CONFIGURATION OPTIONS
111-
// ===============================
112107

113-
// Convert list to vector
114-
std::vector<std::string> inputDataPortsNamesVector;
115-
for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) {
116-
inputDataPortsNamesVector.emplace_back(inputDataPortsNamesList->get(i).asString());
117-
}
108+
pImpl->inputDataPorts = config.check("wearableDataPorts");
118109

119-
yInfo() << logPrefix << "*** ========================";
120-
for (unsigned i = 0; i < inputDataPortsNamesVector.size(); ++i) {
121-
yInfo() << logPrefix << "*** Wearable Data Port" << i + 1 << " :"
122-
<< inputDataPortsNamesVector[i];
123-
}
110+
if (pImpl->inputDataPorts) {
124111

125-
yInfo() << logPrefix << "*** ========================";
112+
// Check that wearableDataPorts option is a list
113+
if (!config.find("wearableDataPorts").isList()) {
114+
yError() << logPrefix << "wearableDataPorts option is not a list";
115+
return false;
116+
}
126117

127-
// Carrier optional configuration
128-
std::string carrier = "";
129-
if (config.check("carrier")) {
130-
carrier = config.find("carrier").asString();
131-
}
118+
yarp::os::Bottle* inputDataPortsNamesList = config.find("wearableDataPorts").asList();
132119

133-
// Initialize the network
134-
pImpl->network = yarp::os::Network();
135-
if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) {
136-
yError() << logPrefix << "YARP server wasn't found active.";
137-
return false;
138-
}
120+
if (inputDataPortsNamesList->size() == 0) {
121+
pImpl->inputDataPorts = false;
122+
}
123+
else {
124+
for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) {
125+
if (!inputDataPortsNamesList->get(i).isString()) {
126+
yError() << logPrefix << "ith entry of wearableDataPorts list is not a string";
127+
return false;
128+
}
129+
}
139130

140-
// ==========================
141-
// CONFIGURE INPUT DATA PORTS
142-
// ==========================
143-
yDebug() << logPrefix << "Configuring input data ports";
131+
// ===============================
132+
// PARSE THE CONFIGURATION OPTIONS
133+
// ===============================
144134

145-
for (unsigned i = 0; i < config.find("wearableDataPorts").asList()->size(); ++i) {
146-
pImpl->inputPortsWearData.emplace_back(new yarp::os::BufferedPort<msg::WearableData>());
147-
pImpl->inputPortsWearData.back()->useCallback(*this);
135+
// Convert list to vector
136+
std::vector<std::string> inputDataPortsNamesVector;
137+
for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) {
138+
inputDataPortsNamesVector.emplace_back(inputDataPortsNamesList->get(i).asString());
139+
}
148140

149-
if (!pImpl->inputPortsWearData.back()->open("...")) {
150-
yError() << logPrefix << "Failed to open local input port";
151-
return false;
152-
}
153-
}
141+
yInfo() << logPrefix << "*** ========================";
142+
for (unsigned i = 0; i < inputDataPortsNamesVector.size(); ++i) {
143+
yInfo() << logPrefix << "*** Wearable Data Port" << i + 1 << " :"
144+
<< inputDataPortsNamesVector[i];
145+
}
154146

155-
// ================
156-
// OPEN INPUT PORTS
157-
// ================
158-
yDebug() << logPrefix << "Opening input ports";
159-
160-
for (unsigned i = 0; i < config.find("wearableDataPorts").asList()->size(); ++i) {
161-
if (!yarp::os::Network::connect(inputDataPortsNamesVector[i],
162-
pImpl->inputPortsWearData[i]->getName(),
163-
carrier)) {
164-
yError() << logPrefix << "Failed to connect " << inputDataPortsNamesVector[i]
165-
<< " with " << pImpl->inputPortsWearData[i]->getName();
166-
return false;
147+
yInfo() << logPrefix << "*** ========================";
148+
149+
// Carrier optional configuration
150+
std::string carrier = "";
151+
if (config.check("carrier")) {
152+
carrier = config.find("carrier").asString();
153+
}
154+
155+
// ==========================
156+
// CONFIGURE INPUT DATA PORTS
157+
// ==========================
158+
yDebug() << logPrefix << "Configuring input data ports";
159+
160+
for (unsigned i = 0; i < config.find("wearableDataPorts").asList()->size(); ++i) {
161+
pImpl->inputPortsWearData.emplace_back(new yarp::os::BufferedPort<msg::WearableData>());
162+
pImpl->inputPortsWearData.back()->useCallback(*this);
163+
164+
if (!pImpl->inputPortsWearData.back()->open("...")) {
165+
yError() << logPrefix << "Failed to open local input port";
166+
return false;
167+
}
168+
}
169+
170+
// ================
171+
// OPEN INPUT PORTS
172+
// ================
173+
yDebug() << logPrefix << "Opening input ports";
174+
175+
for (unsigned i = 0; i < config.find("wearableDataPorts").asList()->size(); ++i) {
176+
if (!yarp::os::Network::connect(inputDataPortsNamesVector[i],
177+
pImpl->inputPortsWearData[i]->getName(),
178+
carrier)) {
179+
yError() << logPrefix << "Failed to connect " << inputDataPortsNamesVector[i]
180+
<< " with " << pImpl->inputPortsWearData[i]->getName();
181+
return false;
182+
}
183+
}
184+
185+
// Initialize the network
186+
pImpl->network = yarp::os::Network();
187+
if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) {
188+
yError() << logPrefix << "YARP server wasn't found active.";
189+
return false;
190+
}
191+
192+
// If it not necessary to wait for the attachAll start the callbacks
193+
// We use callbacks on the input ports, the loop is a no-op
194+
if (!pImpl->waitForAttachAll) {
195+
start();
196+
}
197+
167198
}
168199
}
169-
170-
// We use callbacks on the input ports, the loop is a no-op
171-
start();
200+
172201

173202
yDebug() << logPrefix << "Opened correctly";
174203
return true;
@@ -695,6 +724,135 @@ SensorPtr<const sensor::ISensor> IWearRemapper::getSensor(const sensor::SensorNa
695724
return nullptr;
696725
}
697726

727+
bool IWearRemapper::attachAll(const yarp::dev::PolyDriverList& driverList)
728+
{
729+
for(int p=0; p<driverList.size(); p++)
730+
{
731+
wearable::IWear* iWear = nullptr;
732+
if (!driverList[p]->poly->view(iWear)) {
733+
yError() << logPrefix << "Failed to view the IWear interface from the PolyDriver.";
734+
return false;
735+
}
736+
737+
for (const auto& sensor : iWear->getAccelerometers()) {
738+
const auto* constSensor = static_cast<const sensor::impl::Accelerometer*>(sensor.get());
739+
auto* newSensor = const_cast<sensor::impl::Accelerometer*>(constSensor);
740+
newSensor->setStatus(sensor->getSensorStatus());
741+
pImpl->accelerometers.emplace(sensor->getSensorName(), newSensor);
742+
}
743+
for (const auto& sensor : iWear->getEmgSensors()) {
744+
const auto* constSensor = static_cast<const sensor::impl::EmgSensor*>(sensor.get());
745+
auto* newSensor = const_cast<sensor::impl::EmgSensor*>(constSensor);
746+
newSensor->setStatus(sensor->getSensorStatus());
747+
pImpl->emgSensors.emplace(sensor->getSensorName(), newSensor);
748+
}
749+
for (const auto& sensor : iWear->getForce3DSensors()) {
750+
const auto* constSensor = static_cast<const sensor::impl::Force3DSensor*>(sensor.get());
751+
auto* newSensor = const_cast<sensor::impl::Force3DSensor*>(constSensor);
752+
newSensor->setStatus(sensor->getSensorStatus());
753+
pImpl->force3DSensors.emplace(sensor->getSensorName(), newSensor);
754+
}
755+
for (const auto& sensor : iWear->getForceTorque6DSensors()) {
756+
const auto* constSensor = static_cast<const sensor::impl::ForceTorque6DSensor*>(sensor.get());
757+
auto* newSensor = const_cast<sensor::impl::ForceTorque6DSensor*>(constSensor);
758+
newSensor->setStatus(sensor->getSensorStatus());
759+
pImpl->forceTorque6DSensors.emplace(sensor->getSensorName(), newSensor);
760+
}
761+
for (const auto& sensor : iWear->getFreeBodyAccelerationSensors()) {
762+
const auto* constSensor = static_cast<const sensor::impl::FreeBodyAccelerationSensor*>(sensor.get());
763+
auto* newSensor = const_cast<sensor::impl::FreeBodyAccelerationSensor*>(constSensor);
764+
newSensor->setStatus(sensor->getSensorStatus());
765+
pImpl->freeBodyAccelerationSensors.emplace(sensor->getSensorName(), newSensor);
766+
}
767+
for (const auto& sensor : iWear->getGyroscopes()) {
768+
const auto* constSensor = static_cast<const sensor::impl::Gyroscope*>(sensor.get());
769+
auto* newSensor = const_cast<sensor::impl::Gyroscope*>(constSensor);
770+
newSensor->setStatus(sensor->getSensorStatus());
771+
pImpl->gyroscopes.emplace(sensor->getSensorName(), newSensor);
772+
}
773+
for (const auto& sensor : iWear->getMagnetometers()) {
774+
const auto* constSensor = static_cast<const sensor::impl::Magnetometer*>(sensor.get());
775+
auto* newSensor = const_cast<sensor::impl::Magnetometer*>(constSensor);
776+
newSensor->setStatus(sensor->getSensorStatus());
777+
pImpl->magnetometers.emplace(sensor->getSensorName(), newSensor);
778+
}
779+
for (const auto& sensor : iWear->getOrientationSensors()) {
780+
const auto* constSensor = static_cast<const sensor::impl::OrientationSensor*>(sensor.get());
781+
auto* newSensor = const_cast<sensor::impl::OrientationSensor*>(constSensor);
782+
newSensor->setStatus(sensor->getSensorStatus());
783+
pImpl->orientationSensors.emplace(sensor->getSensorName(), newSensor);
784+
}
785+
for (const auto& sensor : iWear->getPoseSensors()) {
786+
const auto* constSensor = static_cast<const sensor::impl::PoseSensor*>(sensor.get());
787+
auto* newSensor = const_cast<sensor::impl::PoseSensor*>(constSensor);
788+
newSensor->setStatus(sensor->getSensorStatus());
789+
pImpl->poseSensors.emplace(sensor->getSensorName(), newSensor);
790+
}
791+
for (const auto& sensor : iWear->getPositionSensors()) {
792+
const auto* constSensor = static_cast<const sensor::impl::PositionSensor*>(sensor.get());
793+
auto* newSensor = const_cast<sensor::impl::PositionSensor*>(constSensor);
794+
newSensor->setStatus(sensor->getSensorStatus());
795+
pImpl->positionSensors.emplace(sensor->getSensorName(), newSensor);
796+
}
797+
for (const auto& sensor : iWear->getSkinSensors()) {
798+
const auto* constSensor = static_cast<const sensor::impl::SkinSensor*>(sensor.get());
799+
auto* newSensor = const_cast<sensor::impl::SkinSensor*>(constSensor);
800+
newSensor->setStatus(sensor->getSensorStatus());
801+
pImpl->skinSensors.emplace(sensor->getSensorName(), newSensor);
802+
}
803+
for (const auto& sensor : iWear->getTemperatureSensors()) {
804+
const auto* constSensor = static_cast<const sensor::impl::TemperatureSensor*>(sensor.get());
805+
auto* newSensor = const_cast<sensor::impl::TemperatureSensor*>(constSensor);
806+
newSensor->setStatus(sensor->getSensorStatus());
807+
pImpl->temperatureSensors.emplace(sensor->getSensorName(), newSensor);
808+
}
809+
for (const auto& sensor : iWear->getTorque3DSensors()) {
810+
const auto* constSensor = static_cast<const sensor::impl::Torque3DSensor*>(sensor.get());
811+
auto* newSensor = const_cast<sensor::impl::Torque3DSensor*>(constSensor);
812+
newSensor->setStatus(sensor->getSensorStatus());
813+
pImpl->torque3DSensors.emplace(sensor->getSensorName(), newSensor);
814+
}
815+
for (const auto& sensor : iWear->getVirtualLinkKinSensors()) {
816+
const auto* constSensor = static_cast<const sensor::impl::VirtualLinkKinSensor*>(sensor.get());
817+
auto* newSensor = const_cast<sensor::impl::VirtualLinkKinSensor*>(constSensor);
818+
newSensor->setStatus(sensor->getSensorStatus());
819+
pImpl->virtualLinkKinSensors.emplace(sensor->getSensorName(), newSensor);
820+
}
821+
for (const auto& sensor : iWear->getVirtualJointKinSensors()) {
822+
const auto* constSensor = static_cast<const sensor::impl::VirtualJointKinSensor*>(sensor.get());
823+
auto* newSensor = const_cast<sensor::impl::VirtualJointKinSensor*>(constSensor);
824+
newSensor->setStatus(sensor->getSensorStatus());
825+
pImpl->virtualJointKinSensors.emplace(sensor->getSensorName(), newSensor);
826+
}
827+
for (const auto& sensor : iWear->getVirtualSphericalJointKinSensors()) {
828+
const auto* constSensor = static_cast<const sensor::impl::VirtualSphericalJointKinSensor*>(sensor.get());
829+
auto* newSensor = const_cast<sensor::impl::VirtualSphericalJointKinSensor*>(constSensor);
830+
newSensor->setStatus(sensor->getSensorStatus());
831+
pImpl->virtualSphericalJointKinSensors.emplace(sensor->getSensorName(), newSensor);
832+
}
833+
834+
}
835+
836+
// If there are not input ports there is no need to wait for the first data and to start the no-op loop
837+
if (!pImpl->inputDataPorts) {
838+
pImpl->firstRun = false;
839+
return true;
840+
}
841+
else {
842+
// If it is wating for the attach all, the loop can now be started
843+
if (pImpl->waitForAttachAll) {
844+
start();
845+
}
846+
}
847+
848+
return true;
849+
}
850+
851+
bool IWearRemapper::detachAll()
852+
{
853+
return true;
854+
}
855+
698856
VectorOfSensorPtr<const sensor::ISensor>
699857
IWearRemapper::getSensors(const sensor::SensorType type) const
700858
{

0 commit comments

Comments
 (0)