Update devices with VirtualJointKinSensor

robotology · Apr 4, 2019 · 2867767 · 2867767
1 parent 9a450b2
commit 2867767
Show file tree

Hide file tree

Showing 5 changed files with 73 additions and 0 deletions.
diff --git a/devices/IAnalogSensorToIWear/include/IAnalogSensorToIWear.h b/devices/IAnalogSensorToIWear/include/IAnalogSensorToIWear.h
@@ -121,6 +121,9 @@ class wearable::devices::IAnalogSensorToIWear
     SensorPtr<const sensor::IVirtualLinkKinSensor>
     getVirtualLinkKinSensor(const sensor::SensorName name) const override;
 
+    SensorPtr<const sensor::IVirtualJointKinSensor>
+    getVirtualJointKinSensor(const sensor::SensorName name) const override;
+
     SensorPtr<const sensor::IVirtualSphericalJointKinSensor>
     getVirtualSphericalJointKinSensor(const sensor::SensorName name) const override;
 };

diff --git a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp
@@ -308,6 +308,8 @@ wearable::sensor::SensorType sensorTypeFromString(std::string sensorTypeString)
         return wearable::sensor::SensorType::Torque3DSensor;
     else if (sensorTypeString == "VirtualLinkKinSensor")
         return wearable::sensor::SensorType::VirtualLinkKinSensor;
+    else if (sensorTypeString == "VirtualJointKinSensor")
+        return wearable::sensor::SensorType::VirtualJointKinSensor;
     else if (sensorTypeString == "VirtualSphericalJointKinSensor")
         return wearable::sensor::SensorType::VirtualSphericalJointKinSensor;
     else {
@@ -717,6 +719,13 @@ IAnalogSensorToIWear::getVirtualLinkKinSensor(const wearable::sensor::SensorName
     return nullptr;
 }
 
+wearable::SensorPtr<const wearable::sensor::IVirtualJointKinSensor>
+IAnalogSensorToIWear::getVirtualJointKinSensor(
+    const wearable::sensor::SensorName /*name*/) const
+{
+    return nullptr;
+}
+
 wearable::SensorPtr<const wearable::sensor::IVirtualSphericalJointKinSensor>
 IAnalogSensorToIWear::getVirtualSphericalJointKinSensor(
     const wearable::sensor::SensorName /*name*/) const

diff --git a/devices/IWearRemapper/include/IWearRemapper.h b/devices/IWearRemapper/include/IWearRemapper.h
@@ -106,6 +106,9 @@ class wearable::devices::IWearRemapper
     SensorPtr<const sensor::IVirtualLinkKinSensor>
     getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const override;
 
+    SensorPtr<const sensor::IVirtualJointKinSensor>
+    getVirtualJointKinSensor(const sensor::SensorName /*name*/) const override;
+
     SensorPtr<const sensor::IVirtualSphericalJointKinSensor>
     getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const override;
 };

diff --git a/devices/IWearRemapper/src/IWearRemapper.cpp b/devices/IWearRemapper/src/IWearRemapper.cpp
@@ -67,6 +67,8 @@ class IWearRemapper::impl
     std::map<std::string, std::shared_ptr<sensor::impl::Torque3DSensor>> torque3DSensors;
     std::map<std::string, std::shared_ptr<sensor::impl::VirtualLinkKinSensor>>
         virtualLinkKinSensors;
+    std::map<std::string, std::shared_ptr<sensor::impl::VirtualJointKinSensor>>
+        virtualJointKinSensors;
     std::map<std::string, std::shared_ptr<sensor::impl::VirtualSphericalJointKinSensor>>
         virtualSphericalJointKinSensors;
 
@@ -102,6 +104,8 @@ const std::map<wearable::msg::SensorType, wearable::sensor::SensorType> MapSenso
     {wearable::msg::SensorType::TORQUE_3D_SENSOR, wearable::sensor::SensorType::Torque3DSensor},
     {wearable::msg::SensorType::VIRTUAL_LINK_KIN_SENSOR,
      wearable::sensor::SensorType::VirtualLinkKinSensor},
+    {wearable::msg::SensorType::VIRTUAL_JOINT_KIN_SENSOR,
+     wearable::sensor::SensorType::VirtualJointKinSensor},
     {wearable::msg::SensorType::VIRTUAL_SPHERICAL_JOINT_KIN_SENSOR,
      wearable::sensor::SensorType::VirtualSphericalJointKinSensor}};
 
