From 401871012e3c528e78404f9d4c340d8ec29681e0 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Thu, 4 Oct 2018 19:15:56 +0200 Subject: [PATCH 01/36] Icub device initial commit --- devices/CMakeLists.txt | 2 + devices/ICub/CMakeLists.txt | 19 +++ devices/ICub/conf/ICub.xml | 10 ++ devices/ICub/include/ICub.h | 222 ++++++++++++++++++++++++++++++++++++ devices/ICub/src/ICub.cpp | 222 ++++++++++++++++++++++++++++++++++++ 5 files changed, 475 insertions(+) create mode 100644 devices/ICub/CMakeLists.txt create mode 100644 devices/ICub/conf/ICub.xml create mode 100644 devices/ICub/include/ICub.h create mode 100644 devices/ICub/src/ICub.cpp diff --git a/devices/CMakeLists.txt b/devices/CMakeLists.txt index 74b62a65..b341afba 100644 --- a/devices/CMakeLists.txt +++ b/devices/CMakeLists.txt @@ -8,3 +8,5 @@ add_subdirectory(IAnalogSensorToIWear) if(XSENS_MVN_USE_SDK) add_subdirectory(XsensSuit) endif() + +add_subdirectory(ICub) diff --git a/devices/ICub/CMakeLists.txt b/devices/ICub/CMakeLists.txt new file mode 100644 index 00000000..0c610077 --- /dev/null +++ b/devices/ICub/CMakeLists.txt @@ -0,0 +1,19 @@ +# Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +yarp_add_plugin(ICub + src/ICub.cpp + include/ICub.h) + +target_include_directories(ICub PRIVATE + $) + +target_link_libraries(ICub PUBLIC + IWear YARP::YARP_dev) + +yarp_install( + TARGETS ICub + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}) diff --git a/devices/ICub/conf/ICub.xml b/devices/ICub/conf/ICub.xml new file mode 100644 index 00000000..98ad5be0 --- /dev/null +++ b/devices/ICub/conf/ICub.xml @@ -0,0 +1,10 @@ + + + + + 2 + /wholeBodyDynamics/left_arm/endEffectorWrench:o + /wholeBodyDynamics/right_arm/endEffectorWrench:o + + + diff --git a/devices/ICub/include/ICub.h b/devices/ICub/include/ICub.h new file mode 100644 index 00000000..eeab1108 --- /dev/null +++ b/devices/ICub/include/ICub.h @@ -0,0 +1,222 @@ +/* + * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef ICUB_H +#define ICUB_H + +#include "Wearable/IWear/IWear.h" + +#include +#include + +#include + +namespace wearable { + namespace devices { + class ICub; + } // namespace devices +} // namespace wearable + +class wearable::devices::ICub final + : public yarp::dev::DeviceDriver + , public yarp::dev::IPreciselyTimed + , public wearable::IWear +{ +private: + class ICubImpl; + std::unique_ptr pImpl; + +public: + ICub(); + ~ICub() override; + + ICub(const ICub& other) = delete; + ICub(ICub&& other) = delete; + ICub& operator=(const ICub& other) = delete; + ICub& operator=(ICub&& other) = delete; + + // ============= + // DEVICE DRIVER + // ============= + + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // ================ + // IPRECISELY TIMED + // ================ + + yarp::os::Stamp getLastInputStamp() override; + + // ===== + // IWEAR + // ===== + + // GENERIC + // ------- + + WearableName getWearableName() const override; + WearStatus getStatus() const override; + //TimeStamp getTimeStamp() const override; + + // IMPLEMENTED SENSORS + // ------------------- + + SensorPtr + getForceTorque6DSensor(const sensor::SensorName name) const override; + + // UNIMPLEMENTED SENSORS + // --------------------- + + inline SensorPtr + getSensor(const sensor::SensorName /*name*/) const override; + + inline VectorOfSensorPtr + getSensors(const sensor::SensorType) const override; + + inline SensorPtr + getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getMagnetometer(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getOrientationSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getPoseSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getPositionSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getAccelerometer(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getEmgSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getForce3DSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getGyroscope(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getSkinSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getTemperatureSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getTorque3DSensor(const sensor::SensorName /*name*/) const override; + +}; + +inline wearable::SensorPtr +wearable::devices::ICub::getSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::VectorOfSensorPtr +wearable::devices::ICub::getSensors(const sensor::SensorType) const +{ + return {nullptr}; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getMagnetometer(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getOrientationSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getPoseSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getPositionSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getAccelerometer(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getEmgSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getForce3DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getGyroscope(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getSkinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getTemperatureSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::ICub::getTorque3DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +#endif // ICUB_H diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp new file mode 100644 index 00000000..c8bbb568 --- /dev/null +++ b/devices/ICub/src/ICub.cpp @@ -0,0 +1,222 @@ +/* + * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#include "ICub.h" + +#include +#include +#include + +#include +#include +#include +#include + +using namespace wearable::devices; + +const std::string logPrefix = "ICub :"; + +class ICub::ICubImpl +{ +public: + class ICubForceTorque6DSensor; + + const WearableName wearableName = "ICub" + wearable::Separator; + + int nSensors; + std::vector> sensorList; + std::vector sensorNames; + yarp::os::BufferedPort leftHandFTPort; + yarp::os::BufferedPort rightHandFTPort; +}; + +// ========================================== +// ICub implementation of ForceTorque6DSensor +// ========================================== +class ICub::ICubImpl::ICubForceTorque6DSensor + : public wearable::sensor::IForceTorque6DSensor +{ +private: + yarp::os::Bottle* wrench; + +public: + ICub::ICubImpl* icubImpl = nullptr; + + // ------------------------ + // Constructor / Destructor + // ------------------------ + ICubForceTorque6DSensor( + ICub::ICubImpl* impl, + const wearable::sensor::SensorName name = {}, + const wearable::sensor::SensorStatus status = wearable::sensor::SensorStatus::Unknown) + : IForceTorque6DSensor(name, status) + , icubImpl(impl) + {} + + ~ICubForceTorque6DSensor() override = default; + + // ============================== + // IForceTorque6DSensor interface + // ============================== + bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const override { + if (!icubImpl) { + return false; + } + + // Reading wrench from WBD ports + if (this->m_name == "leftWBDFTSensor") { + icubImpl->leftHandFTPort.read(wrench); + } + else if(this->m_name == "rightWBDFTSensor") { + icubImpl->rightHandFTPort.read(wrench); + } + + if(!wrench->isNull() && wrench->size() != 6) { + force3D[0] = wrench->get(0).asDouble(); + force3D[1] = wrench->get(1).asDouble(); + force3D[2] = wrench->get(2).asDouble(); + + torque3D[0] = wrench->get(3).asDouble(); + torque3D[1] = wrench->get(4).asDouble(); + torque3D[2] = wrench->get(5).asDouble(); + } + else { + force3D.fill(0.0); + torque3D.fill(0.0); + } + + return true; + } +}; + +// ========================================== +// ICub constructor/destructor implementation +// ========================================== +ICub::ICub() + : pImpl{new ICubImpl()} +{} + +ICub::~ICub() = default; + +// ====================== +// DeviceDriver interface +// ====================== +bool ICub::open(yarp::os::Searchable& config) +{ + yInfo() << logPrefix << "Starting to configure"; + + yarp::os::Bottle wbdHandsFTSensorsSet = config.findGroup("wbd-hand-ft-sensors"); + if(wbdHandsFTSensorsSet.isNull()) { + yError() << logPrefix << "REQUIRED parameter NOT found"; + return false; + } + else { + + if (!wbdHandsFTSensorsSet.check("nSensors")) { + yError() << logPrefix << "REQUIRED parameter NOT found"; + return false; + } + else { + + pImpl->nSensors = wbdHandsFTSensorsSet.check("nSensors",yarp::os::Value(false)).asInt(); + + if (!wbdHandsFTSensorsSet.check("leftHand") || !wbdHandsFTSensorsSet.check("rightHand")) { + yError() << logPrefix << "REQUIRED parameter or NOT found"; + return false; + } + else { + + // Check YARP network + if(!yarp::os::Network::initialized()) + { + yarp::os::Network::init(); + } + + // Connect to wbd left hand ft port + std::string leftHandFTPortName = wbdHandsFTSensorsSet.check("leftHand",yarp::os::Value(false)).asString(); + + if(pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i")) + { + if(!yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str())) + { + yError() << logPrefix << "Failed to connect " << leftHandFTPortName << " and " << pImpl->leftHandFTPort.getName().c_str(); + return false; + } + else { + pImpl->sensorNames.push_back("leftWBDFTSensor"); + } + } + else + { + yError() << logPrefix << "Failed to open " << pImpl->leftHandFTPort.getName().c_str(); + return false; + } + + // Connect to wbd right hand ft port + std::string rightHandFTPortName = wbdHandsFTSensorsSet.check("rightHand",yarp::os::Value(false)).asString(); + + if(pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i")) + { + if(!yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str())) + { + yError() << logPrefix << "Failed to connect " << rightHandFTPortName << " and " << pImpl->rightHandFTPort.getName().c_str(); + return false; + } + else { + pImpl->sensorNames.push_back("rightWBDFTSensor"); + } + } + else + { + yError() << logPrefix << "Failed to open " << pImpl->rightHandFTPort.getName().c_str(); + return false; + } + + } + + } + + } + + std::string ft6dPrefix = getWearableName() + sensor::IForceTorque6DSensor::getPrefix(); + + for (size_t s = 0; s < pImpl->sensorNames.size(); ++s) { + // Create the new sensors + auto ft6d = std::make_shared( + pImpl.get(), ft6dPrefix + pImpl->sensorNames[s]); + + pImpl->sensorList.push_back(ft6d); + } + + return true; +} + +// --------------- +// Generic Methods +// --------------- + +wearable::WearableName ICub::getWearableName() const +{ + return pImpl->wearableName; +} + +bool ICub::close() +{ + return true; +} + +wearable::SensorPtr +ICub::getForceTorque6DSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + // TODO + + //return a shared point to the required sensor + return static_cast>( + pImpl->sensorList.at(1)); +} From 862d72e6ad57d55433485e53528eda9d81744400 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 00:17:37 +0200 Subject: [PATCH 02/36] Implement sensor methods --- devices/ICub/include/ICub.h | 22 ++-------- devices/ICub/src/ICub.cpp | 87 ++++++++++++++++++++++++++++++------- 2 files changed, 75 insertions(+), 34 deletions(-) diff --git a/devices/ICub/include/ICub.h b/devices/ICub/include/ICub.h index eeab1108..85042f53 100644 --- a/devices/ICub/include/ICub.h +++ b/devices/ICub/include/ICub.h @@ -64,6 +64,10 @@ class wearable::devices::ICub final WearStatus getStatus() const override; //TimeStamp getTimeStamp() const override; + SensorPtr getSensor(const sensor::SensorName name) const override; + + VectorOfSensorPtr getSensors(const sensor::SensorType) const override; + // IMPLEMENTED SENSORS // ------------------- @@ -73,12 +77,6 @@ class wearable::devices::ICub final // UNIMPLEMENTED SENSORS // --------------------- - inline SensorPtr - getSensor(const sensor::SensorName /*name*/) const override; - - inline VectorOfSensorPtr - getSensors(const sensor::SensorType) const override; - inline SensorPtr getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const override; @@ -123,18 +121,6 @@ class wearable::devices::ICub final }; -inline wearable::SensorPtr -wearable::devices::ICub::getSensor(const sensor::SensorName /*name*/) const -{ - return nullptr; -} - -inline wearable::VectorOfSensorPtr -wearable::devices::ICub::getSensors(const sensor::SensorType) const -{ - return {nullptr}; -} - inline wearable::SensorPtr wearable::devices::ICub::getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const { diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index c8bbb568..0e9e9c32 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -19,20 +19,28 @@ using namespace wearable::devices; -const std::string logPrefix = "ICub :"; +const std::string LogPrefix = "ICub :"; class ICub::ICubImpl { public: class ICubForceTorque6DSensor; - const WearableName wearableName = "ICub" + wearable::Separator; - - int nSensors; - std::vector> sensorList; + size_t nSensors; std::vector sensorNames; yarp::os::BufferedPort leftHandFTPort; yarp::os::BufferedPort rightHandFTPort; + + template + struct icubDeviceSensors + { + std::shared_ptr icubSensor; + size_t index; + }; + + std::map> ftSensorsMap; + + const WearableName wearableName = "ICub" + wearable::Separator; }; // ========================================== @@ -108,17 +116,17 @@ ICub::~ICub() = default; // ====================== bool ICub::open(yarp::os::Searchable& config) { - yInfo() << logPrefix << "Starting to configure"; + yInfo() << LogPrefix << "Starting to configure"; yarp::os::Bottle wbdHandsFTSensorsSet = config.findGroup("wbd-hand-ft-sensors"); if(wbdHandsFTSensorsSet.isNull()) { - yError() << logPrefix << "REQUIRED parameter NOT found"; + yError() << LogPrefix << "REQUIRED parameter NOT found"; return false; } else { if (!wbdHandsFTSensorsSet.check("nSensors")) { - yError() << logPrefix << "REQUIRED parameter NOT found"; + yError() << LogPrefix << "REQUIRED parameter NOT found"; return false; } else { @@ -126,7 +134,7 @@ bool ICub::open(yarp::os::Searchable& config) pImpl->nSensors = wbdHandsFTSensorsSet.check("nSensors",yarp::os::Value(false)).asInt(); if (!wbdHandsFTSensorsSet.check("leftHand") || !wbdHandsFTSensorsSet.check("rightHand")) { - yError() << logPrefix << "REQUIRED parameter or NOT found"; + yError() << LogPrefix << "REQUIRED parameter or NOT found"; return false; } else { @@ -144,7 +152,7 @@ bool ICub::open(yarp::os::Searchable& config) { if(!yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str())) { - yError() << logPrefix << "Failed to connect " << leftHandFTPortName << " and " << pImpl->leftHandFTPort.getName().c_str(); + yError() << LogPrefix << "Failed to connect " << leftHandFTPortName << " and " << pImpl->leftHandFTPort.getName().c_str(); return false; } else { @@ -153,7 +161,7 @@ bool ICub::open(yarp::os::Searchable& config) } else { - yError() << logPrefix << "Failed to open " << pImpl->leftHandFTPort.getName().c_str(); + yError() << LogPrefix << "Failed to open " << pImpl->leftHandFTPort.getName().c_str(); return false; } @@ -164,7 +172,7 @@ bool ICub::open(yarp::os::Searchable& config) { if(!yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str())) { - yError() << logPrefix << "Failed to connect " << rightHandFTPortName << " and " << pImpl->rightHandFTPort.getName().c_str(); + yError() << LogPrefix << "Failed to connect " << rightHandFTPortName << " and " << pImpl->rightHandFTPort.getName().c_str(); return false; } else { @@ -173,7 +181,7 @@ bool ICub::open(yarp::os::Searchable& config) } else { - yError() << logPrefix << "Failed to open " << pImpl->rightHandFTPort.getName().c_str(); + yError() << LogPrefix << "Failed to open " << pImpl->rightHandFTPort.getName().c_str(); return false; } @@ -190,7 +198,9 @@ bool ICub::open(yarp::os::Searchable& config) auto ft6d = std::make_shared( pImpl.get(), ft6dPrefix + pImpl->sensorNames[s]); - pImpl->sensorList.push_back(ft6d); + pImpl->ftSensorsMap.emplace( + ft6dPrefix + pImpl->sensorNames[s], + ICubImpl::icubDeviceSensors{ft6d,s}); } return true; @@ -210,13 +220,58 @@ bool ICub::close() return true; } +// --------------------------- +// Implemented Sensors Methods +// --------------------------- + +wearable::SensorPtr +ICub::getSensor(const wearable::sensor::SensorName name) const +{ + wearable::VectorOfSensorPtr sensors = getAllSensors(); + for (const auto& s : sensors) { + if (s->getSensorName() == name) { + return s; + } + } + yWarning() << LogPrefix << "User specified name <" << name << "> not found"; + return nullptr; +} + +wearable::VectorOfSensorPtr +ICub::getSensors(const wearable::sensor::SensorType type) const +{ + wearable::VectorOfSensorPtr outVec; + switch (type) { + case sensor::SensorType::Force3DSensor: { + outVec.reserve(pImpl->nSensors); + for (const auto& ft6d : pImpl->ftSensorsMap) { + outVec.push_back( + static_cast>(ft6d.second.icubSensor)); + } + + break; + } + default: { + yWarning() << LogPrefix << "Selected sensor type (" << static_cast(type) + << ") is not supported by ICub"; + return {}; + } + } + + return outVec; +} + wearable::SensorPtr ICub::getForceTorque6DSensor(const wearable::sensor::SensorName name) const { // Check if user-provided name corresponds to an available sensor - // TODO + if (pImpl->ftSensorsMap.find(static_cast(name)) + == pImpl->ftSensorsMap.end()) { + yError() << LogPrefix << "Invalid sensor name"; + return nullptr; + } //return a shared point to the required sensor return static_cast>( - pImpl->sensorList.at(1)); + pImpl->ftSensorsMap.at(static_cast(name)).icubSensor); } From f35fc918efff04a8bd22f0d1f6326185c44abe93 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 00:47:45 +0200 Subject: [PATCH 03/36] Implement getTimeStamp method --- devices/ICub/include/ICub.h | 4 ++-- devices/ICub/src/ICub.cpp | 23 +++++++++++++++++++++++ 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/devices/ICub/include/ICub.h b/devices/ICub/include/ICub.h index 85042f53..6ac6b0d8 100644 --- a/devices/ICub/include/ICub.h +++ b/devices/ICub/include/ICub.h @@ -17,7 +17,7 @@ #include namespace wearable { - namespace devices { + namespace devices { class ICub; } // namespace devices } // namespace wearable @@ -62,7 +62,7 @@ class wearable::devices::ICub final WearableName getWearableName() const override; WearStatus getStatus() const override; - //TimeStamp getTimeStamp() const override; + TimeStamp getTimeStamp() const override; SensorPtr getSensor(const sensor::SensorName name) const override; diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 0e9e9c32..36f50079 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -26,6 +26,15 @@ class ICub::ICubImpl public: class ICubForceTorque6DSensor; + struct Timestamp + { + double systemTime; + double relative; // [s] + double absolute; // [s] + }; + + std::shared_ptr timestamps = nullptr; + size_t nSensors; std::vector sensorNames; yarp::os::BufferedPort leftHandFTPort; @@ -79,9 +88,11 @@ class ICub::ICubImpl::ICubForceTorque6DSensor // Reading wrench from WBD ports if (this->m_name == "leftWBDFTSensor") { icubImpl->leftHandFTPort.read(wrench); + icubImpl->timestamps->systemTime = yarp::os::Time::now(); } else if(this->m_name == "rightWBDFTSensor") { icubImpl->rightHandFTPort.read(wrench); + icubImpl->timestamps->systemTime = yarp::os::Time::now(); } if(!wrench->isNull() && wrench->size() != 6) { @@ -118,6 +129,12 @@ bool ICub::open(yarp::os::Searchable& config) { yInfo() << LogPrefix << "Starting to configure"; + // Configure clock + if (!yarp::os::Time::isSystemClock()) + { + yarp::os::Time::useSystemClock(); + } + yarp::os::Bottle wbdHandsFTSensorsSet = config.findGroup("wbd-hand-ft-sensors"); if(wbdHandsFTSensorsSet.isNull()) { yError() << LogPrefix << "REQUIRED parameter NOT found"; @@ -215,6 +232,12 @@ wearable::WearableName ICub::getWearableName() const return pImpl->wearableName; } +wearable::TimeStamp ICub::getTimeStamp() const +{ + // Stamp count should be always zero + return {pImpl->timestamps->systemTime,0}; +} + bool ICub::close() { return true; From 0965fc3bec55c0b4d155b1f80bfe4c7e19f04a28 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 00:49:01 +0200 Subject: [PATCH 04/36] Update CMakeLists.txt --- devices/ICub/CMakeLists.txt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/devices/ICub/CMakeLists.txt b/devices/ICub/CMakeLists.txt index 0c610077..c4de3a16 100644 --- a/devices/ICub/CMakeLists.txt +++ b/devices/ICub/CMakeLists.txt @@ -2,6 +2,13 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +yarp_prepare_plugin(icub_device + TYPE wearable::devices::ICub + INCLUDE ICub.h + CATEGORY device + ADVANCED + DEFAULT ON) + yarp_add_plugin(ICub src/ICub.cpp include/ICub.h) From 8e0fe6cced31f6e6deb711caa944fe0d7290f678 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 13:06:57 +0200 Subject: [PATCH 05/36] Update timestamp implementation --- devices/ICub/src/ICub.cpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 36f50079..201b604f 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -26,14 +26,7 @@ class ICub::ICubImpl public: class ICubForceTorque6DSensor; - struct Timestamp - { - double systemTime; - double relative; // [s] - double absolute; // [s] - }; - - std::shared_ptr timestamps = nullptr; + wearable::TimeStamp timeStamp; size_t nSensors; std::vector sensorNames; @@ -88,11 +81,11 @@ class ICub::ICubImpl::ICubForceTorque6DSensor // Reading wrench from WBD ports if (this->m_name == "leftWBDFTSensor") { icubImpl->leftHandFTPort.read(wrench); - icubImpl->timestamps->systemTime = yarp::os::Time::now(); + icubImpl->timeStamp.time = yarp::os::Time::now(); } else if(this->m_name == "rightWBDFTSensor") { icubImpl->rightHandFTPort.read(wrench); - icubImpl->timestamps->systemTime = yarp::os::Time::now(); + icubImpl->timeStamp.time = yarp::os::Time::now(); } if(!wrench->isNull() && wrench->size() != 6) { @@ -235,7 +228,7 @@ wearable::WearableName ICub::getWearableName() const wearable::TimeStamp ICub::getTimeStamp() const { // Stamp count should be always zero - return {pImpl->timestamps->systemTime,0}; + return {pImpl->timeStamp.time,0}; } bool ICub::close() From 144b2e825c3d3a51e83ee7cd2f5a3b4578dda5c5 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 13:37:03 +0200 Subject: [PATCH 06/36] Update CMakeLists and add config file --- devices/ICub/CMakeLists.txt | 7 ++++++- devices/ICub/icub_wearable_device.ini | 5 +++++ 2 files changed, 11 insertions(+), 1 deletion(-) create mode 100644 devices/ICub/icub_wearable_device.ini diff --git a/devices/ICub/CMakeLists.txt b/devices/ICub/CMakeLists.txt index c4de3a16..f4999640 100644 --- a/devices/ICub/CMakeLists.txt +++ b/devices/ICub/CMakeLists.txt @@ -2,7 +2,7 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -yarp_prepare_plugin(icub_device +yarp_prepare_plugin(icub_wearable_device TYPE wearable::devices::ICub INCLUDE ICub.h CATEGORY device @@ -24,3 +24,8 @@ yarp_install( COMPONENT runtime LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}) + +yarp_install( + FILES "${CMAKE_CURRENT_SOURCE_DIR}/icub_wearable_device.ini" + DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) + diff --git a/devices/ICub/icub_wearable_device.ini b/devices/ICub/icub_wearable_device.ini new file mode 100644 index 00000000..0b5505d3 --- /dev/null +++ b/devices/ICub/icub_wearable_device.ini @@ -0,0 +1,5 @@ +[plugin icub_wearable_device] +type device +name icub_wearable_device +library ICub +wrapper iwear_wrapper From 47334b4e031d4efa329a553c89bd198db9605723 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 13:37:52 +0200 Subject: [PATCH 07/36] Implement getLastInputStamp method --- devices/ICub/src/ICub.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 201b604f..3ffd61ca 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -236,6 +236,15 @@ bool ICub::close() return true; } +// ========================= +// IPreciselyTimed interface +// ========================= +yarp::os::Stamp ICub::getLastInputStamp() +{ + // Stamp count should be always zero + return yarp::os::Stamp(0, pImpl->timeStamp.time); +} + // --------------------------- // Implemented Sensors Methods // --------------------------- From 23c5bb5dcdc5a0b1e2bbfae64398fba729b31231 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 14:54:10 +0200 Subject: [PATCH 08/36] Implement getStatus method --- devices/ICub/src/ICub.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 3ffd61ca..9b8f8f28 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -42,6 +42,13 @@ class ICub::ICubImpl std::map> ftSensorsMap; + const std::map icubSensorStatutsMap{ + {"Ok", sensor::SensorStatus::Ok}, + {"Error", sensor::SensorStatus::Error} + }; + + std::string sensorStatus; + const WearableName wearableName = "ICub" + wearable::Separator; }; @@ -82,10 +89,12 @@ class ICub::ICubImpl::ICubForceTorque6DSensor if (this->m_name == "leftWBDFTSensor") { icubImpl->leftHandFTPort.read(wrench); icubImpl->timeStamp.time = yarp::os::Time::now(); + icubImpl->sensorStatus = "Ok"; } else if(this->m_name == "rightWBDFTSensor") { icubImpl->rightHandFTPort.read(wrench); icubImpl->timeStamp.time = yarp::os::Time::now(); + icubImpl->sensorStatus = "Ok"; } if(!wrench->isNull() && wrench->size() != 6) { @@ -122,6 +131,9 @@ bool ICub::open(yarp::os::Searchable& config) { yInfo() << LogPrefix << "Starting to configure"; + // Default sensor status + pImpl->sensorStatus = "Error"; + // Configure clock if (!yarp::os::Time::isSystemClock()) { @@ -225,6 +237,11 @@ wearable::WearableName ICub::getWearableName() const return pImpl->wearableName; } +wearable::WearStatus ICub::getStatus() const +{ + return pImpl->icubSensorStatutsMap.at(pImpl->sensorStatus); +} + wearable::TimeStamp ICub::getTimeStamp() const { // Stamp count should be always zero From d5a31664ff9c4a66004114cd9947dfe21e889645 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 16:22:21 +0200 Subject: [PATCH 09/36] Fix sensor type --- devices/ICub/src/ICub.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 9b8f8f28..216e28b2 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -284,7 +284,7 @@ ICub::getSensors(const wearable::sensor::SensorType type) const { wearable::VectorOfSensorPtr outVec; switch (type) { - case sensor::SensorType::Force3DSensor: { + case sensor::SensorType::ForceTorque6DSensor: { outVec.reserve(pImpl->nSensors); for (const auto& ft6d : pImpl->ftSensorsMap) { outVec.push_back( From 42b21421a49078df144ba4cabc493fde4146b19d Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 16:23:47 +0200 Subject: [PATCH 10/36] Update wrapper details in config --- devices/ICub/CMakeLists.txt | 2 +- devices/ICub/conf/ICub.xml | 14 +++++++++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/devices/ICub/CMakeLists.txt b/devices/ICub/CMakeLists.txt index f4999640..dbddecf5 100644 --- a/devices/ICub/CMakeLists.txt +++ b/devices/ICub/CMakeLists.txt @@ -17,7 +17,7 @@ target_include_directories(ICub PRIVATE $) target_link_libraries(ICub PUBLIC - IWear YARP::YARP_dev) + Wearable::IWear YARP::YARP_dev) yarp_install( TARGETS ICub diff --git a/devices/ICub/conf/ICub.xml b/devices/ICub/conf/ICub.xml index 98ad5be0..d87f6fad 100644 --- a/devices/ICub/conf/ICub.xml +++ b/devices/ICub/conf/ICub.xml @@ -1,10 +1,22 @@ - + 2 /wholeBodyDynamics/left_arm/endEffectorWrench:o /wholeBodyDynamics/right_arm/endEffectorWrench:o + + + 0.01 + /ICub/WearableData/data:o + /ICub/WearableData/metadataRpc:o + + + ICubWearableDevice + + + + From 9ce2cbe2126bc99934bf0de428dc56b3a61143b9 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 5 Oct 2018 17:42:51 +0200 Subject: [PATCH 11/36] Update status handling --- devices/ICub/src/ICub.cpp | 48 +++++++++++++++++++++++++++------------ 1 file changed, 33 insertions(+), 15 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 216e28b2..0bc19506 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -42,13 +42,6 @@ class ICub::ICubImpl std::map> ftSensorsMap; - const std::map icubSensorStatutsMap{ - {"Ok", sensor::SensorStatus::Ok}, - {"Error", sensor::SensorStatus::Error} - }; - - std::string sensorStatus; - const WearableName wearableName = "ICub" + wearable::Separator; }; @@ -70,13 +63,15 @@ class ICub::ICubImpl::ICubForceTorque6DSensor ICubForceTorque6DSensor( ICub::ICubImpl* impl, const wearable::sensor::SensorName name = {}, - const wearable::sensor::SensorStatus status = wearable::sensor::SensorStatus::Unknown) + const wearable::sensor::SensorStatus status = wearable::sensor::SensorStatus::WaitingForFirstRead) : IForceTorque6DSensor(name, status) , icubImpl(impl) {} ~ICubForceTorque6DSensor() override = default; + void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } + // ============================== // IForceTorque6DSensor interface // ============================== @@ -89,15 +84,19 @@ class ICub::ICubImpl::ICubForceTorque6DSensor if (this->m_name == "leftWBDFTSensor") { icubImpl->leftHandFTPort.read(wrench); icubImpl->timeStamp.time = yarp::os::Time::now(); - icubImpl->sensorStatus = "Ok"; + + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); } else if(this->m_name == "rightWBDFTSensor") { icubImpl->rightHandFTPort.read(wrench); icubImpl->timeStamp.time = yarp::os::Time::now(); - icubImpl->sensorStatus = "Ok"; + + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); } - if(!wrench->isNull() && wrench->size() != 6) { + if(!wrench->isNull() && wrench->size() == 6) { force3D[0] = wrench->get(0).asDouble(); force3D[1] = wrench->get(1).asDouble(); force3D[2] = wrench->get(2).asDouble(); @@ -109,6 +108,10 @@ class ICub::ICubImpl::ICubForceTorque6DSensor else { force3D.fill(0.0); torque3D.fill(0.0); + + // Set the sensor status to Error + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Error); } return true; @@ -131,9 +134,6 @@ bool ICub::open(yarp::os::Searchable& config) { yInfo() << LogPrefix << "Starting to configure"; - // Default sensor status - pImpl->sensorStatus = "Error"; - // Configure clock if (!yarp::os::Time::isSystemClock()) { @@ -239,9 +239,27 @@ wearable::WearableName ICub::getWearableName() const wearable::WearStatus ICub::getStatus() const { - return pImpl->icubSensorStatutsMap.at(pImpl->sensorStatus); + wearable::WearStatus status = wearable::WearStatus::Ok; + + for (const auto& s : getAllSensors()) { + + if (s->getSensorStatus() != sensor::SensorStatus::Ok) { + if (s->getSensorStatus() != sensor::SensorStatus::WaitingForFirstRead) { + yError() << LogPrefix << "The status of" << s->getSensorName() << "is not Ok (" + << static_cast(s->getSensorStatus()) << ")"; + return status = wearable::WearStatus::Error; + } + else { + status = wearable::WearStatus::WaitingForFirstRead; + } + } + // TODO: improve handling of the overall status + } + + return status; } + wearable::TimeStamp ICub::getTimeStamp() const { // Stamp count should be always zero From b776173da8e66118e027fe1b963ebd2ddd7f78ce Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Mon, 8 Oct 2018 19:12:42 +0200 Subject: [PATCH 12/36] Implement bufferport callback --- devices/ICub/src/ICub.cpp | 101 +++++++++++++++++++++++++------------- 1 file changed, 66 insertions(+), 35 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 0bc19506..9e1bfcf6 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -30,8 +30,9 @@ class ICub::ICubImpl size_t nSensors; std::vector sensorNames; - yarp::os::BufferedPort leftHandFTPort; - yarp::os::BufferedPort rightHandFTPort; + + bool leftHandFTDataReceived = false; + bool rightHandFTDataReceived = false; template struct icubDeviceSensors @@ -43,6 +44,32 @@ class ICub::ICubImpl std::map> ftSensorsMap; const WearableName wearableName = "ICub" + wearable::Separator; + + class wrenchPort : public yarp::os::BufferedPort { + public: + yarp::os::Bottle wrenchValues; + bool firstDataReceived = false; + using yarp::os::BufferedPort::onRead; + virtual void onRead(yarp::os::Bottle& wrench) { + this->wrenchValues.copy(wrench); + + if(this->firstDataReceived != true) + { + this->firstDataReceived = true; + } + } + + yarp::os::Bottle getWrench() { + return this->wrenchValues; + } + + bool getFirstDataFlag() { + return this->firstDataReceived; + } + }; + + wrenchPort leftHandFTPort; + wrenchPort rightHandFTPort; }; // ========================================== @@ -51,9 +78,6 @@ class ICub::ICubImpl class ICub::ICubImpl::ICubForceTorque6DSensor : public wearable::sensor::IForceTorque6DSensor { -private: - yarp::os::Bottle* wrench; - public: ICub::ICubImpl* icubImpl = nullptr; @@ -63,7 +87,7 @@ class ICub::ICubImpl::ICubForceTorque6DSensor ICubForceTorque6DSensor( ICub::ICubImpl* impl, const wearable::sensor::SensorName name = {}, - const wearable::sensor::SensorStatus status = wearable::sensor::SensorStatus::WaitingForFirstRead) + const wearable::sensor::SensorStatus status = wearable::sensor::SensorStatus::WaitingForFirstRead) //Default sensor status : IForceTorque6DSensor(name, status) , icubImpl(impl) {} @@ -80,30 +104,43 @@ class ICub::ICubImpl::ICubForceTorque6DSensor return false; } + yarp::os::Bottle wrench; + // Reading wrench from WBD ports - if (this->m_name == "leftWBDFTSensor") { - icubImpl->leftHandFTPort.read(wrench); + if (this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { + icubImpl->leftHandFTPort.useCallback(); + wrench = icubImpl->leftHandFTPort.getWrench(); + icubImpl->timeStamp.time = yarp::os::Time::now(); - auto nonConstThis = const_cast(this); - nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); + // Set sensor status to Ok after checking first data flag + if (icubImpl->leftHandFTPort.firstDataReceived) { + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); + } } - else if(this->m_name == "rightWBDFTSensor") { - icubImpl->rightHandFTPort.read(wrench); + else if(this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { + icubImpl->rightHandFTPort.useCallback(); + wrench = icubImpl->rightHandFTPort.getWrench(); + icubImpl->timeStamp.time = yarp::os::Time::now(); - auto nonConstThis = const_cast(this); - nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); + // Set sensor status to Ok after checking first data flag + if (icubImpl->rightHandFTPort.firstDataReceived) { + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); + } } - if(!wrench->isNull() && wrench->size() == 6) { - force3D[0] = wrench->get(0).asDouble(); - force3D[1] = wrench->get(1).asDouble(); - force3D[2] = wrench->get(2).asDouble(); + //TODO: Check if the bottle is valid + if(wrench.size() == 6) { + force3D[0] = wrench.get(0).asDouble(); + force3D[1] = wrench.get(1).asDouble(); + force3D[2] = wrench.get(2).asDouble(); - torque3D[0] = wrench->get(3).asDouble(); - torque3D[1] = wrench->get(4).asDouble(); - torque3D[2] = wrench->get(5).asDouble(); + torque3D[0] = wrench.get(3).asDouble(); + torque3D[1] = wrench.get(4).asDouble(); + torque3D[2] = wrench.get(5).asDouble(); } else { force3D.fill(0.0); @@ -241,21 +278,15 @@ wearable::WearStatus ICub::getStatus() const { wearable::WearStatus status = wearable::WearStatus::Ok; - for (const auto& s : getAllSensors()) { - + /*for (const auto& s : getAllSensors()) { if (s->getSensorStatus() != sensor::SensorStatus::Ok) { - if (s->getSensorStatus() != sensor::SensorStatus::WaitingForFirstRead) { - yError() << LogPrefix << "The status of" << s->getSensorName() << "is not Ok (" - << static_cast(s->getSensorStatus()) << ")"; - return status = wearable::WearStatus::Error; - } - else { - status = wearable::WearStatus::WaitingForFirstRead; - } + yError() << LogPrefix << "The status of" << s->getSensorName() << "is not Ok (" + << static_cast(s->getSensorStatus()) << ")"; + status = wearable::WearStatus::Error; } - // TODO: improve handling of the overall status - } + }*/ + // Default return status is Ok return status; } @@ -312,8 +343,8 @@ ICub::getSensors(const wearable::sensor::SensorType type) const break; } default: { - yWarning() << LogPrefix << "Selected sensor type (" << static_cast(type) - << ") is not supported by ICub"; + //yWarning() << LogPrefix << "Selected sensor type (" << static_cast(type) + // << ") is not supported by ICub"; return {}; } } From c8a58eb1d59e5311084e86cb8944ac92a31076f4 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Tue, 9 Oct 2018 14:38:35 +0200 Subject: [PATCH 13/36] Improve status update logic --- devices/ICub/src/ICub.cpp | 81 ++++++++++++++++++++++++++++++--------- 1 file changed, 62 insertions(+), 19 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 9e1bfcf6..d1dc6e6e 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -51,10 +51,16 @@ class ICub::ICubImpl bool firstDataReceived = false; using yarp::os::BufferedPort::onRead; virtual void onRead(yarp::os::Bottle& wrench) { - this->wrenchValues.copy(wrench); - if(this->firstDataReceived != true) + //std::cout << "Received wrench of size : " << wrench.size() << std::endl; + //std::cout << "Wrench values : " << wrench.toString().c_str() << std::endl; + if (wrench.size() == 6) { + this->wrenchValues.copy(wrench); + } + + while (this->firstDataReceived != true) { + std::cout << "Set firstDataReceived flag : true" << std::endl; this->firstDataReceived = true; } } @@ -90,16 +96,53 @@ class ICub::ICubImpl::ICubForceTorque6DSensor const wearable::sensor::SensorStatus status = wearable::sensor::SensorStatus::WaitingForFirstRead) //Default sensor status : IForceTorque6DSensor(name, status) , icubImpl(impl) - {} + { + //Set the sensor status Ok after receiving the first data on the wrench ports + std::cout << "ICubForceTorque6DSensor constructor" << std::endl; + if (icubImpl->leftHandFTPort.getFirstDataFlag() || icubImpl->rightHandFTPort.getFirstDataFlag()) { + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); + std::cout << "ICubForceTorque6DSensor constructor setting sensor status flag" << std::endl; + } + + } ~ICubForceTorque6DSensor() override = default; - void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } + void setStatus(const wearable::sensor::SensorStatus status) { + //std::cout << "Status : " << status << std::endl; + m_status = status; + switch(m_status) + { + case sensor::SensorStatus::Error: + std::cout << "Sensor Status : Error" << std::endl; + break; + case sensor::SensorStatus::Ok: + //std::cout << "Sensor Status : Ok" << std::endl; + break; + case sensor::SensorStatus::Calibrating: + std::cout << "Sensor Status : Calibrating" << std::endl; + break; + case sensor::SensorStatus::Timeout: + std::cout << "Sensor Status : Timeout" << std::endl; + break; + case sensor::SensorStatus::Unknown: + std::cout << "Sensor Status : Unknown" << std::endl; + break; + case sensor::SensorStatus::WaitingForFirstRead: + std::cout << "Sensor Status : WaitingForFirstRead" << std::endl; + break; + default: + std::cout << "Sensor Status : Default case" << std::endl; + break; + } + } // ============================== // IForceTorque6DSensor interface // ============================== bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const override { + //std::cout << "Inside getForceTorque6D" << std::endl; if (!icubImpl) { return false; } @@ -108,32 +151,25 @@ class ICub::ICubImpl::ICubForceTorque6DSensor // Reading wrench from WBD ports if (this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { + + //std::cout << "Inside leftWBDFTSensor" << std::endl; icubImpl->leftHandFTPort.useCallback(); wrench = icubImpl->leftHandFTPort.getWrench(); icubImpl->timeStamp.time = yarp::os::Time::now(); - - // Set sensor status to Ok after checking first data flag - if (icubImpl->leftHandFTPort.firstDataReceived) { - auto nonConstThis = const_cast(this); - nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); - } } else if(this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { + + //std::cout << "Inside rightWBDFTSensor" << std::endl; icubImpl->rightHandFTPort.useCallback(); wrench = icubImpl->rightHandFTPort.getWrench(); icubImpl->timeStamp.time = yarp::os::Time::now(); - - // Set sensor status to Ok after checking first data flag - if (icubImpl->rightHandFTPort.firstDataReceived) { - auto nonConstThis = const_cast(this); - nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); - } } //TODO: Check if the bottle is valid if(wrench.size() == 6) { + //std::cout << "Correct wrench data received" << std::endl; force3D[0] = wrench.get(0).asDouble(); force3D[1] = wrench.get(1).asDouble(); force3D[2] = wrench.get(2).asDouble(); @@ -143,10 +179,11 @@ class ICub::ICubImpl::ICubForceTorque6DSensor torque3D[2] = wrench.get(5).asDouble(); } else { + //std::cout << "Wrong wrench data received" << std::endl; force3D.fill(0.0); torque3D.fill(0.0); - // Set the sensor status to Error + // Set sensor status to error if wrong data is read auto nonConstThis = const_cast(this); nonConstThis->setStatus(wearable::sensor::SensorStatus::Error); } @@ -215,6 +252,9 @@ bool ICub::open(yarp::os::Searchable& config) return false; } else { + while (!pImpl->leftHandFTPort.getFirstDataFlag()) { + pImpl->leftHandFTPort.useCallback(); + } pImpl->sensorNames.push_back("leftWBDFTSensor"); } } @@ -235,6 +275,9 @@ bool ICub::open(yarp::os::Searchable& config) return false; } else { + while (!pImpl->rightHandFTPort.getFirstDataFlag()) { + pImpl->rightHandFTPort.useCallback(); + } pImpl->sensorNames.push_back("rightWBDFTSensor"); } } @@ -278,13 +321,13 @@ wearable::WearStatus ICub::getStatus() const { wearable::WearStatus status = wearable::WearStatus::Ok; - /*for (const auto& s : getAllSensors()) { + for (const auto& s : getAllSensors()) { if (s->getSensorStatus() != sensor::SensorStatus::Ok) { yError() << LogPrefix << "The status of" << s->getSensorName() << "is not Ok (" << static_cast(s->getSensorStatus()) << ")"; status = wearable::WearStatus::Error; } - }*/ + } // Default return status is Ok return status; From 6de5a1c082a064a85d54b6b32d5a2138760008c7 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Tue, 9 Oct 2018 18:19:36 +0200 Subject: [PATCH 14/36] Remode debug code --- devices/ICub/src/ICub.cpp | 42 +-------------------------------------- 1 file changed, 1 insertion(+), 41 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index d1dc6e6e..f66300b8 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -51,16 +51,12 @@ class ICub::ICubImpl bool firstDataReceived = false; using yarp::os::BufferedPort::onRead; virtual void onRead(yarp::os::Bottle& wrench) { - - //std::cout << "Received wrench of size : " << wrench.size() << std::endl; - //std::cout << "Wrench values : " << wrench.toString().c_str() << std::endl; if (wrench.size() == 6) { this->wrenchValues.copy(wrench); } while (this->firstDataReceived != true) { - std::cout << "Set firstDataReceived flag : true" << std::endl; this->firstDataReceived = true; } } @@ -98,51 +94,21 @@ class ICub::ICubImpl::ICubForceTorque6DSensor , icubImpl(impl) { //Set the sensor status Ok after receiving the first data on the wrench ports - std::cout << "ICubForceTorque6DSensor constructor" << std::endl; if (icubImpl->leftHandFTPort.getFirstDataFlag() || icubImpl->rightHandFTPort.getFirstDataFlag()) { auto nonConstThis = const_cast(this); nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); - std::cout << "ICubForceTorque6DSensor constructor setting sensor status flag" << std::endl; } } ~ICubForceTorque6DSensor() override = default; - void setStatus(const wearable::sensor::SensorStatus status) { - //std::cout << "Status : " << status << std::endl; - m_status = status; - switch(m_status) - { - case sensor::SensorStatus::Error: - std::cout << "Sensor Status : Error" << std::endl; - break; - case sensor::SensorStatus::Ok: - //std::cout << "Sensor Status : Ok" << std::endl; - break; - case sensor::SensorStatus::Calibrating: - std::cout << "Sensor Status : Calibrating" << std::endl; - break; - case sensor::SensorStatus::Timeout: - std::cout << "Sensor Status : Timeout" << std::endl; - break; - case sensor::SensorStatus::Unknown: - std::cout << "Sensor Status : Unknown" << std::endl; - break; - case sensor::SensorStatus::WaitingForFirstRead: - std::cout << "Sensor Status : WaitingForFirstRead" << std::endl; - break; - default: - std::cout << "Sensor Status : Default case" << std::endl; - break; - } - } + void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } // ============================== // IForceTorque6DSensor interface // ============================== bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const override { - //std::cout << "Inside getForceTorque6D" << std::endl; if (!icubImpl) { return false; } @@ -151,16 +117,12 @@ class ICub::ICubImpl::ICubForceTorque6DSensor // Reading wrench from WBD ports if (this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { - - //std::cout << "Inside leftWBDFTSensor" << std::endl; icubImpl->leftHandFTPort.useCallback(); wrench = icubImpl->leftHandFTPort.getWrench(); icubImpl->timeStamp.time = yarp::os::Time::now(); } else if(this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { - - //std::cout << "Inside rightWBDFTSensor" << std::endl; icubImpl->rightHandFTPort.useCallback(); wrench = icubImpl->rightHandFTPort.getWrench(); @@ -169,7 +131,6 @@ class ICub::ICubImpl::ICubForceTorque6DSensor //TODO: Check if the bottle is valid if(wrench.size() == 6) { - //std::cout << "Correct wrench data received" << std::endl; force3D[0] = wrench.get(0).asDouble(); force3D[1] = wrench.get(1).asDouble(); force3D[2] = wrench.get(2).asDouble(); @@ -179,7 +140,6 @@ class ICub::ICubImpl::ICubForceTorque6DSensor torque3D[2] = wrench.get(5).asDouble(); } else { - //std::cout << "Wrong wrench data received" << std::endl; force3D.fill(0.0); torque3D.fill(0.0); From 15f975e55fe3e1c1d45dcc5c94417319cf0c9f7f Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Wed, 10 Oct 2018 14:50:39 +0200 Subject: [PATCH 15/36] Update wrench handling --- devices/ICub/src/ICub.cpp | 35 +++++++++++++++++++++-------------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index f66300b8..7d918082 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -47,12 +47,18 @@ class ICub::ICubImpl class wrenchPort : public yarp::os::BufferedPort { public: - yarp::os::Bottle wrenchValues; + std::vector wrenchValues; bool firstDataReceived = false; using yarp::os::BufferedPort::onRead; virtual void onRead(yarp::os::Bottle& wrench) { - if (wrench.size() == 6) { - this->wrenchValues.copy(wrench); + if (!wrench.isNull()) { + wrenchValues.clear(); // Clear the vector of previous values + for (size_t i = 0; i < wrench.size(); ++i) { + this->wrenchValues.push_back(wrench.get(i).asDouble()); + } + } + else { + yWarning() << LogPrefix << "[wrenchPort] read an invalid wrench bottle"; } while (this->firstDataReceived != true) @@ -61,7 +67,7 @@ class ICub::ICubImpl } } - yarp::os::Bottle getWrench() { + std::vector getWrench() { return this->wrenchValues; } @@ -113,7 +119,7 @@ class ICub::ICubImpl::ICubForceTorque6DSensor return false; } - yarp::os::Bottle wrench; + std::vector wrench; // Reading wrench from WBD ports if (this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { @@ -129,23 +135,24 @@ class ICub::ICubImpl::ICubForceTorque6DSensor icubImpl->timeStamp.time = yarp::os::Time::now(); } - //TODO: Check if the bottle is valid + // Check the size of wrench values if(wrench.size() == 6) { - force3D[0] = wrench.get(0).asDouble(); - force3D[1] = wrench.get(1).asDouble(); - force3D[2] = wrench.get(2).asDouble(); + force3D[0] = wrench[0]; + force3D[1] = wrench[1]; + force3D[2] = wrench[2]; - torque3D[0] = wrench.get(3).asDouble(); - torque3D[1] = wrench.get(4).asDouble(); - torque3D[2] = wrench.get(5).asDouble(); + torque3D[0] = wrench[3]; + torque3D[1] = wrench[4]; + torque3D[2] = wrench[5]; } else { + yWarning() << LogPrefix << "Size of wrench read is wrong. Setting zero values instead."; force3D.fill(0.0); torque3D.fill(0.0); // Set sensor status to error if wrong data is read - auto nonConstThis = const_cast(this); - nonConstThis->setStatus(wearable::sensor::SensorStatus::Error); + //auto nonConstThis = const_cast(this); + //nonConstThis->setStatus(wearable::sensor::SensorStatus::Error); } return true; From 310766a5de08294dda64829d52709166f85ee25f Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Tue, 16 Oct 2018 17:27:23 +0200 Subject: [PATCH 16/36] Add mutex to wrench handling --- devices/ICub/src/ICub.cpp | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 7d918082..b9bc5737 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -49,12 +49,20 @@ class ICub::ICubImpl public: std::vector wrenchValues; bool firstDataReceived = false; + std::mutex mtx; + using yarp::os::BufferedPort::onRead; virtual void onRead(yarp::os::Bottle& wrench) { if (!wrench.isNull()) { - wrenchValues.clear(); // Clear the vector of previous values - for (size_t i = 0; i < wrench.size(); ++i) { - this->wrenchValues.push_back(wrench.get(i).asDouble()); + std::lock_guard lock (mtx); + + this->wrenchValues.clear(); // Clear the vector of previous values + + if (this->wrenchValues.empty()) { + this->wrenchValues.resize(wrench.size()); + for (size_t i = 0; i < wrench.size(); ++i) { + this->wrenchValues.at(i) = wrench.get(i).asDouble(); + } } } else { @@ -68,6 +76,7 @@ class ICub::ICubImpl } std::vector getWrench() { + std::lock_guard lck (mtx); return this->wrenchValues; } @@ -135,15 +144,16 @@ class ICub::ICubImpl::ICubForceTorque6DSensor icubImpl->timeStamp.time = yarp::os::Time::now(); } + // Check the size of wrench values if(wrench.size() == 6) { - force3D[0] = wrench[0]; - force3D[1] = wrench[1]; - force3D[2] = wrench[2]; + force3D[0] = wrench.at(0); + force3D[1] = wrench.at(1); + force3D[2] = wrench.at(2); - torque3D[0] = wrench[3]; - torque3D[1] = wrench[4]; - torque3D[2] = wrench[5]; + torque3D[0] = wrench.at(3); + torque3D[1] = wrench.at(4); + torque3D[2] = wrench.at(5); } else { yWarning() << LogPrefix << "Size of wrench read is wrong. Setting zero values instead."; From 0b78858b4b2874766f1faf34a84647734bd23232 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 23 Nov 2018 14:08:50 +0100 Subject: [PATCH 17/36] Add IAnalogToIWear device config files for FTShoes --- .../ftshoesleft_ianalogsensor_to_iwear.xml | 147 ++++++++++++++++++ .../ftshoesright_ianalogsensor_to_iwear.xml | 147 ++++++++++++++++++ 2 files changed, 294 insertions(+) create mode 100644 conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml create mode 100644 conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml diff --git a/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml b/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml new file mode 100644 index 00000000..6110916b --- /dev/null +++ b/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml @@ -0,0 +1,147 @@ + + + + + + "ecan" + "ecan" + 0 + 0x01 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x02 + 16 + 6 + 5 + 1 + 1 + + + + + 10 + /ft/ftShoe_Left/analog:o + + + + (-0.181101, 0.0, 0.0) + + + ( -1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, 1.0 + ) + + + + + + + ( + -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 + ) + + + false + + + ( + 0.938857303261460 -0.099987001538724 -0.031719474716931 0.934262757200198 -2.081128504120452 1.116222891312317 + -0.008782994221006 0.755393650114154 0.045656043724651 -1.655858713166938 -0.627809636881184 -1.815251879665972 + -0.013675176859862 0.062209239624336 0.991900304766940 -0.285473412550362 0.672237690815685 0.094924727111362 + -0.003522241315859 0.031293495376460 0.005788286130503 0.909299295313223 -0.034622536733099 -0.026646251320573 + -0.029397178956236 0.014719174094312 0.001425795437255 -0.055596915544064 1.100253681658064 0.017844947080158 + -0.003939180755661 0.010569279599475 0.000530902451101 -0.026442188019439 0.021906211171418 1.087258387321892 + ) + + ( + 0.826424294040360 -0.091357422730653 0.029217798555402 0.413143077503409 -3.003143284075124 -0.734326709982322 + -0.043527467867752 0.945992379875415 -0.013974374847989 -2.158570031731984 -0.364385509664276 0.178660582281101 + 0.018456480855675 -0.027680967629762 0.995373889642780 -0.378801203422740 0.405083974855969 -0.399854054343818 + -0.001858452062923 0.028548921877088 -0.001296050687382 0.882390054944027 -0.021267049818161 0.012534000056372 + -0.024912614354884 -0.002610851373780 -0.001454598169520 -0.013007004521544 1.198536139399159 0.056633344983102 + -0.001526052208884 -0.008395096178476 -0.000888111548024 0.027328873291096 -0.022322328373427 0.963011521138837 + ) + + + + + ftShoe_Left_Front + ftShoe_Left_Rear + + + + + + + + + 10 + /ft/ftShoe_Left/analog:o + + + ftShoe_l + + + + + + + + 10 + /ft/ftShoe_Left_Front/analog:o + + + ftShoe_Left_Front + + + + + + 10 + /ft/ftShoe_Left_Rear/analog:o + + + ftShoe_Left_Rear + + + + + + + FTShoeLeftFTSensors + FTShoeLeft + 6 + 0 + ForceTorque6DSensor + + + ftShoe_l + + + + + + + 0.01 + /FTShoeLeft/IWearWrapper/data:o + /FTShoeLeft/IWearWrapper/metadata:o + + + FTShoeLeftIAnalogSensorToIWear + + + + + + diff --git a/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml b/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml new file mode 100644 index 00000000..5cd90be7 --- /dev/null +++ b/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml @@ -0,0 +1,147 @@ + + + + + + "ecan" + "ecan" + 0 + 0x03 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x04 + 16 + 6 + 5 + 1 + 1 + + + + + 10 + /ft/ftShoe_Right/analog:o + + + + (-0.181101, 0.0, 0.0) + + + ( -1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, 1.0 + ) + + + + + + + ( + -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 + ) + + + false + + + ( + 0.927495744506558 0.041016654619440 -0.020768066468872 -0.686739129220131 -1.700341714921413 -0.655844186466448 + 0.032691498454271 0.905387122196879 -0.062060254943741 -2.528433638522609 1.936698940397620 -1.261426627647161 + 0.030307518024746 -0.042708402432872 0.990834772663811 -0.771255812731485 1.379976452711553 1.338383628433477 + 0.003417328018857 0.027192538668070 0.000903874352365 0.934072112166630 0.018298809861597 0.009727347237814 + -0.029683885285561 -0.009657076123040 -0.001319794460849 0.070540847374860 1.112394249787454 -0.081039701093926 + -0.003720738063755 0.002403350518587 -0.001256203878104 -0.004909951055368 0.004652419860706 1.023620297977032 + ) + + ( + 0.752606509808956 0.128485214807810 0.037816833951859 -1.189506447082422 -3.528696946955530 0.367956429295563 + 0.032994788492599 0.947861991843522 0.014131047867024 -2.503874602191483 -0.306564401641517 0.033900728916422 + 0.010291417081055 -0.016786848400983 0.997235043288604 0.044337750405427 0.628784109489714 0.608479361767708 + -0.000534324857619 0.039993343127271 -0.000046443326033 0.846610774589901 -0.114938934447660 -0.164828223499561 + -0.021558854024374 0.008200634539163 -0.003084470512841 0.036116559584761 1.220596115437195 -0.067513275649460 + -0.000434930362439 0.001589856391163 -0.000514737521434 -0.009663262776501 0.025349107573698 1.107985249217049 + ) + + + + + ftShoe_Right_Front + ftShoe_Right_Rear + + + + + + + + + 10 + /ft/ftShoe_Right/analog:o + + + ftShoe_r + + + + + + + + 10 + /ft/ftShoe_Right_Front/analog:o + + + ftShoe_Right_Front + + + + + + 10 + /ft/ftShoe_Right_Rear/analog:o + + + ftShoe_Right_Rear + + + + + + + FTShoeRightFTSensors + FTShoeRight + 6 + 0 + ForceTorque6DSensor + + + ftShoe_r + + + + + + + 0.01 + /FTShoeRight/IWearWrapper/data:o + /FTShoeRight/IWearWrapper/metadata:o + + + FTShoeRightIAnalogSensorToIWear + + + + + + From b29785e909704e8bcb143aa329553978a0f3be8e Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 23 Nov 2018 14:59:13 +0100 Subject: [PATCH 18/36] Update IWearWrapper port names --- conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml | 4 ++-- conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml b/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml index 6110916b..e00fd256 100644 --- a/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml +++ b/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml @@ -134,8 +134,8 @@ 0.01 - /FTShoeLeft/IWearWrapper/data:o - /FTShoeLeft/IWearWrapper/metadata:o + /FTShoeLeft/WearableData/data:o + /FTShoeLeft/WearableData/metadata:o FTShoeLeftIAnalogSensorToIWear diff --git a/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml b/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml index 5cd90be7..c154e1d5 100644 --- a/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml +++ b/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml @@ -134,8 +134,8 @@ 0.01 - /FTShoeRight/IWearWrapper/data:o - /FTShoeRight/IWearWrapper/metadata:o + /FTShoeRight/WearableData/data:o + /FTShoeRight/WearableData/metadata:o FTShoeRightIAnalogSensorToIWear From ffcf276cb618d4aa14179702c82fe1f9ca1e82c5 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 23 Nov 2018 15:10:56 +0100 Subject: [PATCH 19/36] Update IWearWrapper rpcPortName --- conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml | 2 +- conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml b/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml index e00fd256..c358c285 100644 --- a/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml +++ b/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml @@ -135,7 +135,7 @@ 0.01 /FTShoeLeft/WearableData/data:o - /FTShoeLeft/WearableData/metadata:o + /FTShoeLeft/WearableData/metadataRpc:o FTShoeLeftIAnalogSensorToIWear diff --git a/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml b/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml index c154e1d5..109b2627 100644 --- a/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml +++ b/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml @@ -135,7 +135,7 @@ 0.01 /FTShoeRight/WearableData/data:o - /FTShoeRight/WearableData/metadata:o + /FTShoeRight/WearableData/metadataRpc:o FTShoeRightIAnalogSensorToIWear From f7035a62323aef46a2a232aa863a3c2f263236d0 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Tue, 4 Dec 2018 14:03:53 +0100 Subject: [PATCH 20/36] Add ICub config file --- conf/ICub/ICub.xml | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 conf/ICub/ICub.xml diff --git a/conf/ICub/ICub.xml b/conf/ICub/ICub.xml new file mode 100644 index 00000000..d87f6fad --- /dev/null +++ b/conf/ICub/ICub.xml @@ -0,0 +1,22 @@ + + + + + 2 + /wholeBodyDynamics/left_arm/endEffectorWrench:o + /wholeBodyDynamics/right_arm/endEffectorWrench:o + + + + + 0.01 + /ICub/WearableData/data:o + /ICub/WearableData/metadataRpc:o + + + ICubWearableDevice + + + + + From 1c243c8f87af8980e10a0036463e7d8c19841c3e Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 4 Jan 2019 00:59:51 +0100 Subject: [PATCH 21/36] Update PR changes --- devices/ICub/src/ICub.cpp | 106 ++++++++++++++++++-------------------- 1 file changed, 51 insertions(+), 55 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index b9bc5737..ed01e6cd 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -12,6 +12,7 @@ #include #include +#include #include #include #include @@ -21,6 +22,44 @@ using namespace wearable::devices; const std::string LogPrefix = "ICub :"; +class WrenchPort : public yarp::os::BufferedPort { +public: + std::vector wrenchValues; + bool firstRun = true; + mutable std::mutex mtx; + + using yarp::os::BufferedPort::onRead; + virtual void onRead(yarp::os::Bottle& wrench) { + if (!wrench.isNull()) { + std::lock_guard lock (mtx); + + this->wrenchValues.clear(); // Clear the vector of previous values + + this->wrenchValues.resize(wrench.size()); + for (size_t i = 0; i < wrench.size(); ++i) { + this->wrenchValues.at(i) = wrench.get(i).asDouble(); + } + + if (this->firstRun) + { + this->firstRun = false; + } + } + else { + yWarning() << LogPrefix << "[WrenchPort] read an invalid wrench bottle"; + } + } + + std::vector wrench() { + std::lock_guard lck (mtx); + return this->wrenchValues; + } + + bool firstRunFlag() { + return this->firstRun; + } +}; + class ICub::ICubImpl { public: @@ -35,58 +74,17 @@ class ICub::ICubImpl bool rightHandFTDataReceived = false; template - struct icubDeviceSensors + struct ICubDeviceSensors { std::shared_ptr icubSensor; - size_t index; }; - std::map> ftSensorsMap; + std::map> ftSensorsMap; const WearableName wearableName = "ICub" + wearable::Separator; - class wrenchPort : public yarp::os::BufferedPort { - public: - std::vector wrenchValues; - bool firstDataReceived = false; - std::mutex mtx; - - using yarp::os::BufferedPort::onRead; - virtual void onRead(yarp::os::Bottle& wrench) { - if (!wrench.isNull()) { - std::lock_guard lock (mtx); - - this->wrenchValues.clear(); // Clear the vector of previous values - - if (this->wrenchValues.empty()) { - this->wrenchValues.resize(wrench.size()); - for (size_t i = 0; i < wrench.size(); ++i) { - this->wrenchValues.at(i) = wrench.get(i).asDouble(); - } - } - } - else { - yWarning() << LogPrefix << "[wrenchPort] read an invalid wrench bottle"; - } - - while (this->firstDataReceived != true) - { - this->firstDataReceived = true; - } - } - - std::vector getWrench() { - std::lock_guard lck (mtx); - return this->wrenchValues; - } - - bool getFirstDataFlag() { - return this->firstDataReceived; - } - }; - - wrenchPort leftHandFTPort; - wrenchPort rightHandFTPort; + WrenchPort leftHandFTPort; + WrenchPort rightHandFTPort; }; // ========================================== @@ -109,7 +107,7 @@ class ICub::ICubImpl::ICubForceTorque6DSensor , icubImpl(impl) { //Set the sensor status Ok after receiving the first data on the wrench ports - if (icubImpl->leftHandFTPort.getFirstDataFlag() || icubImpl->rightHandFTPort.getFirstDataFlag()) { + if (!icubImpl->leftHandFTPort.firstRunFlag() || !icubImpl->rightHandFTPort.firstRunFlag()) { auto nonConstThis = const_cast(this); nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); } @@ -124,22 +122,20 @@ class ICub::ICubImpl::ICubForceTorque6DSensor // IForceTorque6DSensor interface // ============================== bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const override { - if (!icubImpl) { - return false; - } + assert(icubImpl != nullptr); std::vector wrench; // Reading wrench from WBD ports if (this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { icubImpl->leftHandFTPort.useCallback(); - wrench = icubImpl->leftHandFTPort.getWrench(); + wrench = icubImpl->leftHandFTPort.wrench(); icubImpl->timeStamp.time = yarp::os::Time::now(); } else if(this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { icubImpl->rightHandFTPort.useCallback(); - wrench = icubImpl->rightHandFTPort.getWrench(); + wrench = icubImpl->rightHandFTPort.wrench(); icubImpl->timeStamp.time = yarp::os::Time::now(); } @@ -229,7 +225,7 @@ bool ICub::open(yarp::os::Searchable& config) return false; } else { - while (!pImpl->leftHandFTPort.getFirstDataFlag()) { + while (pImpl->leftHandFTPort.firstRunFlag()) { pImpl->leftHandFTPort.useCallback(); } pImpl->sensorNames.push_back("leftWBDFTSensor"); @@ -252,7 +248,7 @@ bool ICub::open(yarp::os::Searchable& config) return false; } else { - while (!pImpl->rightHandFTPort.getFirstDataFlag()) { + while (pImpl->rightHandFTPort.firstRunFlag()) { pImpl->rightHandFTPort.useCallback(); } pImpl->sensorNames.push_back("rightWBDFTSensor"); @@ -279,7 +275,7 @@ bool ICub::open(yarp::os::Searchable& config) pImpl->ftSensorsMap.emplace( ft6dPrefix + pImpl->sensorNames[s], - ICubImpl::icubDeviceSensors{ft6d,s}); + ICubImpl::ICubDeviceSensors{ft6d}); } return true; @@ -376,7 +372,7 @@ wearable::SensorPtr ICub::getForceTorque6DSensor(const wearable::sensor::SensorName name) const { // Check if user-provided name corresponds to an available sensor - if (pImpl->ftSensorsMap.find(static_cast(name)) + if (pImpl->ftSensorsMap.find(name) == pImpl->ftSensorsMap.end()) { yError() << LogPrefix << "Invalid sensor name"; return nullptr; From c58e258d7f8a3c2c6c7d22dfd3c80ce24cd04539 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 4 Jan 2019 01:39:18 +0100 Subject: [PATCH 22/36] Cleanup open configuration --- devices/ICub/src/ICub.cpp | 143 +++++++++++++++++++------------------- 1 file changed, 72 insertions(+), 71 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index ed01e6cd..2e5d6a53 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -187,85 +187,86 @@ bool ICub::open(yarp::os::Searchable& config) yarp::os::Time::useSystemClock(); } - yarp::os::Bottle wbdHandsFTSensorsSet = config.findGroup("wbd-hand-ft-sensors"); - if(wbdHandsFTSensorsSet.isNull()) { + // =============================== + // CHECK THE CONFIGURATION OPTIONS + // =============================== + + yarp::os::Bottle wbdHandsFTSensorsGroup = config.findGroup("wbd-hand-ft-sensors"); + if(wbdHandsFTSensorsGroup.isNull()) { yError() << LogPrefix << "REQUIRED parameter NOT found"; return false; } - else { - - if (!wbdHandsFTSensorsSet.check("nSensors")) { - yError() << LogPrefix << "REQUIRED parameter NOT found"; - return false; - } - else { - pImpl->nSensors = wbdHandsFTSensorsSet.check("nSensors",yarp::os::Value(false)).asInt(); - - if (!wbdHandsFTSensorsSet.check("leftHand") || !wbdHandsFTSensorsSet.check("rightHand")) { - yError() << LogPrefix << "REQUIRED parameter or NOT found"; - return false; - } - else { - - // Check YARP network - if(!yarp::os::Network::initialized()) - { - yarp::os::Network::init(); - } - - // Connect to wbd left hand ft port - std::string leftHandFTPortName = wbdHandsFTSensorsSet.check("leftHand",yarp::os::Value(false)).asString(); - - if(pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i")) - { - if(!yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str())) - { - yError() << LogPrefix << "Failed to connect " << leftHandFTPortName << " and " << pImpl->leftHandFTPort.getName().c_str(); - return false; - } - else { - while (pImpl->leftHandFTPort.firstRunFlag()) { - pImpl->leftHandFTPort.useCallback(); - } - pImpl->sensorNames.push_back("leftWBDFTSensor"); - } - } - else - { - yError() << LogPrefix << "Failed to open " << pImpl->leftHandFTPort.getName().c_str(); - return false; - } - - // Connect to wbd right hand ft port - std::string rightHandFTPortName = wbdHandsFTSensorsSet.check("rightHand",yarp::os::Value(false)).asString(); - - if(pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i")) - { - if(!yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str())) - { - yError() << LogPrefix << "Failed to connect " << rightHandFTPortName << " and " << pImpl->rightHandFTPort.getName().c_str(); - return false; - } - else { - while (pImpl->rightHandFTPort.firstRunFlag()) { - pImpl->rightHandFTPort.useCallback(); - } - pImpl->sensorNames.push_back("rightWBDFTSensor"); - } - } - else - { - yError() << LogPrefix << "Failed to open " << pImpl->rightHandFTPort.getName().c_str(); - return false; - } - - } + if (!wbdHandsFTSensorsGroup.check("nSensors")) { + yError() << LogPrefix << "REQUIRED parameter NOT found"; + return false; + } - } + if (!wbdHandsFTSensorsGroup.check("leftHand") || !wbdHandsFTSensorsGroup.check("rightHand")) { + yError() << LogPrefix << "REQUIRED parameter or NOT found"; + return false; + } + // Check YARP network + if(!yarp::os::Network::initialized()) + { + yarp::os::Network::init(); } + // =============================== + // PARSE THE CONFIGURATION OPTIONS + // =============================== + + pImpl->nSensors = wbdHandsFTSensorsGroup.check("nSensors",yarp::os::Value(false)).asInt(); + std::string leftHandFTPortName = wbdHandsFTSensorsGroup.check("leftHand",yarp::os::Value(false)).asString(); + std::string rightHandFTPortName = wbdHandsFTSensorsGroup.check("rightHand",yarp::os::Value(false)).asString(); + + // =========================== + // OPEN AND CONNECT YARP PORTS + // =========================== + + // Connect to wbd left hand ft port + if(pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i")) + { + if(!yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str())) + { + yError() << LogPrefix << "Failed to connect " << leftHandFTPortName << " and " << pImpl->leftHandFTPort.getName().c_str(); + return false; + } + else { + while (pImpl->leftHandFTPort.firstRunFlag()) { + pImpl->leftHandFTPort.useCallback(); + } + pImpl->sensorNames.push_back("leftWBDFTSensor"); + } + } + else + { + yError() << LogPrefix << "Failed to open " << pImpl->leftHandFTPort.getName().c_str(); + return false; + } + + // Connect to wbd right hand ft port + if(pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i")) + { + if(!yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str())) + { + yError() << LogPrefix << "Failed to connect " << rightHandFTPortName << " and " << pImpl->rightHandFTPort.getName().c_str(); + return false; + } + else { + while (pImpl->rightHandFTPort.firstRunFlag()) { + pImpl->rightHandFTPort.useCallback(); + } + pImpl->sensorNames.push_back("rightWBDFTSensor"); + } + } + else + { + yError() << LogPrefix << "Failed to open " << pImpl->rightHandFTPort.getName().c_str(); + return false; + } + std::string ft6dPrefix = getWearableName() + sensor::IForceTorque6DSensor::getPrefix(); for (size_t s = 0; s < pImpl->sensorNames.size(); ++s) { From b051044e3bece5bdc5c9c27035ef85499a8a9c60 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 4 Jan 2019 23:49:18 +0100 Subject: [PATCH 23/36] Update port connection --- devices/ICub/src/ICub.cpp | 52 +++++++++++++-------------------------- 1 file changed, 17 insertions(+), 35 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 2e5d6a53..da0bb86c 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -225,48 +225,30 @@ bool ICub::open(yarp::os::Searchable& config) // OPEN AND CONNECT YARP PORTS // =========================== - // Connect to wbd left hand ft port - if(pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i")) - { - if(!yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str())) - { - yError() << LogPrefix << "Failed to connect " << leftHandFTPortName << " and " << pImpl->leftHandFTPort.getName().c_str(); - return false; - } - else { - while (pImpl->leftHandFTPort.firstRunFlag()) { - pImpl->leftHandFTPort.useCallback(); - } - pImpl->sensorNames.push_back("leftWBDFTSensor"); - } - } - else - { - yError() << LogPrefix << "Failed to open " << pImpl->leftHandFTPort.getName().c_str(); + if (! pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i") || + !yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str())) { + yError() << LogPrefix << "Failed to open or connect to " << pImpl->leftHandFTPort.getName().c_str(); return false; } - // Connect to wbd right hand ft port - if(pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i")) - { - if(!yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str())) - { - yError() << LogPrefix << "Failed to connect " << rightHandFTPortName << " and " << pImpl->rightHandFTPort.getName().c_str(); - return false; - } - else { - while (pImpl->rightHandFTPort.firstRunFlag()) { - pImpl->rightHandFTPort.useCallback(); - } - pImpl->sensorNames.push_back("rightWBDFTSensor"); - } + while (pImpl->leftHandFTPort.firstRunFlag()) { + pImpl->leftHandFTPort.useCallback(); } - else - { - yError() << LogPrefix << "Failed to open " << pImpl->rightHandFTPort.getName().c_str(); + + pImpl->sensorNames.push_back("leftWBDFTSensor"); + + if (!pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i") || + !yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str())) { + yError() << LogPrefix << "Failed to open or connect to " << pImpl->rightHandFTPort.getName().c_str(); return false; } + while (pImpl->rightHandFTPort.firstRunFlag()) { + pImpl->rightHandFTPort.useCallback(); + } + + pImpl->sensorNames.push_back("rightWBDFTSensor"); + std::string ft6dPrefix = getWearableName() + sensor::IForceTorque6DSensor::getPrefix(); for (size_t s = 0; s < pImpl->sensorNames.size(); ++s) { From b4d49b765f87c36ed992bd1a715013e684049cd5 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Fri, 4 Jan 2019 23:52:43 +0100 Subject: [PATCH 24/36] Update separator usage --- devices/ICub/src/ICub.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index da0bb86c..fd818ad9 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -81,7 +81,7 @@ class ICub::ICubImpl std::map> ftSensorsMap; - const WearableName wearableName = "ICub" + wearable::Separator; + const WearableName wearableName = "ICub"; WrenchPort leftHandFTPort; WrenchPort rightHandFTPort; @@ -127,13 +127,13 @@ class ICub::ICubImpl::ICubForceTorque6DSensor std::vector wrench; // Reading wrench from WBD ports - if (this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { + if (this->m_name == icubImpl->wearableName + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { icubImpl->leftHandFTPort.useCallback(); wrench = icubImpl->leftHandFTPort.wrench(); icubImpl->timeStamp.time = yarp::os::Time::now(); } - else if(this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { + else if(this->m_name == icubImpl->wearableName + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { icubImpl->rightHandFTPort.useCallback(); wrench = icubImpl->rightHandFTPort.wrench(); @@ -249,7 +249,7 @@ bool ICub::open(yarp::os::Searchable& config) pImpl->sensorNames.push_back("rightWBDFTSensor"); - std::string ft6dPrefix = getWearableName() + sensor::IForceTorque6DSensor::getPrefix(); + std::string ft6dPrefix = getWearableName() + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix(); for (size_t s = 0; s < pImpl->sensorNames.size(); ++s) { // Create the new sensors From 2a5e4fbcf77af092186116dffb229fcf9b32ec17 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 15 Jan 2019 14:36:56 +0100 Subject: [PATCH 25/36] remove Yarp Init check in ICub --- devices/ICub/src/ICub.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index fd818ad9..cb9105ec 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -207,12 +207,6 @@ bool ICub::open(yarp::os::Searchable& config) return false; } - // Check YARP network - if(!yarp::os::Network::initialized()) - { - yarp::os::Network::init(); - } - // =============================== // PARSE THE CONFIGURATION OPTIONS // =============================== From d4153a27bf28a6aae9ddc2ce62f557f9a613ad21 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Wed, 16 Jan 2019 15:31:47 +0100 Subject: [PATCH 26/36] Add ground reaction FT flag handling --- .../ftshoesleft_ianalogsensor_to_iwear.xml | 1 + .../ftshoesright_ianalogsensor_to_iwear.xml | 1 + .../src/IAnalogSensorToIWear.cpp | 23 ++++++++++++++++++- 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml b/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml index c358c285..f78ecc8a 100644 --- a/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml +++ b/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml @@ -124,6 +124,7 @@ 6 0 ForceTorque6DSensor + true ftShoe_l diff --git a/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml b/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml index 109b2627..aab0a1c3 100644 --- a/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml +++ b/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml @@ -124,6 +124,7 @@ 6 0 ForceTorque6DSensor + true ftShoe_r diff --git a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp index 015d9a07..29d47076 100644 --- a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp +++ b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp @@ -130,6 +130,7 @@ class ForceTorque6DSensor : public IForceTorque6DSensor { public: unsigned offset = 0; + bool groundReactionFT; ForceTorque6DSensor(SensorName name, SensorStatus status = SensorStatus::Unknown) : IForceTorque6DSensor(name, status) @@ -149,7 +150,17 @@ class ForceTorque6DSensor : public IForceTorque6DSensor // TODO: The positions of force and torques are hardcoded. Forces should be the first // triplet of elements of the read vector and torques the second one. - return handler.getData(force3D, offset) && handler.getData(torque3D, offset + 3); + bool ok = handler.getData(force3D, offset) && handler.getData(torque3D, offset + 3); + if (groundReactionFT) { + force3D[0] = -1 * force3D[0]; + force3D[1] = -1 * force3D[1]; + force3D[2] = -1 * force3D[2]; + + torque3D[0] = -1 * torque3D[0]; + torque3D[1] = -1 * torque3D[1]; + torque3D[2] = -1 * torque3D[2]; + } + return ok; } }; @@ -281,6 +292,8 @@ struct ParsedOptions size_t numberOfChannels; size_t channelOffset; + + bool getGroundReactionFT; }; class IAnalogSensorToIWear::Impl @@ -337,6 +350,11 @@ bool IAnalogSensorToIWear::open(yarp::os::Searchable& config) return false; } + if (!(config.check("getGroundReactionFT") && config.find("getGroundReactionFT").isBool())) { + yError() << LogPrefix << "Parameter 'getGroundReactionFT' missing or invalid"; + return false; + } + // =============== // READ PARAMETERS // =============== @@ -346,6 +364,7 @@ bool IAnalogSensorToIWear::open(yarp::os::Searchable& config) pImpl->options.numberOfChannels = config.find("numberOfChannels").asInt(); pImpl->options.channelOffset = config.find("channelOffset").asInt(); std::string sensorType = config.find("wearableSensorType").asString(); + pImpl->options.getGroundReactionFT = config.find("getGroundReactionFT").asBool(); pImpl->options.wearableSensorType = sensorTypeFromString(sensorType); yInfo() << LogPrefix << "*** ===================="; @@ -354,6 +373,7 @@ bool IAnalogSensorToIWear::open(yarp::os::Searchable& config) yInfo() << LogPrefix << "*** Wearable name :" << pImpl->options.wearableName; yInfo() << LogPrefix << "*** Number of channels :" << pImpl->options.numberOfChannels; yInfo() << LogPrefix << "*** Channel offset :" << pImpl->options.channelOffset; + yInfo() << LogPrefix << "*** Ground reaction FT :" << pImpl->options.getGroundReactionFT; yInfo() << LogPrefix << "*** ===================="; // ===================== @@ -393,6 +413,7 @@ bool IAnalogSensorToIWear::Impl::allocateSensor(const wearable::sensor::SensorTy case wearable::sensor::SensorType::ForceTorque6DSensor: { auto sensor = std::make_shared(name, SensorStatus::Ok); sensor->offset = options.channelOffset; + sensor->groundReactionFT = options.getGroundReactionFT; iSensor = std::dynamic_pointer_cast(sensor); break; } From 1f510d455e19dff74c0f68b487bf06a6954a6732 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Mon, 21 Jan 2019 12:28:30 +0100 Subject: [PATCH 27/36] Update PR changes --- conf/ICub/ICub.xml | 1 - devices/ICub/src/ICub.cpp | 63 +++++++++++++-------------------------- 2 files changed, 20 insertions(+), 44 deletions(-) diff --git a/conf/ICub/ICub.xml b/conf/ICub/ICub.xml index d87f6fad..34eb7b86 100644 --- a/conf/ICub/ICub.xml +++ b/conf/ICub/ICub.xml @@ -2,7 +2,6 @@ - 2 /wholeBodyDynamics/left_arm/endEffectorWrench:o /wholeBodyDynamics/right_arm/endEffectorWrench:o diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index cb9105ec..6a5a8982 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -33,9 +33,8 @@ class WrenchPort : public yarp::os::BufferedPort { if (!wrench.isNull()) { std::lock_guard lock (mtx); - this->wrenchValues.clear(); // Clear the vector of previous values - this->wrenchValues.resize(wrench.size()); + for (size_t i = 0; i < wrench.size(); ++i) { this->wrenchValues.at(i) = wrench.get(i).asDouble(); } @@ -49,15 +48,6 @@ class WrenchPort : public yarp::os::BufferedPort { yWarning() << LogPrefix << "[WrenchPort] read an invalid wrench bottle"; } } - - std::vector wrench() { - std::lock_guard lck (mtx); - return this->wrenchValues; - } - - bool firstRunFlag() { - return this->firstRun; - } }; class ICub::ICubImpl @@ -73,13 +63,7 @@ class ICub::ICubImpl bool leftHandFTDataReceived = false; bool rightHandFTDataReceived = false; - template - struct ICubDeviceSensors - { - std::shared_ptr icubSensor; - }; - - std::map> ftSensorsMap; + std::map> ftSensorsMap; const WearableName wearableName = "ICub"; @@ -106,8 +90,8 @@ class ICub::ICubImpl::ICubForceTorque6DSensor : IForceTorque6DSensor(name, status) , icubImpl(impl) { - //Set the sensor status Ok after receiving the first data on the wrench ports - if (!icubImpl->leftHandFTPort.firstRunFlag() || !icubImpl->rightHandFTPort.firstRunFlag()) { + // Set the sensor status Ok after receiving the first data on the wrench ports + if (!icubImpl->leftHandFTPort.firstRun || !icubImpl->rightHandFTPort.firstRun) { auto nonConstThis = const_cast(this); nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); } @@ -128,14 +112,16 @@ class ICub::ICubImpl::ICubForceTorque6DSensor // Reading wrench from WBD ports if (this->m_name == icubImpl->wearableName + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { - icubImpl->leftHandFTPort.useCallback(); - wrench = icubImpl->leftHandFTPort.wrench(); + + std::lock_guard lck (icubImpl->leftHandFTPort.mtx); + wrench = icubImpl->leftHandFTPort.wrenchValues; icubImpl->timeStamp.time = yarp::os::Time::now(); } else if(this->m_name == icubImpl->wearableName + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { - icubImpl->rightHandFTPort.useCallback(); - wrench = icubImpl->rightHandFTPort.wrench(); + + std::lock_guard lck (icubImpl->rightHandFTPort.mtx); + wrench = icubImpl->rightHandFTPort.wrenchValues; icubImpl->timeStamp.time = yarp::os::Time::now(); } @@ -197,11 +183,6 @@ bool ICub::open(yarp::os::Searchable& config) return false; } - if (!wbdHandsFTSensorsGroup.check("nSensors")) { - yError() << LogPrefix << "REQUIRED parameter NOT found"; - return false; - } - if (!wbdHandsFTSensorsGroup.check("leftHand") || !wbdHandsFTSensorsGroup.check("rightHand")) { yError() << LogPrefix << "REQUIRED parameter or NOT found"; return false; @@ -211,7 +192,7 @@ bool ICub::open(yarp::os::Searchable& config) // PARSE THE CONFIGURATION OPTIONS // =============================== - pImpl->nSensors = wbdHandsFTSensorsGroup.check("nSensors",yarp::os::Value(false)).asInt(); + pImpl->nSensors = wbdHandsFTSensorsGroup.size()-1; std::string leftHandFTPortName = wbdHandsFTSensorsGroup.check("leftHand",yarp::os::Value(false)).asString(); std::string rightHandFTPortName = wbdHandsFTSensorsGroup.check("rightHand",yarp::os::Value(false)).asString(); @@ -219,25 +200,23 @@ bool ICub::open(yarp::os::Searchable& config) // OPEN AND CONNECT YARP PORTS // =========================== - if (! pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i") || - !yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str())) { + if (!( pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i") && yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str()) ) ) { yError() << LogPrefix << "Failed to open or connect to " << pImpl->leftHandFTPort.getName().c_str(); return false; } - while (pImpl->leftHandFTPort.firstRunFlag()) { + while (pImpl->leftHandFTPort.firstRun) { pImpl->leftHandFTPort.useCallback(); } pImpl->sensorNames.push_back("leftWBDFTSensor"); - if (!pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i") || - !yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str())) { + if (!( pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i") && yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str()) ) ) { yError() << LogPrefix << "Failed to open or connect to " << pImpl->rightHandFTPort.getName().c_str(); return false; } - while (pImpl->rightHandFTPort.firstRunFlag()) { + while (pImpl->rightHandFTPort.firstRun) { pImpl->rightHandFTPort.useCallback(); } @@ -252,7 +231,7 @@ bool ICub::open(yarp::os::Searchable& config) pImpl->ftSensorsMap.emplace( ft6dPrefix + pImpl->sensorNames[s], - ICubImpl::ICubDeviceSensors{ft6d}); + std::shared_ptr{ft6d}); } return true; @@ -273,8 +252,6 @@ wearable::WearStatus ICub::getStatus() const for (const auto& s : getAllSensors()) { if (s->getSensorStatus() != sensor::SensorStatus::Ok) { - yError() << LogPrefix << "The status of" << s->getSensorName() << "is not Ok (" - << static_cast(s->getSensorStatus()) << ")"; status = wearable::WearStatus::Error; } } @@ -327,10 +304,10 @@ ICub::getSensors(const wearable::sensor::SensorType type) const wearable::VectorOfSensorPtr outVec; switch (type) { case sensor::SensorType::ForceTorque6DSensor: { - outVec.reserve(pImpl->nSensors); + outVec.reserve(pImpl->ftSensorsMap.size()); for (const auto& ft6d : pImpl->ftSensorsMap) { outVec.push_back( - static_cast>(ft6d.second.icubSensor)); + static_cast>(ft6d.second)); } break; @@ -355,7 +332,7 @@ ICub::getForceTorque6DSensor(const wearable::sensor::SensorName name) const return nullptr; } - //return a shared point to the required sensor + // Return a shared point to the required sensor return static_cast>( - pImpl->ftSensorsMap.at(static_cast(name)).icubSensor); + pImpl->ftSensorsMap.at(name)); } From 4ad9c08c778c8d3b11013f510e1f00b60aa72e6c Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Mon, 21 Jan 2019 12:47:52 +0100 Subject: [PATCH 28/36] Beautify using wearables clang format file --- devices/ICub/src/ICub.cpp | 139 ++++++++++++++++++++------------------ 1 file changed, 73 insertions(+), 66 deletions(-) diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp index 6a5a8982..0951cab1 100644 --- a/devices/ICub/src/ICub.cpp +++ b/devices/ICub/src/ICub.cpp @@ -8,9 +8,9 @@ #include "ICub.h" +#include #include #include -#include #include #include @@ -22,16 +22,18 @@ using namespace wearable::devices; const std::string LogPrefix = "ICub :"; -class WrenchPort : public yarp::os::BufferedPort { +class WrenchPort : public yarp::os::BufferedPort +{ public: std::vector wrenchValues; bool firstRun = true; mutable std::mutex mtx; using yarp::os::BufferedPort::onRead; - virtual void onRead(yarp::os::Bottle& wrench) { + virtual void onRead(yarp::os::Bottle& wrench) + { if (!wrench.isNull()) { - std::lock_guard lock (mtx); + std::lock_guard lock(mtx); this->wrenchValues.resize(wrench.size()); @@ -39,8 +41,7 @@ class WrenchPort : public yarp::os::BufferedPort { this->wrenchValues.at(i) = wrench.get(i).asDouble(); } - if (this->firstRun) - { + if (this->firstRun) { this->firstRun = false; } } @@ -60,7 +61,7 @@ class ICub::ICubImpl size_t nSensors; std::vector sensorNames; - bool leftHandFTDataReceived = false; + bool leftHandFTDataReceived = false; bool rightHandFTDataReceived = false; std::map> ftSensorsMap; @@ -74,8 +75,7 @@ class ICub::ICubImpl // ========================================== // ICub implementation of ForceTorque6DSensor // ========================================== -class ICub::ICubImpl::ICubForceTorque6DSensor - : public wearable::sensor::IForceTorque6DSensor +class ICub::ICubImpl::ICubForceTorque6DSensor : public wearable::sensor::IForceTorque6DSensor { public: ICub::ICubImpl* icubImpl = nullptr; @@ -84,51 +84,55 @@ class ICub::ICubImpl::ICubForceTorque6DSensor // Constructor / Destructor // ------------------------ ICubForceTorque6DSensor( - ICub::ICubImpl* impl, - const wearable::sensor::SensorName name = {}, - const wearable::sensor::SensorStatus status = wearable::sensor::SensorStatus::WaitingForFirstRead) //Default sensor status - : IForceTorque6DSensor(name, status) - , icubImpl(impl) + ICub::ICubImpl* impl, + const wearable::sensor::SensorName name = {}, + const wearable::sensor::SensorStatus status = + wearable::sensor::SensorStatus::WaitingForFirstRead) // Default sensor status + : IForceTorque6DSensor(name, status) + , icubImpl(impl) { // Set the sensor status Ok after receiving the first data on the wrench ports if (!icubImpl->leftHandFTPort.firstRun || !icubImpl->rightHandFTPort.firstRun) { auto nonConstThis = const_cast(this); nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); } - } ~ICubForceTorque6DSensor() override = default; - void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } + void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } // ============================== // IForceTorque6DSensor interface // ============================== - bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const override { + bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const override + { assert(icubImpl != nullptr); std::vector wrench; // Reading wrench from WBD ports - if (this->m_name == icubImpl->wearableName + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { + if (this->m_name + == icubImpl->wearableName + wearable::Separator + + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { - std::lock_guard lck (icubImpl->leftHandFTPort.mtx); + std::lock_guard lck(icubImpl->leftHandFTPort.mtx); wrench = icubImpl->leftHandFTPort.wrenchValues; icubImpl->timeStamp.time = yarp::os::Time::now(); } - else if(this->m_name == icubImpl->wearableName + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { + else if (this->m_name + == icubImpl->wearableName + wearable::Separator + + sensor::IForceTorque6DSensor::getPrefix() + "rightWBDFTSensor") { - std::lock_guard lck (icubImpl->rightHandFTPort.mtx); + std::lock_guard lck(icubImpl->rightHandFTPort.mtx); wrench = icubImpl->rightHandFTPort.wrenchValues; icubImpl->timeStamp.time = yarp::os::Time::now(); } - // Check the size of wrench values - if(wrench.size() == 6) { + if (wrench.size() == 6) { force3D[0] = wrench.at(0); force3D[1] = wrench.at(1); force3D[2] = wrench.at(2); @@ -143,8 +147,8 @@ class ICub::ICubImpl::ICubForceTorque6DSensor torque3D.fill(0.0); // Set sensor status to error if wrong data is read - //auto nonConstThis = const_cast(this); - //nonConstThis->setStatus(wearable::sensor::SensorStatus::Error); + // auto nonConstThis = const_cast(this); + // nonConstThis->setStatus(wearable::sensor::SensorStatus::Error); } return true; @@ -168,8 +172,7 @@ bool ICub::open(yarp::os::Searchable& config) yInfo() << LogPrefix << "Starting to configure"; // Configure clock - if (!yarp::os::Time::isSystemClock()) - { + if (!yarp::os::Time::isSystemClock()) { yarp::os::Time::useSystemClock(); } @@ -178,7 +181,7 @@ bool ICub::open(yarp::os::Searchable& config) // =============================== yarp::os::Bottle wbdHandsFTSensorsGroup = config.findGroup("wbd-hand-ft-sensors"); - if(wbdHandsFTSensorsGroup.isNull()) { + if (wbdHandsFTSensorsGroup.isNull()) { yError() << LogPrefix << "REQUIRED parameter NOT found"; return false; } @@ -192,46 +195,54 @@ bool ICub::open(yarp::os::Searchable& config) // PARSE THE CONFIGURATION OPTIONS // =============================== - pImpl->nSensors = wbdHandsFTSensorsGroup.size()-1; - std::string leftHandFTPortName = wbdHandsFTSensorsGroup.check("leftHand",yarp::os::Value(false)).asString(); - std::string rightHandFTPortName = wbdHandsFTSensorsGroup.check("rightHand",yarp::os::Value(false)).asString(); - - // =========================== - // OPEN AND CONNECT YARP PORTS - // =========================== - - if (!( pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i") && yarp::os::Network::connect(leftHandFTPortName,pImpl->leftHandFTPort.getName().c_str()) ) ) { - yError() << LogPrefix << "Failed to open or connect to " << pImpl->leftHandFTPort.getName().c_str(); - return false; - } + pImpl->nSensors = wbdHandsFTSensorsGroup.size() - 1; + std::string leftHandFTPortName = + wbdHandsFTSensorsGroup.check("leftHand", yarp::os::Value(false)).asString(); + std::string rightHandFTPortName = + wbdHandsFTSensorsGroup.check("rightHand", yarp::os::Value(false)).asString(); + + // =========================== + // OPEN AND CONNECT YARP PORTS + // =========================== + + if (!(pImpl->leftHandFTPort.open("/ICub/leftHantFTSensor:i") + && yarp::os::Network::connect(leftHandFTPortName, + pImpl->leftHandFTPort.getName().c_str()))) { + yError() << LogPrefix << "Failed to open or connect to " + << pImpl->leftHandFTPort.getName().c_str(); + return false; + } - while (pImpl->leftHandFTPort.firstRun) { - pImpl->leftHandFTPort.useCallback(); - } + while (pImpl->leftHandFTPort.firstRun) { + pImpl->leftHandFTPort.useCallback(); + } - pImpl->sensorNames.push_back("leftWBDFTSensor"); + pImpl->sensorNames.push_back("leftWBDFTSensor"); - if (!( pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i") && yarp::os::Network::connect(rightHandFTPortName,pImpl->rightHandFTPort.getName().c_str()) ) ) { - yError() << LogPrefix << "Failed to open or connect to " << pImpl->rightHandFTPort.getName().c_str(); - return false; - } + if (!(pImpl->rightHandFTPort.open("/ICub/rightHantFTSensor:i") + && yarp::os::Network::connect(rightHandFTPortName, + pImpl->rightHandFTPort.getName().c_str()))) { + yError() << LogPrefix << "Failed to open or connect to " + << pImpl->rightHandFTPort.getName().c_str(); + return false; + } - while (pImpl->rightHandFTPort.firstRun) { - pImpl->rightHandFTPort.useCallback(); - } + while (pImpl->rightHandFTPort.firstRun) { + pImpl->rightHandFTPort.useCallback(); + } - pImpl->sensorNames.push_back("rightWBDFTSensor"); + pImpl->sensorNames.push_back("rightWBDFTSensor"); - std::string ft6dPrefix = getWearableName() + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix(); + std::string ft6dPrefix = + getWearableName() + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix(); for (size_t s = 0; s < pImpl->sensorNames.size(); ++s) { // Create the new sensors - auto ft6d = std::make_shared( + auto ft6d = std::make_shared( pImpl.get(), ft6dPrefix + pImpl->sensorNames[s]); - pImpl->ftSensorsMap.emplace( - ft6dPrefix + pImpl->sensorNames[s], - std::shared_ptr{ft6d}); + pImpl->ftSensorsMap.emplace(ft6dPrefix + pImpl->sensorNames[s], + std::shared_ptr{ft6d}); } return true; @@ -260,11 +271,10 @@ wearable::WearStatus ICub::getStatus() const return status; } - wearable::TimeStamp ICub::getTimeStamp() const { // Stamp count should be always zero - return {pImpl->timeStamp.time,0}; + return {pImpl->timeStamp.time, 0}; } bool ICub::close() @@ -306,14 +316,13 @@ ICub::getSensors(const wearable::sensor::SensorType type) const case sensor::SensorType::ForceTorque6DSensor: { outVec.reserve(pImpl->ftSensorsMap.size()); for (const auto& ft6d : pImpl->ftSensorsMap) { - outVec.push_back( - static_cast>(ft6d.second)); + outVec.push_back(static_cast>(ft6d.second)); } break; } default: { - //yWarning() << LogPrefix << "Selected sensor type (" << static_cast(type) + // yWarning() << LogPrefix << "Selected sensor type (" << static_cast(type) // << ") is not supported by ICub"; return {}; } @@ -326,13 +335,11 @@ wearable::SensorPtr ICub::getForceTorque6DSensor(const wearable::sensor::SensorName name) const { // Check if user-provided name corresponds to an available sensor - if (pImpl->ftSensorsMap.find(name) - == pImpl->ftSensorsMap.end()) { + if (pImpl->ftSensorsMap.find(name) == pImpl->ftSensorsMap.end()) { yError() << LogPrefix << "Invalid sensor name"; return nullptr; } // Return a shared point to the required sensor - return static_cast>( - pImpl->ftSensorsMap.at(name)); + return static_cast>(pImpl->ftSensorsMap.at(name)); } From 5b643b3a3711a8db227c1454d6a4cce72167219f Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 22 Jan 2019 14:22:58 +0100 Subject: [PATCH 29/36] Fix CMakeLists with YARP_FORCE_DYNAMIC_PLUGINS --- CMakeLists.txt | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 313e8935..5cdaa960 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,12 +21,9 @@ endif() # Libraries type if(MSVC) - option(BUILD_SHARED_LIBS "Compile WBToolbox as a shared library" OFF) + option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" OFF) else() - option(BUILD_SHARED_LIBS "Compile WBToolbox as a shared library" ON) -endif() -if(NOT BUILD_SHARED_LIBS) - set(CMAKE_POSITION_INDEPENDENT_CODE ON) + option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) endif() # Control where binaries and libraries are placed in the build folder. @@ -63,10 +60,11 @@ add_subdirectory(impl) # Prepare YARP wrapper and devices find_package(YARP 3.0 REQUIRED) -set(BUILD_SHARED_LIBS ON) -set(CMAKE_POSITION_INDEPENDENT_CODE ON) +set(YARP_FORCE_DYNAMIC_PLUGINS ON) yarp_configure_plugins_installation(wearable) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + add_subdirectory(msgs) add_subdirectory(devices) add_subdirectory(wrappers) From c1da2694bf720800f0348f7ef3422db5250907d3 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Mon, 28 Jan 2019 18:31:37 +0100 Subject: [PATCH 30/36] Implement skin sensor and add config file --- conf/SkinInsoles/SkinInsoles.xml | 70 +++++++++++++++++++ .../src/IAnalogSensorToIWear.cpp | 48 ++++++++++++- impl/SensorsImpl/SensorsImpl.cpp | 13 ++++ .../Wearable/IWear/Sensors/impl/SensorsImpl.h | 5 ++ .../Wearable/IWear/Sensors/ISkinSensor.h | 5 +- 5 files changed, 137 insertions(+), 4 deletions(-) create mode 100644 conf/SkinInsoles/SkinInsoles.xml diff --git a/conf/SkinInsoles/SkinInsoles.xml b/conf/SkinInsoles/SkinInsoles.xml new file mode 100644 index 00000000..33e2dcf8 --- /dev/null +++ b/conf/SkinInsoles/SkinInsoles.xml @@ -0,0 +1,70 @@ + + + + + + 0 + 5 6 7 8 9 10 + 20 + socketcan + + + + 50 + 0 + 0xff + false + + + true + 3 + 0x2200 + + + + + + 20 + 1152 + skinWrapper + + 0 1151 0 1151 + + + + + skin_insoles + + + + + + + SkinInsolesSensor + SkinInsoles + 1152 + 0 + SkinSensor + + + skin_insoles + + + + + + + 0.01 + /SkinInsoles/WearableData/data:o + /SkinInsoles/WearableData/metadataRpc:o + + + SkinInsolesIAnalogSensorToIWear + + + + + + diff --git a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp index 015d9a07..c92d16fb 100644 --- a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp +++ b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp @@ -120,6 +120,14 @@ static class IAnalogSensorHandler std::copy(buffer.data() + offset, buffer.data() + offset + 4, array.data()); return true; } + + // This is for skin data + bool getData(std::vector& vector, const size_t offset = 0) + { + std::copy(buffer.data() + offset, buffer.end() + offset, vector.data()); + return true; + } + } handler; // ================================ @@ -228,6 +236,31 @@ class TemperatureSensor : public ITemperatureSensor } }; +class SkinSensor : public ISkinSensor +{ +public: + unsigned offset = 0; + + SkinSensor(SensorName name, SensorStatus status = SensorStatus::Unknown) + : ISkinSensor(name, status) + {} + + void setStatus(const SensorStatus status) { m_status = status; } + + bool getPressure(std::vector& pressure) const override + { + if (!handler.readData()) { + return false; + } + + // Dirty workaround to set the status from a const method + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(handler.getStatus()); + + return handler.getData(pressure, offset); + } +}; + // TODO: implement all the other sensors // ==================== @@ -408,6 +441,12 @@ bool IAnalogSensorToIWear::Impl::allocateSensor(const wearable::sensor::SensorTy iSensor = std::dynamic_pointer_cast(sensor); break; } + case wearable::sensor::SensorType::SkinSensor: { + auto sensor = std::make_shared(name, SensorStatus::Ok); + sensor->offset = options.channelOffset; + iSensor = std::dynamic_pointer_cast(sensor); + break; + } default: // TODO: implement the remaining sensors return false; @@ -641,9 +680,14 @@ IAnalogSensorToIWear::getPositionSensor(const wearable::sensor::SensorName /*nam } wearable::SensorPtr -IAnalogSensorToIWear::getSkinSensor(const wearable::sensor::SensorName /*name*/) const +IAnalogSensorToIWear::getSkinSensor(const wearable::sensor::SensorName name) const { - return nullptr; + if (!(pImpl->iSensor && (pImpl->iSensor->getSensorName() == name))) { + yError() << LogPrefix << "Failed to get sensor" << name; + return nullptr; + } + + return std::dynamic_pointer_cast(pImpl->iSensor); } wearable::SensorPtr diff --git a/impl/SensorsImpl/SensorsImpl.cpp b/impl/SensorsImpl/SensorsImpl.cpp index 478cc429..f62d5d80 100644 --- a/impl/SensorsImpl/SensorsImpl.cpp +++ b/impl/SensorsImpl/SensorsImpl.cpp @@ -246,6 +246,19 @@ SkinSensor::SkinSensor(wearable::sensor::SensorName n, wearable::sensor::SensorS : wearable::sensor::ISkinSensor(n, s) {} +bool SkinSensor::getPressure(std::vector& pressure) const +{ + std::lock_guard lock(m_mutex); + pressure = m_values; + return true; +} + +void SkinSensor::setBuffer(const std::vector values) +{ + std::lock_guard lock(m_mutex); + m_values = values; +} + // ================= // TemperatureSensor // ================= diff --git a/impl/SensorsImpl/include/Wearable/IWear/Sensors/impl/SensorsImpl.h b/impl/SensorsImpl/include/Wearable/IWear/Sensors/impl/SensorsImpl.h index 81fb3090..48accd2f 100644 --- a/impl/SensorsImpl/include/Wearable/IWear/Sensors/impl/SensorsImpl.h +++ b/impl/SensorsImpl/include/Wearable/IWear/Sensors/impl/SensorsImpl.h @@ -11,6 +11,7 @@ #include "Wearable/IWear/IWear.h" #include +#include namespace wearable { namespace sensor { @@ -201,11 +202,15 @@ class wearable::sensor::impl::SkinSensor : public wearable::sensor::ISkinSensor { public: mutable std::mutex m_mutex; + std::vector m_values; SkinSensor(wearable::sensor::SensorName n = {}, wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); ~SkinSensor() override = default; + bool getPressure(std::vector& pressure) const override; + + void setBuffer(const std::vector values); inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } }; diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/ISkinSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/ISkinSensor.h index 31d2c8f7..08249bc4 100644 --- a/interfaces/IWear/include/Wearable/IWear/Sensors/ISkinSensor.h +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/ISkinSensor.h @@ -10,6 +10,7 @@ #define WEARABLE_ISKIN_SENSOR_H #include "Wearable/IWear/Sensors/ISensor.h" +#include namespace wearable { namespace sensor { @@ -28,8 +29,9 @@ class wearable::sensor::ISkinSensor : public wearable::sensor::ISensor virtual ~ISkinSensor() = default; + virtual bool getPressure(std::vector& pressure) const = 0; + inline static const std::string getPrefix(); - // TODO: to be implemented }; inline const std::string wearable::sensor::ISkinSensor::getPrefix() @@ -37,5 +39,4 @@ inline const std::string wearable::sensor::ISkinSensor::getPrefix() return "skin" + wearable::Separator; } -// const wearable::sensor::SensorPrefix wearable::sensor::ISkinSensor::namePrefix = "skin_"; #endif // WEARABLE_ISKIN_SENSOR_H From f3aedf1b89c6f7a242fa04539414901f50fd84e1 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 22 Jan 2019 14:55:24 +0100 Subject: [PATCH 31/36] rpc port can be enabled or disabled in IWearRemapper with wearableRPCPorts param --- devices/IWearRemapper/src/IWearRemapper.cpp | 192 +++++++++++--------- 1 file changed, 108 insertions(+), 84 deletions(-) diff --git a/devices/IWearRemapper/src/IWearRemapper.cpp b/devices/IWearRemapper/src/IWearRemapper.cpp index 2417c159..c47799b5 100644 --- a/devices/IWearRemapper/src/IWearRemapper.cpp +++ b/devices/IWearRemapper/src/IWearRemapper.cpp @@ -40,6 +40,7 @@ class IWearRemapper::impl yarp::os::Network network; TimeStamp timestamp; bool firstRun = true; + bool useRPC = false; mutable std::recursive_mutex mutex; @@ -135,16 +136,26 @@ bool IWearRemapper::open(yarp::os::Searchable& config) } // RPC ports - if (!(config.check("wearableRPCPorts") && config.find("wearableRPCPorts").isList())) { - yError() << logPrefix << "wearableRPCPorts option does not exist or it is not a list"; - return false; + // The usage of rpc port is optional depending wether or not the parameter is present + yarp::os::Bottle* inputRPCPortsNamesList = nullptr; + if (!config.check("wearableRPCPorts")) + { + yInfo() << logPrefix << "wearableRPCPorts does not exist. RPC port will not be used"; } - yarp::os::Bottle* inputRPCPortsNamesList = config.find("wearableRPCPorts").asList(); - for (unsigned i = 0; i < inputRPCPortsNamesList->size(); ++i) { - if (!inputRPCPortsNamesList->get(i).isString()) { - yError() << logPrefix << "ith entry of wearableRPCPorts list is not a string"; + else + { + pImpl->useRPC = true; + if (!config.find("wearableRPCPorts").isList()) { + yError() << logPrefix << "wearableRPCPorts option is not a list"; return false; } + inputRPCPortsNamesList = config.find("wearableRPCPorts").asList(); + for (unsigned i = 0; i < inputRPCPortsNamesList->size(); ++i) { + if (!inputRPCPortsNamesList->get(i).isString()) { + yError() << logPrefix << "ith entry of wearableRPCPorts list is not a string"; + return false; + } + } } // Output port @@ -168,8 +179,11 @@ bool IWearRemapper::open(yarp::os::Searchable& config) // Convert list to vector std::vector inputRPCPortsNamesVector; - for (unsigned i = 0; i < inputRPCPortsNamesList->size(); ++i) { - inputRPCPortsNamesVector.emplace_back(inputRPCPortsNamesList->get(i).asString()); + if (pImpl->useRPC) + { + for (unsigned i = 0; i < inputRPCPortsNamesList->size(); ++i) { + inputRPCPortsNamesVector.emplace_back(inputRPCPortsNamesList->get(i).asString()); + } } yInfo() << logPrefix << "*** ========================"; @@ -177,10 +191,16 @@ bool IWearRemapper::open(yarp::os::Searchable& config) yInfo() << logPrefix << "*** Wearable Data Port" << i + 1 << " :" << inputDataPortsNamesVector[i]; } - for (unsigned i = 0; i < inputDataPortsNamesVector.size(); ++i) { - yInfo() << logPrefix << "*** Wearable RPC Port" << i + 1 << " :" - << inputRPCPortsNamesVector[i]; - ; + if (pImpl->useRPC) + { + for (unsigned i = 0; i < inputDataPortsNamesVector.size(); ++i) { + yInfo() << logPrefix << "*** Wearable RPC Port" << i + 1 << " :" + << inputRPCPortsNamesVector[i]; + } + } + else + { + yInfo() << logPrefix << "*** Wearable RPC Port not configured"; } yInfo() << logPrefix << "*** Output port :" << outputPortName; yInfo() << logPrefix << "*** ========================"; @@ -221,90 +241,93 @@ bool IWearRemapper::open(yarp::os::Searchable& config) // ==================== // OPEN INPUT RPC PORTS // ==================== - yDebug() << logPrefix << "Opening input RPC ports"; + if(pImpl->useRPC) + { + yDebug() << logPrefix << "Opening input RPC ports"; - if (inputDataPortsNamesVector.size() != inputRPCPortsNamesVector.size()) { - yError() << logPrefix << "wearableDataPorts and wearableRPCPorts lists should contain " - << "the same number of elements."; - return false; - } - - using SensorTypeMetadata = std::vector; - using WearableMetadata = std::map; - - std::vector metadata; - - // Get metadata from remote rpc ports - for (const auto& inputRpcPortName : inputRPCPortsNamesVector) { - // Open a local port for the rpc data - yarp::os::RpcClient localRpcPort; - if (!localRpcPort.open("...")) { - yInfo() << logPrefix << "Failed to open local port"; + if (inputDataPortsNamesVector.size() != inputRPCPortsNamesVector.size()) { + yError() << logPrefix << "wearableDataPorts and wearableRPCPorts lists should contain " + << "the same number of elements."; return false; } - // Wait the connection with the remote port - while (true) { - yarp::os::Network::connect(localRpcPort.getName(), inputRpcPortName); - if (yarp::os::Network::isConnected(localRpcPort.getName(), inputRpcPortName)) { - yInfo() << logPrefix << "Connected with" << inputRpcPortName; - break; - } - yInfo() << logPrefix << "Waiting connection with" << inputRpcPortName; - yarp::os::Time::delay(10); - } - - // Attach the rpc client - wearable::msg::WearableMetadataService service; - if (!service.yarp().attachAsClient(localRpcPort)) { - yError() << logPrefix << "Failed to attach to" << localRpcPort.getName(); - return false; - } + using SensorTypeMetadata = std::vector; + using WearableMetadata = std::map; - // Ask metadata and store them - metadata.push_back(service.getMetadata()); + std::vector metadata; - // Close the temporary port - localRpcPort.close(); - } + // Get metadata from remote rpc ports + for (const auto& inputRpcPortName : inputRPCPortsNamesVector) { + // Open a local port for the rpc data + yarp::os::RpcClient localRpcPort; + if (!localRpcPort.open("...")) { + yInfo() << logPrefix << "Failed to open local port"; + return false; + } - // ==================================== - // PROCESS METADATA READ FROM RPC PORTS - // ==================================== - yDebug() << logPrefix << "Processing RPC metadata"; - - auto findStoredSensorName = - [](const std::string& name, - std::map>& map) -> bool { - for (const auto& entryPair : map) { - for (const std::string& sensorName : entryPair.second) { - if (sensorName == name) { - return true; + // Wait the connection with the remote port + while (true) { + yarp::os::Network::connect(localRpcPort.getName(), inputRpcPortName); + if (yarp::os::Network::isConnected(localRpcPort.getName(), inputRpcPortName)) { + yInfo() << logPrefix << "Connected with" << inputRpcPortName; + break; } + yInfo() << logPrefix << "Waiting connection with" << inputRpcPortName; + yarp::os::Time::delay(10); } - } - return false; - }; - // Store the sensor names checking that there are no name duplicates in the - // connected implementation of IWear - for (const WearableMetadata& wearableMetadataFromSingleSource : metadata) { + // Attach the rpc client + wearable::msg::WearableMetadataService service; + if (!service.yarp().attachAsClient(localRpcPort)) { + yError() << logPrefix << "Failed to attach to" << localRpcPort.getName(); + return false; + } - for (const auto& mapEntry : wearableMetadataFromSingleSource) { + // Ask metadata and store them + metadata.push_back(service.getMetadata()); - const wearable::msg::SensorType& sensorType = mapEntry.first; - const SensorTypeMetadata& sensorTypeMetadata = mapEntry.second; + // Close the temporary port + localRpcPort.close(); + } - for (const wearable::msg::WearableSensorMetadata& sensorMD : sensorTypeMetadata) { - // Check there isn't any sensor name collision - if (findStoredSensorName(sensorMD.name, pImpl->rpcSensorNames)) { - yError() << logPrefix << "Sensor with name " << sensorMD.name - << " alredy exists."; - return false; + // ==================================== + // PROCESS METADATA READ FROM RPC PORTS + // ==================================== + yDebug() << logPrefix << "Processing RPC metadata"; + + auto findStoredSensorName = + [](const std::string& name, + std::map>& map) -> bool { + for (const auto& entryPair : map) { + for (const std::string& sensorName : entryPair.second) { + if (sensorName == name) { + return true; + } + } + } + return false; + }; + + // Store the sensor names checking that there are no name duplicates in the + // connected implementation of IWear + for (const WearableMetadata& wearableMetadataFromSingleSource : metadata) { + + for (const auto& mapEntry : wearableMetadataFromSingleSource) { + + const wearable::msg::SensorType& sensorType = mapEntry.first; + const SensorTypeMetadata& sensorTypeMetadata = mapEntry.second; + + for (const wearable::msg::WearableSensorMetadata& sensorMD : sensorTypeMetadata) { + // Check there isn't any sensor name collision + if (findStoredSensorName(sensorMD.name, pImpl->rpcSensorNames)) { + yError() << logPrefix << "Sensor with name " << sensorMD.name + << " alredy exists."; + return false; + } + // Add the sensor name + yInfo() << logPrefix << "Found sensor" << sensorMD.name; + pImpl->rpcSensorNames[MapSensorType.at(sensorType)].push_back(sensorMD.name); } - // Add the sensor name - yInfo() << logPrefix << "Found sensor" << sensorMD.name; - pImpl->rpcSensorNames[MapSensorType.at(sensorType)].push_back(sensorMD.name); } } } @@ -946,7 +969,8 @@ IWearRemapper::impl::getSensor(const sensor::SensorName name, const sensor::SensorType type, std::map>& storage) { - if (!validSensorName(name, type)) { + // TODO check if this check on the sensor name is required, and how it is done in case rpc is not available + if (useRPC && !validSensorName(name, type)) { yError() << logPrefix << "Sensor" << name << "not found in the list read from the wrapper"; return nullptr; } From 27b53649eebcee9fad3d121d13c1bfd349753054 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Fri, 1 Feb 2019 10:35:05 +0100 Subject: [PATCH 32/36] Add PreciselyTimed interface to IWearRemapper --- devices/IWearRemapper/include/IWearRemapper.h | 7 +++++++ devices/IWearRemapper/src/IWearRemapper.cpp | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/devices/IWearRemapper/include/IWearRemapper.h b/devices/IWearRemapper/include/IWearRemapper.h index 16c78b8f..83d7c786 100644 --- a/devices/IWearRemapper/include/IWearRemapper.h +++ b/devices/IWearRemapper/include/IWearRemapper.h @@ -12,6 +12,7 @@ #include "Wearable/IWear/IWear.h" #include +#include #include #include @@ -31,6 +32,7 @@ class wearable::devices::IWearRemapper , public wearable::IWear , public yarp::os::TypedReaderCallback , public yarp::os::PeriodicThread + , public yarp::dev::IPreciselyTimed { private: class impl; @@ -50,6 +52,10 @@ class wearable::devices::IWearRemapper // TypedReaderCallback void onRead(msg::WearableData& wearData) override; + // PreciselyTimed interface + yarp::os::Stamp getLastInputStamp() override; + + // IWear interface WearableName getWearableName() const override; WearStatus getStatus() const override; TimeStamp getTimeStamp() const override; @@ -102,6 +108,7 @@ class wearable::devices::IWearRemapper SensorPtr getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const override; + }; #endif // IWEARREMAPPER_H diff --git a/devices/IWearRemapper/src/IWearRemapper.cpp b/devices/IWearRemapper/src/IWearRemapper.cpp index c47799b5..d8aeaff5 100644 --- a/devices/IWearRemapper/src/IWearRemapper.cpp +++ b/devices/IWearRemapper/src/IWearRemapper.cpp @@ -825,6 +825,13 @@ void IWearRemapper::onRead(msg::WearableData& receivedWearData) } } +yarp::os::Stamp IWearRemapper::getLastInputStamp() +{ + std::lock_guard lock(pImpl->mutex); + // Stamp count should be always zero + return yarp::os::Stamp(0, getTimeStamp().time); +} + WearableName IWearRemapper::getWearableName() const { std::lock_guard lock(pImpl->mutex); From 20e62440f013fcaa2a3a01eaeb0a5c7c5d5c7f45 Mon Sep 17 00:00:00 2001 From: Yeshasvi Tirupachuri Date: Sun, 3 Feb 2019 17:27:32 +0100 Subject: [PATCH 33/36] update config file names and add yarpmanager files --- app/Wearables-yarpmanager.ini | 32 ++++++++++++ .../xml/FTShoeLeftWearableDevice.xml | 0 .../xml/FTShoeRightWearableDevice.xml | 0 .../xml/ICubWearableDevice.xml | 0 .../xml/SkinInsolesWearableDevice.xml | 0 .../xml/XsensSuitWearableDevice.xml | 0 app/xml/applications/WearableDevices.xml | 50 +++++++++++++++++++ 7 files changed, 82 insertions(+) create mode 100644 app/Wearables-yarpmanager.ini rename conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml => app/xml/FTShoeLeftWearableDevice.xml (100%) rename conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml => app/xml/FTShoeRightWearableDevice.xml (100%) rename conf/ICub/ICub.xml => app/xml/ICubWearableDevice.xml (100%) rename conf/SkinInsoles/SkinInsoles.xml => app/xml/SkinInsolesWearableDevice.xml (100%) rename conf/Xsens/xsens_windows_all_wrappers.xml => app/xml/XsensSuitWearableDevice.xml (100%) create mode 100644 app/xml/applications/WearableDevices.xml diff --git a/app/Wearables-yarpmanager.ini b/app/Wearables-yarpmanager.ini new file mode 100644 index 00000000..c163290c --- /dev/null +++ b/app/Wearables-yarpmanager.ini @@ -0,0 +1,32 @@ +# Path to the folder which contains application description files +apppath = "xml/applications" +load_subfolders = yes + +# Path to the folder which contains module description files +#modpath = "xml/modules" + +# Path to the folder which contains module description files +#respath = "xml/resources" + +# Fault tolerance +# parameters: yes|No +watchdog = no + +# Module failure awareness (if watchdog is enabled) +# parameters: Ignore|terminate|prompt|recover +module_failure = prompt + +# Connection failure awareness (if watchdog is enabled) +# parameters: Ignore|terminate|prompt +connection_failure = prompt + +# Automatically establish all connections +# parameters: Yes|no +auto_connect = no + +# Appearance (for yarpmanager) +# parameters: No|dark|light +color_theme = dark + +# External editor (e.g. gedit, notepad) +external_editor = gvim diff --git a/conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml b/app/xml/FTShoeLeftWearableDevice.xml similarity index 100% rename from conf/FTShoes/ftshoesleft_ianalogsensor_to_iwear.xml rename to app/xml/FTShoeLeftWearableDevice.xml diff --git a/conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml b/app/xml/FTShoeRightWearableDevice.xml similarity index 100% rename from conf/FTShoes/ftshoesright_ianalogsensor_to_iwear.xml rename to app/xml/FTShoeRightWearableDevice.xml diff --git a/conf/ICub/ICub.xml b/app/xml/ICubWearableDevice.xml similarity index 100% rename from conf/ICub/ICub.xml rename to app/xml/ICubWearableDevice.xml diff --git a/conf/SkinInsoles/SkinInsoles.xml b/app/xml/SkinInsolesWearableDevice.xml similarity index 100% rename from conf/SkinInsoles/SkinInsoles.xml rename to app/xml/SkinInsolesWearableDevice.xml diff --git a/conf/Xsens/xsens_windows_all_wrappers.xml b/app/xml/XsensSuitWearableDevice.xml similarity index 100% rename from conf/Xsens/xsens_windows_all_wrappers.xml rename to app/xml/XsensSuitWearableDevice.xml diff --git a/app/xml/applications/WearableDevices.xml b/app/xml/applications/WearableDevices.xml new file mode 100644 index 00000000..0585e241 --- /dev/null +++ b/app/xml/applications/WearableDevices.xml @@ -0,0 +1,50 @@ + + WearableDevices + An application for running wearable devices + + + + yarprobotinterface + --config XsensSuitWearableDevice.xml + YARP_FORWARD_LOG_ENABLE=1 + Run xsens suit wearable device + localhost + + + + + yarprobotinterface + --config FTShoeLeftWearableDevice.xml + YARP_FORWARD_LOG_ENABLE=1 + Run left ftshoe wearable device + localhost + + + + + yarprobotinterface + --config FTShoeRightWearableDevice.xml + YARP_FORWARD_LOG_ENABLE=1 + Run right ftshoe wearable device + localhost + + + + + yarprobotinterface + --config SkinInsolesWearableDevice.xml + YARP_FORWARD_LOG_ENABLE=1 + Run skin insoles wearable device + localhost + + + + + yarprobotinterface + --config ICubWearableDevice.xml + YARP_FORWARD_LOG_ENABLE=1 + Run icub wearable device + localhost + + + From 3386f3c31caeeff0caff735362c726db099d9ff4 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 4 Feb 2019 10:15:40 +0100 Subject: [PATCH 34/36] Add use RPC parameter --- devices/IWearRemapper/src/IWearRemapper.cpp | 43 ++++++++++++++------- 1 file changed, 28 insertions(+), 15 deletions(-) diff --git a/devices/IWearRemapper/src/IWearRemapper.cpp b/devices/IWearRemapper/src/IWearRemapper.cpp index d8aeaff5..6b5cc39e 100644 --- a/devices/IWearRemapper/src/IWearRemapper.cpp +++ b/devices/IWearRemapper/src/IWearRemapper.cpp @@ -40,7 +40,7 @@ class IWearRemapper::impl yarp::os::Network network; TimeStamp timestamp; bool firstRun = true; - bool useRPC = false; + bool useRPC = true; mutable std::recursive_mutex mutex; @@ -136,25 +136,38 @@ bool IWearRemapper::open(yarp::os::Searchable& config) } // RPC ports - // The usage of rpc port is optional depending wether or not the parameter is present - yarp::os::Bottle* inputRPCPortsNamesList = nullptr; - if (!config.check("wearableRPCPorts")) - { - yInfo() << logPrefix << "wearableRPCPorts does not exist. RPC port will not be used"; + if (!(config.check("useRPC"))) { + yWarning() << logPrefix << "OPTIONAL parameter useRPC NOT found"; } - else + else { + if (!config.find("useRPC").isBool()) { + yError() << logPrefix << "useRPC option is not a bool"; + } + pImpl->useRPC = config.find("useRPC").asBool(); + yWarning() << "useRPC set to" << pImpl->useRPC; + } + + yarp::os::Bottle* inputRPCPortsNamesList = nullptr; + if (pImpl->useRPC) { - pImpl->useRPC = true; - if (!config.find("wearableRPCPorts").isList()) { - yError() << logPrefix << "wearableRPCPorts option is not a list"; - return false; + if (!config.check("wearableRPCPorts")) + { + yInfo() << logPrefix << "wearableRPCPorts parameter does not exist. RPC port will not be used"; + pImpl->useRPC = false; } - inputRPCPortsNamesList = config.find("wearableRPCPorts").asList(); - for (unsigned i = 0; i < inputRPCPortsNamesList->size(); ++i) { - if (!inputRPCPortsNamesList->get(i).isString()) { - yError() << logPrefix << "ith entry of wearableRPCPorts list is not a string"; + else + { + if (!config.find("wearableRPCPorts").isList()) { + yError() << logPrefix << "wearableRPCPorts option is not a list"; return false; } + inputRPCPortsNamesList = config.find("wearableRPCPorts").asList(); + for (unsigned i = 0; i < inputRPCPortsNamesList->size(); ++i) { + if (!inputRPCPortsNamesList->get(i).isString()) { + yError() << logPrefix << "ith entry of wearableRPCPorts list is not a string"; + return false; + } + } } } From 8aa47b8b9d5b581ca1d64af7e6bd414864906fef Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 5 Feb 2019 16:33:59 +0100 Subject: [PATCH 35/36] fix backcompatibility with YARP master --- devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp index a02df81c..4d42ac86 100644 --- a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp +++ b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp @@ -124,7 +124,7 @@ static class IAnalogSensorHandler // This is for skin data bool getData(std::vector& vector, const size_t offset = 0) { - std::copy(buffer.data() + offset, buffer.end() + offset, vector.data()); + std::copy(buffer.data() + offset, buffer.data() + buffer.size() + offset, vector.data()); return true; } From 6d9959013c3d9da96d3038a041bedfb43a85d5b1 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Thu, 7 Feb 2019 09:51:51 +0100 Subject: [PATCH 36/36] Fix Xsens Calibrator device --- XSensMVN/src/XSensMVNCalibrator.cpp | 14 ++++---------- app/xml/XsensSuitWearableDevice.xml | 2 +- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/XSensMVN/src/XSensMVNCalibrator.cpp b/XSensMVN/src/XSensMVNCalibrator.cpp index e6bc7be2..70c1c989 100644 --- a/XSensMVN/src/XSensMVNCalibrator.cpp +++ b/XSensMVN/src/XSensMVNCalibrator.cpp @@ -169,7 +169,7 @@ namespace xsensmvn { } // Wait for the discard operation to be completed - while (!m_operationCompleted && !m_calibrationAborted) { + while (m_suitsConnector.isCalibrationPerformed(calibrationType) && !m_calibrationAborted) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } if (m_calibrationAborted) { @@ -183,8 +183,9 @@ namespace xsensmvn { // Get the phases of the selected calibration type XsIntArray calibPhases = m_suitsConnector.calibrationPhaseList(); - // Start the calibration data collection + // Start the calibration data collection (Wait three seconds to give the subject enough time to take position) xsInfo << "Starting " << calibrationType << " calibration" << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); m_suitsConnector.startCalibration(); // Follow step-by-step the calibration phases of the selected type @@ -270,14 +271,7 @@ namespace xsensmvn { } else { // Notify the user the calibration can be applied - xsInfo << "Ready to apply the obtained calibration. Stand still in starting position. " - "Applying in: "; - - // Wait three seconds to give the subject enough time to take position - for (int s = 5; s >= 0; --s) { - xsInfo << s << " s"; - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - } + xsInfo << "Ready to apply the obtained calibration"; // Apply the calibration to the MVN Engine and wait for a positive feedback m_suitsConnector.finalizeCalibration(); diff --git a/app/xml/XsensSuitWearableDevice.xml b/app/xml/XsensSuitWearableDevice.xml index 3378bcef..5b4149e7 100644 --- a/app/xml/XsensSuitWearableDevice.xml +++ b/app/xml/XsensSuitWearableDevice.xml @@ -28,7 +28,7 @@ true true - false + true