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();