@@ -780,6 +784,35 @@ void IWearRemapper::onRead(msg::WearableData& receivedWearData)
         sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status));
     }
 
+    for (auto& s : receivedWearData.virtualJointKinSensors) {
+        const auto& inputSensorName = s.first;
+        const auto& wearDataInputSensor = s.second;
+        // ==========================
+        // FORWARD TO THE OUTPUT PORT
+        // ==========================
+        auto& wearData = pImpl->outputPortWearData.prepare();
+        wearData.virtualJointKinSensors[inputSensorName] = wearDataInputSensor;
+        // ====================
+        // EXPOSE THE INTERFACE
+        // ====================
+        auto isensor = getVirtualJointKinSensor(inputSensorName);
+        if (!isensor) {
+            yError() << logPrefix << "Failed to get VirtualJointKinSensor"
+                     << inputSensorName;
+            askToStop();
+            return;
+        }
+        const auto* constSensor =
+            static_cast<const sensor::impl::VirtualJointKinSensor*>(isensor.get());
+        auto* sensor = const_cast<sensor::impl::VirtualJointKinSensor*>(constSensor);
+        // Copy its data to the buffer used for exposing the IWear interface
+        sensor->setBuffer({wearDataInputSensor.data.angle},
+                          {wearDataInputSensor.data.velocity},
+                          {wearDataInputSensor.data.acceleration});
+        // Set the status
+        sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status));
+    }
+
     for (auto& s : receivedWearData.virtualSphericalJointKinSensors) {
         const auto& inputSensorName = s.first;
         const auto& wearDataInputSensor = s.second;
@@ -954,6 +987,11 @@ IWearRemapper::getSensors(const sensor::SensorType type) const
                 sensors.push_back(s.second);
             }
             break;
+        case sensor::SensorType::VirtualJointKinSensor:
+            for (const auto& s : pImpl->virtualJointKinSensors) {
+                sensors.push_back(s.second);
+            }
+        break;
         case sensor::SensorType::VirtualSphericalJointKinSensor:
             for (const auto& s : pImpl->virtualSphericalJointKinSensors) {
                 sensors.push_back(s.second);
@@ -1108,6 +1146,17 @@ IWearRemapper::getVirtualLinkKinSensor(const sensor::SensorName name) const
             name, sensor::SensorType::VirtualLinkKinSensor, pImpl->virtualLinkKinSensors);
 }
 
+wearable::SensorPtr<const sensor::IVirtualJointKinSensor>
+IWearRemapper::getVirtualJointKinSensor(const sensor::SensorName name) const
+{
+    std::lock_guard<std::recursive_mutex> lock(pImpl->mutex);
+    return pImpl->getSensor<const sensor::IVirtualJointKinSensor,
+                            sensor::impl::VirtualJointKinSensor>(
+        name,
+        sensor::SensorType::VirtualJointKinSensor,
+        pImpl->virtualJointKinSensors);
+}
+
 wearable::SensorPtr<const sensor::IVirtualSphericalJointKinSensor>
 IWearRemapper::getVirtualSphericalJointKinSensor(const sensor::SensorName name) const
 {

diff --git a/devices/XsensSuit/include/XsensSuit.h b/devices/XsensSuit/include/XsensSuit.h
@@ -135,6 +135,9 @@ class wearable::devices::XsensSuit final
 
     inline SensorPtr<const sensor::ITorque3DSensor>
     getTorque3DSensor(const sensor::SensorName /*name*/) const override;
+
+    inline SensorPtr<const sensor::IVirtualJointKinSensor>
+    getVirtualJointKinSensor(const sensor::SensorName /*name*/) const override;
 };
 
 inline wearable::SensorPtr<const wearable::sensor::IAccelerometer>
@@ -185,4 +188,10 @@ wearable::devices::XsensSuit::getTorque3DSensor(const sensor::SensorName /*name*
     return nullptr;
 }
 
+inline wearable::SensorPtr<const wearable::sensor::IVirtualJointKinSensor>
+wearable::devices::XsensSuit::getVirtualJointKinSensor(const sensor::SensorName /*name*/) const
+{
+    return nullptr;
+}
+
 #endif // XSENSSUIT_H