diff --git a/.gitignore b/.gitignore index d88da75..c57f998 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,5 @@ CMakeLists.txt.* *.autosave *.original *.*~ +.vscode/c_cpp_properties.json +.vscode/settings.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 1970aa1..7e9bcba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 3.16) -set(PROJECT_VERSION "1.8.0") +set(PROJECT_VERSION "1.9.0") set (WEARABLES_PROJECT_NAME Wearables) diff --git a/devices/HapticGlove/conf/HapticGlove.xml b/devices/HapticGlove/conf/HapticGlove.xml index c476277..06d1562 100644 --- a/devices/HapticGlove/conf/HapticGlove.xml +++ b/devices/HapticGlove/conf/HapticGlove.xml @@ -17,7 +17,7 @@ - 0.01 + 0.1 /WearableData/HapticGlove/LeftHand/data:o /WearableData/HapticGlove/LeftHand/metadataRpc:o @@ -29,8 +29,8 @@ - 0.01 - /WearableData/HapticGlove/LeftHand/Actuators/input:i + 0.1 + /WearableData/HapticGlove/LeftHand/Actuators/input:i LeftHapticGloveWearableDevice @@ -53,7 +53,7 @@ - 0.01 + 0.1 /WearableData/HapticGlove/RightHand/data:o /WearableData/HapticGlove/RightHand/metadataRpc:o @@ -65,8 +65,8 @@ - 0.01 - /WearableData/HapticGlove/RightHand/Actuators/input:i + 0.1 + /WearableData/HapticGlove/RightHand/Actuators/input:i RightHapticGloveWearableDevice diff --git a/devices/HapticGlove/src/HapticGlove.cpp b/devices/HapticGlove/src/HapticGlove.cpp index 95fda51..b9231f5 100644 --- a/devices/HapticGlove/src/HapticGlove.cpp +++ b/devices/HapticGlove/src/HapticGlove.cpp @@ -80,6 +80,7 @@ class HapticGlove::SenseGloveImpl // Number of actuators const int nActuators = 11; // humanFingerNames.size()*2 + hand/palm thumper + const int nActuatorsPerGlove = 5; // Number of the actuators per glove std::unique_ptr pGlove; /**< Pointer to the glove object. */ @@ -305,6 +306,14 @@ bool HapticGlove::open(yarp::os::Searchable& config) m_pImpl->hapticActuatorPrefix = getWearableName() + actuator::IHaptic::getPrefix(); + for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++) { + m_pImpl->sensegloveHapticActuatorVector.push_back( + std::make_shared( + m_pImpl.get(), + m_pImpl->hapticActuatorPrefix + + "HapticFeedback")); + } + for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++) { m_pImpl->sensegloveHapticActuatorVector.push_back( std::make_shared( @@ -339,6 +348,12 @@ bool HapticGlove::open(yarp::os::Searchable& config) m_pImpl->gloveData.humanJointSensorNameIdMap.emplace( std::make_pair(m_pImpl->jointSensorPrefix + m_pImpl->gloveData.humanJointNames[i], i)); + for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++) + m_pImpl->gloveData.humanHapticActuatorNameIdMap.emplace( + std::make_pair(m_pImpl->hapticActuatorPrefix + + "HapticFeedback", + i)); + for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++) m_pImpl->gloveData.humanHapticActuatorNameIdMap.emplace( std::make_pair(m_pImpl->hapticActuatorPrefix + m_pImpl->gloveData.humanFingerNames[i] @@ -537,22 +552,29 @@ class HapticGlove::SenseGloveImpl::SenseGloveHapticActuator : public wearable::a bool setHapticCommand(double& value) const override { + yError() << LogPrefix << "Wrong method has been called! To set the haptic command please use the setHapticsCommand method."; + return false; + } - std::lock_guard lock(m_gloveImpl->mutex); + bool setHapticCommands(const std::vector& forceValue, const std::vector& vibrotactileValue) const override + { - if (!m_gloveImpl->isAvailable(m_actuatorName, - m_gloveImpl->gloveData.humanHapticActuatorNameIdMap)) { - yError() << LogPrefix << "The actuator name (" << m_actuatorName - << ") is not found in the list of actuators."; + std::lock_guard lock(m_gloveImpl->mutex); + if (forceValue.size() != m_gloveImpl->nActuatorsPerGlove || vibrotactileValue.size() != m_gloveImpl->nActuatorsPerGlove) + { + yError() << "The sizes of the forceValue and the vibrotactileValue vectors are not correct!"; return false; } - m_gloveImpl->gloveData.fingersHapticFeedback - [m_gloveImpl->gloveData.humanHapticActuatorNameIdMap[m_actuatorName]] = value; - + for (size_t i = 0; i < m_gloveImpl->nFingers; i++) + { + m_gloveImpl->gloveData.fingersHapticFeedback[i] = forceValue[i]; + m_gloveImpl->gloveData.fingersHapticFeedback[i + 5] = vibrotactileValue[i]; + } return true; } + inline void setStatus(const actuator::ActuatorStatus& status) { m_status = status; } private: diff --git a/interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h b/interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h index 9430b80..2d5fcc7 100644 --- a/interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h +++ b/interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h @@ -27,6 +27,8 @@ class wearable::actuator::IHaptic : public wearable::actuator::IActuator inline static const std::string getPrefix(); virtual bool setHapticCommand(double& value) const = 0; + + virtual bool setHapticCommands(const std::vector& forceValue, const std::vector& vibrotactileValue) const = 0; }; inline const std::string wearable::actuator::IHaptic::getPrefix() diff --git a/msgs/thrift/WearableActuators.thrift b/msgs/thrift/WearableActuators.thrift index 86efeed..701abf2 100644 --- a/msgs/thrift/WearableActuators.thrift +++ b/msgs/thrift/WearableActuators.thrift @@ -27,3 +27,13 @@ struct WearableActuatorCommand { 2: double value; 3: double duration; } + +// ========================== +// Glove Actuator Command data type +// ========================== + +struct GloveActuatorCommand { + 1: ActuatorInfo info; + 2: list forceValue; + 3: list vibroTactileValue; +} diff --git a/wrappers/IWearActuators/include/IWearActuatorsWrapper.h b/wrappers/IWearActuators/include/IWearActuatorsWrapper.h index 6a28c0f..bff00e4 100644 --- a/wrappers/IWearActuators/include/IWearActuatorsWrapper.h +++ b/wrappers/IWearActuators/include/IWearActuatorsWrapper.h @@ -12,6 +12,7 @@ #include #include "thrift/WearableActuatorCommand.h" +#include "thrift/GloveActuatorCommand.h" namespace wearable { namespace wrappers { @@ -25,6 +26,7 @@ class wearable::wrappers::IWearActuatorsWrapper , public yarp::dev::IMultipleWrapper , public yarp::os::PeriodicThread , public yarp::os::TypedReaderCallback + , public yarp::os::TypedReaderCallback { private: class impl; @@ -40,6 +42,7 @@ class wearable::wrappers::IWearActuatorsWrapper // TypedReaderCallback void onRead(msg::WearableActuatorCommand& wearableActuatorCommand) override; + void onRead(msg::GloveActuatorCommand& gloveActuatorCommand) override; // PeriodicThread void run() override; diff --git a/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp b/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp index fb3f0ef..922f49d 100644 --- a/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp +++ b/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp @@ -23,11 +23,14 @@ class IWearActuatorsWrapper::impl : public wearable::msg::WearableActuatorComman std::string attachedWearableDeviceKey = "defaultIWearActuatorsWrapperDevice"; std::string actuatorCommandInputPortName; + std::string gloveActuatorCommandInputPortName; yarp::os::BufferedPort actuatorCommandInputPort; + yarp::os::BufferedPort gloveActuatorCommandInputPort; std::unordered_map> actuatorsMap; msg::WearableActuatorCommand wearableActuatorCommand; + msg::GloveActuatorCommand gloveActuatorCommand; wearable::IWear* iWear = nullptr; }; @@ -54,11 +57,43 @@ void IWearActuatorsWrapper::run() bool IWearActuatorsWrapper::open(yarp::os::Searchable& config) { - if (!config.check("actuatorCommandInputPortName") || !config.find("actuatorCommandInputPortName").isString()) { - yError() << LogPrefix << "actuatorCommandInputPortName parameter not found"; + if (!config.check("actuatorCommandInputPortName") || !config.check("gloveActuatorCommandInputPortName")) + { + yError() << LogPrefix << "No actuator command input ports found."; return false; } + // Find and configure yarp port related to the actuator commands + if (!config.find("actuatorCommandInputPortName").isString()) { + yWarning() << LogPrefix << "actuatorCommandInputPortName parameter not found. Haptic feedback will not be available."; + } + else + { + pImpl->actuatorCommandInputPortName = config.find("actuatorCommandInputPortName").asString(); + if(!pImpl->actuatorCommandInputPort.open(pImpl->actuatorCommandInputPortName)) + { + yError() << "Failed to open " << pImpl->actuatorCommandInputPortName << " yarp port"; + return false; + } + + // Set the callback to use onRead() method of this device + pImpl->actuatorCommandInputPort.useCallback(*this); + + } + if (!config.check("gloveActuatorCommandInputPortName") || !config.find("gloveActuatorCommandInputPortName").isString()) { + yWarning() << LogPrefix << "gloveActuatorCommandInputPortName parameter not found! The port will not be opened."; + } + else + { + pImpl->gloveActuatorCommandInputPortName = config.find("gloveActuatorCommandInputPortName").asString(); + if(!pImpl->gloveActuatorCommandInputPort.open(pImpl->gloveActuatorCommandInputPortName)) + { + yError() << "Failed to open " << pImpl->gloveActuatorCommandInputPortName << " yarp port"; + return false; + } + // Set the callback to use onRead() method of this device + pImpl->gloveActuatorCommandInputPort.useCallback(*this); + } if (!config.check("period")) { yInfo() << LogPrefix << "Using default period: " << DefaultPeriod << "s"; @@ -69,20 +104,6 @@ bool IWearActuatorsWrapper::open(yarp::os::Searchable& config) const double period = config.check("period", yarp::os::Value(DefaultPeriod)).asFloat64(); setPeriod(period); - pImpl->actuatorCommandInputPortName = config.find("actuatorCommandInputPortName").asString(); - - - // Configure yarp ports - - if(!pImpl->actuatorCommandInputPort.open(pImpl->actuatorCommandInputPortName)) - { - yError() << "Failed to open " << pImpl->actuatorCommandInputPortName << " yarp port"; - return false; - } - - // Set the callback to use onRead() method of this device - pImpl->actuatorCommandInputPort.useCallback(*this); - return true; } @@ -136,6 +157,39 @@ void IWearActuatorsWrapper::onRead(msg::WearableActuatorCommand& wearableActuato } } +void IWearActuatorsWrapper::onRead(msg::GloveActuatorCommand& gloveActuatorCommand) +{ + // Unpack the actuator in from incoming command + wearable::msg::ActuatorInfo info = gloveActuatorCommand.info; + + // Check if the commanded actuator name is available + if (pImpl->actuatorsMap.find(info.name) == pImpl->actuatorsMap.end()) + { + yWarning() << "Requested actuator with name " << info.name << " is not available in " << pImpl->attachedWearableDeviceKey << " wearable device \n \t Ignoring wearable actuation command."; + } + else // process the wearable actuator command + { + wearable::actuator::ActuatorType aType = pImpl->actuatorsMap[info.name]->getActuatorType(); + if (aType == wearable::actuator::ActuatorType::Haptic) + { + // Check if the actuator type in the wearable command is correct + if(info.type == wearable::msg::ActuatorType::HAPTIC) + { + // Get haptic actuator + wearable::ElementPtr castActuator = std::static_pointer_cast(pImpl->actuatorsMap[info.name]); + + // Send haptic command + castActuator->setHapticCommands(gloveActuatorCommand.forceValue, gloveActuatorCommand.vibroTactileValue); + } + } + else + { + yError() << "Actuator type is not correct!"; + return; + } + } +} + bool IWearActuatorsWrapper::close() { pImpl->actuatorCommandInputPort.close();