Fix Haptic glove transmission - GloveDevice

.gitignore

@@ -8,3 +8,5 @@ CMakeLists.txt.*
 *.autosave
 *.original
 *.*~
+.vscode/c_cpp_properties.json
+.vscode/settings.json

devices/HapticGlove/conf/HapticGlove.xml

@@ -17,7 +17,7 @@
     </device>
 
     <device type="iwear_wrapper" name="LeftHapticGloveWearableDeviceWrapper">
-        <param extern-name="period" name="period">0.01</param>
+        <param extern-name="period" name="period">0.1</param>
         <param name="dataPortName">/WearableData/HapticGlove/LeftHand/data:o</param>
         <param name="rpcPortName">/WearableData/HapticGlove/LeftHand/metadataRpc:o</param>
         <action phase="startup" level="5" type="attach">
@@ -29,8 +29,8 @@
     </device>
 
     <device type="iwearactuators_wrapper" name="LeftHapticGloveWearableDeviceActuatorsWrapper">
-        <param extern-name="period" name="period">0.01</param>
-        <param name="actuatorCommandInputPortName">/WearableData/HapticGlove/LeftHand/Actuators/input:i</param>
+        <param extern-name="period" name="period">0.1</param>
+        <param name="gloveActuatorCommandInputPortName">/WearableData/HapticGlove/LeftHand/Actuators/input:i</param>
         <action phase="startup" level="5" type="attach">
             <paramlist name="networks">
                 <elem name="HapticGloveWearableDeviceActuatorWrapperLabel"> LeftHapticGloveWearableDevice </elem>
@@ -53,7 +53,7 @@
     </device>
 
     <device type="iwear_wrapper" name="RightHapticGloveWearableDeviceWrapper">
-        <param extern-name="period" name="period">0.01</param>
+        <param extern-name="period" name="period">0.1</param>
         <param name="dataPortName">/WearableData/HapticGlove/RightHand/data:o</param>
         <param name="rpcPortName">/WearableData/HapticGlove/RightHand/metadataRpc:o</param>
         <action phase="startup" level="5" type="attach">
@@ -65,8 +65,8 @@
     </device>
 
     <device type="iwearactuators_wrapper" name="RightHapticGloveWearableDeviceActuatorsWrapper">
-        <param extern-name="period" name="period">0.01</param>
-        <param name="actuatorCommandInputPortName">/WearableData/HapticGlove/RightHand/Actuators/input:i</param>
+        <param extern-name="period" name="period">0.1</param>
+        <param name="gloveActuatorCommandInputPortName">/WearableData/HapticGlove/RightHand/Actuators/input:i</param>
         <action phase="startup" level="5" type="attach">
             <paramlist name="networks">
                 <elem name="HapticGloveWearableDeviceActuatorWrapperLabel"> RightHapticGloveWearableDevice </elem>

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<senseGlove::SenseGloveHelper> 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<SenseGloveImpl::SenseGloveHapticActuator>(
+                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<SenseGloveImpl::SenseGloveHapticActuator>(
@@ -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<std::mutex> lock(m_gloveImpl->mutex);
+    bool setHapticCommands(std::vector<double>& forceValue, std::vector<double>& 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<std::mutex> 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:

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(std::vector<double>& forceValue, std::vector<double>& vibrotactileValue) const = 0;
 };
 
 inline const std::string wearable::actuator::IHaptic::getPrefix()

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<double> forceValue;
+  3: list<double> vibroTactileValue;
+}

wrappers/IWearActuators/include/IWearActuatorsWrapper.h

@@ -12,6 +12,7 @@
 #include <yarp/os/TypedReaderCallback.h>
 
 #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<msg::WearableActuatorCommand>
+    , public yarp::os::TypedReaderCallback<msg::GloveActuatorCommand>
 {
 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;

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<wearable::msg::WearableActuatorCommand> actuatorCommandInputPort;
+    yarp::os::BufferedPort<wearable::msg::GloveActuatorCommand> gloveActuatorCommandInputPort;
 
     std::unordered_map<std::string, wearable::ElementPtr<const actuator::IActuator>> actuatorsMap;
 
     msg::WearableActuatorCommand wearableActuatorCommand;
+    msg::GloveActuatorCommand gloveActuatorCommand;
 
     wearable::IWear* iWear = nullptr;
 };
@@ -59,6 +62,19 @@ bool IWearActuatorsWrapper::open(yarp::os::Searchable& config)
         return false;
     }
 
+    // Find and configure yarp port related to the actuator commands
+    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;
+        }
+    }
 
     if (!config.check("period")) {
         yInfo() << LogPrefix << "Using default period: " << DefaultPeriod << "s";
@@ -71,7 +87,6 @@ bool IWearActuatorsWrapper::open(yarp::os::Searchable& config)
 
     pImpl->actuatorCommandInputPortName = config.find("actuatorCommandInputPortName").asString();
 
-
     // Configure yarp ports
 
     if(!pImpl->actuatorCommandInputPort.open(pImpl->actuatorCommandInputPortName))
@@ -82,6 +97,7 @@ bool IWearActuatorsWrapper::open(yarp::os::Searchable& config)
 
     // Set the callback to use onRead() method of this device
     pImpl->actuatorCommandInputPort.useCallback(*this);
+    pImpl->gloveActuatorCommandInputPort.useCallback(*this);
 
     return true;
 }
@@ -136,6 +152,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<const wearable::actuator::IHaptic> castActuator = std::static_pointer_cast<const wearable::actuator::IHaptic>(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();