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) 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/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/app/xml/FTShoeLeftWearableDevice.xml b/app/xml/FTShoeLeftWearableDevice.xml new file mode 100644 index 00000000..f78ecc8a --- /dev/null +++ b/app/xml/FTShoeLeftWearableDevice.xml @@ -0,0 +1,148 @@ + + + + + + "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 + true + + + ftShoe_l + + + + + + + 0.01 + /FTShoeLeft/WearableData/data:o + /FTShoeLeft/WearableData/metadataRpc:o + + + FTShoeLeftIAnalogSensorToIWear + + + + + + diff --git a/app/xml/FTShoeRightWearableDevice.xml b/app/xml/FTShoeRightWearableDevice.xml new file mode 100644 index 00000000..aab0a1c3 --- /dev/null +++ b/app/xml/FTShoeRightWearableDevice.xml @@ -0,0 +1,148 @@ + + + + + + "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 + true + + + ftShoe_r + + + + + + + 0.01 + /FTShoeRight/WearableData/data:o + /FTShoeRight/WearableData/metadataRpc:o + + + FTShoeRightIAnalogSensorToIWear + + + + + + diff --git a/app/xml/ICubWearableDevice.xml b/app/xml/ICubWearableDevice.xml new file mode 100644 index 00000000..34eb7b86 --- /dev/null +++ b/app/xml/ICubWearableDevice.xml @@ -0,0 +1,21 @@ + + + + + /wholeBodyDynamics/left_arm/endEffectorWrench:o + /wholeBodyDynamics/right_arm/endEffectorWrench:o + + + + + 0.01 + /ICub/WearableData/data:o + /ICub/WearableData/metadataRpc:o + + + ICubWearableDevice + + + + + diff --git a/app/xml/SkinInsolesWearableDevice.xml b/app/xml/SkinInsolesWearableDevice.xml new file mode 100644 index 00000000..33e2dcf8 --- /dev/null +++ b/app/xml/SkinInsolesWearableDevice.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/conf/Xsens/xsens_windows_all_wrappers.xml b/app/xml/XsensSuitWearableDevice.xml similarity index 98% rename from conf/Xsens/xsens_windows_all_wrappers.xml rename to app/xml/XsensSuitWearableDevice.xml index 3378bcef..5b4149e7 100644 --- a/conf/Xsens/xsens_windows_all_wrappers.xml +++ b/app/xml/XsensSuitWearableDevice.xml @@ -28,7 +28,7 @@ true true - false + true 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 + + + 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/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp index 015d9a07..4d42ac86 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.data() + buffer.size() + offset, vector.data()); + return true; + } + } handler; // ================================ @@ -130,6 +138,7 @@ class ForceTorque6DSensor : public IForceTorque6DSensor { public: unsigned offset = 0; + bool groundReactionFT; ForceTorque6DSensor(SensorName name, SensorStatus status = SensorStatus::Unknown) : IForceTorque6DSensor(name, status) @@ -149,7 +158,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; } }; @@ -228,6 +247,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 // ==================== @@ -281,6 +325,8 @@ struct ParsedOptions size_t numberOfChannels; size_t channelOffset; + + bool getGroundReactionFT; }; class IAnalogSensorToIWear::Impl @@ -337,6 +383,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 +397,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 +406,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 +446,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; } @@ -408,6 +462,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 +701,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/devices/ICub/CMakeLists.txt b/devices/ICub/CMakeLists.txt new file mode 100644 index 00000000..dbddecf5 --- /dev/null +++ b/devices/ICub/CMakeLists.txt @@ -0,0 +1,31 @@ +# 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_prepare_plugin(icub_wearable_device + TYPE wearable::devices::ICub + INCLUDE ICub.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(ICub + src/ICub.cpp + include/ICub.h) + +target_include_directories(ICub PRIVATE + $) + +target_link_libraries(ICub PUBLIC + Wearable::IWear YARP::YARP_dev) + +yarp_install( + TARGETS ICub + 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/conf/ICub.xml b/devices/ICub/conf/ICub.xml new file mode 100644 index 00000000..d87f6fad --- /dev/null +++ b/devices/ICub/conf/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 + + + + + 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 diff --git a/devices/ICub/include/ICub.h b/devices/ICub/include/ICub.h new file mode 100644 index 00000000..6ac6b0d8 --- /dev/null +++ b/devices/ICub/include/ICub.h @@ -0,0 +1,208 @@ +/* + * 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; + + SensorPtr getSensor(const sensor::SensorName name) const override; + + VectorOfSensorPtr getSensors(const sensor::SensorType) const override; + + // IMPLEMENTED SENSORS + // ------------------- + + SensorPtr + getForceTorque6DSensor(const sensor::SensorName name) const override; + + // UNIMPLEMENTED SENSORS + // --------------------- + + 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::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..0951cab1 --- /dev/null +++ b/devices/ICub/src/ICub.cpp @@ -0,0 +1,345 @@ +/* + * 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 +#include + +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.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"; + } + } +}; + +class ICub::ICubImpl +{ +public: + class ICubForceTorque6DSensor; + + wearable::TimeStamp timeStamp; + + size_t nSensors; + std::vector sensorNames; + + bool leftHandFTDataReceived = false; + bool rightHandFTDataReceived = false; + + std::map> ftSensorsMap; + + const WearableName wearableName = "ICub"; + + WrenchPort leftHandFTPort; + WrenchPort rightHandFTPort; +}; + +// ========================================== +// ICub implementation of ForceTorque6DSensor +// ========================================== +class ICub::ICubImpl::ICubForceTorque6DSensor : public wearable::sensor::IForceTorque6DSensor +{ +public: + ICub::ICubImpl* icubImpl = nullptr; + + // ------------------------ + // 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) + { + // 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; } + + // ============================== + // IForceTorque6DSensor interface + // ============================== + 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") { + + 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") { + + 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) { + force3D[0] = wrench.at(0); + force3D[1] = wrench.at(1); + force3D[2] = wrench.at(2); + + 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."; + 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); + } + + 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"; + + // Configure clock + if (!yarp::os::Time::isSystemClock()) { + yarp::os::Time::useSystemClock(); + } + + // =============================== + // 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; + } + + if (!wbdHandsFTSensorsGroup.check("leftHand") || !wbdHandsFTSensorsGroup.check("rightHand")) { + yError() << LogPrefix << "REQUIRED parameter or NOT found"; + return false; + } + + // =============================== + // 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; + } + + 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()))) { + yError() << LogPrefix << "Failed to open or connect to " + << pImpl->rightHandFTPort.getName().c_str(); + return false; + } + + while (pImpl->rightHandFTPort.firstRun) { + pImpl->rightHandFTPort.useCallback(); + } + + pImpl->sensorNames.push_back("rightWBDFTSensor"); + + 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( + pImpl.get(), ft6dPrefix + pImpl->sensorNames[s]); + + pImpl->ftSensorsMap.emplace(ft6dPrefix + pImpl->sensorNames[s], + std::shared_ptr{ft6d}); + } + + return true; +} + +// --------------- +// Generic Methods +// --------------- + +wearable::WearableName ICub::getWearableName() const +{ + return pImpl->wearableName; +} + +wearable::WearStatus ICub::getStatus() const +{ + wearable::WearStatus status = wearable::WearStatus::Ok; + + for (const auto& s : getAllSensors()) { + if (s->getSensorStatus() != sensor::SensorStatus::Ok) { + status = wearable::WearStatus::Error; + } + } + + // Default return status is Ok + return status; +} + +wearable::TimeStamp ICub::getTimeStamp() const +{ + // Stamp count should be always zero + return {pImpl->timeStamp.time, 0}; +} + +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 +// --------------------------- + +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::ForceTorque6DSensor: { + outVec.reserve(pImpl->ftSensorsMap.size()); + for (const auto& ft6d : pImpl->ftSensorsMap) { + outVec.push_back(static_cast>(ft6d.second)); + } + + 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 + 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)); +} 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 2417c159..6b5cc39e 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 = true; mutable std::recursive_mutex mutex; @@ -135,15 +136,38 @@ 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; + if (!(config.check("useRPC"))) { + yWarning() << logPrefix << "OPTIONAL parameter useRPC NOT found"; } - 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"; - return false; + 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) + { + if (!config.check("wearableRPCPorts")) + { + yInfo() << logPrefix << "wearableRPCPorts parameter does not exist. RPC port will not be used"; + pImpl->useRPC = false; + } + 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; + } + } } } @@ -168,8 +192,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 +204,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 +254,93 @@ bool IWearRemapper::open(yarp::os::Searchable& config) // ==================== // OPEN INPUT RPC PORTS // ==================== - 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; + if(pImpl->useRPC) + { + yDebug() << logPrefix << "Opening input RPC ports"; - // 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); } } } @@ -802,6 +838,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); @@ -946,7 +989,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; } 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