diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 707fb5a94..d861340fa 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1 +1 @@ -* @lrapetti +* @traversaro @S-Dafarra @dariosortino diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 000000000..b73537336 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,35 @@ +--- +name: Bug report +about: Create a report to help us improve + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: +1. Go to '...' +2. Click on '....' +3. Scroll down to '....' +4. See error + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Screenshots** +If applicable, add screenshots to help explain your problem. + +**Desktop (please complete the following information):** + - OS: [e.g. iOS] + - Browser [e.g. chrome, safari] + - Version [e.g. 22] + +**Smartphone (please complete the following information):** + - Device: [e.g. iPhone6] + - OS: [e.g. iOS8.1] + - Browser [e.g. stock browser, safari] + - Version [e.g. 22] + +**Additional context** +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 000000000..066b2d920 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,17 @@ +--- +name: Feature request +about: Suggest an idea for this project + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 36e52814b..23a178171 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,199 +1,62 @@ -name: C++ CI Workflow - -# template derived from https://github.com/robotology/how-to-export-cpp-library/tree/master/.github/workflows -# and https://github.com/robotology/idyntree/blob/master/.github/workflows/ci.yml +name: C++ CI Workflow with dependencies installed via conda on: - push: - pull_request: - schedule: - # run a cron job for a nightly build - # * is a special character in YAML so you have to quote this string - # Execute a "nightly" build at 2 AM UTC - - cron: '0 2 * * *' - -env: - YCM_TAG: v0.15.3 - YARP_TAG: v3.8.1 - iDynTree_TAG: v12.0.0 - wearables_TAG: v1.7.1 - icub_main_TAG: v2.2.1 - osqp_TAG: v0.6.3 - OsqpEigen_TAG: v0.8.1 - matioCpp_TAG: v0.2.4 - robometry_TAG: v1.2.1 + push: + pull_request: + workflow_dispatch: + schedule: + # * is a special character in YAML so you have to quote this string + # Execute a "nightly" build at 2 AM UTC + - cron: '0 2 * * *' jobs: - build: - name: '[${{matrix.os}}@${{matrix.build_type}}]' - runs-on: ${{matrix.os}} - strategy: - matrix: - build_type: [Release] - os: [ubuntu-latest, windows-latest, macos-12] - fail-fast: false - - steps: - # Clone the repository in $GITHUB_WORKSPACE - - uses: actions/checkout@master - - # Use mamba for Windows and macos dependencies - - uses: mamba-org/setup-micromamba@v1 - if: matrix.os == 'windows-latest' || contains(matrix.os, 'macos') - with: - environment-file: ci_env.yml - channel-priority: true - - # Print the environment variables to simplify development and debugging - - name: Environment Variables - if: matrix.os == 'ubuntu-latest' - shell: bash - run: env - - # ============ - # DEPENDENCIES - # ============ - - - name: Dependencies [Ubuntu] - if: matrix.os == 'ubuntu-latest' - run: | - sudo apt update - sudo apt install git build-essential cmake libace-dev coinor-libipopt-dev libboost-system-dev libboost-filesystem-dev \ - libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev libqt5charts5-dev \ - libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python3-dev valgrind libassimp-dev libirrlicht-dev libmatio-dev libglfw3-dev - - - name: Source-based Dependencies [Ubuntu] - if: steps.cache-source-deps.outputs.cache-hit != 'true' && matrix.os == 'ubuntu-latest' - shell: bash - run: | - # YCM - cd ${GITHUB_WORKSPACE} - git clone -b ${YCM_TAG} https://github.com/robotology/ycm - cd ycm - mkdir -p build - cd build - cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{matrix.build_type}} --target install - - # YARP - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/yarp - cd yarp - git checkout ${YARP_TAG} - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{matrix.build_type}} --target install - - # icub-main - cd ${GITHUB_WORKSPACE} - git clone -b ${icub_main_TAG} https://github.com/robotology/icub-main - cd icub-main - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{matrix.build_type}} --target install - - # iDynTree - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/iDynTree - cd iDynTree - git checkout ${iDynTree_TAG} - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DIDYNTREE_USES_IRRLICHT:BOOL=ON \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{matrix.build_type}} --target install - - # wearables - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/wearables - cd wearables - git checkout ${wearables_TAG} - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{matrix.build_type}} --target install - - # osqp - cd ${GITHUB_WORKSPACE} - git clone --recursive -b ${osqp_TAG} https://github.com/oxfordcontrol/osqp - cd osqp - mkdir -p build - cd build - cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{ matrix.build_type }} --target install - - # OsqpEigen - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/osqp-eigen - cd osqp-eigen - git checkout ${OsqpEigen_TAG} - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{matrix.build_type}} --target install - - # matioCpp - cd ${GITHUB_WORKSPACE} - git clone https://github.com/dic-iit/matio-cpp - cd matio-cpp - git checkout ${matioCpp_TAG} - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{matrix.build_type}} --target install - # robometry - cd ${GITHUB_WORKSPACE} - git clone https://github.com/robotology/robometry - cd robometry - git checkout ${robometry_TAG} - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. - cmake --build . --config ${{matrix.build_type}} --target install - - # =================== - # CMAKE-BASED PROJECT - # =================== - # We will just configure and build the project now. Further modifications and tests can be added - # Configure step - - name: Configure [Windows] - if: matrix.os == 'windows-latest' - shell: bash -l {0} - run: | - mkdir -p build - cd build - cmake -A x64 cmake -G"Visual Studio 17 2022" -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_BUILD_TYPE=${{matrix.build_type}} \ - -DHUMANSTATEPROVIDER_ENABLE_VISUALIZER:BOOL=ON \ - -DHUMANSTATEPROVIDER_ENABLE_LOGGER:BOOL=ON \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - - - name: Configure [Ubuntu/macOS] - if: matrix.os == 'ubuntu-latest' || contains(matrix.os, 'macos') - shell: bash -l {0} - run: | - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_BUILD_TYPE=${{matrix.build_type}} \ - -DHUMANSTATEPROVIDER_ENABLE_VISUALIZER:BOOL=ON \ - -DHUMANSTATEPROVIDER_ENABLE_LOGGER:BOOL=ON \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install .. - - # Build step - - name: Build - shell: bash - run: | - cd build - cmake --build . --config ${{matrix.build_type}} - - + build: + name: '[${{ matrix.os }}@${{ matrix.build_type }}@conda]' + runs-on: ${{ matrix.os }} + strategy: + matrix: + build_type: [Release] + os: [ubuntu-latest, windows-2019, macOS-latest] + fail-fast: false + + steps: + - uses: actions/checkout@v2 + + - uses: mamba-org/setup-micromamba@v1 + with: + environment-file: ci_env.yml + + - name: Configure VS Toolchain (Windows) + if: contains(matrix.os, 'windows') + uses: ilammy/msvc-dev-cmd@v1.12.1 + + - name: Setup compilation env variables (Windows) + if: contains(matrix.os, 'windows') + shell: bash -l {0} + run: | + bash_vc_install=${VCToolsInstallDir//\\//} + compiler_path=${bash_vc_install}bin/Hostx64/x64/cl.exe + echo "CC=${compiler_path}" >> $GITHUB_ENV + echo "CXX=${compiler_path}" >> $GITHUB_ENV + + - name: Configure + shell: bash -l {0} + run: | + mkdir -p build + cd build + cmake -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install -DBUILD_TESTING:BOOL=ON \ + -DWEARABLES_COMPILE_PYTHON_BINDINGS=ON -DHUMANSTATEPROVIDER_ENABLE_VISUALIZER:BOOL=ON \ + -DHUMANSTATEPROVIDER_ENABLE_LOGGER:BOOL=ON .. + + - name: Build + shell: bash -l {0} + run: | + cd build + cmake --build . --config ${{ matrix.build_type }} + + - name: Test + shell: bash -l {0} + run: | + cd build + ctest --repeat until-pass:5 --output-on-failure -C ${{ matrix.build_type }} . diff --git a/.gitignore b/.gitignore index 21da9b5fe..b820f7bba 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,17 @@ build* *.txt.user* *.vscode* .vs/ +build/ +build*/ +install/ +CMakeLists.txt.* +.DS_Store +*.bak +*.orig +*.autosave +*.original +*.*~ +.vscode/c_cpp_properties.json +.vscode/settings.json +.vs/* CMakeSettings.json diff --git a/CHANGELOG.md b/CHANGELOG.md index f5f510386..16ab2ab68 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,5 @@ # Changelog + This file documents notable changes to this project done before November 2023. For changes after that date, please refer to the release notes of each release at https://github.com/robotology/human-dynamics-estimation/releases. ## [3.0.0] - 2023-11-09 diff --git a/CMakeLists.txt b/CMakeLists.txt index c2f447374..690c49775 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 3.5) project(HumanDynamicsEstimation LANGUAGES CXX - VERSION 3.1.1) + VERSION 4.0.0) # ===================== # PROJECT CONFIGURATION @@ -55,7 +55,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Flag to enable python bindings option(HDE_COMPILE_PYTHON_BINDINGS "Flag that enables building the bindings" OFF) -# Plugins are force to be Shared/Dynamic Library +# Plugins are force to be Shared/Dynamic Library set(YARP_FORCE_DYNAMIC_PLUGINS ON) # Control where binaries and libraries are placed in the build folder. @@ -74,17 +74,65 @@ if(NOT MSVC) USE_LINK_PATH) endif() + +#Function to automatize the process of creating python bindings +include(AddWearablesPythonModule) + + +# Tweak linker flags in Linux +if(UNIX AND NOT APPLE) + get_filename_component(LINKER_BIN ${CMAKE_LINKER} NAME) + if(${LINKER_BIN} STREQUAL "ld") + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--unresolved-symbols=report-all") + endif() +endif() + +if(WIN32) + #Add compile definition to suppress warning related to header + add_compile_definitions(_SILENCE_EXPERIMENTAL_FILESYSTEM_DEPRECATION_WARNING) +endif() + +# Flag to enable Paexo wearable device +option(WEARABLES_COMPILE_PYTHON_BINDINGS "Flag that enables building the bindings" OFF) + +# Flag to enable XSensSuit wearable device +find_package(XsensXME QUIET) +option(ENABLE_XsensSuit "Flag that enables building XsensSuit wearable device" ${XsensXME_FOUND}) +if(ENABLE_XsensSuit) + add_subdirectory(XSensMVN) +endif() + +# Flag to enable Paexo wearable device +option(ENABLE_Paexo "Flag that enables building Paexo wearable device" OFF) + +# Flag to enable SenseGlove wearable device +option(ENABLE_HapticGlove "Flag that enables building Haptic Sense Glove wearable device" OFF) + +# Flag to enable IWearFrameVisualizer module +find_package(iDynTree QUIET) +option(ENABLE_FrameVisualizer "Flag that enables building IWearFrameVisualizer module" ${iDynTree_FOUND}) + +# Flag to enable IWearLogger device +find_package(robometry QUIET) +option(ENABLE_Logger "Flag that enables building Wearable Logger device" ${robometry_FOUND}) + +# Flag to enable ICub wearable device +option(ENABLE_ICub "Flag that enables building iCub wearable device" ${iDynTree_FOUND}) + + # Add the uninstall target include(AddUninstallTarget) # =================== # PROJECT DIRECTORIES # =================== +include(InstallBasicPackageFiles) yarp_configure_plugins_installation(hde) add_subdirectory(conf) add_subdirectory(msgs) add_subdirectory(interfaces) +add_subdirectory(impl) add_subdirectory(devices) add_subdirectory(modules) add_subdirectory(servers) @@ -93,7 +141,6 @@ add_subdirectory(publishers) add_subdirectory(bindings) add_subdirectory(HumanDynamicsEstimationLibrary) -include(InstallBasicPackageFiles) install_basic_package_files(${PROJECT_NAME} VERSION ${${PROJECT_NAME}_VERSION} COMPATIBILITY AnyNewerVersion diff --git a/README.md b/README.md index 54084f537..2b6f492de 100644 --- a/README.md +++ b/README.md @@ -149,3 +149,65 @@ The development is also supported by the [Istituto Italiano di Tecnologia](http: ## Mantainers * Lorenzo Rapetti ([@lrapetti](https://github.com/lrapetti)) + +# Wearables README + +[![C++ CI Action Status](https://github.com/robotology/wearables/workflows/C++%20CI%20Workflow/badge.svg)](https://github.com/robotology/wearables/actions/workflows/ci.yml) + +# Setting up +## Dependencies +Here following there is a list of the required dependencies you need for using this repository. Instead, the optional dependencies, specific for each wearable-device, are listed in [wearable-device-sources](#wearable-device-sources). + +### Required +* [CMake](https://cmake.org/download/) open-source, cross-platform family of tools designed to build, test and package software. +* [YARP](http://www.yarp.it/) software package to handle the data comunication. + +It must be noted that `YARP`, together with a subset of the optional dependencies, can be installed together in one place using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild) enabling its [`Human-Dynamics` profile](https://github.com/robotology/robotology-superbuild#human-dynamics). + +### Optional +* [robometry](https://github.com/robotology/robometry) device: if present, the `IWearLogger` device is installed by default. +* [iDynTree](https://github.com/robotology/idyntree): if present, the `IWearFrameVisualizer` module and `ICub` device are installed by default. +* [pybind11](https://github.com/pybind/pybind11): if present, the user can enable `WEARABLES_COMPILE_PYTHON_BINDINGS` to enable the compilation of the python bindings. + +## Build the suite +### Linux/macOS + +```sh +git clone https://github.com/robotology/wearables.git +cd wearables +mkdir build && cd build +cmake ../ +make +[sudo] make install +``` +Notice: `sudo` is not necessary if you specify the `CMAKE_INSTALL_PREFIX`. In this case it is necessary to add in the `.bashrc` or `.bash_profile` the following lines: +``` sh +export WEARABLES_INSTALL_DIR=/path/where/you/installed +export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${WEARABLES_INSTALL_DIR}/share/yarp +``` +Note that this is not necessary if you install `wearables` via the `robotology-superbuild`. + +### Optional flags +The compilation of some of the software components in `wearables` can be enabled using the following cmake flags: +- `ENABLE_FrameVisualizer`: enable the compilation of the `IWearFrameVisualizer` module that allows to visualize wearble inertial measurements. +- `ENABLE_Logger`: enable the compilation of the `IWearLogger` device that allows to save wearable data as `.mat` or stream the data on YARP as analog vector data. +- `ENABLE_`: enable the compilation of the optional wearable device (see documentation in [Wearable device sources](#wearable-device-sources)). + + +# Wearable device sources +Wearable devices contained in [`/devices/`](/devices) are [YARP devices](http://www.yarp.it/git-master/note_devices.html) that exposes sensors data using `IWear` interface. List of the dependencies and documentation for running some of the wearable data sources can be found at the links in the table below. The compilation of some of theese devices can be enable/disabled changing the CMAKE option `ENABLE_`. + +| Device Name | Description | OS | Dependencies| Documentation | +|---------------|------|---------------|----------------------------------------------------------|------| +| IAnalogSensor | Exposes YARP [`IAnalogSensor` Interface](http://www.yarp.it/git-master/classyarp_1_1dev_1_1IAnalogSensor.html). | Linux/MacOS/Windows | - | - | +| FTShoes | Exposes YARP [`ftShoe`](https://github.com/robotology/forcetorque-yarp-devices/tree/master/ftShoe) data. | Linux/MacOS/Windows | [`forcetorque-yarp-devices`](https://github.com/robotology/forcetorque-yarp-devices) | [:books:](/doc/How-to-run-FTshoes.md) | +| XsensSuit | Exposes [`XsensSuit`](https://www.xsens.com/motion-capture) data. | Windows | xsens MVN SDK 2018.0.3 | [:books:](/doc/How-to-run-XsensSuit.md) | +| ICub | Exposes [iCub](https://icub.iit.it/) robot data. | Linux/MacOS/Windows | [`iDynTree`](https://github.com/robotology/idyntree) | [:books:](/doc/How-to-run-iCub-as-wearable-source.md) | +| Paexo | Exposes [Paexo](https://paexo.com/?lang=en) data. | Linux | `iFeelDriver` (Contact the maintainer for more details) | - | +| HapticGlove | Exposes [SenseGlove](https://www.senseglove.com/product/developers-kit/) data. | Linux/Windows | [`SenseGloveSDK`](https://github.com/Adjuvo/SenseGlove-API) | [:books:](./doc/How-to-compile-and-run-HapticGlove.md) | +| IFrameTransform | Exposes YARP [`IFrameTransform` Interface](http://www.yarp.it/git-master/classyarp_1_1dev_1_1IFrameTransform.html). | Linux/MacOS/Windows | - | - | + +## Mantainers + +* Lorenzo Rapetti ([@lrapetti](https://github.com/lrapetti)) +>>>>>>> wearables/master diff --git a/XSensMVN/CMakeLists.txt b/XSensMVN/CMakeLists.txt new file mode 100644 index 000000000..d379df443 --- /dev/null +++ b/XSensMVN/CMakeLists.txt @@ -0,0 +1,36 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +set(XSENS_MVN_SOURCES + ${CMAKE_CURRENT_SOURCE_DIR}/src/XSensMVNCalibrator.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/XSensMVNDriverImpl.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/XSensMVNDriver.cpp) + +set(XSENS_MVN_INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensLogger.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensCalibrationQualities.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensMVNCalibrator.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensMVNDriverImpl.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/XSensMVNDriver.h) + +add_library(XSensMVN + ${XSENS_MVN_SOURCES} + ${XSENS_MVN_INCLUDES}) + +target_include_directories(XSensMVN + PUBLIC $ + $) + +target_link_libraries(XSensMVN ${XsensXME_LIBRARIES} IXsensMVNControl) + +install(TARGETS XSensMVN + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + +install( + FILES ${XSENS_MVN_INCLUDES} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/XSensMVN) + +add_subdirectory(test) diff --git a/XSensMVN/include/XSensCalibrationQualities.h b/XSensMVN/include/XSensCalibrationQualities.h new file mode 100644 index 000000000..9a12e0ec4 --- /dev/null +++ b/XSensMVN/include/XSensCalibrationQualities.h @@ -0,0 +1,21 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef XSENS_CALIBRATION_QUALITIES_H +#define XSENS_CALIBRATION_QUALITIES_H + +#include + +namespace xsensmvn { + + enum CalibrationQuality + { + UNKNOWN = 0, // Unknown quality or not yet performed + FAILED = 1, // Failed to retrieve a meaningful calibration + POOR = 2, // Some serious issues, a new calibration is recommended + ACCEPTABLE = 3, // Some anomalies, but nothing serious + GOOD = 4, // Good resulting quality, no problems detected + }; +} + +#endif // XSENS_CALIBRATION_QUALITIES_H diff --git a/XSensMVN/include/XSensLogger.h b/XSensMVN/include/XSensLogger.h new file mode 100644 index 000000000..194d7c15c --- /dev/null +++ b/XSensMVN/include/XSensLogger.h @@ -0,0 +1,58 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef XSENS_LOGGER_H +#define XSENS_LOGGER_H + +/* Hack required to print verbose log messages when not in Release + * It is not possible to directly use the NDEBUG variable since XSens defines regardless from the + * compile option + * This brings as consequence that the Logger should be included in the .h files ALWAYS before XSens + * SDK files + */ +#ifndef NDEBUG +#define XS_VERBOSE_LOG +#endif + +#include +#include +#include + +// Define macros to print YARP-like info, warning, and error messages with exactly the same usage +// of std::cout (operator <<) +#ifndef xsError +#define xsError \ + std::cerr << std::endl \ + << printLogMessage("[ERROR]", __FILE__, std::to_string(__LINE__), __FUNCTION__) +#endif + +#ifndef xsWarning +#define xsWarning \ + std::cout << std::endl \ + << printLogMessage("[WARNING]", __FILE__, std::to_string(__LINE__), __FUNCTION__) +#endif + +#ifndef xsInfo +#define xsInfo \ + std::cout << std::endl \ + << printLogMessage("[INFO]", __FILE__, std::to_string(__LINE__), __FUNCTION__) +#endif + +// Utility function to print "advanced" log messages +inline std::string printLogMessage(const std::string& msgType, + const std::string& file = "", + const std::string& line = "", + const std::string& function = "") +{ + std::string filename = file.substr(file.find_last_of("/\\") + 1); + std::string ss; + ss = msgType + " "; + + // If debug print addictional metadata +#ifdef XS_VERBOSE_LOG + ss = ss + filename + "@" + function + ":" + line + " - "; +#endif + return ss; +}; + +#endif // XSENS_LOGGER_H diff --git a/XSensMVN/include/XSensMVNCalibrator.h b/XSensMVN/include/XSensMVNCalibrator.h new file mode 100644 index 000000000..25eaa7594 --- /dev/null +++ b/XSensMVN/include/XSensMVNCalibrator.h @@ -0,0 +1,93 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef XSENS_MVN_CALIBRATOR_H +#define XSENS_MVN_CALIBRATOR_H + +#include "XSensCalibrationQualities.h" +#include "XSensLogger.h" + +#include +#include + +#include +#include +#include +#include +#include + +namespace xsensmvn { + class XSensMVNCalibrator; +} // namespace xsensmvn + +class xsensmvn::XSensMVNCalibrator : public XmeCallback +{ +private: + /* ---------- * + * Variables * + * ---------- */ + + // Reference to the XmeControl object owned from the driver thread + XmeControl& m_suitsConnector; + + // Minimum quality required for applying calibration + xsensmvn::CalibrationQuality m_minimumAccaptableQuality; + + // Last applied calibration infos + xsensmvn::CalibrationQuality m_achievedCalibrationQuality; + std::string m_usedCalibrationType; + + // Atomic variable for internal state machine + std::atomic_bool m_calibrationAborted; + std::atomic_bool m_calibrationInProgress; + std::atomic_bool m_calibrationProcessed; + std::atomic_bool m_operationCompleted; + + /* ---------- * + * Functions * + * ---------- */ + + void cleanup(); + +public: + /* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ + + XSensMVNCalibrator(XmeControl& connector, + const xsensmvn::CalibrationQuality minAcceptableQuality = + xsensmvn::CalibrationQuality::ACCEPTABLE); + virtual ~XSensMVNCalibrator(); + + /* ---------- * + * Functions * + * ---------- */ + + // Calibrator controls + bool calibrateWithType(std::string calibrationType); + bool abortCalibration(); + + // Body-dimensions set/get + bool setBodyDimensions(const std::map& bodyDimensions); + bool getBodyDimensions(std::map& dimensions) const; + bool getBodyDimension(const std::string bodyName, double& dim); + + // Minimum calibration quality considered to be satisfactory set/get + bool setMinimumAcceptableCalibrationQuality(const xsensmvn::CalibrationQuality quality); + xsensmvn::CalibrationQuality& getMinimumAcceptableCalibrationQuality(); + + // Last calibration info get + void getLastCalibrationInfo(std::string& type, CalibrationQuality quality); + + // Status get + bool isCalibrationInProgress(); + + /* -------------------- * + * XSens XME Callbacks * + * -------------------- */ + virtual void onCalibrationAborted(XmeControl* dev); + virtual void onCalibrationComplete(XmeControl* dev); + virtual void onCalibrationProcessed(XmeControl* dev); +}; + +#endif // XSENS_MVN_CALIBRATOR_H diff --git a/XSensMVN/include/XSensMVNDriver.h b/XSensMVN/include/XSensMVNDriver.h new file mode 100644 index 000000000..0332401ad --- /dev/null +++ b/XSensMVN/include/XSensMVNDriver.h @@ -0,0 +1,242 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef XSENS_MVN_DRIVER_H +#define XSENS_MVN_DRIVER_H + +#include "XSensCalibrationQualities.h" + +#include "IXsensMVNControl.h" + +#include +#include +#include +#include + +namespace xsensmvn { + + class XSensMVNDriver; + class XSensMVNDriverImpl; + + /* ------------ * + * Definitions * + * ------------ */ + + using Vector3 = std::array; + using Quaternion = std::array; + using bodyDimensions = std::map; + + struct LinkData + { + std::string name; + Vector3 position; + Vector3 linearVelocity; + Vector3 linearAcceleration; + Quaternion orientation; + Vector3 angularVelocity; + Vector3 angularAcceleration; + }; + + struct SensorData + { + std::string name; + Vector3 position; + Quaternion orientation; + Vector3 freeBodyAcceleration; + Vector3 magneticField; + }; + + struct JointData + { + std::string name; + Vector3 angles; // RPY fix frame X-Y-Z + Vector3 velocities; + Vector3 accelerations; + }; + + struct Timestamp + { + double systemTime; + double relative; // [s] + double absolute; // [s] + }; + + struct LinkDataVector + { + std::shared_ptr time = nullptr; + std::vector data; + }; + + struct SensorDataVector + { + std::shared_ptr time = nullptr; + std::vector data; + }; + + struct JointDataVector + { + std::shared_ptr time = nullptr; + std::vector data; + }; + + struct DriverDataSample + { + std::shared_ptr timestamps = nullptr; + LinkDataVector links; + SensorDataVector sensors; + JointDataVector joints; + + DriverDataSample() + : timestamps(new Timestamp) + { + links.time = timestamps; + sensors.time = timestamps; + joints.time = timestamps; + } + + inline void reset() + { + timestamps->absolute = 0.0; + timestamps->relative = 0.0; + timestamps->systemTime = 0.0; + + for (auto& element : links.data) { + const std::string name = element.name; + element = {}; + element.name = name; + } + + for (auto& element : sensors.data) { + const std::string name = element.name; + element = {}; + element.name = name; + } + + for (auto& element : joints.data) { + const std::string name = element.name; + element = {}; + element.name = name; + } + } + }; + + struct DriverDataStreamConfig + { + bool enableLinkData; + bool enableSensorData; + bool enableJointData; + }; + + struct DriverConfiguration + { + const std::string licensePath; + const std::string suitConfiguration; + const std::string acquisitionScenario; + const std::string defaultCalibrationType; + const xsensmvn::CalibrationQuality minimumRequiredCalibrationQuality; + const int scanTimeout; + const int samplingRate; + const bodyDimensions bodyDimensions; + const DriverDataStreamConfig dataStreamConfiguration; + const bool saveMVNRecording; + const bool saveCurrentCalibration; + }; + + enum class DriverStatus + { + Disconnected = 0, + Scanning, + Connected, + Calibrating, + CalibratedAndReadyToRecord, + Recording, + Unknown, + }; + +} // namespace xsensmvn + +class xsensmvn::XSensMVNDriver : public IXsensMVNControl +{ +private: + /* ---------- * + * Variables * + * ---------- */ + + // Struct to internally store (cache) a single data sample + std::shared_ptr m_dataSample = nullptr; + std::shared_ptr m_dataMutex = nullptr; + + // Pointer to the driver implementation + std::unique_ptr m_pimpl; + + // Move the data from the implementation internal memory to the local one + // void cacheData(); + +public: + /* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ + + XSensMVNDriver() = delete; + XSensMVNDriver(const xsensmvn::DriverConfiguration& conf); + ~XSensMVNDriver() override; + + /* ---------- * + * Functions * + * ---------- */ + + // Prevent copy + XSensMVNDriver(const XSensMVNDriver& other) = delete; + XSensMVNDriver& operator=(const XSensMVNDriver& other) = delete; + + // Driver opening and closing + bool configureAndConnect(); + bool terminate(); + + // Minimum calibration quality considered to be satisfactory set/get + bool setMinimumAcceptableCalibrationQuality(const xsensmvn::CalibrationQuality quality) const; + const xsensmvn::CalibrationQuality& getMinimumAcceptableCalibrationQuality() const; + + const xsensmvn::DriverConfiguration& getDriverConfiguration() const; + + // Data accessor + // Data are returned as reference, but the user should not store it because the data are + // automaically updated by the driver callback. This is intended to be used only to avoid + // copying all the data in case just one is needed. + const DriverDataSample getDataSample(); + const LinkDataVector getLinkDataSample(); + const SensorDataVector getSensorDataSample(); + const JointDataVector getJointDataSample(); + + // Metadata accessor + // double getSampleRelativeTime() const; + // double getSampleAbsoluteTime() const; + + // Labels accessor + std::vector getSuitLinkLabels() const; + std::vector getSuitSensorLabels() const; + std::vector getSuitJointLabels() const; + + // Status get + xsensmvn::DriverStatus getStatus() const; + + // Timestamp get + xsensmvn::Timestamp getTimeStamps() const; + + /* --------------------------- * + * IXsensMVNControl Interface * + * --------------------------- */ + + bool startAcquisition() override; + bool stopAcquisition() override; + + bool calibrate(const std::string& calibrationType = {}) override; + bool abortCalibration() override; + + // Body-dimensions set/get + bool setBodyDimensions(const std::map& bodyDimensions) override; + bool getBodyDimensions(std::map& dimensions) const override; + bool getBodyDimension(const std::string bodyName, double& dimension) const override; +}; + +#endif // XSENS_MVN_DRIVER_H diff --git a/XSensMVN/include/XSensMVNDriverImpl.h b/XSensMVN/include/XSensMVNDriverImpl.h new file mode 100644 index 000000000..24ab3406d --- /dev/null +++ b/XSensMVN/include/XSensMVNDriverImpl.h @@ -0,0 +1,151 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef XSENS_MVN_DRIVER_IMPL_H +#define XSENS_MVN_DRIVER_IMPL_H + +#include "XSensMVNCalibrator.h" +#include "XSensMVNDriver.h" + +#include +#include +#include + +#include +#include +#include + +namespace xsensmvn { + class XSensMVNDriverImpl; +} // namespace xsensmvn + +class xsensmvn::XSensMVNDriverImpl : public XmeCallback +{ + +private: + struct XSensDataSample + { + long long relativeTime; + long long absoluteTime; + XmePose humanPose; + XmeSuitSample suitData; + }; + + struct CalibrationInfo + { + std::string type; + xsensmvn::CalibrationQuality quality; + }; + + struct SuitLabels + { + std::mutex labelsLock; + std::vector segmentNames; + std::vector sensorNames; + std::vector jointNames; + }; + + /* ---------- * + * Variables * + * ---------- */ + + // Status of the driver + std::atomic m_driverStatus; + bool m_stopProcessor; + + // XSens-style data variables + XSensDataSample m_lastDataSample; + bool m_newSampleAvailable; + + // Link, Sensor, and Joint names lists + SuitLabels m_suitLabels; + + // XSens object prointers + std::unique_ptr m_license; + std::unique_ptr m_connection; + + // Data processor thread + std::thread m_processor; + + // Condition variables and mutexs + std::condition_variable m_processorVariable; + std::condition_variable m_connectionVariable; + + mutable std::recursive_mutex m_objectMutex; + std::mutex m_connectionMutex; + std::mutex m_processorMutex; + + /* ---------- * + * Functions * + * ---------- */ + + void processDataSamples(); + + bool fillSegmentNames(); + bool fillSensorNames(); + bool fillJointNames(); + +public: + /* ---------- * + * Variables * + * ---------- */ + + // Driver output data + std::shared_ptr m_outDataMutex; + std::shared_ptr m_lastProcessedDataSample; + + // Calibrator object pointer + std::unique_ptr m_calibrator; + + // Driver configuration + const DriverConfiguration m_driverConfiguration; + + // Calibration Infos + CalibrationInfo m_calibrationInfo; + + /* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ + XSensMVNDriverImpl(const xsensmvn::DriverConfiguration& conf, + std::shared_ptr dataSampleStorage, + std::shared_ptr dataStorageMutex); + XSensMVNDriverImpl(const XSensMVNDriverImpl&) = delete; + XSensMVNDriverImpl& operator=(const XSensMVNDriverImpl&) = delete; + + ~XSensMVNDriverImpl() override; + + /* ---------- * + * Functions * + * ---------- */ + + bool configureAndConnect(); + + bool startAcquisition(); + bool stopAcquisition(); + + bool cleanAndClose(); + + bool calibrate(const std::string calibrationType); + + xsensmvn::DriverStatus getDriverStatus() const { return m_driverStatus; } + std::vector getSuitLinkLabels(); + std::vector getSuitSensorLabels(); + std::vector getSuitJointLabels(); + + /* -------------------- * + * XSens XME Callbacks * + * -------------------- */ + + // Hardware has been successfully disconnected + void onHardwareDisconnected(XmeControl* dev) override; + // Unspecified hardware error + void onHardwareError(XmeControl* dev) override; + // Hardware is ready after being detected + void onHardwareReady(XmeControl* dev) override; + // Called when device battery becomes lower than 10% + void onLowBatteryLevel(XmeControl* dev) override; + // Xme acquisition status callbacks + void onPoseReady(XmeControl* dev) override; // A new data sample is available +}; + +#endif // XSENS_MVN_DRIVER_IMPL_H diff --git a/XSensMVN/include/XSensMVNDriverImplBCKP.h b/XSensMVN/include/XSensMVNDriverImplBCKP.h new file mode 100644 index 000000000..d38413825 --- /dev/null +++ b/XSensMVN/include/XSensMVNDriverImplBCKP.h @@ -0,0 +1,140 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef XSENS_MVN_DRIVER_IMPL_H +#define XSENS_MVN_DRIVER_IMPL_H + +#include "XSensLogger.h" +#include "XSensMVNCalibrator.h" + +#include "XSensMVNDriver.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +// struct XmeControl; +// class XsensCallbackHandler; + +namespace xsensmvn { + // class XSensMVNCalibrator; + class XSensMVNDriverImpl; +} // namespace xsensmvn + +class xsensmvn::XSensMVNDriverImpl : public XmeCallback +{ + +private: + // Structure to store single-sample data + struct XSensDataSample + { + long long relativeTime; + long long absoluteTime; + XmePose humanPose; + XmeSuitSample suitData; + }; + + struct CalibrationInfo + { + std::string type; + // xsensmvn::CalibrationQuality quality; + bool appliedToEngine; + }; + + /* ---------- * + * Variables * + * ---------- */ + + // Driver Configuration + const DriverConfiguration m_driverConfiguration; + + // Status of the driver + std::atomic m_driverStatus; + + // XSens-style data variables + XSensDataSample m_lastDataSample; + bool m_newSampleAvailable; + + // Driver output data + xsensmvn::DriverDataSample m_lastProcessedDataSample; + + // XSens object prointers + std::unique_ptr m_license; + std::unique_ptr m_connection; + + // Calibrator object pointer + std::unique_ptr m_calibrator; + + // Data processor thread + std::thread m_processor; + + // Variables for multithreading + std::condition_variable m_processorVariable; + std::condition_variable m_connectionVariable; + + std::mutex m_connectionMutex; + std::mutex m_processorMutex; + + mutable std::recursive_mutex m_objectMutex; + mutable std::mutex m_outDataMutex; + + // Status variables + std::atomic_bool m_hardwareConnected; + + bool m_acquiring; + bool m_stopProcessor; + + std::atomic> m_segmentNames; + std::atomic> m_sensorNames; + std::atomic> m_jointNames; + + /* ---------- * + * Functions * + * ---------- */ + + void processDataSamples(); + + bool fillSegmentNames(); + bool fillSensorNames(); + bool fillJointNames(); + + bool configureAndConnect(); + +public: + /* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ + XSensMVNDriverImpl(const xsensmvn::DriverConfiguration conf); + XSensMVNDriverImpl(const XSensMVNDriverImpl&) = delete; + XSensMVNDriverImpl& operator=(const XSensMVNDriverImpl&) = delete; + + virtual ~XSensMVNDriverImpl() override { m_connection->addCallbackHandler(this); } + + /* -------------------- * + * XSens XME Callbacks * + * -------------------- */ + + // Hardware has been successfully disconnected + virtual void onHardwareDisconnected(XmeControl* dev) override; + // Unspecified hardware error + virtual void onHardwareError(XmeControl* dev) override; + // Hardware is ready after being detected + virtual void onHardwareReady(XmeControl* dev) override; + // Called when device battery becomes lower than 10% + virtual void onLowBatteryLevel(XmeControl* dev) override; + // Xme acquisition status callbacks + virtual void onPoseReady(XmeControl* dev) override; // A new data sample is available +}; + +#endif // XSENS_MVN_DRIVER_IMPL_H diff --git a/XSensMVN/include/XSensMVNDriverStates.h b/XSensMVN/include/XSensMVNDriverStates.h new file mode 100644 index 000000000..a7703660f --- /dev/null +++ b/XSensMVN/include/XSensMVNDriverStates.h @@ -0,0 +1,26 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef XSENS_MVN_DRIVER_STATES_H +#define XSENS_MVN_DRIVER_STATES_H + +/* Hack required to print verbose log messages when not in Release + * It is not possible to directly use the NDEBUG variable since XSens defines regardless from the + * compile option + * This brings as consequence that the Logger should be included in the .h files ALWAYS before XSens + * SDK files + */ +namespace xsens { + enum class DriverStatus + { + Disconnected = 0, + Scanning, + Connected, + Calibrating, + CalibratedAndReadyToRecord, + Recording, + Unknown, + }; +} + +#endif // XSENS_MVN_DRIVER_STATES_H diff --git a/XSensMVN/src/XSensMVNCalibrator.cpp b/XSensMVN/src/XSensMVNCalibrator.cpp new file mode 100644 index 000000000..bda6bdf7b --- /dev/null +++ b/XSensMVN/src/XSensMVNCalibrator.cpp @@ -0,0 +1,350 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "XsensMVNCalibrator.h" + +#include +#include + +#include +#include +#include +#include +#include + +namespace xsensmvn { + + /* ---------- * + * Utilities * + * ---------- */ + + // Map from XSens XMECalibrationQuality to xsens::CalibrationQuality + const std::map CalibrationQualitiesMap{ + {XmeCalibrationQuality::XCalQ_Unknown, xsensmvn::CalibrationQuality::UNKNOWN}, + {XmeCalibrationQuality::XCalQ_Failed, xsensmvn::CalibrationQuality::FAILED}, + {XmeCalibrationQuality::XCalQ_Poor, xsensmvn::CalibrationQuality::POOR}, + {XmeCalibrationQuality::XCalQ_Acceptable, xsensmvn::CalibrationQuality::ACCEPTABLE}, + {XmeCalibrationQuality::XCalQ_Good, xsensmvn::CalibrationQuality::GOOD}}; + + // Map from xsens::CalibrationQuality to std::string QualityLabel + const std::map CalibrationQualityLabels{ + {xsensmvn::CalibrationQuality::UNKNOWN, "Unknown"}, + {xsensmvn::CalibrationQuality::GOOD, "Good"}, + {xsensmvn::CalibrationQuality::ACCEPTABLE, "Acceptable"}, + {xsensmvn::CalibrationQuality::POOR, "Poor"}, + {xsensmvn::CalibrationQuality::FAILED, "Failed"}}; + + /* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ + + XSensMVNCalibrator::XSensMVNCalibrator(XmeControl& connector, + const xsensmvn::CalibrationQuality minAcceptableQuality) + : m_suitsConnector(connector) + , m_minimumAccaptableQuality(minAcceptableQuality) + , m_achievedCalibrationQuality(CalibrationQuality::UNKNOWN) + , m_usedCalibrationType("") + , m_calibrationAborted(false) + , m_calibrationInProgress(false) + , m_calibrationProcessed(false) + , m_operationCompleted(true) + { + m_suitsConnector.addCallbackHandler(static_cast(this)); + } + + XSensMVNCalibrator::~XSensMVNCalibrator() + { + cleanup(); + m_suitsConnector.removeCallbackHandler(static_cast(this)); + } + + /* ----------------- * + * Public Functions * + * ----------------- */ + + void XSensMVNCalibrator::getLastCalibrationInfo(std::string& type, CalibrationQuality quality) + { + type = m_usedCalibrationType; + quality = m_achievedCalibrationQuality; + } + + // Set user-specific body dimensions and apply them to the MVN engine + bool XSensMVNCalibrator::setBodyDimensions(const std::map& bodyDimensions) + { + // Check if the suit is connected + if (!m_suitsConnector.status().isConnected()) { + xsError << "Device not connected. Unable to set body dimensions." << std::endl; + return false; + } + + // If a calibration is in progress it is not possible to set the body dimensions + if (!m_operationCompleted) { + xsError << "Calibration in progress. Unable to set body dimensions." << std::endl; + return false; + } + + XsStringArray bodyDimList = m_suitsConnector.bodyDimensionLabelList(); + + if (bodyDimensions.empty()) { + xsError << "Empty body dimension list"; + return false; + } + // rise the flag to signal an operation is ongoing + m_operationCompleted = false; + + // We do not cache the dimensions as we obtain always the most updated values from Xsens + for (const auto& pair : bodyDimensions) { + if (bodyDimList.find(pair.first) == -1) { + xsWarning << "Body dimension: " << pair.first << " NOT found. Skipping."; + continue; + } + m_suitsConnector.setBodyDimension(pair.first, pair.second); + } + + // wait for completion + while (!m_operationCompleted) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + xsInfo << "Body dimensions successfully updated." << std::endl; + return m_operationCompleted; + } + + // Get user-specific body dimensions from the MVN engine + bool XSensMVNCalibrator::getBodyDimensions(std::map& dimensions) const + { + // Check if the suit is connected + if (!m_suitsConnector.status().isConnected()) { + xsError << "Device not connected. Unable to retrieve body dimensions." << std::endl; + return false; + } + + // Get a list of bodies for which dimensions are defined + XsStringArray bodyDimList = m_suitsConnector.bodyDimensionLabelList(); + for (const auto& body : bodyDimList) { + // get the estimated dimension of the body + double value = m_suitsConnector.bodyDimensionValueEstimate(body); + + // Check if the body dimension is known (-1 == unknown) + if (static_cast(value) != -1) { + // Push the pair to the output vector + dimensions.insert(std::map::value_type(body.c_str(), value)); + } + } + + xsInfo << "Body dimensions successfully retrieved from device." << std::endl; + return true; + } + + bool XSensMVNCalibrator::getBodyDimension(const std::string bodyName, double& dim) + { + std::map dimensions; + getBodyDimensions(dimensions); + const auto it = dimensions.find(bodyName); + if (it != dimensions.end()) { + xsWarning << "Impossible to find " << bodyName; + return false; + } + dim = it->second; + return true; + } + + // Calibrate MVN engine following the specified calibrationType routine + bool XSensMVNCalibrator::calibrateWithType(std::string calibrationType) + { + m_calibrationInProgress = true; + + // Check if a previous calibration of the same type is already in use, if so, discard it. + if (m_suitsConnector.isCalibrationPerformed(calibrationType)) { + xsInfo << "Discarding previous " << calibrationType << " calibration"; + m_usedCalibrationType = ""; + m_achievedCalibrationQuality = CalibrationQuality::UNKNOWN; + m_operationCompleted = false; + m_suitsConnector.clearCalibration(calibrationType); + } + + // Wait for the discard operation to be completed + while (m_suitsConnector.isCalibrationPerformed(calibrationType) && !m_calibrationAborted) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + if (m_calibrationAborted) { + cleanup(); + return false; + } + + // Initialize MVN engine to start the calibration routine + m_suitsConnector.initializeCalibration(calibrationType); + + // Get the phases of the selected calibration type + XsIntArray calibPhases = m_suitsConnector.calibrationPhaseList(); + + // 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 + for (unsigned int phase = 0; phase < calibPhases.size() - 1; ++phase) { + int startingFrame = calibPhases[phase]; + int endingFrame = calibPhases[phase + 1]; + xsInfo << m_suitsConnector.calibrationPhaseText(static_cast(phase)) << std::endl; + + while ((startingFrame < endingFrame) && !m_calibrationAborted) { + XmePose calibPose = m_suitsConnector.calibrationPose(startingFrame++); + // Sleep for 16ms as suggested by XSens MVN2018 Calibration guidelines "recommended + // sleep time is 16 milliseconds because the recording playback is shown at 60 Hz" + std::this_thread::sleep_for(std::chrono::milliseconds(16)); + std::cout << "."; + } + if (m_calibrationAborted) { + cleanup(); + return false; + } + + std::cout << std::endl; + } + + // Stop the calibration data collection + m_suitsConnector.stopCalibration(); + xsInfo << "Data collection for calibration completed." << std::endl; + + // Just to play safe sleep 100 ms to ensure the stopCalibration command has been received + m_calibrationProcessed = false; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + xsInfo << "Processing calibration data just collected." << std::endl; + + // Wait for the onCalibrationProcessed Callback + while (!m_calibrationProcessed && !m_calibrationAborted) { + std::cout << "."; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // Check if the positive wake has been due to an abortCalibration event + if (m_calibrationAborted) { + cleanup(); + return false; + } + + xsInfo << "Calibration data processing completed"; + xsInfo << "Retrieving calibration results" << std::endl; + + XmeCalibrationResult calibrationResult = + m_suitsConnector.calibrationResult(calibrationType); + + XmeCalibrationQuality quality = calibrationResult.m_quality; + XsStringArray warnings = calibrationResult.m_warnings; + + // Notify the user about the calibration quality and the received hints / warnings + xsInfo << "Calibration Quality: " + << xsensmvn::CalibrationQualityLabels.at(CalibrationQualitiesMap.at(quality)); + + if (!warnings.empty()) { + xsInfo << "Calibration result warnings:"; + for (const auto& wrn : warnings) { + xsInfo << wrn; + } + } + + if (CalibrationQualitiesMap.at(calibrationResult.m_quality) < m_minimumAccaptableQuality) { + // Notify the user that the calibration quality is lower than the minimum required one + xsInfo << "Minimum required quality: " + << xsensmvn::CalibrationQualityLabels.at(m_minimumAccaptableQuality).c_str(); + xsInfo << " Achieved quality: " + << xsensmvn::CalibrationQualityLabels.at( + CalibrationQualitiesMap.at(calibrationResult.m_quality)); + xsInfo << "Condition not met. Discarding. Please try again."; + + // Calibration cannot be considered acceptable. The fastest and safest way to discard it + // is to manually trigger an abortCalibration event + abortCalibration(); + while (!m_calibrationAborted) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + cleanup(); + return false; + } + else { + // Notify the user the calibration can be applied + xsInfo << "Ready to apply the obtained calibration"; + + // Apply the calibration to the MVN Engine and wait for a positive feedback + m_suitsConnector.finalizeCalibration(); + while (!m_operationCompleted && !m_calibrationAborted) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // Check if the positive wake has been due to an abortCalibration event + if (m_calibrationAborted) { + cleanup(); + return false; + } + xsInfo << "Done! Calibration Completed."; + } + + m_usedCalibrationType = calibrationType; + m_achievedCalibrationQuality = CalibrationQualitiesMap.at(quality); + + // Calibration completed + m_calibrationInProgress = false; + return true; + } + + // Abort the current ongoing calibration + bool XSensMVNCalibrator::abortCalibration() + { + if (m_calibrationInProgress) { + m_suitsConnector.abortCalibration(); + return true; + } + else { + return false; + } + } + + // Check if there is a calibration currently in progress + bool XSensMVNCalibrator::isCalibrationInProgress() { return m_calibrationInProgress; } + + // Set the minimum calibration quality considered to be satisfactory + bool XSensMVNCalibrator::setMinimumAcceptableCalibrationQuality( + const xsensmvn::CalibrationQuality quality) + { + m_minimumAccaptableQuality = quality; + return true; + } + + // Get the minimum calibration quality considered to be satisfactory + xsensmvn::CalibrationQuality& XSensMVNCalibrator::getMinimumAcceptableCalibrationQuality() + { + return m_minimumAccaptableQuality; + } + + /* ------------------------------------------ * + * Public XSens XME Callback Implementations * + * ------------------------------------------ */ + + // Called by XSens MVN Engine after successfully abort the calibration + void XSensMVNCalibrator::onCalibrationAborted(XmeControl* dev) { m_calibrationAborted = true; } + + // Called by XSens MVN Engine after the completion of any request made to the device + void XSensMVNCalibrator::onCalibrationComplete(XmeControl* dev) { m_operationCompleted = true; } + + // Called by XSens MVN Engine after the completion of the calibration data processing + void XSensMVNCalibrator::onCalibrationProcessed(XmeControl* dev) + { + m_calibrationProcessed = true; + } + + /* ------------------ * + * Private Functions * + * ------------------ */ + + // Reset internal state machine atomic variables + void XSensMVNCalibrator::cleanup() + { + m_usedCalibrationType = ""; + m_achievedCalibrationQuality = CalibrationQuality::UNKNOWN; + m_calibrationProcessed = m_operationCompleted = m_calibrationInProgress = false; + m_calibrationAborted = false; + } +} // namespace xsensmvn diff --git a/XSensMVN/src/XSensMVNDriver.cpp b/XSensMVN/src/XSensMVNDriver.cpp new file mode 100644 index 000000000..bad87c58e --- /dev/null +++ b/XSensMVN/src/XSensMVNDriver.cpp @@ -0,0 +1,144 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "XSensMVNDriver.h" +#include "XSensMVNDriverImpl.h" + +using namespace xsensmvn; + +/* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ + +XSensMVNDriver::XSensMVNDriver(const DriverConfiguration& conf) + : m_dataSample(new DriverDataSample) + , m_dataMutex(new std::mutex) + , m_pimpl(nullptr) +{ + m_pimpl.reset(new XSensMVNDriverImpl(conf, m_dataSample, m_dataMutex)); +}; + +XSensMVNDriver::~XSensMVNDriver() +{ + m_pimpl.reset(nullptr); +} + +/* ---------- * + * Functions * + * ---------- */ + +bool XSensMVNDriver::configureAndConnect() +{ + return m_pimpl->configureAndConnect(); +} + +bool XSensMVNDriver::terminate() +{ + return m_pimpl->cleanAndClose(); +} + +bool XSensMVNDriver::setMinimumAcceptableCalibrationQuality( + const xsensmvn::CalibrationQuality quality) const +{ + return m_pimpl->m_calibrator->setMinimumAcceptableCalibrationQuality(quality); +} + +const xsensmvn::CalibrationQuality& XSensMVNDriver::getMinimumAcceptableCalibrationQuality() const +{ + return m_pimpl->m_calibrator->getMinimumAcceptableCalibrationQuality(); +} + +const DriverConfiguration& XSensMVNDriver::getDriverConfiguration() const +{ + return m_pimpl->m_driverConfiguration; +} + +const DriverDataSample XSensMVNDriver::getDataSample() +{ + std::lock_guard readLock(*m_dataMutex); + return *m_dataSample; +} + +const LinkDataVector XSensMVNDriver::getLinkDataSample() +{ + std::lock_guard readLock(*m_dataMutex); + return m_dataSample->links; +} + +const SensorDataVector XSensMVNDriver::getSensorDataSample() +{ + std::lock_guard readLock(*m_dataMutex); + return m_dataSample->sensors; +} + +const JointDataVector XSensMVNDriver::getJointDataSample() +{ + std::lock_guard readLock(*m_dataMutex); + return m_dataSample->joints; +} + +std::vector XSensMVNDriver::getSuitLinkLabels() const +{ + return m_pimpl->getSuitLinkLabels(); +} + +std::vector XSensMVNDriver::getSuitSensorLabels() const +{ + return m_pimpl->getSuitSensorLabels(); +} + +std::vector XSensMVNDriver::getSuitJointLabels() const +{ + return m_pimpl->getSuitJointLabels(); +} + +DriverStatus XSensMVNDriver::getStatus() const +{ + return m_pimpl->getDriverStatus(); +} + +xsensmvn::Timestamp XSensMVNDriver::getTimeStamps() const +{ + std::lock_guard readLock(*m_dataMutex); + return *(m_dataSample->timestamps); +} + +/* ------------------------------------------ * + * IXsensMVNControl Interface Implementation * + * ------------------------------------------ */ + +bool XSensMVNDriver::startAcquisition() +{ + return m_pimpl->startAcquisition(); +} + +bool XSensMVNDriver::stopAcquisition() +{ + return m_pimpl->stopAcquisition(); +} + +bool XSensMVNDriver::calibrate(const std::string& calibrationType) +{ + // No need to check if calibrationType is empty, the check is inside the implementation + return m_pimpl->calibrate(calibrationType); +} + +bool XSensMVNDriver::abortCalibration() +{ + return m_pimpl->m_calibrator->abortCalibration(); +} + +bool XSensMVNDriver::setBodyDimensions(const std::map& bodyDimensions) +{ + return m_pimpl->m_calibrator->setBodyDimensions(bodyDimensions); +} + +bool XSensMVNDriver::getBodyDimensions(std::map& dimensions) const +{ + return m_pimpl->m_calibrator->getBodyDimensions(dimensions); +} + +bool XSensMVNDriver::getBodyDimension(const std::string bodyName, double& dimension) const +{ + return m_pimpl->m_calibrator->getBodyDimension(bodyName, dimension); +} diff --git a/XSensMVN/src/XSensMVNDriverImpl.cpp b/XSensMVN/src/XSensMVNDriverImpl.cpp new file mode 100644 index 000000000..c6999958d --- /dev/null +++ b/XSensMVN/src/XSensMVNDriverImpl.cpp @@ -0,0 +1,892 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "XSensMVNDriverImpl.h" + +#include +#include +#include +#include +#include + +#include + +using namespace xsensmvn; + +/* ---------- * + * Utilities * + * ---------- */ + +/* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ +XSensMVNDriverImpl::XSensMVNDriverImpl( + const xsensmvn::DriverConfiguration& conf, + std::shared_ptr dataSampleStorage, + std::shared_ptr dataStorageMutex) + : m_driverStatus(DriverStatus::Disconnected) + , m_stopProcessor(false) + , m_newSampleAvailable(false) + , m_license(nullptr) + , m_connection(nullptr) + , m_outDataMutex(dataStorageMutex) + , m_lastProcessedDataSample(dataSampleStorage) + , m_calibrator(nullptr) + , m_driverConfiguration(conf) + , m_calibrationInfo({}) +{} + +XSensMVNDriverImpl::~XSensMVNDriverImpl() +{ + if(m_driverConfiguration.saveMVNRecording) + { + // Save .mvn recording file + m_connection->stopRecording(); + while(m_connection->status().isRecording() || m_connection->status().isFlushing()){ + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + m_connection->saveAndCloseFile(); + } + + m_connection->removeCallbackHandler(static_cast(this)); +} + +/* ----------------- * + * Public Functions * + * ----------------- */ + +// This method is used by the processor thread +// TODO: explain better what it does +void XSensMVNDriverImpl::processDataSamples() +{ + xsInfo << "Starting processing thread"; + while (true) { + + XSensDataSample lastSample; + + { + // Processor lock scope + // unique_lock is necessary to unlock the mutex manually + std::unique_lock processorLock(m_processorMutex); + + // Wait for being notified if new sample is available or if it is time to stop + m_processorVariable.wait(processorLock); + + // Check if the thread has been notified by fini() + if (m_stopProcessor) { + // Time to stop + xsInfo << "Closing sample processor thread"; + break; + } + + // If not acquiring, release processor lock and go to the next cycle + // It is possible to directly check the value of m_driverStatus since defined + // atomic + if (m_driverStatus != DriverStatus::Recording) { + continue; + } + + // Move locally the last data sample + lastSample = std::move(m_lastDataSample); + m_newSampleAvailable = false; + } + + // Copy last retrieved data sample to the driver structure + { + std::lock_guard copyLock(*m_outDataMutex); + + // Reset the structure keeping only the name + m_lastProcessedDataSample->reset(); + + // Fill timestamps structure : absolute, relative, systemClock(Unix time) + // Copy absolute and relative XSens time + m_lastProcessedDataSample->timestamps->absolute = lastSample.absoluteTime / 1000.0; + m_lastProcessedDataSample->timestamps->relative = + static_cast(lastSample.relativeTime) / 1000.0; + // TODO: is this system time representation what we really want? + m_lastProcessedDataSample->timestamps->systemTime = + std::chrono::duration_cast>( + std::chrono::high_resolution_clock::now().time_since_epoch()) + .count(); // From yarp/os/SystemClock.cpp + + if (m_driverConfiguration.dataStreamConfiguration.enableLinkData) { + + // If the link data vector is empty is the first run, so allocate the space + if (m_lastProcessedDataSample->links.data.empty()) { + m_lastProcessedDataSample->links.data.resize( + lastSample.humanPose.m_segmentStates.size()); + } + + // Check if expected and actual sample dimensions match + if (m_lastProcessedDataSample->links.data.size() + != lastSample.humanPose.m_segmentStates.size()) { + xsError << "The number of links do not match previous samples"; + xsError << "Skipping sample"; + continue; + } + + unsigned segmentIx = 0; + for (const auto& link : lastSample.humanPose.m_segmentStates) { + + xsensmvn::LinkData newLink = {}; + + for (unsigned i = 0; i < 3; ++i) { + newLink.position.at(i) = link.m_position.at(i); + newLink.linearVelocity.at(i) = link.m_velocity.at(i); + newLink.linearAcceleration.at(i) = link.m_acceleration.at(i); + newLink.angularVelocity.at(i) = link.m_angularVelocity.at(i); + newLink.angularAcceleration.at(i) = link.m_angularAcceleration.at(i); + } + newLink.orientation = {{link.m_orientation.w(), + link.m_orientation.x(), + link.m_orientation.y(), + link.m_orientation.z()}}; + + newLink.name = m_suitLabels.segmentNames.at(segmentIx); + + m_lastProcessedDataSample->links.data[segmentIx] = newLink; + ++segmentIx; + }; + } + + if (m_driverConfiguration.dataStreamConfiguration.enableSensorData) { + + // If the link data vector is empty is the first run, so allocate the space + if (m_lastProcessedDataSample->sensors.data.empty()) { + m_lastProcessedDataSample->sensors.data.resize( + lastSample.suitData.sensorKinematics().size()); + } + + // Check if expected and actual sample dimensions match + if (m_lastProcessedDataSample->sensors.data.size() + != lastSample.suitData.sensorKinematics().size()) { + xsError << "The number of sensors do not match previous samples"; + xsError << "Skipping sample"; + continue; + } + + unsigned sensorIx = 0; + for (const auto& sensor : lastSample.suitData.sensorKinematics()) { + + SensorData newSensor{}; + + for (unsigned i = 0; i < 3; ++i) { + newSensor.position.at(i) = sensor.m_posG.at(i); + newSensor.freeBodyAcceleration.at(i) = sensor.m_accG.at(i); + newSensor.magneticField.at(i) = sensor.m_mag.at(i); + } + newSensor.orientation = { + {sensor.m_q.w(), sensor.m_q.x(), sensor.m_q.y(), sensor.m_q.z()}}; + + newSensor.name = m_suitLabels.sensorNames.at(sensorIx); + + m_lastProcessedDataSample->sensors.data[sensorIx] = newSensor; + ++sensorIx; + }; + } + + if (m_driverConfiguration.dataStreamConfiguration.enableJointData) { + // Xsens store angles this way: dof are the rows, X, Y, Z the columns + // They are computed using ZXY permutation using current frame formalism + XmeEulerPermutation anglePermutation = XmeEulerPermutation::XEP_ZXY_YUp; + XsMatrix angles = m_connection->jointAngles( + lastSample.humanPose, anglePermutation, XmePoseReference::XPR_AnatomicalPose); + + // If the link data vector is empty is the first run, so allocate the space + if (m_lastProcessedDataSample->joints.data.empty()) { + m_lastProcessedDataSample->joints.data.resize(angles.rows()); + } + + // Check if expected and actual sample dimensions match + if (m_lastProcessedDataSample->joints.data.size() != angles.rows()) { + xsError << "The number of joints do not match previous samples"; + xsError << "Skipping sample"; + continue; + } + + // Loop through all the joints in the XSens model + for (size_t jointIx = 0; jointIx < angles.rows(); ++jointIx) { + JointData newJoint{}; + + // Fill newly created joint with data coming from XSems + // Since the order of XSens matrix columns depends on the chosen permutation it + // is safer to copy them manually + switch (anglePermutation) { + case XmeEulerPermutation::XEP_ZXY_YUp: + newJoint.angles.at(0) = angles.value(jointIx, 1); + newJoint.angles.at(1) = angles.value(jointIx, 2); + newJoint.angles.at(2) = angles.value(jointIx, 0); + break; + case XmeEulerPermutation::XEP_XZY_YUp: + newJoint.angles.at(0) = angles.value(jointIx, 0); + newJoint.angles.at(1) = angles.value(jointIx, 2); + newJoint.angles.at(2) = angles.value(jointIx, 1); + break; + } + + newJoint.name = m_suitLabels.jointNames.at(jointIx); + + // Push the newly created joint in the joint vector of the last processed frame + m_lastProcessedDataSample->joints.data[jointIx] = newJoint; + } + } + } + } + xsInfo << "Closing data processing thread"; +} + +bool XSensMVNDriverImpl::configureAndConnect() +{ + // This function is called only in the constructor. The mutex is not strictly necessary. + // TODO: probably a non-recursive mutex would be enough. + std::lock_guard globalGuard(m_objectMutex); + + switch (m_driverStatus) { + case DriverStatus::Disconnected: + case DriverStatus::Unknown: + // Continue the execution + break; + case DriverStatus::Scanning: + xsError << "Driver is currently scanning. This state shouldn't be reached here."; + return false; + case DriverStatus::Connected: + case DriverStatus::Calibrating: + case DriverStatus::CalibratedAndReadyToRecord: + case DriverStatus::Recording: + xsError << "Device already connected"; + return false; + } + + // Create a license instance + m_license.reset(new XmeLicense()); + if (!m_license) { + xsError << "Unable to create a valid license instance. Check hardware dongle."; + return false; + } + + std::experimental::filesystem::path tmpFolder = + std::experimental::filesystem::temp_directory_path(); + xsInfo << "Temporary directory is " << tmpFolder.c_str() << std::endl; + + xmeSetPaths(m_driverConfiguration.licensePath.c_str(), "", tmpFolder.c_str(), true); + + // Create a connection instance + m_connection.reset(XmeControl::construct()); + if (!m_connection) { + xsError << "Could not open Xsens DLL. Check to use the correct DLL."; + return false; + } + // Configure the connection to enable callbacks support + m_connection->addCallbackHandler(static_cast(this)); + + //---------------------------------------- + // Configure the MVN motion capture system + //---------------------------------------- + + // Get the list of the supported suit configurations + xsInfo << "--- Supported MVN suit configurations ---"; + XsStringArray suitConfs = m_connection->configurationList(); + for (const auto& xssc : suitConfs) { + xsInfo << " - " << xssc.c_str(); + } + xsInfo << "--------------------------------" << std::endl; + + // Check if the user-selected configuration is supported + int suitConfIsValid = suitConfs.find(m_driverConfiguration.suitConfiguration.c_str(), false); + if (suitConfIsValid == -1) { + xsError << "Selected suit configuration: " + << m_driverConfiguration.suitConfiguration.c_str() << "not supported"; + cleanAndClose(); + return false; + } + + // Configure the system to use the selected MVN suit configuration + m_connection->setConfiguration(m_driverConfiguration.suitConfiguration.c_str()); + xsInfo << "Using selected suit configuration:" + << m_driverConfiguration.suitConfiguration.c_str() << std::endl; + + // Get the list of the supported suit configurations + xsInfo << "--- Supported MVN acquisition scenarios ---"; + XsStringArray acqScenarios = m_connection->userScenarioList(); + for (const auto& xsas : acqScenarios) { + xsInfo << " - " << xsas.c_str(); + } + xsInfo << "--------------------------------" << std::endl; + + int acqScenarioIsValid = + acqScenarios.find(m_driverConfiguration.acquisitionScenario.c_str(), false); + if (acqScenarioIsValid == -1) { + xsWarning << "Selected acquisition scenario: " + << m_driverConfiguration.acquisitionScenario.c_str() << "not supported"; + xsWarning << "Using DEFAULT scenario: " << m_connection->userScenario().c_str() + << std::endl; + } + else { + m_connection->setUserScenario(m_driverConfiguration.suitConfiguration.c_str()); + xsInfo << "Using selected acquisition scenario:" << m_connection->userScenario().c_str() + << std::endl; + } + + //---------------------------------- + // Connect to the Xsens MVN hardware + //---------------------------------- + + // Scan for the suit until either the connection is successfull or the timeout is reached + // Since this function is called asynchronously, it should be synchronized + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + { + std::unique_lock connectionLock(m_connectionMutex); + m_driverStatus = DriverStatus::Scanning; + m_connection->setScanMode(true); + if (m_driverConfiguration.scanTimeout * 1000 > 0) { + // Timeout enabled + m_connectionVariable.wait_until( + connectionLock, + now + std::chrono::milliseconds(m_driverConfiguration.scanTimeout * 1000)); + } + else { + // Timout disabled, keep scannig until the system is found + m_connectionVariable.wait(connectionLock); + } + m_connection->setScanMode(false); + } + + // Double-check if Xsens MVN system is connected + if (!m_connection->status().isConnected() && m_driverStatus != DriverStatus::Connected) { + xsError << "Unable to connect to the XSens MVN suit"; + cleanAndClose(); + return false; + } + + xsInfo << "Successfully connected to the XSens suit"; + + //----------------------------- + // Create a calibrator instance + //----------------------------- + + m_calibrator.reset(new xsensmvn::XSensMVNCalibrator( + *m_connection, m_driverConfiguration.minimumRequiredCalibrationQuality)); + if (!m_calibrator) { + xsError << "Unable to create the calibrator"; + cleanAndClose(); + return false; + } + + //------------------------------------------- + // Configure subject-specific body dimensions + //------------------------------------------- + + xsInfo << "--- Supported subject dimension parameters ---"; + // Get configurable body dimensions from XSens + XsStringArray xsBodyDimensions = m_connection->bodyDimensionLabelList(); + if (xsBodyDimensions.empty()) { + xsInfo << "Supported body dimension retrieved from device is empty"; + cleanAndClose(); + return false; + } + + for (const auto& xsbd : xsBodyDimensions) { + xsInfo << " - " << xsbd.c_str(); + } + xsInfo << "--------------------------------" << std::endl; + + if (!m_driverConfiguration.bodyDimensions.empty()) { + if (!m_calibrator->setBodyDimensions(m_driverConfiguration.bodyDimensions)) { + xsError << "Unable to configure the system with the provided body dimensions"; + cleanAndClose(); + return false; + } + } + + // Retrieve sensor, segment, and joint names from the device + if (!(fillSensorNames() && fillSegmentNames() && fillJointNames())) { + xsError << "Failed to fill sensor, segment, and joint name lists"; + cleanAndClose(); + return false; + }; + + //------------------------------------- + // Retrieve supported calibration types + //------------------------------------- + + XsStringArray calibrationTypeSet = m_connection->calibrationLabelList(); + xsInfo << "--- Supported calibration types ---"; + for (const auto& cr : calibrationTypeSet) { + xsInfo << " - " << cr.c_str(); + } + xsInfo << "------------------------------------"; + + // Get the default calibration routine name from config file + if (calibrationTypeSet.find(m_driverConfiguration.defaultCalibrationType.c_str()) == -1) { + xsError << "Provided default calibration type: " + << m_driverConfiguration.defaultCalibrationType << " not supported"; + cleanAndClose(); + return false; + } + xsInfo << "Default calibration type set: " << m_driverConfiguration.defaultCalibrationType; + + //-------------------------------- + // Retrieve supported sample rates + //-------------------------------- + + xsInfo << "--- Supported acquisition frequencies [Hz] ---"; + XsIntArray supportedUpdateRates = m_connection->determineSupportedUpdateRates( + m_connection->detectedDevices(), + static_cast(m_connection->sensorCountForConfiguration(m_connection->configuration())), + false); + for (const auto& ur : supportedUpdateRates) { + xsInfo << " - " << ur << " [Hz]"; + } + xsInfo << "------------------------------------"; + if (supportedUpdateRates.find(m_driverConfiguration.samplingRate) == -1) { + xsWarning << "Requested sampling rate: " << m_driverConfiguration.samplingRate + << " not supported."; + xsWarning << "Using default sampling rate: " << supportedUpdateRates.at(0) << " Hz"; + m_connection->setSampleRate(static_cast(supportedUpdateRates.at(0))); + } + else { + m_connection->setSampleRate(static_cast(m_driverConfiguration.samplingRate)); + } + // Sleep for at least (EMPIRICALLY RETRIEVED VALUE) to allow XSens to update its sampling rate + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + xsInfo << "Sampling rate set to: " << m_connection->sampleRate() << " Hz"; + + //--------------------------------------- + // Create the datasample processor thread + //--------------------------------------- + // Create a sample processor thread to free data processing from MVN callbacks + // TODO: switch to async calls?? std::future? Is it performant enough create a thread + // every time data is received? Probably if no allocations are done this is ok. + // This would mean lock-free code :) + m_stopProcessor = false; + m_processor = std::thread(&XSensMVNDriverImpl::processDataSamples, this); + + return m_driverStatus == DriverStatus::Connected; +} + +bool XSensMVNDriverImpl::startAcquisition() +{ + std::lock_guard globalGuard(m_objectMutex); + + // Check the internal status and continue only if the driver is CalibratedAndReadyToRecord + switch (m_driverStatus) { + case DriverStatus::Disconnected: + case DriverStatus::Unknown: + case DriverStatus::Scanning: + xsError << "Device not ready or not connected"; + return false; + case DriverStatus::Calibrating: + xsError << "Device is calibrating. Calibration must end in order to " + << "start the acquisition."; + return false; + case DriverStatus::CalibratedAndReadyToRecord: + // Continue the execution + break; + case DriverStatus::Recording: + xsError << "Device already recording"; + return false; + case DriverStatus::Connected: + // In theory the suit can stream without being calibrated but we do not support it + xsError << "Device connected but not calibrated. " + << "You need to calibrate the device first."; + return false; + } + + // If ready to acquire, ask directly to the SDK if everything is ok + if (!m_connection || !m_connection->status().isConnected() + || m_connection->status().isOutOfRange()) { + xsError << "Suit not connected or out of range"; + xsWarning << "The internal driver status does not match Xsens status"; + return false; + } + + if (!m_calibrator || m_calibrator->isCalibrationInProgress()) { + xsError << "Calibration in progress. Unable to start data acquisition"; + xsWarning << "The internal driver status does not match Xsens status"; + return false; + } + + // Check if the acquisition is already in progress + if (m_connection->realTimePoseMode()) { + xsError << "Acquisition already in progress"; + xsWarning << "The internal driver status does not match Xsens status"; + return false; + } + + // Start the acquisition + m_connection->setRealTimePoseMode(true); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + if (!m_connection->realTimePoseMode()) { + xsError << "Failed to activate real time data flow"; + return false; + } + + m_driverStatus = DriverStatus::Recording; + xsInfo << "Acquisition successfully started"; + return true; +} + +bool XSensMVNDriverImpl::stopAcquisition() +{ + std::lock_guard globalGuard(m_objectMutex); + + // Check the internal status and continue only if the driver is CalibratedAndReadyToRecord + switch (m_driverStatus) { + case DriverStatus::Disconnected: + case DriverStatus::Unknown: + case DriverStatus::Scanning: + xsError << "Device not ready or not connected"; + return false; + case DriverStatus::Calibrating: + xsError << "Calibration in progress. Unable to stop data acquisition"; + return false; + case DriverStatus::CalibratedAndReadyToRecord: + case DriverStatus::Connected: + xsError << "Device not recording. Unable to stop data acquisition"; + return false; + case DriverStatus::Recording: + // Continue the execution + break; + } + + // If ready to acquire, ask directly to the SDK if everything is ok + if (!m_connection || !m_connection->status().isConnected() + || m_connection->status().isOutOfRange()) { + xsError << "Suit not connected or out of range"; + xsWarning << "The internal driver status does not match Xsens status"; + return false; + } + + if (!m_calibrator || m_calibrator->isCalibrationInProgress()) { + xsError << "Calibration in progress. Unable to stop data acquisition"; + xsWarning << "The internal driver status does not match Xsens status"; + return false; + } + + // Check if the acquisition is already in progress + if (!m_connection->realTimePoseMode()) { + xsError << "Acquisition not in progress"; + xsWarning << "The internal driver status does not match Xsens status"; + return false; + } + + // Stop the acquisition + m_connection->setRealTimePoseMode(false); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + if (m_connection->realTimePoseMode()) { + xsError << "Failed to stop real time data flow"; + return false; + } + + m_driverStatus = DriverStatus::CalibratedAndReadyToRecord; + xsInfo << "Acquisition successfully stopped"; + return true; +} + +bool XSensMVNDriverImpl::calibrate(const std::string calibrationType) +{ + std::string calibType = calibrationType; + + { + std::lock_guard globalGuard(m_objectMutex); + + // Handle empty calibrationType parameter + if (calibType.empty()) { + if (m_driverConfiguration.defaultCalibrationType.empty()) { + xsError << "Neither custom nor default calibration type set. Aborting"; + return false; + } + calibType = m_driverConfiguration.defaultCalibrationType; + } + + // Check the internal status and continue only if the driver is CalibratedAndReadyToRecord + switch (m_driverStatus) { + case DriverStatus::Disconnected: + case DriverStatus::Unknown: + case DriverStatus::Scanning: + xsError << "Device not ready or not connected"; + return false; + case DriverStatus::Calibrating: + xsError << "Calibration already in progress. Unable to calibrate"; + xsError << "Please wait for completion or abort the ongoing acquisition and retry"; + return false; + case DriverStatus::CalibratedAndReadyToRecord: + // TODO: it would be possible to store multiple calibrations but it is not yet + // supported + xsInfo << "The device has already been calibrated. Discarding the previous " + "calibration."; + break; + case DriverStatus::Connected: + // Continue the execution + break; + case DriverStatus::Recording: + xsError << "Acquisition in progress. Unable to calibrate"; + xsError << "Please stop the ongoing acquisition, then retry"; + return false; + } + + // If ready to acquire, ask directly to the SDK if everything is ok + if (!m_connection || !m_connection->status().isConnected() + || m_connection->status().isOutOfRange()) { + xsError << "Suit not connected or out of range"; + xsWarning << "The internal driver status does not match Xsens status"; + return false; + } + + if (!m_calibrator || m_calibrator->isCalibrationInProgress()) { + xsError << "Calibration in progress. Unable to start another calibration"; + xsWarning << "The internal driver status does not match Xsens status"; + return false; + } + } + + // Set the status to calibrating + m_driverStatus = DriverStatus::Calibrating; + + // Ask the calibrator to perform the calibration + bool calibrationCompleted = m_calibrator->calibrateWithType(calibType); + + // Activate data flow + m_driverStatus = DriverStatus::Connected; + if (calibrationCompleted) { + // Record the success of the calibration + m_driverStatus = DriverStatus::CalibratedAndReadyToRecord; + + // Save calibration and .mvn record + if(m_driverConfiguration.saveMVNRecording || m_driverConfiguration.saveCurrentCalibration) + { + //Start recording .mvn file + time_t rawtime; + struct tm * timeinfo; + char buffer[80]; + + time(&rawtime); + timeinfo = localtime(&rawtime); + + strftime(buffer, sizeof(buffer), "%d-%m-%Y %H:%M:%S", timeinfo); + std::string time_string(buffer); + + // Replace separators with underscore + std::replace(time_string.begin(), time_string.end(), ' ', '_'); //Replace space with underscore + std::replace(time_string.begin(), time_string.end(), '-', '_'); //Replace - with underscore + std::replace(time_string.begin(), time_string.end(), ':', '_'); //Replace : with underscore + + if(m_driverConfiguration.saveMVNRecording) + { + const XsString mvnFileName("recording_" + time_string); + + xsInfo << "Recording to " << mvnFileName.toStdString() << ".mvn file"; + + m_connection->createMvnFile(mvnFileName); + m_connection->startRecording(); + } + + if (m_driverConfiguration.saveCurrentCalibration) + { + const XsString mvnFileName("recording_" + time_string + "_calibration.mvn"); + + xsInfo << "Calibration saved to " << mvnFileName.toStdString() << " file"; + + m_connection->saveCurrentCalibration(mvnFileName); + } + } + + m_calibrator->getLastCalibrationInfo(m_calibrationInfo.type, m_calibrationInfo.quality); + } + + return calibrationCompleted; +} + +std::vector XSensMVNDriverImpl::getSuitLinkLabels() +{ + std::lock_guard guard(m_suitLabels.labelsLock); + return m_suitLabels.segmentNames; +} + +std::vector XSensMVNDriverImpl::getSuitSensorLabels() +{ + std::lock_guard guard(m_suitLabels.labelsLock); + return m_suitLabels.sensorNames; +} + +std::vector XSensMVNDriverImpl::getSuitJointLabels() +{ + std::lock_guard guard(m_suitLabels.labelsLock); + return m_suitLabels.jointNames; +} + +/* ------------------------------------------ * + * Public XSens XME Callback Implementations * + * ------------------------------------------ */ + +void XSensMVNDriverImpl::onHardwareDisconnected(XmeControl* /*dev*/) +{ + xsInfo << "Xsens MVN hardware disconnected."; + m_driverStatus = DriverStatus::Disconnected; + m_connectionVariable.notify_one(); +} + +void XSensMVNDriverImpl::onHardwareError(XmeControl* dev) +{ + XmeStatus status = dev->status(); + xsWarning << "Hardware error received."; + xsWarning << "Suit connected: " << (status.isConnected() ? "YES" : "NO"); + xsWarning << "Scanning for XSens MVN suit: " << (status.isScanning() ? "YES" : "NO"); +} + +void XSensMVNDriverImpl::onHardwareReady(XmeControl* /*dev*/) +{ + xsInfo << "Xsens MVN hardware connected successfully. Ready to start"; + m_driverStatus = DriverStatus::Connected; + m_connectionVariable.notify_one(); +} + +void XSensMVNDriverImpl::onLowBatteryLevel(XmeControl* /*dev*/) +{ + xsWarning << "Low battery level. Recharge it as soon as possible."; +} + +void XSensMVNDriverImpl::onPoseReady(XmeControl* dev) +{ + // Copy last available sample to temporary structure to avoid waiting for the lock + XSensDataSample newSample{dev->pose(XME_LAST_AVAILABLE_FRAME).m_relativeTime, + dev->pose(XME_LAST_AVAILABLE_FRAME).m_absoluteTime, + dev->pose(XME_LAST_AVAILABLE_FRAME), + dev->suitSample(XME_LAST_AVAILABLE_FRAME)}; + + // Copy acquired sample to the processing queue and notify the processor thread + std::lock_guard poseCopyGuard(m_processorMutex); + m_lastDataSample = std::move(newSample); + m_newSampleAvailable = true; + m_processorVariable.notify_one(); +} + +/* ------------------ * + * Private Functions * + * ------------------ */ + +bool XSensMVNDriverImpl::cleanAndClose() +{ + std::lock_guard connectionGuard(m_objectMutex); + + // Kill calibrator thread + m_calibrator.reset(); + + // Stop the acquisition, disconnect the hardware, and destroy the device connector + if (m_connection) { + if (m_driverStatus == DriverStatus::Recording) { + stopAcquisition(); + m_driverStatus = DriverStatus::CalibratedAndReadyToRecord; + } + m_connection->disconnectHardware(); + { + std::unique_lock connectionLock(m_connectionMutex); + m_connectionVariable.wait(connectionLock); + + // Clear the callback handlers list + m_connection->clearCallbackHandlers(); + m_connection->destruct(); + } + } + + m_driverStatus = DriverStatus::Disconnected; + + // Close frame processor thread + { + std::unique_lock lock(m_processorMutex); + m_stopProcessor = true; + + // Notify the processor thread it's time to end + m_processorVariable.notify_one(); + } + + // The processor lock is now realeased, join the thread to wait for its end + if (m_processor.joinable()) { + m_processor.join(); + } + + if (m_license) { + xmeTerminate(); + } + + return true; +} + +bool XSensMVNDriverImpl::fillSegmentNames() +{ + // Lock the mutex to prevent other threads changing the segment list + // std::lock_guard connectionGuard(m_objectMutex); + + if (!m_connection || m_connection->segmentCount() == 0) { + xsWarning << "Unable to retrieve segment names from XSens or empty segment list"; + return false; + } + + // Get segment count and allocate memory for output vector + int segmentCount = m_connection->segmentCount(); + { + std::lock_guard labelGuard(m_suitLabels.labelsLock); + m_suitLabels.segmentNames.reserve(static_cast(segmentCount)); + // Segment ID is 1-based, vector is 0-based, segmentID = index + 1 + for (int ix = 0; ix < segmentCount; ++ix) { + m_suitLabels.segmentNames.push_back(m_connection->segmentName(ix + 1).toStdString()); + } + } + return true; +} + +bool XSensMVNDriverImpl::fillSensorNames() +{ + // Lock the mutex to prevent other threads changing the segment list + // std::lock_guard connectionGuard(m_objectMutex); + + if (!m_connection || m_connection->sensorCount() == 0) { + xsWarning << "Unable to retrieve sensor names from XSens or empty sensor list"; + return false; + } + + unsigned sensorCount = static_cast(m_connection->sensorCount()); + // Since sensor names are not directly available, get the suit sensors status + XmeDeviceStatusArray sensorsStatus = m_connection->status().suitStatus().m_sensors; + + // Double check if sensorCount and sensorStatus.size() are equals + if (sensorCount != sensorsStatus.size()) { + xsWarning << "Mismatch in sensor count and sensor states dimensions"; + return false; + } + + { + std::lock_guard labelGuard(m_suitLabels.labelsLock); + m_suitLabels.sensorNames.reserve(sensorCount); + for (unsigned ix = 0; ix < sensorCount; ++ix) { + // Get the name of the segment associated to the ix-th sensor (everything is + // 0-based) + m_suitLabels.sensorNames.push_back( + m_connection->segmentName(sensorsStatus[ix].m_segmentId).toStdString()); + } + } + return true; +} + +bool XSensMVNDriverImpl::fillJointNames() +{ + // Lock the mutex to prevent other threads changing the segment list + // std::lock_guard connectionGuard(m_objectMutex); + + if (!m_connection) { + xsWarning << "Unable to retrieve joint names"; + return false; + } + + { + std::lock_guard labelGuard(m_suitLabels.labelsLock); + m_suitLabels.jointNames.reserve(m_connection->joints().size()); + + for (const auto& joint : m_connection->joints()) { + m_suitLabels.jointNames.push_back(joint.name().toStdString()); + } + } + return true; +} diff --git a/XSensMVN/src/XSensMVNDriverImpl_BCKP.cpp b/XSensMVN/src/XSensMVNDriverImpl_BCKP.cpp new file mode 100644 index 000000000..dcd87ecdb --- /dev/null +++ b/XSensMVN/src/XSensMVNDriverImpl_BCKP.cpp @@ -0,0 +1,1138 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "XSensMVNDriverImpl.h" + +#include +#include +#include + +#include + +// void yarp::dev::XsensMVN::XsensMVNPrivate::resizeVectorToOuterAndInnerSize( +// std::vector& vector, +// unsigned outerSize, +// unsigned innerSize) +//{ +// size_t oldSize = vector.size(); +// vector.resize(outerSize); +// for (unsigned i = oldSize; i < outerSize; ++i) { +// vector[i].resize(innerSize, 0.0); +// } +//} +using namespace xsensmvn; + +/* ---------- * + * Utilities * + * ---------- */ + +/* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ +XSensMVNDriverImpl::XSensMVNDriverImpl(const DriverConfiguration conf) + : m_driverConfiguration(conf) + , m_newSampleAvailable(false) + , m_license(nullptr) + , m_connection(nullptr) + , m_hardwareConnected(false) + , m_stopProcessor(false) + , m_segmentNames({}) + , m_sensorNames({}) + , m_jointNames({}) +{ + + configureAndConnect(); +} + +// XSensMVNDriverImpl::~XSensMVNDriverImpl() {} // namespace xsensmvn + +/* ------------------------------------------ * + * Public XSens XME Callback Implementations * + * ------------------------------------------ */ + +void XSensMVNDriverImpl::onHardwareDisconnected(XmeControl*) +{ + xsInfo << "Xsens MVN hardware disconnected."; + m_driverStatus = DriverStatus::Disconnected; + + std::lock_guard poseCopyGuard(m_connectionMutex); + m_hardwareConnected = false; + m_connectionVariable.notify_one(); +} + +void XSensMVNDriverImpl::onHardwareError(XmeControl* dev) +{ + XmeStatus status = dev->status(); + xsWarning << "Hardware error received."; + xsWarning << "Suit connected: %s" << (status.isConnected() ? "YES" : "NO"); + xsWarning << "Scanning for XSens MVN suit: " << (status.isScanning() ? "YES" : "NO"); +} + +void XSensMVNDriverImpl::onHardwareReady(XmeControl* dev) +{ + xsInfo << "Xsens MVN hardware connected successfully. Ready to start"; + m_driverStatus = DriverStatus::Connected; + + std::lock_guard poseCopyGuard(m_connectionMutex); + m_hardwareConnected = true; + m_connectionVariable.notify_one(); +} + +void XSensMVNDriverImpl::onLowBatteryLevel(XmeControl* dev) +{ + xsWarning << "Low battery level. Recharge it as soon as possible."; +} + +void XSensMVNDriverImpl::onPoseReady(XmeControl* dev) +{ + // Copy last available sample to temporary structure to avoid waiting for the lock + XSensDataSample newSample{dev->pose(XME_LAST_AVAILABLE_FRAME).m_relativeTime, + dev->pose(XME_LAST_AVAILABLE_FRAME).m_absoluteTime, + dev->pose(XME_LAST_AVAILABLE_FRAME), + dev->suitSample(XME_LAST_AVAILABLE_FRAME)}; + + // Copy acquired sample to the processing queue and notify the processor thread + std::lock_guard poseCopyGuard(m_processorMutex); + m_lastDataSample = std::move(newSample); + m_newSampleAvailable = true; + m_processorVariable.notify_one(); +} + +void XSensMVNDriverImpl::processDataSamples() +{ + xsInfo << "Starting processing thread"; + while (true) { + + // Processor lock scope + std::unique_lock processorLock(m_processorMutex); + + // Wait for being notified if new sample is available or if it is time to stop + m_processorVariable.wait(processorLock, + [&]() { return m_stopProcessor || m_newSampleAvailable; }); + + // Check if the thread has been notified by fini() + if (m_stopProcessor) { + // Time to stop + xsInfo << "Closing sample processor thread"; + break; + } + + // If not acquiring, release processor lock and go to the next cycle + // It is possible to directly check the value of m_driverStatus since defined + // atomic + if (m_driverStatus == DriverStatus::Recording) { + processorLock.unlock(); + continue; + } + + // Move locally the last data sample + XSensDataSample lastSample = std::move(m_lastDataSample); + m_newSampleAvailable = false; + + // Unlock processor scope + processorLock.unlock(); + + // Copy last retrieved data sample to the driver structure + { + std::lock_guard copyLock(m_outDataMutex); + + // TODO: fill suit name + + // Copy absolute and relative XSens time + m_lastProcessedDataSample.absoluteTime = lastSample.absoluteTime / 1000.0; + m_lastProcessedDataSample.relativeTime = lastSample.relativeTime / 1000.0; + + if (m_driverConfiguration.dataStreamConfiguration.enableLinkData) { + // TODO: check if expected and actual sample dimensions match + + m_lastProcessedDataSample.links.reserve( + lastSample.humanPose.m_segmentStates.size()); + + int segmentIx = 0; + for (const auto& link : lastSample.humanPose.m_segmentStates) { + + xsensmvn::LinkData newLink{}; + + for (unsigned i = 0; i < 3; ++i) { + newLink.position.at(i) = link.m_position.at(i); + newLink.linearVelocity.at(i) = link.m_velocity.at(i); + newLink.linearAcceleration.at(i) = link.m_acceleration.at(i); + newLink.angularVelocity.at(i) = link.m_angularVelocity.at(i); + newLink.angularAcceleration.at(i) = link.m_angularAcceleration.at(i); + } + newLink.orientation = {{link.m_orientation.w(), + link.m_orientation.x(), + link.m_orientation.y(), + link.m_orientation.z()}}; + newLink.name = m_segmentNames.at(segmentIx); + m_lastProcessedDataSample.links.push_back(newLink); + }; + } + + if (m_driverConfiguration.dataStreamConfiguration.enableSensorData) { + m_lastProcessedDataSample.sensors.reserve( + lastSample.suitData.sensorKinematics().size()); + + int sensorIx = 0; + for (const auto& sensor : lastSample.suitData.sensorKinematics()) { + + SensorData newSensor{}; + + for (unsigned i = 0; i < 3; ++i) { + newSensor.position.at(i) = sensor.m_posG.at(i); + newSensor.freeBodyAcceleration.at(i) = sensor.m_accG.at(i); + newSensor.magneticField.at(i) = sensor.m_mag.at(i); + } + newSensor.orientation = { + {sensor.m_q.w(), sensor.m_q.x(), sensor.m_q.y(), sensor.m_q.z()}}; + + newSensor.name = m_sensorNames.at(sensorIx); + m_lastProcessedDataSample.sensors.push_back(newSensor); + }; + } + + if (m_driverConfiguration.dataStreamConfiguration.enableJointData) { + // TODO: add support for joint angles + xsWarning << "Joint angles not supported yet."; + } + } + xsInfo << "Frame processed"; + } + xsInfo << "Closing data processing thread"; +} + +/* ------------------ * + * Private Functions * + * ------------------ */ + +bool XSensMVNDriverImpl::configureAndConnect() +{ + + std::lock_guard globalGuard(m_objectMutex); + + // Check if the device is already connected + if (m_connection) { + xsError << "Device already connected"; + return false; + } + + // Create a license istance + m_license.reset(new XmeLicense()); + if (!m_license) { + xsError << "Unable to create a valid license istance. Check hardware dongle."; + return false; + } + + std::experimental::filesystem::path tmpFoder = + std::experimental::filesystem::temp_directory_path(); + xsInfo << "Temporary directory is " << tmpFoder.c_str(); + + xmeSetPaths(m_driverConfiguration.licensePath.c_str(), "", tmpFoder.c_str(), true); + + // Create a connection istance + m_connection.reset(XmeControl::construct()); + if (!m_connection) { + xsError << "Could not open Xsens DLL. Check to use the correct DLL."; + return false; + } + // Configure the connection to enable callbacks support + m_connection->addCallbackHandler(static_cast(this)); + return true; +} + +bool XSensMVNDriverImpl::fillSegmentNames() +{ + // Lock the mutex to prevent other threads changing the segment list + std::lock_guard connectionGuard(m_objectMutex); + + if (!m_connection || m_connection->segmentCount() == 0) { + xsWarning << "Unable to retrieve segment names from XSens or empty segment list"; + return false; + } + + // Get segment count and allocate memory for output vector + int segmentCount = m_connection->segmentCount(); + m_segmentNames.reserve(static_cast(segmentCount)); + + // Segment ID is 1-based, vector is 0-based, segmentID = index + 1 + for (int ix = 0; ix < segmentCount; ++ix) { + m_segmentNames.push_back(m_connection->segmentName(ix + 1).toStdString()); + } + + return true; +} + +bool XSensMVNDriverImpl::fillSensorNames() +{ + // Lock the mutex to prevent other threads changing the segment list + std::lock_guard connectionGuard(m_objectMutex); + + if (!m_connection || m_connection->sensorCount() == 0) { + xsWarning << "Unable to retrieve sensor names from XSens or empty sensor list"; + return false; + } + + unsigned sensorCount = static_cast(m_connection->sensorCount()); + // Since sensor names are not directly available, get the suit sensors status + XmeDeviceStatusArray sensorsStatus = m_connection->status().suitStatus().m_sensors; + + // Double check if sensorCount and sensorStatus.size() are equals + if (sensorCount != sensorsStatus.size()) { + xsWarning << "Mismatch in sensor count and sensor states dimensions"; + return false; + } + + m_sensorNames.reserve(sensorCount); + for (unsigned ix = 0; ix < sensorCount; ++ix) { + // Get the name of the segment associated to the ix-th sensor (everything is + // 0-based) + m_sensorNames.push_back( + m_connection->segmentName(sensorsStatus[ix].m_segmentId).toStdString()); + } + + return true; +} + +bool XSensMVNDriverImpl::fillJointNames() +{ + // Lock the mutex to prevent other threads changing the segment list + std::lock_guard connectionGuard(m_objectMutex); + + if (!m_connection) { + xsWarning << "Unable to retrieve joint names"; + return false; + } + + m_jointNames.reserve(m_connection->joints().size()); + + for (const auto& joint : m_connection->joints()) { + m_jointNames.push_back(joint.name().toStdString()); + } + + return true; +} +// namespace xsensmvn +// bool yarp::dev::XsensMVN::XsensMVNPrivate::init(yarp::os::Searchable& config) +// { +// std::lock_guard globalGuard(m_objectMutex); + +// // Check if the device is already connected +// if (m_connection) { +// yError("Device already connected. Unable to configure"); +// return false; +// } + +// // Create a license istance +// m_license.reset(new XmeLicense()); +// if (!m_license) { +// yError("Unable to create a valid license istance. Check hardware dongle."); +// return false; +// } + +// //--------------------------------------------- +// // Retrieve paths and configure xme to use them +// //--------------------------------------------- +// // The location of the other files (xmedef, *.mvnc) can be configured at runtime by +// calling +// // xmeSetPaths. This expects in this order: +// // - the path to runtime dependencies (xmedef.xsb, and .mvnc files), +// // - the path to (props.xsb - probably not useful), +// // - the path where the file xme.log will be stored (a temporary directory) +// // - bool: overwrite previous logs +// // Note: If any of these strings are left empty (i.e. "") the default value will be +// used + +// // Retrieve temporary folder +// std::experimental::filesystem::path tmpFoder = +// std::experimental::filesystem::temp_directory_path(); +// yInfo("Temporary directory, retrieved with std::experimental, is %s", +// tmpFoder.c_str()); + +// // Read from conf file the rundepsFolder +// yarp::os::ConstString rundepsFolder = +// config +// .check("xsens-rundeps-dir", +// yarp::os::Value(""), +// "Path to XSens MVN SDK runtime dependencies") +// .asString(); +// yInfo("Reading runtime dependencies from %s", rundepsFolder.c_str()); + +// // Configure xme to use the specified folders +// xmeSetPaths(rundepsFolder.c_str(), "", tmpFoder.c_str(), true); + +// // Create a connection istance +// m_connection.reset(XmeControl::construct()); +// if (!m_connection) { +// yError("Could not open Xsens DLL. Check to use the correct DLL."); +// return false; +// } +// // Configure the connection to enable callbacks support +// m_connection->addCallbackHandler(this); + +// //---------------------------------------- +// // Configure the MVN motion capture system +// //---------------------------------------- + +// // Get the list of the supported suit configurations +// yInfo("--- Supported MVN suit configurations ---"); +// XsStringArray suitConfs = m_connection->configurationList(); +// for (XsStringArray::const_iterator it = suitConfs.begin(); it != suitConfs.end(); +// ++it) { +// yInfo(" - %s", it->c_str()); +// } +// yInfo("--------------------------------"); + +// // Get the user-selected suit configuration +// yarp::os::ConstString suitConf = +// config.check("suit-config", yarp::os::Value(""), "MVN suit configuration to use") +// .asString(); + +// // Check if the user-selected configuration is supported +// bool confIsSupported = false; +// for (const XsString& conf : suitConfs) { +// if (conf == suitConf.c_str()) { +// confIsSupported = true; +// break; +// } +// } +// if (!confIsSupported) { +// yError("Selected suit configuration %s not supported", suitConf.c_str()); +// fini(); +// return false; +// } + +// // Configure the system to use the selected MVN suit configuration +// m_connection->setConfiguration(suitConf.c_str()); +// yInfo("Using suit configuration: %s", suitConf.c_str()); + +// // TODO: add option to select the available MVN scenario + +// // Get the user-selected absolute reference system name +// // Information used ONLY at YARP level, is just an alias for the output data reference +// frame +// // info +// yarp::os::ConstString referenceFrame = config +// .check("reference-frame-alias", +// yarp::os::Value("xsens_world"), +// "Absolute reference frame alias") +// .asString(); + +// //---------------------------------- +// // Connect to the Xsens MVN hardware +// //---------------------------------- + +// // Get the timeout [ms] for connecting with the XSens MVN hardware +// int scanTimeout = +// config.check("scan-timeout", yarp::os::Value(10), "Scan timeout [s]. -1 to +// disable.") +// .asInt32() +// * 1000; + +// // Scan for the suit until either the connection is successfull or the timeout is +// reached +// // Since this function is called asynchronously, it should be synchronized +// std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); +// { +// std::unique_lock connectionLock(m_connectionMutex); +// m_hardwareConnected = false; +// m_connection->setScanMode(true); +// if (scanTimeout > 0) { +// // Timeout enabled +// m_connectionVariable.wait_until(connectionLock, +// now + std::chrono::milliseconds(scanTimeout), +// [&]() { return m_hardwareConnected; }); +// } +// else { +// // Timout disabled, keep scannig until the system is found +// m_connectionVariable.wait(connectionLock, [&]() { return m_hardwareConnected; +// }); +// } +// m_connection->setScanMode(false); +// } + +// // Double-check if Xsens MVN system is connected +// if (!m_connection->status().isConnected()) { +// yError("Unable to connect to the XSens MVN suit"); +// fini(); +// return false; +// } + +// yInfo("XSens suit connected"); + +// //---------------------------- +// // Create a calibrator istance +// //---------------------------- + +// m_calibrator.reset(new xsens::XsensMVNCalibrator(*m_connection)); +// if (!m_calibrator) { +// yError("Unable to create the calibrator"); +// fini(); +// return false; +// } +// m_calibrator->addDelegate(*this); + +// //------------------------------------------- +// // Configure subject-specific body dimensions +// //------------------------------------------- + +// yInfo("Configuring subject-specific body dimensions"); + +// // Get configurable body dimensions from XSens +// XsStringArray bodyDimensionSet = m_connection->bodyDimensionLabelList(); +// if (bodyDimensionSet.empty()) { +// yError("Unknown error occurred in retrieving supported body dimension from +// device."); fini(); return false; +// } + +// // Get subject-specific body dimensions from the configuration file and push them to +// // bodyDimensionMap +// yarp::os::Bottle subjectBodyDimensionSet = +// config.findGroup("body-dimensions", "Subject-specific body dimensions"); +// std::map bodyDimensionsMap; + +// yInfo("--- Configurable body dimensions supported by XSens MVN : user-provided value +// [m] " +// "---"); +// for (const auto& bd : bodyDimensionSet) { +// yarp::os::Value dimension = subjectBodyDimensionSet.find(bd.c_str()); +// if (dimension.isNull() || !dimension.isFloat64()) { +// yInfo(" - %s : Not Provided", bd.c_str()); +// continue; +// } +// double dimValue = dimension.asFloat64(); +// yInfo(" - %s : %lf [m]", bd.c_str(), dimValue); +// bodyDimensionsMap.insert( +// std::map::value_type(bd.c_str(), dimValue)); +// } +// yInfo("------------------------------------------------------------------------------------" +// "--"); + +// // Provide the bodyDimensionMap to the calibrator and (indirectly) to MVN Engine +// if (!bodyDimensionsMap.empty()) { +// if (!setBodyDimensions(bodyDimensionsMap)) { +// yError("Unable to configure the system with the provided body dimensions"); +// fini(); +// return false; +// } +// } + +// //--------------------------------------------- +// // Retrieve model segment and suit sensor lists +// //--------------------------------------------- + +// const std::vector segmentNameList = +// getSegmentInfos(); + +// yInfo("--- XSens body segments --- [XSensSegmentID] - ReferenceFrame - Name ---"); +// // XSensSegmentID is 1-based, segmentNamesVector is 0-based +// // segmentID = index + 1 +// for (unsigned ix = 0; ix < segmentNameList.size(); ++ix) { +// yInfo(" - [%d] - %s - %s", +// ix + 1, +// segmentNameList[ix].frameReference.c_str(), +// segmentNameList[ix].frameName.c_str()); +// } +// yInfo("------------------------------------------------------------------------"); + +// // TODO: get sensors list + +// //------------------------------------- +// // Retrieve supported calibration types +// //------------------------------------- + +// XsStringArray calibrationTypeSet = m_connection->calibrationLabelList(); +// yInfo("--- Supported calibration types ---"); +// for (const auto& cr : calibrationTypeSet) { +// yInfo(" - %s", cr.c_str()); +// } +// yInfo("------------------------------------"); + +// // Get the default calibration routine name from config file +// yarp::os::ConstString defaultCalibrationType = config +// .check("default-calibration-type", +// yarp::os::Value("Npose"), +// "Default calibration type") +// .asString(); + +// // Copy the value from yarp::os::ConstString to the std::string +// m_defaultCalibrationType.assign(defaultCalibrationType.c_str(), +// defaultCalibrationType.length()); + +// yInfo("Default calibration type: %s", m_defaultCalibrationType.c_str()); + +// //--------------------------- +// // Initialize data structures +// //--------------------------- +// // Initialize size of vectors (model is set before calibration) +// resizeVectorToOuterAndInnerSize(m_lastSegmentPosesRead, m_connection->segmentCount(), +// 7); resizeVectorToOuterAndInnerSize( +// m_lastSegmentVelocitiesRead, m_connection->segmentCount(), 6); +// resizeVectorToOuterAndInnerSize( +// m_lastSegmentAccelerationsRead, m_connection->segmentCount(), 6); + +// resizeVectorToOuterAndInnerSize(m_lastSensorPositionsRead, +// m_connection->sensorCount(), 3); resizeVectorToOuterAndInnerSize( +// m_lastSensorOrientationsRead, m_connection->sensorCount(), 4); +// resizeVectorToOuterAndInnerSize( +// m_lastSensorFreeAccelerationsRead, m_connection->sensorCount(), 3); + +// //--------------------------------------- +// // Create the datasample processor thread +// //--------------------------------------- +// // Create a sample processor thread to free data processing from MVN callbacks +// m_stopProcessor = false; +// m_processor = std::thread(&yarp::dev::XsensMVN::XsensMVNPrivate::processNewFrame, +// this); + +// return m_hardwareConnected; +// } + +// bool yarp::dev::XsensMVN::XsensMVNPrivate::fini() +// { +// std::lock_guard globalGuard(m_objectMutex); + +// // Check for existence and remove calibrator thread +// if (m_calibrator) { +// m_calibrator->removeDelegate(*this); +// } + +// // Stop the acquisition, disconnect the hardware, and destroy the device connector +// if (m_connection) { +// stopAcquisition(); +// m_connection->disconnectHardware(); +// { +// std::unique_lock connectionLock(m_connectionMutex); +// m_connectionVariable.wait(connectionLock, +// [&]() { return +// !m_connection->status().isConnected(); }); + +// // Clear the callback handlers list +// m_connection->clearCallbackHandlers(); +// m_connection->destruct(); +// } +// } + +// // Close frame processor thread +// { +// std::unique_lock lock(m_processorMutex); +// m_stopProcessor = true; + +// // Notify the processor thread it's time to end +// m_processorVariable.notify_one(); +// } + +// // The processor lock is now realeased, join the thread to wait for its end +// if (m_processor.joinable()) { +// m_processor.join(); +// } + +// if (m_license) { +// xmeTerminate(); +// } + +// return true; +// } + +// const std::vector +// yarp::dev::XsensMVN::XsensMVNPrivate::getSegmentInfos() const +// { +// // Initialize empty output vector +// std::vector segments{}; + +// // Lock the mutex to prevent other threads changing the segment list +// std::lock_guard globalGuard(m_objectMutex); + +// if (!m_connection || m_connection->segmentCount() < 0) { +// yWarning("Unable to retrieve segment names from XSens or empty segment list"); +// return segments; +// } + +// // Get segment count and allocate memory for output vector +// unsigned segmentCount = static_cast(m_connection->segmentCount()); +// segments.reserve(segmentCount); + +// // Segment ID is 1-based, vector is 0-based, segmentID = index + 1 +// for (unsigned ix = 0; ix < segmentCount; ++ix) { +// yarp::experimental::dev::FrameReference frameInfo = { +// m_referenceFrame, m_connection->segmentName(ix + 1).c_str()}; +// segments.push_back(frameInfo); +// } + +// return segments; +// } + +// const std::vector +// yarp::dev::XsensMVN::XsensMVNPrivate::getSensorInfos() const +// { +// // Initialize empty output vector +// std::vector sensors{}; + +// // Lock the mutex to prevent other threads changing the segment list +// std::lock_guard globalGuard(m_objectMutex); + +// if (!m_connection || m_connection->sensorCount() < 0) { +// yWarning("Unable to retrieve sensor list from XSens or empty sensor list"); +// return sensors; +// } +// // Get sensor count +// unsigned sensorCount = static_cast(m_connection->sensorCount()); + +// // Since sensor names are not directly available, get the suit sensors status +// XmeDeviceStatusArray sensorsStatus = m_connection->status().suitStatus().m_sensors; + +// // Double check if sensorCount and sensorStatus.size() are equals +// if (sensorCount != static_cast(sensorsStatus.size())) { +// yWarning("Mismatch in sensor count and sensor states dimensions"); +// return sensors; +// } + +// // allocate memory for output vector +// sensors.reserve(sensorCount); + +// for (unsigned ix = 0; ix < sensorCount; ++ix) { +// // Get the name of the segment associated to the ix-th sensor (everything is +// 0-based) XsString sensorInSegmentName = +// m_connection->segmentName(sensorsStatus[ix].m_segmentId); +// // Imu quantities in MVN 2018 are expressed w.r.t. global frame +// yarp::experimental::dev::FrameReference frameInfo = {m_referenceFrame, +// sensorInSegmentName.c_str()}; +// sensors.push_back(frameInfo); +// } +// return sensors; +// } + +// bool yarp::dev::XsensMVN::XsensMVNPrivate::startAcquisition() +// { +// std::lock_guard globalGuard(m_objectMutex); + +// // Check if the device is available, is connected and is inside WiFi range +// if (!m_connection || !m_connection->status().isConnected() +// || m_connection->status().isOutOfRange()) { +// yError("Device not connected or out of range. Unable to start data acquisition"); +// return false; +// } + +// // Check that there are no calibration in progress +// if (m_calibrator && m_calibrator->isCalibrationInProgress()) { +// yError("Calibration in progress. Unable to start data acquisition"); +// return false; +// } + +// // TODO: Check if the device is calibrated, otherwise acquisition cannot start + +// // Check if the acquisition is alredy in progress +// if (m_connection->realTimePoseMode() && m_acquiring +// && m_driverStatus == yarp::experimental::dev::IFrameProviderStatusOK) { +// yWarning("Acquisition already in progress"); +// return false; +// } + +// // Start acquisition +// m_acquiring = true; +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusOK; +// m_connection->setRealTimePoseMode(true); +// yInfo("Acquisition successfully started"); +// return true; +// } + +// bool yarp::dev::XsensMVN::XsensMVNPrivate::stopAcquisition() +// { +// std::lock_guard globalGuard(m_objectMutex); + +// // Check if the device is available, is connected and is inside WiFi range +// if (!m_connection || !m_connection->status().isConnected() +// || m_connection->status().isOutOfRange()) { +// yError("Device not connected or out of range. Unable to stop data acquisition"); +// return false; +// } + +// // Stop acquisition +// m_connection->setRealTimePoseMode(false); +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusNoData; +// m_acquiring = false; +// yInfo("Acquisition successfully stopped"); +// return true; +// } + +// bool yarp::dev::XsensMVN::XsensMVNPrivate::setBodyDimensions( +// const std::map& dimensions) +// { +// std::lock_guard globalGuard(m_objectMutex); + +// // Check if device and calibrator objects are available +// if (!m_connection || !m_calibrator) { +// yError("Device or calibrator not available. Unable to set body dimensions"); +// return false; +// } + +// // Ask the calibrator to set the specified body dimensions +// return m_calibrator->setBodyDimensions(dimensions); +// } + +// // Read fron the device the estimated body dimensions according to the scaling done by +// XSens +// // after every call to setBodyDimension +// bool yarp::dev::XsensMVN::XsensMVNPrivate::getBodyDimensions( +// std::map& dimensions) const +// { +// std::lock_guard globalGuard(m_objectMutex); + +// // Check if device and calibrator objects are available +// if (!m_connection || !m_calibrator) { +// yError("Device or calibrator not available. Unable to set body dimensions"); +// return false; +// } +// return m_calibrator->getBodyDimensions(dimensions); +// } + +// // Perform XSens calibration using the specified calibrationType +// bool yarp::dev::XsensMVN::XsensMVNPrivate::calibrateWithType(const std::string& +// calibrationType) +// { +// std::string tmpCalibType = calibrationType; +// { +// std::lock_guard globalGuard(m_objectMutex); + +// // Check device and calibrator availability +// if (!m_connection || !m_calibrator) { +// yError("Device or calibrator not available. Unable to start calibration +// routine"); return false; +// } + +// // Check if there is an acquisition in progress +// if (m_acquiring) { +// yError("Acquisition in progress. Unable to start calibration routine."); +// yError("First stop current acquisition, then retry"); +// return false; +// } + +// // Handle empty calibrationType parameter +// if (tmpCalibType.empty()) { +// if (m_defaultCalibrationType.empty()) { +// yError("Neither custom nor default calibration type set. Aborting"); +// return false; +// } +// tmpCalibType = m_defaultCalibrationType; +// } +// } + +// // Activate data flow +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusOK; + +// // Ask the calibrator to perform the calibration +// bool calibrationCompleted = m_calibrator->calibrateWithType(tmpCalibType); + +// // Deactivate data flow +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusNoData; + +// if (calibrationCompleted) { +// m_calibrationType = tmpCalibType; +// } + +// return calibrationCompleted; +// } + +// // TODO: Check what is the abort calibration flow and arrange stuff accordingly +// bool yarp::dev::XsensMVN::XsensMVNPrivate::abortCalibration() +// { +// // I don't know if this makes sense, but how can I abort the calibration +// // if I have to take the same lock of the calibrate function? +// { +// std::lock_guard globalGuard(m_objectMutex); + +// // Check device and calibrator availability +// if (!m_connection || !m_calibrator) { +// yError("Device or calibrator not available. Unable to abort calibration +// routine"); return false; +// } +// } +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusNoData; +// m_calibrator->abortCalibration(); +// return true; +// } + +// void yarp::dev::XsensMVN::XsensMVNPrivate::processNewFrame() +// { +// yDebug("Starting sample processor thread"); +// while (true) { + +// // Processor lock scope +// std::unique_lock processorLock(m_processorMutex); + +// // Wait for being notified if new sample is available or if it is time to stop +// m_processorVariable.wait(processorLock, +// [&]() { return m_stopProcessor || m_lastSampleAvailable; +// }); + +// // Check if the thread has been notified by fini() +// if (m_stopProcessor) { +// // Time to stop +// yDebug("Closing sample processor thread"); +// break; +// } + +// // Move locally the last data sample +// SampleData lastSample = std::move(m_lastSampleData); +// m_lastSampleAvailable = false; + +// // Unlock processor scope +// processorLock.unlock(); + +// // Global lock scope +// { +// std::lock_guard globalLock(m_objectMutex); +// if (!m_acquiring) { +// continue; +// } +// } + +// // Process the new data sample +// { +// std::lock_guard readLock(m_dataMutex); + +// // Check if expected and actual sensor and segment counts match +// if (m_lastSegmentPosesRead.size() != +// lastSample.humanPose.m_segmentStates.size() +// || m_lastSensorOrientationsRead.size() +// != lastSample.suitData.sensorKinematics().size()) { +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusError; +// yWarning("Sample Processor Thread: Error in data processing, size +// mismatch. " +// "Skipping current sample"); +// continue; +// } + +// // TODO: add option to select which timestamp to use +// int64_t xsAbsoluteTime = lastSample.humanPose.m_absoluteTime; +// double time = xsAbsoluteTime / 1000.0; + +// m_lastTimestamp = yarp::os::Stamp(lastSample.humanPose.m_frameNumber, time); + +// for (unsigned sx = 0; sx < lastSample.humanPose.m_segmentStates.size(); ++sx) +// { +// const XmeSegmentState& segmentData = +// lastSample.humanPose.m_segmentStates.at(sx); +// yarp::sig::Vector& segmentPosition = m_lastSegmentPosesRead.at(sx); +// yarp::sig::Vector& segmentVelocity = m_lastSegmentVelocitiesRead.at(sx); +// yarp::sig::Vector& segmentAcceleration = +// m_lastSegmentAccelerationsRead.at(sx); + +// for (unsigned i = 0; i < 3; ++i) { +// // linear part +// segmentPosition(i) = segmentData.m_position.at(i); +// segmentVelocity(i) = segmentData.m_velocity.at(i); +// segmentAcceleration(i) = segmentData.m_acceleration.at(i); +// // angular part for velocity and acceleration +// segmentVelocity(3 + i) = segmentData.m_angularVelocity.at(i); +// segmentAcceleration(3 + i) = segmentData.m_angularAcceleration.at(i); +// } +// // Do the quaternion explicitly to avoid issues in format +// segmentPosition(3) = segmentData.m_orientation.w(); +// segmentPosition(4) = segmentData.m_orientation.x(); +// segmentPosition(5) = segmentData.m_orientation.y(); +// segmentPosition(6) = segmentData.m_orientation.z(); +// } + +// for (unsigned sx = 0; sx < lastSample.suitData.sensorKinematics().size(); +// ++sx) { +// const XmeSensorKinematics& sensorData = +// lastSample.suitData.sensorKinematics().at(sx); +// yarp::sig::Vector& sensorPosition = m_lastSensorPositionsRead.at(sx); +// yarp::sig::Vector& sensorOrientation = +// m_lastSensorOrientationsRead.at(sx); yarp::sig::Vector& +// sensorFreeAcceleration = +// m_lastSensorFreeAccelerationsRead.at(sx); + +// for (unsigned i = 0; i < 3; ++i) { +// sensorPosition(i) = sensorData.m_posG.at(i); +// sensorFreeAcceleration(i) = sensorData.m_accG.at(i); +// } +// sensorOrientation(0) = sensorData.m_q.w(); +// sensorOrientation(1) = sensorData.m_q.x(); +// sensorOrientation(2) = sensorData.m_q.y(); +// sensorOrientation(3) = sensorData.m_q.z(); +// } + +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusOK; +// } +// } +// yDebug("Sample processor thread closed"); +// } + +// yarp::experimental::dev::IFrameProviderStatus +// yarp::dev::XsensMVN::XsensMVNPrivate::getLastSegmentReadTimestamp(yarp::os::Stamp& +// timestamp) +// { +// std::lock_guard readLock(m_dataMutex); +// timestamp = m_lastTimestamp; +// return m_driverStatus; +// } + +// yarp::experimental::dev::IIMUFrameProviderStatus +// yarp::dev::XsensMVN::XsensMVNPrivate::getLastSensorReadTimestamp(yarp::os::Stamp& +// timestamp) +// { +// std::lock_guard readLock(m_dataMutex); +// timestamp = m_lastTimestamp; +// return +// yarp::experimental::dev::IIMUFrameProviderStatus(static_cast(m_driverStatus)); +// } + +// yarp::experimental::dev::IFrameProviderStatus +// yarp::dev::XsensMVN::XsensMVNPrivate::getLastSegmentInformation( +// yarp::os::Stamp& timestamp, +// std::vector& lastPoses, +// std::vector& lastVelocities, +// std::vector& lastAccelerations) +// { +// // These also ensure all the sizes are the same +// if (lastPoses.size() < m_lastSegmentPosesRead.size()) { +// // This will cause an allocation +// resizeVectorToOuterAndInnerSize(lastPoses, m_lastSegmentPosesRead.size(), 7); +// } +// if (lastVelocities.size() < m_lastSegmentPosesRead.size()) { +// // This will cause an allocation +// resizeVectorToOuterAndInnerSize(lastVelocities, m_lastSegmentPosesRead.size(), 6); +// } +// if (lastAccelerations.size() < m_lastSegmentPosesRead.size()) { +// // This will cause an allocation +// resizeVectorToOuterAndInnerSize(lastAccelerations, m_lastSegmentPosesRead.size(), +// 6); +// } + +// { +// std::lock_guard readLock(m_dataMutex); + +// if (m_acquiring) { +// // we should receive data +// double now = yarp::os::Time::now(); +// if ((now - m_lastTimestamp.getTime()) > 1.0) { +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusTimeout; +// } +// // yInfo("Reading data with time %lf at YARP Time %lf", +// // m_lastSegmentTimestamp.getTime(), now); +// /*else { +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusOK; +// }*/ +// } +// // get anyway data out +// timestamp = m_lastTimestamp; +// for (unsigned i = 0; i < m_lastSegmentPosesRead.size(); ++i) { +// lastPoses[i] = m_lastSegmentPosesRead[i]; +// lastVelocities[i] = m_lastSegmentVelocitiesRead[i]; +// lastAccelerations[i] = m_lastSegmentAccelerationsRead[i]; +// } +// } + +// return m_driverStatus; +// } + +// yarp::experimental::dev::IIMUFrameProviderStatus +// yarp::dev::XsensMVN::XsensMVNPrivate::getLastSuitInformations( +// yarp::os::Stamp& timestamp, +// std::vector& lastOrientations, +// std::vector& lastPositions, +// std::vector& lastFreeAccelerations) +// { +// // These also ensure all the sizes are the same +// if (lastOrientations.size() < m_lastSensorOrientationsRead.size()) { +// // This will cause an allocation +// resizeVectorToOuterAndInnerSize( +// lastOrientations, m_lastSensorOrientationsRead.size(), 4); +// } +// if (lastPositions.size() < m_lastSensorPositionsRead.size()) { +// // This will cause an allocation +// resizeVectorToOuterAndInnerSize(lastPositions, m_lastSensorPositionsRead.size(), +// 3); +// } +// if (lastFreeAccelerations.size() < m_lastSensorFreeAccelerationsRead.size()) { +// // This will cause an allocation +// resizeVectorToOuterAndInnerSize( +// lastFreeAccelerations, m_lastSensorFreeAccelerationsRead.size(), 3); +// } + +// { +// std::lock_guard readLock(m_dataMutex); + +// if (m_acquiring) { +// // we should receive data +// double now = yarp::os::Time::now(); +// if ((now - m_lastTimestamp.getTime()) > 1.0) { +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusTimeout; +// } +// // yInfo("Reading data with time %lf at YARP Time %lf", +// // m_lastSegmentTimestamp.getTime(), now); +// /*else { +// m_driverStatus = yarp::experimental::dev::IFrameProviderStatusOK; +// }*/ +// } +// // get anyway data out +// timestamp = m_lastTimestamp; +// for (unsigned i = 0; i < m_lastSensorOrientationsRead.size(); ++i) { +// lastOrientations[i] = m_lastSensorOrientationsRead[i]; +// lastPositions[i] = m_lastSensorPositionsRead[i]; +// lastFreeAccelerations[i] = m_lastSensorFreeAccelerationsRead[i]; +// } +// } +// return +// yarp::experimental::dev::IIMUFrameProviderStatus(static_cast(m_driverStatus)); +// } + +// /*----------------- +// Callback funtions +// -----------------*/ + +// void yarp::dev::XsensMVN::XsensMVNPrivate::onHardwareReady(XmeControl* dev) +// { +// yInfo("Xsens MVN hardware connected successfully. Ready to start"); +// std::unique_lock lock(m_connectionMutex); +// m_hardwareConnected = true; +// m_connectionVariable.notify_one(); +// } + +// void yarp::dev::XsensMVN::XsensMVNPrivate::onHardwareError(XmeControl* dev) +// { +// XmeStatus status = dev->status(); +// yWarning("Hardware error received."); +// yWarning("Suit connected: %s", (status.isConnected() ? "YES" : "NO")); +// yWarning("Scanning for XSens MVN suit: %s", (status.isScanning() ? "YES" : "NO")); +// } + +// void yarp::dev::XsensMVN::XsensMVNPrivate::onPoseReady(XmeControl* dev) +// { +// // Copy last available sample to temporary structure to avoid waiting for the lock +// SampleData newSample; +// newSample.humanPose = dev->pose(XME_LAST_AVAILABLE_FRAME); +// newSample.suitData = dev->suitSample(XME_LAST_AVAILABLE_FRAME); + +// std::unique_lock lock(m_processorMutex); +// // Copy acquired sample to the processing queue and notify the processor thread +// m_lastSampleData = std::move(newSample); +// m_lastSampleAvailable = true; +// m_processorVariable.notify_one(); +// } + +// void yarp::dev::XsensMVN::XsensMVNPrivate::onHardwareDisconnected(XmeControl*) +// { +// yInfo("Xsens MVN hardware disconnected."); +// std::unique_lock lock(m_connectionMutex); +// m_hardwareConnected = false; +// m_connectionVariable.notify_one(); +// } + +// // TODO: not sure what it does, to be investigated +// void yarp::dev::XsensMVN::XsensMVNPrivate::calibratorHasReceivedNewCalibrationPose( +// const xsens::XsensMVNCalibrator* const sender, +// std::vector newPose) +// { +// // only manage my own calibrator +// // if (sender != m_calibrator) +// // return; +// std::lock_guard readLock(m_dataMutex); + +// for (unsigned index = 0; index < newPose.size(); ++index) { +// m_lastSegmentPosesRead[index] = newPose[index]; +// m_lastSegmentVelocitiesRead[index].zero(); +// m_lastSegmentAccelerationsRead[index].zero(); +// } +// } diff --git a/XSensMVN/test/CMakeLists.txt b/XSensMVN/test/CMakeLists.txt new file mode 100644 index 000000000..355930569 --- /dev/null +++ b/XSensMVN/test/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +# Build the test unit executables +# =============================== +add_executable(testCalibrator ${CMAKE_CURRENT_SOURCE_DIR}/testCalibrator.cpp) +target_link_libraries(testCalibrator XSensMVN) + +add_executable(testDriver ${CMAKE_CURRENT_SOURCE_DIR}/testDriver.cpp) +target_link_libraries(testDriver XSensMVN) diff --git a/XSensMVN/test/testCalibrator.cpp b/XSensMVN/test/testCalibrator.cpp new file mode 100644 index 000000000..f88ad5f2f --- /dev/null +++ b/XSensMVN/test/testCalibrator.cpp @@ -0,0 +1,61 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "XSensCalibrationQualities.h" +#include "XSensMVNCalibrator.h" + +#include "xme.h" + +int main(int argc, const char* argv[]) +{ + // Create a licence object + XmeLicense lic; + + // Hardcoded path to the runtime dependencies + std::string rundepsFolder = "C:/Program Files/Xsens/MVN SDK 2018.0.3/SDK Files/rundeps"; + + // Configure the paths to the runtime required folders + xmeSetPaths(rundepsFolder.c_str(), "", "", true); + + try { + XmeControl* ctl = XmeControl::construct(); + + // Connect to the suit + xsInfo << "Starting scan"; + ctl->setScanMode(true); + + while (!ctl->status().isConnected()) + std::this_thread::sleep_for(std::chrono::microseconds(10)); + + ctl->setScanMode(false); + + xsInfo << "Suit connected."; + + // List the available calibration types + XsStringArray calibTypes = ctl->calibrationLabelList(); + xsInfo << "Calibration types:"; + for (auto& type : calibTypes) { + xsInfo << type; + } + + // Create the calibrator + xsensmvn::XSensMVNCalibrator calib(*ctl, xsensmvn::CalibrationQuality::ACCEPTABLE); + + // Perform the calibration + calib.calibrateWithType(std::string("Npose")); + + // Disconnect from the XSens suit and wait for a positive feedback + ctl->disconnectHardware(); + while (ctl->status().isConnected()) + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + catch (XsException& exception) { + xsError << "An error occurred: " << exception.text(); + xmeTerminate(); + return 1; + } + + // Clean up and exit + xmeTerminate(); + return EXIT_SUCCESS; +} diff --git a/XSensMVN/test/testDriver.cpp b/XSensMVN/test/testDriver.cpp new file mode 100644 index 000000000..55f6ed3d0 --- /dev/null +++ b/XSensMVN/test/testDriver.cpp @@ -0,0 +1,105 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "XSensLogger.h" +#include "XSensMVNDriver.h" + +#include "xme.h" + +#include +#include + +int main(int argc, const char* argv[]) +{ + + xsensmvn::DriverDataStreamConfig dsconf = {true, true, true}; + + xsensmvn::bodyDimensions bd = {{"ankleHeight", 0.07}, + {"armSpan", 1.71}, + {"bodyHeight", 1.71}, + {"footSize", 0.26}, + {"hipHeight", 0.87}, + {"hipWidth", 0.25}, + {"kneeHeight", 0.50}, + {"shoulderWidth", 0.34}, + {"shoeSoleHeight", 0.02}}; + + xsensmvn::DriverConfiguration conf = { + "C:/Program Files/Xsens/MVN SDK 2018.0.3/SDK Files/rundeps", + "FullBody", + "", + "Tpose", + xsensmvn::CalibrationQuality::FAILED, + 20, + 120, + bd, + dsconf}; + + xsInfo << std::endl << std::endl << "Creating"; + xsensmvn::XSensMVNDriver driver(conf); + + xsInfo << std::endl << std::endl << "Configuring"; + + driver.configureAndConnect(); + + xsInfo << std::endl << std::endl << "Connected, ready to be calibrated"; + + if (driver.calibrate("Npose")) { + xsInfo << std::endl << std::endl << "Calibrated, ready to acquire data"; + }; + + if (driver.startAcquisition()) { + xsInfo << std::endl << std::endl << "Recording"; + + const auto jointLabels = driver.getSuitJointLabels(); + xsInfo << "----------- Joint Labels -------------"; + for (const auto& j : jointLabels) { + xsInfo << " - " << j; + } + xsInfo << "--------------------------------------"; + + const auto linkLabels = driver.getSuitLinkLabels(); + xsInfo << "----------- Link Labels -------------"; + for (const auto& l : linkLabels) { + xsInfo << " - " << l; + } + xsInfo << "--------------------------------------"; + + for (int j = 0; j < 50; ++j) { + xsensmvn::LinkDataVector linkSample; + linkSample = driver.getLinkDataSample(); + if (!linkSample.data.empty()) { + // xsInfo << "Xsens relative time: " << linkSample.time->relative; + // xsInfo << "Xsens absolute time: " << linkSample.time->absolute; + xsInfo << std::endl + << std::endl + << "System time (UNIX time): " << linkSample.time->systemTime; + + for (size_t l = 0; l < linkLabels.size(); ++l) { + xsInfo << linkSample.data.at(l).name + << " position: " << linkSample.data.at(l).position.at(0) << " " + << linkSample.data.at(l).position.at(1) << " " + << linkSample.data.at(l).position.at(2); + } + } + + xsensmvn::JointDataVector jointSample = driver.getJointDataSample(); + if (!jointSample.data.empty()) { + xsInfo << std::endl + << std::endl + << "System time (UNIX time): " << jointSample.time->systemTime; + for (size_t j = 0; j < jointLabels.size(); ++j) { + xsInfo << jointSample.data.at(j).name + << " jointAngles XYZ: " << jointSample.data.at(j).angles.at(0) << " " + << jointSample.data.at(j).angles.at(1) << " " + << jointSample.data.at(j).angles.at(2); + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + } + + xsInfo << "Done. Closing."; + xsInfo << "Closing success: " << driver.terminate(); + return EXIT_SUCCESS; +} diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt new file mode 100644 index 000000000..b7eb01471 --- /dev/null +++ b/app/CMakeLists.txt @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +# HDE xml files installation + +set (WEARABLES_XML_FILES xml/FTShoeLeftWearableDevice.xml + xml/FTShoeRightWearableDevice.xml + xml/FTShoesWearableDevice.xml + xml/ICubWearableDevice.xml + xml/SkinInsolesWearableDevice.xml + xml/XsensSuitWearableDevice.xml + xml/applications/WearableDevices-Dumper.xml + xml/applications/HumanDynamicsEstimation-WearableDumper.xml + xml/applications/WearableDevices.xml) + +install(FILES ${WEARABLES_XML_FILES} + DESTINATION ${CMAKE_INSTALL_DATADIR}/${WEARABLES_PROJECT_NAME}) diff --git a/app/Wearables-yarpmanager.ini b/app/Wearables-yarpmanager.ini new file mode 100644 index 000000000..c163290c6 --- /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/AMTIForcePlatesWearableDevice.xml b/app/xml/AMTIForcePlatesWearableDevice.xml new file mode 100644 index 000000000..ae29ec8ec --- /dev/null +++ b/app/xml/AMTIForcePlatesWearableDevice.xml @@ -0,0 +1,112 @@ + + + + + + 100 + off + data + + + + 2897 + 0 + + + FPplatform + + + + + + + + 2896 + 0 + + + FPplatform + + + + + + + 10 + /amti/first/analog:o + + + first_plaftform + + + + + + + 10 + /amti/second/analog:o + + + second_plaftform + + + + + + + FTShoeRightFTSensors + FTShoeRight + 6 + 0 + ForceTorque6DSensor + true + + + first_plaftform + + + + + + + FTShoeLeftFTSensors + FTShoeLeft + 6 + 0 + ForceTorque6DSensor + true + + + second_plaftform + + + + + + + + 0.01 + /FTShoeRight/WearableData/data:o + /FTShoeRight/WearableData/metadataRpc:o + + + AMTIFirstPlatformtIAnalogSensorToIWear + + + + + + + 0.01 + /FTShoeLeft/WearableData/data:o + /FTShoeLeft/WearableData/metadataRpc:o + + + AMTISecondPlatformtIAnalogSensorToIWear + + + + + + + diff --git a/app/xml/FTShoeLeftWearableDevice.xml b/app/xml/FTShoeLeftWearableDevice.xml new file mode 100644 index 000000000..54dfb41d1 --- /dev/null +++ b/app/xml/FTShoeLeftWearableDevice.xml @@ -0,0 +1,128 @@ + + + + + + + "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 + + + 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 000000000..58aeae038 --- /dev/null +++ b/app/xml/FTShoeRightWearableDevice.xml @@ -0,0 +1,128 @@ + + + + + + + "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 + + + 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/FTShoesWearableDevice.xml b/app/xml/FTShoesWearableDevice.xml new file mode 100644 index 000000000..a65cc4e33 --- /dev/null +++ b/app/xml/FTShoesWearableDevice.xml @@ -0,0 +1,262 @@ + + + + + + + "ecan" + "ecan" + 0 + 0x01 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x02 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x03 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x04 + 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 + + + ftShoe_Left_Front + ftShoe_Left_Rear + + + + + + 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_Left/analog:o + + + ftShoe_l + + + + + + 10 + /ft/ftShoe_Right/analog:o + + + ftShoe_r + + + + + + + + 10 + /ft/ftShoe_Left_Front/analog:o + + + ftShoe_Left_Front + + + + + + 10 + /ft/ftShoe_Left_Rear/analog:o + + + ftShoe_Left_Rear + + + + + + 10 + /ft/ftShoe_Right_Front/analog:o + + + ftShoe_Right_Front + + + + + + 10 + /ft/ftShoe_Right_Rear/analog:o + + + ftShoe_Right_Rear + + + + + + + + FTShoeLeftFTSensors + FTShoeLeft + 6 + 0 + ForceTorque6DSensor + true + + + ftShoe_l + + + + + + FTShoeRightFTSensors + FTShoeRight + 6 + 0 + ForceTorque6DSensor + true + + + ftShoe_r + + + + + + + + 0.01 + /FTShoeLeft/WearableData/data:o + /FTShoeLeft/WearableData/metadataRpc:o + + + FTShoeLeftIAnalogSensorToIWear + + + + + + 0.01 + /FTShoeRight/WearableData/data:o + /FTShoeRight/WearableData/metadataRpc:o + + + FTShoeRightIAnalogSensorToIWear + + + + + + diff --git a/app/xml/FTShoesWearableDevice_2.xml b/app/xml/FTShoesWearableDevice_2.xml new file mode 100644 index 000000000..a67ea142a --- /dev/null +++ b/app/xml/FTShoesWearableDevice_2.xml @@ -0,0 +1,241 @@ + + + + + + + "ecan" + "ecan" + 0 + 0x01 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x02 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x03 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x04 + 16 + 6 + 5 + 1 + 1 + + + + + 10 + /ft/ftShoe_Left_2/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 + + + ftShoe_Left_Front + ftShoe_Left_Rear + + + + + + 10 + /ft/ftShoe_Right_2/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 + + + ftShoe_Right_Front + ftShoe_Right_Rear + + + + + + + + 10 + /ft/ftShoe_Left_2/analog:o + + + ftShoe_l + + + + + + 10 + /ft/ftShoe_Right_2/analog:o + + + ftShoe_r + + + + + + + + 10 + /ft/ftShoe_Left_Front_2/analog:o + + + ftShoe_Left_Front + + + + + + 10 + /ft/ftShoe_Left_Rear_2/analog:o + + + ftShoe_Left_Rear + + + + + + 10 + /ft/ftShoe_Right_Front_2/analog:o + + + ftShoe_Right_Front + + + + + + 10 + /ft/ftShoe_Right_Rear_2/analog:o + + + ftShoe_Right_Rear + + + + + + + + FTShoeLeftFTSensors + FTShoeLeft + 6 + 0 + ForceTorque6DSensor + true + + + ftShoe_l + + + + + + FTShoeRightFTSensors + FTShoeRight + 6 + 0 + ForceTorque6DSensor + true + + + ftShoe_r + + + + + + + + 0.01 + /FTShoeLeft/WearableData_2/data:o + /FTShoeLeft/WearableData_2/metadataRpc:o + + + FTShoeLeftIAnalogSensorToIWear + + + + + + 0.01 + /FTShoeRight/WearableData_2/data:o + /FTShoeRight/WearableData_2/metadataRpc:o + + + FTShoeRightIAnalogSensorToIWear + + + + + + diff --git a/app/xml/ICubWearableDevice.xml b/app/xml/ICubWearableDevice.xml new file mode 100644 index 000000000..248fbeaf3 --- /dev/null +++ b/app/xml/ICubWearableDevice.xml @@ -0,0 +1,31 @@ + + + + + model.urdf + root_link + + /wholeBodyDynamics/left_arm/endEffectorWrench:o + /wholeBodyDynamics/right_arm/endEffectorWrench:o + /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o + /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o + + + (head torso left_arm right_arm left_leg right_leg) + /icubSim + /ICubWearableRemoteClient + + + + + 0.01 + /ICub/WearableData/data:o + /ICub/WearableData/metadataRpc:o + + + ICubWearableDevice + + + + + diff --git a/app/xml/PRO01_FTShoesWearableDevice.xml b/app/xml/PRO01_FTShoesWearableDevice.xml new file mode 100644 index 000000000..62ba204c9 --- /dev/null +++ b/app/xml/PRO01_FTShoesWearableDevice.xml @@ -0,0 +1,283 @@ + + + + + + + "ecan" + "ecan" + 0 + 0x01 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x02 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x03 + 16 + 6 + 5 + 1 + 1 + + + "ecan" + "ecan" + 0 + 0x04 + 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 + ) + + + true + + + ( + -0.4383, -0.8971, -0.0367, 1.8887, 1.4826, -0.4497, + 0.8902, -0.7420, -0.0951, 3.3255, -0.0336, 0.7560, + -0.0373, 0.0383, 1.0202, -0.9179, 0.2259, 0.3153, + 0.0017, -0.0373, -0.2889, -0.1609, -0.0201, -0.0786, + -0.0080, 0.0535, 0.0978, 0.6018, -0.0408, -0.0064, + -0.2022, -0.3434, -0.0126, 0.6566, 0.6046, 0.8013 + ) + + ( + 0.4241, 0.9560, 0.0317, -3.2329, -1.7754, 0.3937, + -0.7868, 0.5544, -0.0225, -2.2028, 3.0248, 1.0689, + 0.0132, 0.0203, 0.9914, 0.0640, 0.8944, 0.0800, + -0.0278, 0.0254, 0.1568, 0.3688, 1.1749, 0.1257, + -0.0132, -0.0387, 0.3433, -0.6378, 0.9329, 0.1063, + 0.2072, -0.3476, 0.0034, 1.2476, -0.7781, 0.5480 + ) + + + + + ftShoe_Left_Front + ftShoe_Left_Rear + + + + + + 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 + ) + + + true + + + ( + -0.5352, -1.0615, 0.0758, 3.8782, -0.4849, -0.4849, + 0.7976, -0.4305, -0.0166, 0.2742, -0.7137, -1.0847, + -0.0070, -0.0441, 1.0208, 2.5757, 0.8728, -0.1731, + -0.0135, -0.0246, -0.0624, -0.5595, -0.0137, -0.1040, + 0.0069, 0.0568, -0.2951, -0.0155, -0.3156, 0.2627, + 0.1845, -0.3471, 0.0080, 0.7452, -0.2717, 0.4891 + ) + + ( + 0.4477, 0.8099, 0.0242, -1.0268, -1.3692, -0.8212, + -0.7655, 0.4236, -0.0557, -2.2798, 4.3884, -0.5540, + 0.0026, -0.0302, 0.9999, 0.1001, 0.5442, -0.9613, + -0.0312, 0.0116, 0.3747, 0.3537, 1.3133, -0.3311, + -0.0206, -0.0340, -0.0361, -0.7568, 0.6437, -0.0702, + -0.1970, -0.3028, -0.0119, 0.3850, 0.6892, 1.1337 + ) + + + + + ftShoe_Right_Front + ftShoe_Right_Rear + + + + + + + + 10 + /ft/ftShoe_Left/analog:o + + + ftShoe_l + + + + + + 10 + /ft/ftShoe_Right/analog:o + + + ftShoe_r + + + + + + + + 10 + /ft/ftShoe_Left_Front/analog:o + + + ftShoe_Left_Front + + + + + + 10 + /ft/ftShoe_Left_Rear/analog:o + + + ftShoe_Left_Rear + + + + + + 10 + /ft/ftShoe_Right_Front/analog:o + + + ftShoe_Right_Front + + + + + + 10 + /ft/ftShoe_Right_Rear/analog:o + + + ftShoe_Right_Rear + + + + + + + + FTShoeLeftFTSensors + FTShoeLeft + 6 + 0 + ForceTorque6DSensor + true + + + ftShoe_l + + + + + + FTShoeRightFTSensors + FTShoeRight + 6 + 0 + ForceTorque6DSensor + true + + + ftShoe_r + + + + + + + + 0.01 + /FTShoeLeft/WearableData/data:o + /FTShoeLeft/WearableData/metadataRpc:o + + + FTShoeLeftIAnalogSensorToIWear + + + + + + 0.01 + /FTShoeRight/WearableData/data:o + /FTShoeRight/WearableData/metadataRpc:o + + + FTShoeRightIAnalogSensorToIWear + + + + + + diff --git a/app/xml/SkinInsolesWearableDevice.xml b/app/xml/SkinInsolesWearableDevice.xml new file mode 100644 index 000000000..b4a197879 --- /dev/null +++ b/app/xml/SkinInsolesWearableDevice.xml @@ -0,0 +1,71 @@ + + + + + + + 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/app/xml/XsensSuitWearableDevice.xml b/app/xml/XsensSuitWearableDevice.xml new file mode 100644 index 000000000..034bab705 --- /dev/null +++ b/app/xml/XsensSuitWearableDevice.xml @@ -0,0 +1,73 @@ + + + + + + + + "C:\\Program Files\\Xsens\\MVN SDK 2018.0.3\\SDK Files\\rundeps" + + + + + FullBody + + + singleLevel + + + Npose + + + Poor + + 60 + + + 120 + + false + + true + + + true + true + true + + + + 0.07 + 1.71 + 1.71 + 0.26 + 0.87 + 0.25 + 0.50 + 0.34 + 0.02 + + + + + 0.01 + /XSensSuit/WearableData/data:o + /XSensSuit/WearableData/metadataRpc:o + + + XsensSuitDevice + + + + + + + /XSensSuit/Control/rpc:i + + + XsensSuitDevice + + + + + diff --git a/app/xml/applications/HumanDynamicsEstimation-WearableDumper.xml b/app/xml/applications/HumanDynamicsEstimation-WearableDumper.xml new file mode 100644 index 000000000..ea14a51fc --- /dev/null +++ b/app/xml/applications/HumanDynamicsEstimation-WearableDumper.xml @@ -0,0 +1,46 @@ + + HumanDumper + Dumper for Wearable Data to run Human Dynamics Estimation + + localhost + + + yarpdatadumper + --name /human_dump/wearable/FTshoes/left --type bottle --txTime --rxTime + ${generic_node} + yarpdatadumper + + + + yarpdatadumper + --name /human_dump/wearable/FTshoes/right --type bottle --txTime --rxTime + ${generic_node} + yarpdatadumper + + + + yarpdatadumper + --name /human_dump/wearable/xsens --type bottle --txTime --rxTime + ${generic_node} + yarpdatadumper + + + + /FTShoeLeft/WearableData/data:o + /human_dump/wearable/FTshoes/left + udp + + + + /FTShoeRight/WearableData/data:o + /human_dump/wearable/FTshoes/right + udp + + + + /XSensSuit/WearableData/data:o + /human_dump/wearable/xsens + udp + + + diff --git a/app/xml/applications/WearableDevices-Dumper.xml b/app/xml/applications/WearableDevices-Dumper.xml new file mode 100644 index 000000000..d99ae3424 --- /dev/null +++ b/app/xml/applications/WearableDevices-Dumper.xml @@ -0,0 +1,57 @@ + + WearableDevices-Dumper + An application for dumping wearable data + + + yarpdatadumper + --name /wearableData/FTshoes/left --type bottle --txTime --rxTime + ${generic_node} + yarpdatadumper + + + + yarpdatadumper + --name /wearableData/FTshoes/right --type bottle --txTime --rxTime + ${generic_node} + yarpdatadumper + + + + yarpdatadumper + --name /wearableData/xsens --type bottle --txTime --rxTime + ${generic_node} + yarpdatadumper + + + + yarpdatadumper + --name /wearableData/SkinInsoles --type bottle --txTime --rxTime + ${generic_node} + yarpdatadumper + + + + /FTShoeLeft/WearableData/data:o + /wearableData/FTshoes/left + udp + + + + /FTShoeRight/WearableData/data:o + /wearableData/FTshoes/right + udp + + + + /XSensSuit/WearableData/data:o + /wearableData/xsens + udp + + + + /SkinInsoles/WearableData/data:o + /wearableData/SkinInsoles + udp + + + diff --git a/app/xml/applications/WearableDevices.xml b/app/xml/applications/WearableDevices.xml new file mode 100644 index 000000000..0585e241a --- /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/app/xml/iWearLoggerExampleTemplate.xml b/app/xml/iWearLoggerExampleTemplate.xml new file mode 100644 index 000000000..e040668ce --- /dev/null +++ b/app/xml/iWearLoggerExampleTemplate.xml @@ -0,0 +1,53 @@ + + + + + + + + + + 0.01 + + + + (matlab, yarp) + + false + false + false + false + true + true + true + false + true + false + false + false + false + false + false + false + false + + true + test_iwear_logger + /path/to/some/folder/ + 100000 + true + 120.0 + 300 + true + + + + + ProducerDevice + + + + + + + diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt index acfbcd928..27c3571a7 100644 --- a/bindings/CMakeLists.txt +++ b/bindings/CMakeLists.txt @@ -24,46 +24,10 @@ if(HDE_COMPILE_PYTHON_BINDINGS) set(HDE_PYTHON_INSTALL_DIR ${_PYTHON_INSTDIR_CLEAN}) endif() endif() - set(PYTHON_INSTDIR ${HDE_PYTHON_INSTALL_DIR}/hde) - - # Folder of the Python package within the build tree. - # It is used for the Python tests. - set(HDE_PYTHON_PACKAGE "${CMAKE_BINARY_DIR}/hde") # Add the bindings directory - add_subdirectory(python) - - # Create the __init__.py file - file(GENERATE - OUTPUT "${HDE_PYTHON_PACKAGE}/__init__.py" - CONTENT "from .bindings import *${NEW_LINE}") - - # Install the __init__.py file - install(FILES "${HDE_PYTHON_PACKAGE}/__init__.py" - DESTINATION ${PYTHON_INSTDIR}) - - # Install pip metadata files to ensure that HDE installed via CMake is listed by pip list - # See https://packaging.python.org/specifications/recording-installed-packages/ - # and https://packaging.python.org/en/latest/specifications/core-metadata/#core-metadata - option(HDE_PYTHON_PIP_METADATA_INSTALL "Use CMake to install Python pip metadata. Set to off if some other tool already installs it." ON) - mark_as_advanced(HDE_PYTHON_PIP_METADATA_INSTALL) - set(HDE_PYTHON_PIP_METADATA_INSTALLER "cmake" CACHE STRING "Specify the string to identify the pip Installer. Default: cmake, change this if you are using another tool.") - mark_as_advanced(HDE_PYTHON_PIP_METADATA_INSTALLER) - if(HDE_PYTHON_PIP_METADATA_INSTALL) - get_filename_component(PYTHON_METADATA_PARENT_DIR ${PYTHON_INSTDIR} DIRECTORY) - if(WIN32) - set(NEW_LINE "\n\r") - else() - set(NEW_LINE "\n") - endif() - file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/METADATA "") - file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Metadata-Version: 2.1${NEW_LINE}") - file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Name: hde${NEW_LINE}") - file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Version: ${hde_VERSION}${NEW_LINE}") - file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/INSTALLER "${HDE_PYTHON_PIP_METADATA_INSTALLER}${NEW_LINE}") - install( - FILES "${CMAKE_CURRENT_BINARY_DIR}/METADATA" "${CMAKE_CURRENT_BINARY_DIR}/INSTALLER" - DESTINATION ${PYTHON_METADATA_PARENT_DIR}/hde-${hde_VERSION}.dist-info) - endif() + add_subdirectory(python-wearables) -endif() \ No newline at end of file + # Add the bindings directory + add_subdirectory(python-hde) +endif() diff --git a/bindings/python-hde/CMakeLists.txt b/bindings/python-hde/CMakeLists.txt new file mode 100644 index 000000000..cc3a1ffdf --- /dev/null +++ b/bindings/python-hde/CMakeLists.txt @@ -0,0 +1,67 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +set(PYTHON_INSTDIR ${HDE_PYTHON_INSTALL_DIR}/hde) + +# Folder of the Python package within the build tree. +# It is used for the Python tests. +set(HDE_PYTHON_PACKAGE "${CMAKE_BINARY_DIR}/hde") + +add_subdirectory(msgs) + +get_property(pybind_headers GLOBAL PROPERTY pybind_headers) +get_property(pybind_sources GLOBAL PROPERTY pybind_sources) +get_property(pybind_include_dirs GLOBAL PROPERTY pybind_include_dirs) +get_property(pybind_link_libraries GLOBAL PROPERTY pybind_link_libraries) + +pybind11_add_module(pybind11_hde MODULE + hde.cpp + ${pybind_sources} + ${pybind_headers} + ) + +target_include_directories(pybind11_hde PUBLIC "$") + +target_link_libraries(pybind11_hde PRIVATE + ${pybind_link_libraries}) + +# # The generated Python dynamic module must have the same name as the pybind11 +# # module, i.e. `bindings`. +set_target_properties(pybind11_hde PROPERTIES + LIBRARY_OUTPUT_DIRECTORY "${HDE_PYTHON_PACKAGE}" + OUTPUT_NAME "bindings") + +# Output package is: +# +# hde +# |-- __init__.py (generated from main bindings CMake file) +# `-- bindings. + +install(TARGETS pybind11_hde DESTINATION ${PYTHON_INSTDIR}) + +# Create the __init__.py file +file(GENERATE + OUTPUT "${HDE_PYTHON_PACKAGE}/__init__.py" + CONTENT "from .bindings import *${NEW_LINE}") +# Install the __init__.py file +install(FILES "${HDE_PYTHON_PACKAGE}/__init__.py" + DESTINATION ${PYTHON_INSTDIR}) +# Install pip metadata files to ensure that HDE installed via CMake is listed by pip list +# See https://packaging.python.org/specifications/recording-installed-packages/ +# and https://packaging.python.org/en/latest/specifications/core-metadata/#core-metadata +if(HDE_PYTHON_PIP_METADATA_INSTALL) + get_filename_component(PYTHON_METADATA_PARENT_DIR ${PYTHON_INSTDIR} DIRECTORY) + if(WIN32) + set(NEW_LINE "\n\r") + else() + set(NEW_LINE "\n") + endif() + file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/METADATA "") + file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Metadata-Version: 2.1${NEW_LINE}") + file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Name: hde${NEW_LINE}") + file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Version: ${PROJECT_VERSION}${NEW_LINE}") + file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/INSTALLER "${HDE_PYTHON_PIP_METADATA_INSTALLER}${NEW_LINE}") + install( + FILES "${CMAKE_CURRENT_BINARY_DIR}/METADATA" "${CMAKE_CURRENT_BINARY_DIR}/INSTALLER" + DESTINATION ${PYTHON_METADATA_PARENT_DIR}/hde-${PROJECT_VERSION}.dist-info) +endif() diff --git a/bindings/python/hde.cpp b/bindings/python-hde/hde.cpp similarity index 100% rename from bindings/python/hde.cpp rename to bindings/python-hde/hde.cpp diff --git a/bindings/python/msgs/CMakeLists.txt b/bindings/python-hde/msgs/CMakeLists.txt similarity index 100% rename from bindings/python/msgs/CMakeLists.txt rename to bindings/python-hde/msgs/CMakeLists.txt diff --git a/bindings/python/msgs/include/hde/bindings/msgs/BufferedPort.h b/bindings/python-hde/msgs/include/hde/bindings/msgs/BufferedPort.h similarity index 100% rename from bindings/python/msgs/include/hde/bindings/msgs/BufferedPort.h rename to bindings/python-hde/msgs/include/hde/bindings/msgs/BufferedPort.h diff --git a/bindings/python/msgs/include/hde/bindings/msgs/HumanState.h b/bindings/python-hde/msgs/include/hde/bindings/msgs/HumanState.h similarity index 100% rename from bindings/python/msgs/include/hde/bindings/msgs/HumanState.h rename to bindings/python-hde/msgs/include/hde/bindings/msgs/HumanState.h diff --git a/bindings/python/msgs/include/hde/bindings/msgs/Module.h b/bindings/python-hde/msgs/include/hde/bindings/msgs/Module.h similarity index 100% rename from bindings/python/msgs/include/hde/bindings/msgs/Module.h rename to bindings/python-hde/msgs/include/hde/bindings/msgs/Module.h diff --git a/bindings/python/msgs/src/HumanState.cpp b/bindings/python-hde/msgs/src/HumanState.cpp similarity index 100% rename from bindings/python/msgs/src/HumanState.cpp rename to bindings/python-hde/msgs/src/HumanState.cpp diff --git a/bindings/python/msgs/src/Module.cpp b/bindings/python-hde/msgs/src/Module.cpp similarity index 100% rename from bindings/python/msgs/src/Module.cpp rename to bindings/python-hde/msgs/src/Module.cpp diff --git a/bindings/python-wearables/CMakeLists.txt b/bindings/python-wearables/CMakeLists.txt new file mode 100644 index 000000000..e4ed2941e --- /dev/null +++ b/bindings/python-wearables/CMakeLists.txt @@ -0,0 +1,67 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +set(PYTHON_INSTDIR ${HDE_PYTHON_INSTALL_DIR}/wearables) + +# Folder of the Python package within the build tree. +# It is used for the Python tests. +set(WEARABLES_PYTHON_PACKAGE "${CMAKE_BINARY_DIR}/wearables") + +add_subdirectory(msgs) + +get_property(pybind_headers GLOBAL PROPERTY pybind_headers) +get_property(pybind_sources GLOBAL PROPERTY pybind_sources) +get_property(pybind_include_dirs GLOBAL PROPERTY pybind_include_dirs) +get_property(pybind_link_libraries GLOBAL PROPERTY pybind_link_libraries) + +pybind11_add_module(pybind11_wearables MODULE + wearables.cpp + ${pybind_sources} + ${pybind_headers} + ) + +target_include_directories(pybind11_wearables PUBLIC "$") + +target_link_libraries(pybind11_wearables PRIVATE + ${pybind_link_libraries}) + +# # The generated Python dynamic module must have the same name as the pybind11 +# # module, i.e. `bindings`. +set_target_properties(pybind11_wearables PROPERTIES + LIBRARY_OUTPUT_DIRECTORY "${WEARABLES_PYTHON_PACKAGE}" + OUTPUT_NAME "bindings") + +# Output package is: +# +# bipedal_locomotion +# |-- __init__.py (generated from main bindings CMake file) +# `-- bindings. + +install(TARGETS pybind11_wearables DESTINATION ${PYTHON_INSTDIR}) + +# Install the __init__.py file +install(FILES "${WEARABLES_PYTHON_PACKAGE}/__init__.py" + DESTINATION ${PYTHON_INSTDIR}) +# Install pip metadata files to ensure that WEARABLES installed via CMake is listed by pip list +# See https://packaging.python.org/specifications/recording-installed-packages/ +# and https://packaging.python.org/en/latest/specifications/core-metadata/#core-metadata +option(HDE_PYTHON_PIP_METADATA_INSTALL "Use CMake to install Python pip metadata. Set to off if some other tool already installs it." ON) +mark_as_advanced(HDE_PYTHON_PIP_METADATA_INSTALL) +set(HDE_PYTHON_PIP_METADATA_INSTALLER "cmake" CACHE STRING "Specify the string to identify the pip Installer. Default: cmake, change this if you are using another tool.") +mark_as_advanced(HDE_PYTHON_PIP_METADATA_INSTALLER) +if(HDE_PYTHON_PIP_METADATA_INSTALL) + get_filename_component(PYTHON_METADATA_PARENT_DIR ${PYTHON_INSTDIR} DIRECTORY) + if(WIN32) + set(NEW_LINE "\n\r") + else() + set(NEW_LINE "\n") + endif() + file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/METADATA "") + file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Metadata-Version: 2.1${NEW_LINE}") + file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Name: wearables${NEW_LINE}") + file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/METADATA "Version: ${PROJECT_VERSION}${NEW_LINE}") + file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/INSTALLER "${HDE_PYTHON_PIP_METADATA_INSTALLER}${NEW_LINE}") + install( + FILES "${CMAKE_CURRENT_BINARY_DIR}/METADATA" "${CMAKE_CURRENT_BINARY_DIR}/INSTALLER" + DESTINATION ${PYTHON_METADATA_PARENT_DIR}/wearables-${PROJECT_VERSION}.dist-info) +endif() \ No newline at end of file diff --git a/bindings/python-wearables/msgs/CMakeLists.txt b/bindings/python-wearables/msgs/CMakeLists.txt new file mode 100644 index 000000000..8869caebe --- /dev/null +++ b/bindings/python-wearables/msgs/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +set(H_PREFIX include/Wearable/bindings/msgs) + +add_wearables_python_module( + NAME MsgsBindings + SOURCES src/WearableData.cpp src/Module.cpp + HEADERS ${H_PREFIX}/WearableData.h ${H_PREFIX}/BufferedPort.h ${H_PREFIX}/Module.h + LINK_LIBRARIES Wearable::WearableData YARP::YARP_os) diff --git a/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/BufferedPort.h b/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/BufferedPort.h new file mode 100644 index 000000000..481e6427b --- /dev/null +++ b/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/BufferedPort.h @@ -0,0 +1,44 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLES_BINDINGS_MSGS_BUFFERED_PORT_H +#define WEARABLES_BINDINGS_MSGS_BUFFERED_PORT_H + +#include + +#include +#include + +#include + +namespace wearables { + namespace bindings { + namespace msgs { + + template + void CreateBufferedPort(pybind11::module& module, const std::string& name) + { + namespace py = ::pybind11; + py::class_<::yarp::os::BufferedPort>(module, name.c_str()) + .def(py::init()) + .def("open", + py::overload_cast(&::yarp::os::BufferedPort::open), + py::arg("name")) + .def("close", &::yarp::os::BufferedPort::close) + .def("isClosed", &::yarp::os::BufferedPort::isClosed) + .def("prepare", + &::yarp::os::BufferedPort::prepare, + py::return_value_policy::reference_internal) + .def("write", + &::yarp::os::BufferedPort::write, + py::arg("forceStrict") = false) + .def("read", + &::yarp::os::BufferedPort::read, + py::arg("shouldWait") = true, + py::return_value_policy::reference_internal); + } + } // namespace msgs + } // namespace bindings +} // namespace wearables + +#endif // WEARABLES_BINDINGS_MSGS_BUFFERED_PORT_H diff --git a/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/Module.h b/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/Module.h new file mode 100644 index 000000000..173b48de2 --- /dev/null +++ b/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/Module.h @@ -0,0 +1,19 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLES_BINDINGS_MSGS_MODULE_H +#define WEARABLES_BINDINGS_MSGS_MODULE_H + +#include + +namespace wearables { + namespace bindings { + namespace msgs { + + void CreateModule(pybind11::module& module); + + } // namespace msgs + } // namespace bindings +} // namespace wearables + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_YARP_UTILITES_MODULE_H diff --git a/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/WearableData.h b/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/WearableData.h new file mode 100644 index 000000000..f764f1716 --- /dev/null +++ b/bindings/python-wearables/msgs/include/Wearable/bindings/msgs/WearableData.h @@ -0,0 +1,19 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLES_BINDINGS_MSGS_WEARABLE_DATA_H +#define WEARABLES_BINDINGS_MSGS_WEARABLE_DATA_H + +#include + +namespace wearables { + namespace bindings { + namespace msgs { + + void CreateWearableData(pybind11::module& module); + + } // namespace msgs + } // namespace bindings +} // namespace wearables + +#endif // WEARABLES_BINDINGS_MSGS_WEARABLE_DATA_H diff --git a/bindings/python-wearables/msgs/src/Module.cpp b/bindings/python-wearables/msgs/src/Module.cpp new file mode 100644 index 000000000..8034f8087 --- /dev/null +++ b/bindings/python-wearables/msgs/src/Module.cpp @@ -0,0 +1,20 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include + +#include + +namespace wearables { + namespace bindings { + namespace msgs { + + void CreateModule(pybind11::module& module) + { + module.doc() = "YarpUtilities module."; + + CreateWearableData(module); + } + } // namespace msgs + } // namespace bindings +} // namespace wearables diff --git a/bindings/python-wearables/msgs/src/WearableData.cpp b/bindings/python-wearables/msgs/src/WearableData.cpp new file mode 100644 index 000000000..51d0fa3f6 --- /dev/null +++ b/bindings/python-wearables/msgs/src/WearableData.cpp @@ -0,0 +1,202 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#include "thrift/Accelerometer.h" +#include "thrift/EmgData.h" +#include "thrift/QuaternionWXYZ.h" +#include "thrift/SensorInfo.h" +#include "thrift/VectorXYZ.h" +#include +#include + +#include +#include + +#include +#include + +namespace wearables { + namespace bindings { + namespace msgs { + + void CreateSensorStatus(pybind11::module& module) + { + namespace py = ::pybind11; + + py::enum_<::wearable::msg::SensorStatus>(module, "SensorStatus") + .value("ERROR", ::wearable::msg::ERROR) + .value("OK", ::wearable::msg::OK) + .value("CALIBRATING", ::wearable::msg::CALIBRATING) + .value("DATA_OVERFLOW", ::wearable::msg::DATA_OVERFLOW) + .value("TIMEOUT", ::wearable::msg::TIMEOUT) + .value("UNKNOWN", ::wearable::msg::UNKNOWN) + .value("WAITING_FOR_FIRST_READ", ::wearable::msg::WAITING_FOR_FIRST_READ) + .export_values(); + } + + void CreateSensorInfo(pybind11::module& module) + { + namespace py = ::pybind11; + + py::class_<::wearable::msg::SensorInfo>(module, "SensorInfo") + .def(py::init()) + .def_readwrite("name", &::wearable::msg::SensorInfo::name) + .def_readwrite("status", &::wearable::msg::SensorInfo::status); + } + + void CreateVectors(pybind11::module& module) + { + namespace py = ::pybind11; + + py::class_<::wearable::msg::VectorXYZ>(module, "VectorXYZ") + .def(py::init()) + .def_readwrite("x", &::wearable::msg::VectorXYZ::x) + .def_readwrite("y", &::wearable::msg::VectorXYZ::y) + .def_readwrite("z", &::wearable::msg::VectorXYZ::z); + + py::class_<::wearable::msg::VectorRPY>(module, "VectorRPY") + .def(py::init()) + .def_readwrite("r", &::wearable::msg::VectorRPY::r) + .def_readwrite("p", &::wearable::msg::VectorRPY::p) + .def_readwrite("y", &::wearable::msg::VectorRPY::y); + + py::class_<::wearable::msg::QuaternionWXYZ>(module, "QuaternonWXYZ") + .def(py::init()) + .def_readwrite("w", &::wearable::msg::QuaternionWXYZ::w) + .def_readwrite("x", &::wearable::msg::QuaternionWXYZ::x) + .def_readwrite("y", &::wearable::msg::QuaternionWXYZ::y) + .def_readwrite("z", &::wearable::msg::QuaternionWXYZ::z); + } + + void CreateSensorData(pybind11::module& module) + { + namespace py = ::pybind11; + + py::class_<::wearable::msg::ForceTorque6DSensorData>(module, + "ForceTorque6DSensorData") + .def(py::init()) + .def_readwrite("force", &::wearable::msg::ForceTorque6DSensorData::force) + .def_readwrite("torque", &::wearable::msg::ForceTorque6DSensorData::torque); + + py::class_<::wearable::msg::PoseSensorData>(module, "PoseSensorData") + .def(py::init()) + .def_readwrite("orientation", &::wearable::msg::PoseSensorData::orientation) + .def_readwrite("position", &::wearable::msg::PoseSensorData::position); + + py::class_<::wearable::msg::VirtualLinkKinSensorData>(module, + "VirtualLinkKinSensorData") + .def(py::init()) + .def_readwrite("orientation", + &::wearable::msg::VirtualLinkKinSensorData::orientation) + .def_readwrite("position", &::wearable::msg::VirtualLinkKinSensorData::position) + .def_readwrite("linearVelocity", + &::wearable::msg::VirtualLinkKinSensorData::linearVelocity) + .def_readwrite("angularVelocity", + &::wearable::msg::VirtualLinkKinSensorData::angularVelocity) + .def_readwrite("linearAcceleration", + &::wearable::msg::VirtualLinkKinSensorData::linearAcceleration) + .def_readwrite("angularAcceleration", + &::wearable::msg::VirtualLinkKinSensorData::angularAcceleration); + + py::class_<::wearable::msg::VirtualJointKinSensorData>(module, + "VirtualJointKinSensorData") + .def(py::init()) + .def_readwrite("acceleration", + &::wearable::msg::VirtualJointKinSensorData::acceleration) + .def_readwrite("velocity", + &::wearable::msg::VirtualJointKinSensorData::velocity) + .def_readwrite("position", + &::wearable::msg::VirtualJointKinSensorData::position); + + py::class_<::wearable::msg::VirtualSphericalJointKinSensorData>( + module, "VirtualSphericalJointKinSensorData") + .def(py::init()) + .def_readwrite( + "acceleration", + &::wearable::msg::VirtualSphericalJointKinSensorData::acceleration) + .def_readwrite("velocity", + &::wearable::msg::VirtualSphericalJointKinSensorData::velocity) + .def_readwrite("angle", + &::wearable::msg::VirtualSphericalJointKinSensorData::angle); + + py::class_<::wearable::msg::EmgData>(module, "EmgData") + .def(py::init()) + .def_readwrite("value", &::wearable::msg::EmgData::value) + .def_readwrite("normalization", &::wearable::msg::EmgData::normalization); + } + + template + void CreateSensorStructure(pybind11::module& module, const std::string& name) + { + namespace py = ::pybind11; + + py::class_(module, name.c_str()) + .def(py::init()) + .def_readwrite("info", &Sensor::info) + .def_readwrite("data", &Sensor::data) + .def("__str__", &Sensor::toString) + .def("toString", &Sensor::toString); + } + + void CreateSensorsStructure(pybind11::module& module) + { + using namespace ::wearable::msg; + CreateSensorStructure(module, "Accelerometer"); + CreateSensorStructure(module, "EmgSensor"); + CreateSensorStructure(module, + "FreeBodyAccelerationSensor"); + CreateSensorStructure(module, "Force3DSensor"); + CreateSensorStructure(module, "ForceTorque6DSensor"); + CreateSensorStructure(module, "Gyroscope"); + CreateSensorStructure(module, "Magnetometer"); + CreateSensorStructure(module, "OrientationSensor"); + CreateSensorStructure(module, "PoseSensor"); + CreateSensorStructure(module, "PositionSensor"); + CreateSensorStructure(module, "SkinSensor"); + CreateSensorStructure(module, "TemperatureSensor"); + CreateSensorStructure(module, "Torque3DSensor"); + CreateSensorStructure(module, "VirtualLinkKinSensor"); + CreateSensorStructure(module, "VirtualJointKinSensor"); + CreateSensorStructure( + module, "VirtualSphericalJointKinSensor"); + } + + void CreateWearableData(pybind11::module& module) + { + namespace py = ::pybind11; + using namespace ::wearable::msg; + + CreateSensorStatus(module); + CreateSensorInfo(module); + CreateVectors(module); + CreateSensorData(module); + CreateSensorsStructure(module); + + py::class_(module, "WearableData") + .def(py::init()) + .def_readwrite("producerName", &WearableData::producerName) + .def_readwrite("accelerometers", &WearableData::accelerometers) + .def_readwrite("emgSensors", &WearableData::emgSensors) + .def_readwrite("force3DSensors", &WearableData::force3DSensors) + .def_readwrite("forceTorque6DSensors", &WearableData::forceTorque6DSensors) + .def_readwrite("freeBodyAccelerationSensors", &WearableData::freeBodyAccelerationSensors) + .def_readwrite("gyroscopes", &WearableData::gyroscopes) + .def_readwrite("magnetometers", &WearableData::magnetometers) + .def_readwrite("orientationSensors", &WearableData::orientationSensors) + .def_readwrite("poseSensors", &WearableData::poseSensors) + .def_readwrite("positionSensors", &WearableData::positionSensors) + .def_readwrite("skinSensors", &WearableData::skinSensors) + .def_readwrite("temperatureSensors", &WearableData::temperatureSensors) + .def_readwrite("torque3DSensors", &WearableData::torque3DSensors) + .def_readwrite("virtualLinkKinSensors", &WearableData::virtualLinkKinSensors) + .def_readwrite("virtualJointKinSensors", &WearableData::virtualJointKinSensors) + .def_readwrite("virtualSphericalJointKinSensors", &WearableData::virtualSphericalJointKinSensors) + .def("__str__", &WearableData::toString) + .def("toString", &WearableData::toString); + + CreateBufferedPort(module, "BufferedPortWearableData"); + } + } // namespace msgs + } // namespace bindings +} // namespace wearables diff --git a/bindings/python-wearables/wearables.cpp b/bindings/python-wearables/wearables.cpp new file mode 100644 index 000000000..1c6890c58 --- /dev/null +++ b/bindings/python-wearables/wearables.cpp @@ -0,0 +1,18 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#include + +#include + +// Create the Python module +PYBIND11_MODULE(bindings, m) +{ + namespace py = ::pybind11; + + m.doc() = "wearables bindings"; + + py::module msgModule = m.def_submodule("msg"); + wearables::bindings::msgs::CreateModule(msgModule); +} diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt deleted file mode 100644 index bc92d2421..000000000 --- a/bindings/python/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) -# SPDX-License-Identifier: BSD-3-Clause - -add_subdirectory(msgs) - -get_property(pybind_headers GLOBAL PROPERTY pybind_headers) -get_property(pybind_sources GLOBAL PROPERTY pybind_sources) -get_property(pybind_include_dirs GLOBAL PROPERTY pybind_include_dirs) -get_property(pybind_link_libraries GLOBAL PROPERTY pybind_link_libraries) - -pybind11_add_module(pybind11_hde MODULE - hde.cpp - ${pybind_sources} - ${pybind_headers} - ) - -target_include_directories(pybind11_hde PUBLIC "$") - -target_link_libraries(pybind11_hde PRIVATE - ${pybind_link_libraries}) - -# # The generated Python dynamic module must have the same name as the pybind11 -# # module, i.e. `bindings`. -set_target_properties(pybind11_hde PROPERTIES - LIBRARY_OUTPUT_DIRECTORY "${HDE_PYTHON_PACKAGE}" - OUTPUT_NAME "bindings") - -# Output package is: -# -# hde -# |-- __init__.py (generated from main bindings CMake file) -# `-- bindings. - -install(TARGETS pybind11_hde DESTINATION ${PYTHON_INSTDIR}) \ No newline at end of file diff --git a/ci_env.yml b/ci_env.yml index 2f78df877..b11438a4d 100644 --- a/ci_env.yml +++ b/ci_env.yml @@ -3,15 +3,15 @@ channels: - conda-forge - robotology dependencies: - - cmake + - cmake - compilers - - make + - make - ninja - - pkg-config + - pkg-config + - pybind11 - yarp - - icub-main - - wearables - - idyntree - - libmatio-cpp - - robometry + - icub-main + - idyntree + - libmatio-cpp + - robometry - osqp-eigen diff --git a/cmake/AddInstallRPATHSupport.cmake b/cmake/AddInstallRPATHSupport.cmake new file mode 100644 index 000000000..6665921e2 --- /dev/null +++ b/cmake/AddInstallRPATHSupport.cmake @@ -0,0 +1,237 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +#[=======================================================================[.rst: +AddInstallRPATHSupport +---------------------- + +Add support to RPATH during installation to the project and the targets + +.. command:: add_install_rpath_support + + Add support to RPATH during installation to the project:: + + .. code-block:: cmake + + add_install_rpath_support([BIN_DIRS dir [dir]] + [LIB_DIRS dir [dir]] + [INSTALL_NAME_DIR [dir]] + [DEPENDS condition [condition]] + [USE_LINK_PATH]) + + Normally (depending on the platform) when you install a shared + library you can either specify its absolute path as the install name, + or leave just the library name itself. In the former case the library + will be correctly linked during run time by all executables and other + shared libraries, but it must not change its install location. This + is often the case for libraries installed in the system default + library directory (e.g. ``/usr/lib``). + In the latter case, instead, the library can be moved anywhere in the + file system but at run time the dynamic linker must be able to find + it. This is often accomplished by setting environmental variables + (i.e. ``LD_LIBRARY_PATH`` on Linux). + This procedure is usually not desirable for two main reasons: + + - by setting the variable you are changing the default behaviour + of the dynamic linker thus potentially breaking executables (not as + destructive as ``LD_PRELOAD``) + - the variable will be used only by applications spawned by the shell + and not by other processes. + + RPATH aims in solving the issues introduced by the second + installation method. Using run-path dependent libraries you can + create a directory structure containing executables and dependent + libraries that users can relocate without breaking it. + A run-path dependent library is a dependent library whose complete + install name is not known when the library is created. + Instead, the library specifies that the dynamic loader must resolve + the library’s install name when it loads the executable that depends + on the library. The executable or the other shared library will + hardcode in the binary itself the additional search directories + to be passed to the dynamic linker. This works great in conjunction + with relative paths. + This command will enable support to RPATH to your project. + It will enable the following things: + + - If the project builds shared libraries it will generate a run-path + enabled shared library, i.e. its install name will be resolved + only at run time. + - In all cases (building executables and/or shared libraries) + dependent shared libraries with RPATH support will have their name + resolved only at run time, by embedding the search path directly + into the built binary. + + The command has the following parameters: + + Options: + - ``USE_LINK_PATH``: if passed the command will automatically adds to + the RPATH the path to all the dependent libraries. + + Arguments: + - ``BIN_DIRS`` list of directories when the targets (executable and + plugins) will be installed. + - ``LIB_DIRS`` list of directories to be added to the RPATH. These + directories will be added "relative" w.r.t. the ``BIN_DIRS`` and + ``LIB_DIRS``. + - ``INSTALL_NAME_DIR`` directory where the libraries will be installed. + This variable will be used only if ``CMAKE_SKIP_RPATH`` or + ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the + ``INSTALL_NAME_DIR`` on all targets + - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable + RPATH, for example ``FOO; NOT BAR``. + + Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further + details. + +.. command:: target_append_install_rpath + + Add extra paths to RPATH for a specific target:: + + .. code-block:: cmake + + target_append_install_rpath( + + [LIB_DIRS dir [dir]] + [DEPENDS condition [condition]]) + + Arguments: + - ``INSTALL_DESTINATION`` path where the target will be installed. + - ``LIB_DIRS`` list of directories to be added to the RPATH. These + directories will be added "relative" w.r.t. the ``INSTALL_DESTINATION``. + - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable + RPATH, for example ``FOO; NOT BAR``. + +#]=======================================================================] + +include(CMakeParseArguments) + + +macro(__AddInstallRPATHSupport_GET_SYSTEM_LIB_DIRS _var) + # Find system implicit lib directories + set(${_var} ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES}) + if(EXISTS "/etc/debian_version") # is this a debian system ? + if(CMAKE_LIBRARY_ARCHITECTURE) + list(APPEND ${_var} "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" + "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") + endif() + endif() +endmacro() + + +macro(__AddInstallRPATHSupport_APPEND_RELATIVE_RPATH _var _bin_dir _lib_dir) + file(RELATIVE_PATH _rel_path ${_bin_dir} ${_lib_dir}) + if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + list(APPEND ${_var} "@loader_path/${_rel_path}") + else() + list(APPEND ${_var} "\$ORIGIN/${_rel_path}") + endif() +endmacro() + + + +function(ADD_INSTALL_RPATH_SUPPORT) + + set(_options USE_LINK_PATH) + set(_oneValueArgs INSTALL_NAME_DIR) + set(_multiValueArgs BIN_DIRS + LIB_DIRS + DEPENDS) + + cmake_parse_arguments(_ARS "${_options}" + "${_oneValueArgs}" + "${_multiValueArgs}" + "${ARGN}") + + # if either RPATH or INSTALL_RPATH is disabled + # and the INSTALL_NAME_DIR variable is set, then hardcode the install name + if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH) + if(DEFINED _ARS_INSTALL_NAME_DIR) + set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE) + endif() + endif() + + if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) + return() + endif() + + + set(_rpath_available 1) + if(DEFINED _ARS_DEPENDS) + foreach(_dep ${_ARS_DEPENDS}) + string(REGEX REPLACE " +" ";" _dep "${_dep}") + if(NOT (${_dep})) + set(_rpath_available 0) + endif() + endforeach() + endif() + + if(_rpath_available) + + # Enable RPATH on OSX. + set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE) + + __AddInstallRPATHSupport_get_system_lib_dirs(_system_lib_dirs) + + # This is relative RPATH for libraries built in the same project + foreach(lib_dir ${_ARS_LIB_DIRS}) + list(FIND _system_lib_dirs "${lib_dir}" isSystemDir) + if("${isSystemDir}" STREQUAL "-1") + foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS}) + __AddInstallRPATHSupport_append_relative_rpath(CMAKE_INSTALL_RPATH ${bin_dir} ${lib_dir}) + endforeach() + endif() + endforeach() + if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "") + list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH) + endif() + set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE) + + # add the automatically determined parts of the RPATH + # which point to directories outside the build tree to the install RPATH + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE) + + endif() + +endfunction() + + +function(TARGET_APPEND_INSTALL_RPATH _target) + set(_options ) + set(_oneValueArgs INSTALL_DESTINATION) + set(_multiValueArgs LIB_DIRS + DEPENDS) + + if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) + return() + endif() + + cmake_parse_arguments(_ARS "${_options}" + "${_oneValueArgs}" + "${_multiValueArgs}" + "${ARGN}") + + set(_rpath_available 1) + if(DEFINED _ARS_DEPENDS) + foreach(_dep ${_ARS_DEPENDS}) + string(REGEX REPLACE " +" ";" _dep "${_dep}") + if(NOT (${_dep})) + set(_rpath_available 0) + endif() + endforeach() + endif() + + if(_rpath_available) + + __AddInstallRPATHSupport_get_system_lib_dirs(_system_lib_dirs) + + get_target_property(_current_rpath ${_target} INSTALL_RPATH) + foreach(lib_dir ${_ARS_LIB_DIRS}) + list(FIND _system_lib_dirs "${lib_dir}" isSystemDir) + if("${isSystemDir}" STREQUAL "-1") + __AddInstallRPATHSupport_append_relative_rpath(_current_rpath ${_ARS_INSTALL_DESTINATION} ${lib_dir}) + endif() + endforeach() + set_target_properties(${_target} PROPERTIES INSTALL_RPATH "${_current_rpath}") + endif() + +endfunction() diff --git a/cmake/AddWearablesPythonModule.cmake b/cmake/AddWearablesPythonModule.cmake new file mode 100644 index 000000000..8f564e870 --- /dev/null +++ b/cmake/AddWearablesPythonModule.cmake @@ -0,0 +1,46 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +function(add_wearables_python_module) + + set(options ) + set(oneValueArgs NAME) + set(multiValueArgs + SOURCES + HEADERS + LINK_LIBRARIES + TESTS + TESTS_RUNTIME_CONDITIONS) + + set(prefix "wearables") + + cmake_parse_arguments(${prefix} + "${options}" + "${oneValueArgs}" + "${multiValueArgs}" + ${ARGN}) + + set(name ${${prefix}_NAME}) + set(is_interface ${${prefix}_IS_INTERFACE}) + set(sources ${${prefix}_SOURCES}) + set(headers ${${prefix}_HEADERS}) + set(link_libraries ${${prefix}_LINK_LIBRARIES}) + set(subdirectories ${${prefix}_SUBDIRECTORIES}) + set(tests ${${prefix}_TESTS}) + set(tests_runtime_conditions ${${prefix}_TESTS_RUNTIME_CONDITIONS}) + + foreach(file ${headers}) + set_property(GLOBAL APPEND PROPERTY pybind_headers ${CMAKE_CURRENT_SOURCE_DIR}/${file}) + endforeach() + + foreach(file ${sources}) + set_property(GLOBAL APPEND PROPERTY pybind_sources ${CMAKE_CURRENT_SOURCE_DIR}/${file}) + endforeach() + + set_property(GLOBAL APPEND PROPERTY pybind_include_dirs ${CMAKE_CURRENT_SOURCE_DIR}/include) + + set_property(GLOBAL APPEND PROPERTY pybind_link_libraries ${link_libraries}) + + message(STATUS "Added files for bindings named ${name} in ${PROJECT_NAME}.") + +endfunction() diff --git a/cmake/FindSenseGlove.cmake b/cmake/FindSenseGlove.cmake new file mode 100644 index 000000000..1818bb8f8 --- /dev/null +++ b/cmake/FindSenseGlove.cmake @@ -0,0 +1,46 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +# Finds the Sense Glove SDK +# +# This will define the following variables:: +# +# SenseGlove_FOUND - True if the system has the Sense Glove SDK +# SenseGlove - The name of the libraries to link against. +############################### + +# Check Directory of the SenseGlove_DIR +if(NOT DEFINED ENV{SenseGlove_DIR}) + message( FATAL_ERROR "Environment variable {SenseGlove_DIR} is not defined." ) +else() + message(STATUS "Environment variable {SenseGlove_DIR}: $ENV{SenseGlove_DIR}" ) +endif() + +if (CMAKE_BUILD_TYPE MATCHES "Debug") + set(BUILD_TYPE "Debug") +else() + set(BUILD_TYPE "Release") +endif() +message(STATUS "SenseGlove is linking to BUILD_TYPE: ${BUILD_TYPE}") + + +if(WIN32) + file(GLOB SenseGlove_LIB $ENV{SenseGlove_DIR}/Core/SGCoreCpp/lib/win/${BUILD_TYPE}/SGCoreCpp.lib ) + set(LIB_TYPE "STATIC") +else() + file(GLOB SenseGlove_LIB $ENV{SenseGlove_DIR}/Core/SGCoreCpp/lib/linux/${BUILD_TYPE}/libSGCoreCpp.so ) + set(LIB_TYPE "SHARED") +endif() + +set(SenseGlove_INCLUDE_DIRS $ENV{SenseGlove_DIR}/Core/SGCoreCpp/incl ) + +message(STATUS "Variable {SenseGlove_LIB}: ${SenseGlove_LIB}" ) +message(STATUS "variable {SenseGlove_INCLUDE_DIRS}: ${SenseGlove_INCLUDE_DIRS}" ) + +##### Find SenseGlove ##### + +add_library(SenseGlove ${LIB_TYPE} IMPORTED GLOBAL ${SenseGlove_LIB}) +set_target_properties(SenseGlove PROPERTIES IMPORTED_LOCATION ${SenseGlove_LIB}) +target_include_directories(SenseGlove INTERFACE ${SenseGlove_INCLUDE_DIRS}) + +set(SenseGlove_FOUND TRUE) diff --git a/cmake/FindXsensXME.cmake b/cmake/FindXsensXME.cmake new file mode 100644 index 000000000..c70af7468 --- /dev/null +++ b/cmake/FindXsensXME.cmake @@ -0,0 +1,92 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +#.rst: +# FindXsensXME +# ----------- +# +# Find the Xsens MVN Engine (XME) library API. +# +# IMPORTED Targets +# ^^^^^^^^^^^^^^^^ +# +# This module defines the following :prop_tgt:`IMPORTED` targets if +# XME drivers has been found:: +# +# Xsens::XME +# +# Result Variables +# ^^^^^^^^^^^^^^^^ +# +# This module defines the following variables:: +# +# XsensXME_FOUND - System has XME +# XsensXME_INCLUDE_DIRS - Include directories for XME +# XsensXME_LIBRARIES - libraries to link against XME +# +# Readed enviromental variables +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +# +# This module reads hints about search locations from variables:: +# +# XsensXME_ROOT - Directory containing the SDK files. + +if(WIN32) + + if(NOT CMAKE_SIZEOF_VOID_P STREQUAL "8") + #32bit + set(XME_ARCH "32") + set(XME_DIR_PATH "Win32") + else() + #64bit + set(XME_ARCH "64") + set(XME_DIR_PATH "x64") + endif() + + + find_path(Xsens_INCLUDE_DIR xme.h + HINTS "$ENV{XsensXME_ROOT}/${XME_DIR_PATH}/include") + + find_library(Xsens_xme_LIBRARY "xme${XME_ARCH}" + HINTS "$ENV{XsensXME_ROOT}/${XME_DIR_PATH}/lib") + + find_library(Xsens_xstypes_LIBRARY "xstypes${XME_ARCH}" + HINTS "$ENV{XsensXME_ROOT}/${XME_DIR_PATH}/lib") + + + mark_as_advanced(Xsens_INCLUDE_DIR + Xsens_xme_LIBRARY + Xsens_xstypes_LIBRARY) + + if (Xsens_xme_LIBRARY + AND Xsens_xstypes_LIBRARY + AND Xsens_INCLUDE_DIR + AND NOT TARGET Xsens::XME) + + add_library(Xsens::XME UNKNOWN IMPORTED) + + set_target_properties(Xsens::XME PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${Xsens_INCLUDE_DIR}" + IMPORTED_LINK_INTERFACE_LANGUAGES "C" + IMPORTED_LOCATION "${Xsens_xme_LIBRARY}" + INTERFACE_LINK_LIBRARIES "${Xsens_xstypes_LIBRARY}") + + set(XsensXME_LIBRARIES Xsens::XME) + set(XsensXME_INCLUDE_DIRS "${Xsens_INCLUDE_DIR}") + endif() +else() + set(XsensXME_FOUND FALSE) +endif() + + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(XsensXME + FOUND_VAR XsensXME_FOUND + REQUIRED_VARS XsensXME_LIBRARIES XsensXME_INCLUDE_DIRS) + +# Set package properties if FeatureSummary was included +if(COMMAND set_package_properties) + set_package_properties(XsensXME PROPERTIES DESCRIPTION "API for Xsens MVN suit" + URL "https://www.xsens.com/products/xsens-mvn/") +endif() diff --git a/cmake/InstallBasicPackageFiles.cmake b/cmake/InstallBasicPackageFiles.cmake deleted file mode 100644 index c7a65a689..000000000 --- a/cmake/InstallBasicPackageFiles.cmake +++ /dev/null @@ -1,722 +0,0 @@ -# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) -# SPDX-License-Identifier: BSD-3-Clause -# -#.rst: -# InstallBasicPackageFiles -# ------------------------ -# -# A helper module to make your package easier to be found by other -# projects. -# -# -# .. command:: install_basic_package_files -# -# Create and install a basic version of cmake config files for your -# project:: -# -# install_basic_package_files( -# VERSION -# COMPATIBILITY -# [ARCH_INDEPENDENT] -# [EXPORT ] # (default = "") -# [NO_SET_AND_CHECK_MACRO] -# [NO_CHECK_REQUIRED_COMPONENTS_MACRO] -# [VARS_PREFIX ] # (default = "") -# [EXPORT_DESTINATION ] -# [INSTALL_DESTINATION ] -# [NAMESPACE ] # (default = "::") -# [EXTRA_PATH_VARS_SUFFIX path1 [path2 ...]] -# [CONFIG_TEMPLATE ] -# [UPPERCASE_FILENAMES | LOWERCASE_FILENAMES] -# [DEPENDENCIES " [...]" ...] -# [PRIVATE_DEPENDENCIES " [...]" ...] -# [INCLUDE_FILE | INCLUDE_CONTENT ] -# [COMPONENT ] # (default = "") -# ) -# -# Depending on ``UPPERCASE_FILENAMES`` and ``LOWERCASE_FILENAMES``, this -# function generates 3 files: -# -# - ``ConfigVersion.cmake`` or ``-config-version.cmake`` -# - ``Config.cmake`` or ``-config.cmake`` -# - ``Targets.cmake`` or ``-targets.cmake`` -# -# If neither ``UPPERCASE_FILENAMES`` nor ``LOWERCASE_FILENAMES`` is -# set, a file ``ConfigVersion.cmake.in`` or -# ``-config-version.cmake.in`` is searched, and the convention -# is chosed according to the file found. If no file was found, the -# uppercase convention is used. -# -# The ``DEPENDENCIES`` argument can be used to set a list of dependencies -# that will be searched using the :command:`find_dependency` command -# from the :module:`CMakeFindDependencyMacro` module. -# Dependencies can be followed by any of the possible :command:`find_dependency` -# argument. -# In this case, all the arguments must be specified within double quotes (e.g. -# ``" 1.0.0 EXACT"``, or ``" CONFIG"``). -# The ``PRIVATE_DEPENDENCIES`` argument is similar to ``DEPENDENCIES``, but -# these dependencies are included only when :variable:`BUILD_SHARED_LIBS` is -# ``OFF``. -# If a libraries is declared ``STATIC``, ``OBJECT`` or ``INTERFACE``, and they -# link to some dependency, these should be added using the ``DEPENDENCIES`` -# argument, since the ``PRIVATE_DEPENDENCIES`` argument would work only when -# :variable:`BUILD_SHARED_LIBS` is disabled. -# -# When using a custom template file, the ``@PACKAGE_DEPENDENCIES@`` -# string is replaced with the code checking for the dependencies -# specified by these two argument. -# -# Each file is generated twice, one for the build directory and one for -# the installation directory. The ``INSTALL_DESTINATION`` argument can be -# passed to install the files in a location different from the default -# one (``CMake`` on Windows, ``${CMAKE_INSTALL_LIBDIR}/cmake/${Name}`` -# on other platforms. The ``EXPORT_DESTINATION`` argument can be passed to -# generate the files in the build tree in a location different from the default -# one (``CMAKE_BINARY_DIR``). If this is a relative path, it is -# considered relative to the ``CMAKE_CURRENT_BINARY_DIR`` directory. -# -# The ``ConfigVersion.cmake`` file is generated using -# ``write_basic_package_version_file``. The ``VERSION``, ``COMPATIBILITY``, and -# ``ARCH_INDEPENDENT``arguments are passed to this function. -# -# ``VERSION`` shall be in the form ``[.[.[.]]]]``. -# If no ``VERSION`` is given, the ``PROJECT_VERSION`` variable is used. -# If this hasn’t been set, it errors out. The ``VERSION`` argument is also used -# to replace the ``@PACKAGE_VERSION@`` string in the configuration file. -# -# ``COMPATIBILITY`` shall be any of the options accepted by the -# :command:`write_basic_package_version_file` command -# (``AnyNewerVersion``, ``SameMajorVersion``, ``SameMinorVersion`` [CMake 3.11], -# or ``ExactVersion``). -# These options are explained in :command:`write_basic_package_version_file` -# command documentation. -# If your project has more elaborated version matching rules, you will need to -# write your own custom ConfigVersion.cmake file instead of using this macro. -# -# If the ``ARCH_INDEPENDENT`` option is enabled, the installed package version -# will be considered compatible even if it was built for a different -# architecture than the requested architecture. -# -# The ``Config.cmake`` file is generated using -# ``configure_package_config_file``. The ``NO_SET_AND_CHECK_MACRO``, and -# ``NO_CHECK_REQUIRED_COMPONENTS_MACRO``, and arguments are passed to this -# function. -# -# By default ``install_basic_package_files`` also generates the two helper -# macros ``set_and_check()`` and ``check_required_components()`` into the -# ``Config.cmake`` file. ``set_and_check()`` should be used instead of the -# normal :command:`set()` command for setting directories and file locations. -# Additionally to setting the variable it also checks that the referenced file -# or directory actually exists and fails with a ``FATAL_ERROR`` otherwise. -# This makes sure that the created ``Config.cmake`` file does not contain -# wrong references. -# When using the ``NO_SET_AND_CHECK_MACRO, this macro is not generated into the -# ``Config.cmake`` file. -# -# By default, ``install_basic_package_files`` append a call to -# ``check_required_components()`` in ``Config.cmake`` file if the -# package supports components. This macro checks whether all requested, -# non-optional components have been found, and if this is not the case, sets the -# ``_FOUND`` variable to ``FALSE``, so that the package is considered to -# be not found. It does that by testing the ``__FOUND`` -# variables for all requested required components. When using the -# ``NO_CHECK_REQUIRED_COMPONENTS_MACRO`` option, this macro is not generated -# into the ``Config.cmake`` file. -# -# Finally, the files in the build and install directory are exactly the same. -# -# See the documentation of :module:`CMakePackageConfigHelpers` module for -# further information and references therein. -# -# If the ``CONFIG_TEMPLATE`` argument is passed, the specified file -# is used as template for generating the configuration file, otherwise -# this module expects to find a ``Config.cmake.in`` or -# ``-config.cmake.in`` file either in current source directory. -# If the file does not exist, a very basic file is created. -# -# A set of variables are checked and passed to -# ``configure_package_config_file`` as ``PATH_VARS``. For each of the -# ``SUFFIX`` considered, if one of the variables:: -# -# _(BUILD|INSTALL)_ -# (BUILD|INSTALL)__ -# -# is defined, the ``_`` variable will be defined -# before configuring the package. In order to use that variable in the -# config file, you have to add a line:: -# -# set_and_check(_ \"@PACKAGE__@\") -# -# if the path must exist or just:: -# -# set(_ \"@PACKAGE__@\") -# -# if the path could be missing. -# -# These variable will have different values whether you are using the -# package from the build tree or from the install directory. Also these -# files will contain only relative paths, meaning that you can move the -# whole installation and the CMake files will still work. -# -# Default ``PATH_VARS`` suffixes are:: -# -# BINDIR BIN_DIR -# SBINDIR SBIN_DIR -# LIBEXECDIR LIBEXEC_DIR -# SYSCONFDIR SYSCONF_DIR -# SHAREDSTATEDIR SHAREDSTATE_DIR -# LOCALSTATEDIR LOCALSTATE_DIR -# LIBDIR LIB_DIR -# INCLUDEDIR INCLUDE_DIR -# OLDINCLUDEDIR OLDINCLUDE_DIR -# DATAROOTDIR DATAROOT_DIR -# DATADIR DATA_DIR -# INFODIR INFO_DIR -# LOCALEDIR LOCALE_DIR -# MANDIR MAN_DIR -# DOCDIR DOC_DIR -# -# more suffixes can be added using the ``EXTRA_PATH_VARS_SUFFIX`` -# argument. -# -# -# The ``Targets.cmake`` is generated using :command:`export(EXPORT)` in -# the build tree and :command:`install(EXPORT)` in the installation directory. -# The targets are exported using the value for the ``NAMESPACE`` argument as -# namespace. -# The export can be passed using the ``EXPORT`` argument. -# -# If the ``INCLUDE_FILE`` argument is passed, the content of the specified file -# (which might contain ``@variables@``) is appended to the generated -# ``Config.cmake`` file. -# If the ``INCLUDED_CONTENT`` argument is passed, the specified content -# (which might contain ``@variables@``) is appended to the generated -# ``Config.cmake`` file. -# When a ``CONFIG_TEMPLATE`` is passed, or a ``ConfigVersion.cmake.in`` or -# a ``-config-version.cmake.in file is available, these 2 arguments are -# used to replace the ``@INCLUDED_CONTENT@`` string in this file. -# This allows one to inject custom code to this file, useful e.g. to set -# additional variables which are loaded by downstream projects. -# -# If the ``COMPONENT`` argument is passed, it is forwarded to the -# :command:`install` commands, otherwise ```` is used. - - -if(COMMAND install_basic_package_files) - return() -endif() - - -include(GNUInstallDirs) -include(CMakePackageConfigHelpers) -include(CMakeParseArguments) - - -function(INSTALL_BASIC_PACKAGE_FILES _Name) - - # TODO check that _Name does not contain "-" characters - - set(_options ARCH_INDEPENDENT - NO_SET_AND_CHECK_MACRO - NO_CHECK_REQUIRED_COMPONENTS_MACRO - UPPERCASE_FILENAMES - LOWERCASE_FILENAMES - NO_COMPATIBILITY_VARS # Deprecated - ENABLE_COMPATIBILITY_VARS) # Deprecated - set(_oneValueArgs VERSION - COMPATIBILITY - EXPORT - FIRST_TARGET # Deprecated - TARGETS_PROPERTY # Deprecated - VARS_PREFIX - EXPORT_DESTINATION - INSTALL_DESTINATION - DESTINATION # Deprecated - NAMESPACE - CONFIG_TEMPLATE - INCLUDE_FILE - INCLUDE_CONTENT - COMPONENT) - set(_multiValueArgs EXTRA_PATH_VARS_SUFFIX - TARGETS # Deprecated - TARGETS_PROPERTIES # Deprecated - DEPENDENCIES - PRIVATE_DEPENDENCIES) - cmake_parse_arguments(_IBPF "${_options}" "${_oneValueArgs}" "${_multiValueArgs}" "${ARGN}") - - if(NOT DEFINED _IBPF_VARS_PREFIX) - set(_IBPF_VARS_PREFIX ${_Name}) - endif() - - if(NOT DEFINED _IBPF_VERSION) - message(FATAL_ERROR "VERSION argument is required") - endif() - - if(NOT DEFINED _IBPF_COMPATIBILITY) - message(FATAL_ERROR "COMPATIBILITY argument is required") - endif() - - unset(_arch_independent) - if(_IBPF_ARCH_INDEPENDENT) - set(_arch_independent ARCH_INDEPENDENT) - endif() - - if(_IBPF_UPPERCASE_FILENAMES AND _IBPF_LOWERCASE_FILENAMES) - message(FATAL_ERROR "UPPERCASE_FILENAMES and LOWERCASE_FILENAMES arguments cannot be used together") - endif() - - if(DEFINED _IBPF_INCLUDE_FILE AND DEFINED _IBPF_INCLUDE_CONTENT) - message(FATAL_ERROR "INCLUDE_FILE and INCLUDE_CONTENT arguments cannot be used together") - endif() - - # Prepare install and export commands - unset(_targets) - set(_install_cmd EXPORT ${_Name}) - set(_export_cmd EXPORT ${_Name}) - - if(DEFINED _IBPF_EXPORT) - if(DEFINED _IBPF_TARGETS OR DEFINED _IBPF_TARGETS_PROPERTIES OR DEFINED _IBPF_TARGETS_PROPERTIES) - message(FATAL_ERROR "EXPORT cannot be used with TARGETS, TARGETS_PROPERTY or TARGETS_PROPERTIES") - endif() - - set(_export_cmd EXPORT ${_IBPF_EXPORT}) - set(_install_cmd EXPORT ${_IBPF_EXPORT}) - - elseif(DEFINED _IBPF_TARGETS) - message(DEPRECATION "TARGETS is deprecated. Use EXPORT instead") - - if(DEFINED _IBPF_TARGETS_PROPERTY OR DEFINED _IBPF_TARGETS_PROPERTIES) - message(FATAL_ERROR "TARGETS cannot be used with TARGETS_PROPERTY or TARGETS_PROPERTIES") - endif() - - set(_targets ${_IBPF_TARGETS}) - set(_export_cmd TARGETS ${_IBPF_TARGETS}) - - elseif(DEFINED _IBPF_TARGETS_PROPERTY) - message(DEPRECATION "TARGETS_PROPERTY is deprecated. Use EXPORT instead") - - if(DEFINED _IBPF_TARGETS_PROPERTIES) - message(FATAL_ERROR "TARGETS_PROPERTIES cannot be used with TARGETS_PROPERTIES") - endif() - - get_property(_targets GLOBAL PROPERTY ${_IBPF_TARGETS_PROPERTY}) - set(_export_cmd TARGETS ${_targets}) - - elseif(DEFINED _IBPF_TARGETS_PROPERTIES) - message(DEPRECATION "TARGETS_PROPERTIES is deprecated. Use EXPORT instead") - - set(_targets "") # Defined but empty - foreach(_prop ${_IBPF_TARGETS_PROPERTIES}) - get_property(_prop_val GLOBAL PROPERTY ${_prop}) - list(APPEND _targets ${_prop_val}) - endforeach() - set(_export_cmd TARGETS ${_targets}) - - endif() - - # Path for installed cmake files - if(DEFINED _IBPF_DESTINATION) - message(DEPRECATION "DESTINATION is deprecated. Use INSTALL_DESTINATION instead") - if(NOT DEFINED _IBPF_INSTALL_DESTINATION) - set(_IBPF_INSTALL_DESTINATION ${_IBPF_DESTINATION}) - endif() - endif() - - # FIXME CMake 3.7 use the same path - # FIXME Use ARCH_INDEPENDENT to choose destination - if(NOT DEFINED _IBPF_INSTALL_DESTINATION) - if(WIN32 AND NOT CYGWIN) - set(_IBPF_INSTALL_DESTINATION CMake) - else() - set(_IBPF_INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${_Name}) - endif() - endif() - - # FIRST_TARGET is no longer used - if(DEFINED _IBPF_FIRST_TARGET) - message(DEPRECATION "FIRST_TARGET is deprecated.") - endif() - - # NO_COMPATIBILITY_VARS and ENABLE_COMPATIBILITY_VARS cannot be used together - if(_IBPF_NO_COMPATIBILITY_VARS AND _ENABLE_COMPATIBILITY_VARS) - message(FATAL_ERROR "NO_COMPATIBILITY_VARS and ENABLE_COMPATIBILITY_VARS cannot be used together") - endif() - # NO_COMPATIBILITY_VARS is deprecated - if(_IBPF_NO_COMPATIBILITY_VARS) - message(DEPRECATION "NO_COMPATIBILITY_VARS is deprecated.") - endif() - # ENABLE_COMPATIBILITY_VARS is deprecated - if(_IBPF_ENABLE_COMPATIBILITY_VARS) - message(DEPRECATION "ENABLE_COMPATIBILITY_VARS is deprecated.") - endif() - # ENABLE_COMPATIBILITY_VARS does not work with EXPORT - if(NOT DEFINED _targets AND _IBPF_ENABLE_COMPATIBILITY_VARS) - message(FATAL_ERROR "ENABLE_COMPATIBILITY_VARS does not work with EXPORT") - endif() - # ENABLE_COMPATIBILITY_VARS can be enabled for projects still using targets - if(DEFINED _targets AND NOT _IBPF_NO_COMPATIBILITY_VARS AND NOT _IBPF_ENABLE_COMPATIBILITY_VARS) - message(AUTHOR_WARNING "Compatibility variables are no longer generated. Use ENABLE_COMPATIBILITY_VARS to re-enable them (deprecated) or define them using either INCLUDE_FILE or INCLUDE_CONTENT (recommended).") - endif() - - if(NOT DEFINED _IBPF_EXPORT_DESTINATION) - set(_IBPF_EXPORT_DESTINATION "${CMAKE_BINARY_DIR}") - elseif(NOT IS_ABSOLUTE "${_IBPF_EXPORT_DESTINATION}") - set(_IBPF_EXPORT_DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/${_IBPF_EXPORT_DESTINATION}") - endif() - - if(NOT DEFINED _IBPF_NAMESPACE) - set(_IBPF_NAMESPACE "${_Name}::") - endif() - - if(NOT DEFINED _IBPF_COMPONENT) - set(_IBPF_COMPONENT "${_Name}") - endif() - - if(_IBPF_NO_SET_AND_CHECK_MACRO) - list(APPEND configure_package_config_file_extra_args NO_SET_AND_CHECK_MACRO) - endif() - - if(_IBPF_NO_CHECK_REQUIRED_COMPONENTS_MACRO) - list(APPEND configure_package_config_file_extra_args NO_CHECK_REQUIRED_COMPONENTS_MACRO) - endif() - - - - # Set input file for config, and ensure that _IBPF_UPPERCASE_FILENAMES - # and _IBPF_LOWERCASE_FILENAMES are set correctly - unset(_config_cmake_in) - set(_generate_file 0) - if(DEFINED _IBPF_CONFIG_TEMPLATE) - if(NOT EXISTS "${_IBPF_CONFIG_TEMPLATE}") - message(FATAL_ERROR "Config template file \"${_IBPF_CONFIG_TEMPLATE}\" not found") - endif() - set(_config_cmake_in "${_IBPF_CONFIG_TEMPLATE}") - if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES) - if("${_IBPF_CONFIG_TEMPLATE}" MATCHES "${_Name}Config.cmake.in") - set(_IBPF_UPPERCASE_FILENAMES 1) - elseif("${_IBPF_CONFIG_TEMPLATE}" MATCHES "${_name}-config.cmake.in") - set(_IBPF_LOWERCASE_FILENAMES 1) - else() - set(_IBPF_UPPERCASE_FILENAMES 1) - endif() - endif() - else() - string(TOLOWER "${_Name}" _name) - if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_Name}Config.cmake.in") - set(_config_cmake_in "${CMAKE_CURRENT_SOURCE_DIR}/${_Name}Config.cmake.in") - if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES) - set(_IBPF_UPPERCASE_FILENAMES 1) - endif() - elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_name}-config.cmake.in") - set(_config_cmake_in "${CMAKE_CURRENT_SOURCE_DIR}/${_name}-config.cmake.in") - if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES) - set(_IBPF_LOWERCASE_FILENAMES 1) - endif() - else() - set(_generate_file 1) - if(_IBPF_LOWERCASE_FILENAMES) - set(_config_cmake_in "${CMAKE_CURRENT_BINARY_DIR}/${_name}-config.cmake.in") - else() - set(_config_cmake_in "${CMAKE_CURRENT_BINARY_DIR}/${_Name}Config.cmake.in") - set(_IBPF_UPPERCASE_FILENAMES 1) - endif() - endif() - endif() - - # Set input file containing user variables - if(DEFINED _IBPF_INCLUDE_FILE) - if(NOT IS_ABSOLUTE "${_IBPF_INCLUDE_FILE}") - if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_IBPF_INCLUDE_FILE}") - set(_IBPF_INCLUDE_FILE "${CMAKE_CURRENT_SOURCE_DIR}/${_IBPF_INCLUDE_FILE}") - endif() - endif() - if(NOT EXISTS "${_IBPF_INCLUDE_FILE}") - message(FATAL_ERROR "File \"${_IBPF_INCLUDE_FILE}\" not found") - endif() - file(READ ${_IBPF_INCLUDE_FILE} _IBPF_INCLUDE_CONTENT) - endif() - - if(DEFINED _IBPF_INCLUDE_CONTENT) - string(CONFIGURE ${_IBPF_INCLUDE_CONTENT} - _IBPF_INCLUDE_CONTENT - @ONLY) - set(INCLUDED_CONTENT -"#### Expanded from INCLUDE_FILE/INCLUDE_CONTENT by install_basic_package_files() #### - -${_IBPF_INCLUDE_CONTENT} - -##################################################################################### -") - endif() - - # Backwards compatibility - if(NOT _generate_file AND DEFINED _IBPF_INCLUDE_FILE) - file(READ ${_config_cmake_in} _config_cmake_in_content) - if("${_config_cmake_in_content}" MATCHES "@INCLUDED_FILE_CONTENT@") - message(DEPRECATION "The @INCLUDED_FILE_CONTENT@ variable is deprecated in favour of @INCLUDED_CONTENT@") - set(INCLUDED_FILE_CONTENT "${INCLUDED_CONTENT}") - endif() - endif() - - # Select output file names - if(_IBPF_UPPERCASE_FILENAMES) - set(_config_filename ${_Name}Config.cmake) - set(_version_filename ${_Name}ConfigVersion.cmake) - set(_targets_filename ${_Name}Targets.cmake) - elseif(_IBPF_LOWERCASE_FILENAMES) - set(_config_filename ${_name}-config.cmake) - set(_version_filename ${_name}-config-version.cmake) - set(_targets_filename ${_name}-targets.cmake) - endif() - - - # If the template config file does not exist, write a basic one - if(_generate_file) - # Generate the compatibility code - unset(_compatibility_vars) - if(_IBPF_ENABLE_COMPATIBILITY_VARS) - unset(_get_include_dir_code) - unset(_set_include_dir_code) - unset(_target_list) - foreach(_target ${_targets}) - list(APPEND _target_list ${_IBPF_NAMESPACE}${_target}) - endforeach() - if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDEDIR OR - DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDEDIR OR - DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_INCLUDEDIR OR - DEFINED INSTALL_${_IBPF_VARS_PREFIX}_INCLUDEDIR) - list(APPEND _include_dir_list "\"\@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDEDIR\@\"") - elseif(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDE_DIR OR - DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDE_DIR OR - DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_INCLUDE_DIR OR - DEFINED INSTALL_${_IBPF_VARS_PREFIX}_INCLUDE_DIR) - list(APPEND _include_dir_list "\"\@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDE_DIR\@\"") - else() - unset(_include_dir_list) - foreach(_target ${_targets}) - list(APPEND _include_dir_list "\$") - endforeach() - string(REPLACE ";" " " _include_dir_list "${_include_dir_list}") - string(REPLACE ";" " " _target_list "${_target_list}") - set(_set_include_dir "") - endif() - set(_compatibility_vars -"# Compatibility\nset(${_Name}_LIBRARIES ${_target_list}) -set(${_Name}_INCLUDE_DIRS ${_include_dir_list}) -if(NOT \"\${${_Name}_INCLUDE_DIRS}\" STREQUAL \"\") - list(REMOVE_DUPLICATES ${_Name}_INCLUDE_DIRS) -endif() -") - endif() - - # Write the file - file(WRITE "${_config_cmake_in}" -"set(${_IBPF_VARS_PREFIX}_VERSION \@PACKAGE_VERSION\@) - -\@PACKAGE_INIT\@ - -\@PACKAGE_DEPENDENCIES\@ - -include(\"\${CMAKE_CURRENT_LIST_DIR}/${_targets_filename}\") - -${_compatibility_vars} - -\@INCLUDED_CONTENT\@ -") - endif() - - # Make relative paths absolute (needed later on) and append the - # defined variables to _(build|install)_path_vars_suffix - foreach(p BINDIR BIN_DIR - SBINDIR SBIN_DIR - LIBEXECDIR LIBEXEC_DIR - SYSCONFDIR SYSCONF_DIR - SHAREDSTATEDIR SHAREDSTATE_DIR - LOCALSTATEDIR LOCALSTATE_DIR - LIBDIR LIB_DIR - INCLUDEDIR INCLUDE_DIR - OLDINCLUDEDIR OLDINCLUDE_DIR - DATAROOTDIR DATAROOT_DIR - DATADIR DATA_DIR - INFODIR INFO_DIR - LOCALEDIR LOCALE_DIR - MANDIR MAN_DIR - DOCDIR DOC_DIR - ${_IBPF_EXTRA_PATH_VARS_SUFFIX}) - if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p}) - list(APPEND _build_path_vars_suffix ${p}) - list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}") - endif() - if(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p}) - list(APPEND _build_path_vars_suffix ${p}) - list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}") - endif() - if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p}) - list(APPEND _install_path_vars_suffix ${p}) - list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}") - endif() - if(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p}) - list(APPEND _install_path_vars_suffix ${p}) - list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}") - endif() - endforeach() - - - # ConfigVersion.cmake file (same for build tree and intall) - write_basic_package_version_file("${_IBPF_EXPORT_DESTINATION}/${_version_filename}" - VERSION ${_IBPF_VERSION} - COMPATIBILITY ${_IBPF_COMPATIBILITY} - ${_arch_independent}) - install(FILES "${_IBPF_EXPORT_DESTINATION}/${_version_filename}" - DESTINATION ${_IBPF_INSTALL_DESTINATION} - COMPONENT ${_IBPF_COMPONENT}) - - - # Prepare PACKAGE_DEPENDENCIES variable - set(_need_private_deps 0) - if(NOT BUILD_SHARED_LIBS) - set(_need_private_deps 1) - endif() - - unset(PACKAGE_DEPENDENCIES) - if(DEFINED _IBPF_DEPENDENCIES) - set(PACKAGE_DEPENDENCIES "#### Expanded from @PACKAGE_DEPENDENCIES@ by install_basic_package_files() ####\n\ninclude(CMakeFindDependencyMacro)\n") - - # FIXME When CMake 3.9 or greater is required, remove this madness and just - # use find_dependency - if (CMAKE_VERSION VERSION_LESS 3.9) - string(APPEND PACKAGE_DEPENDENCIES " -set(_${_Name}_FIND_PARTS_REQUIRED) -if (${_Name}_FIND_REQUIRED) - set(_${_Name}_FIND_PARTS_REQUIRED REQUIRED) -endif() -set(_${_Name}_FIND_PARTS_QUIET) -if (${_Name}_FIND_QUIETLY) - set(_${_Name}_FIND_PARTS_QUIET QUIET) -endif() -") - - foreach(_dep ${_IBPF_DEPENDENCIES}) - set(_match 0) - string(REGEX REPLACE "[ \n\t]+" ";" _dep_list "${_dep}") - list(LENGTH _dep_list _len) - if(_len EQUAL 1) - set(_match 0) - elseif(_len EQUAL 2) - list(GET _dep_list 1 _dep1) - if(NOT "${_dep1}" MATCHES "^[0-9][0-9\\.]*$") - set(_match 1) - endif() - elseif(_len EQUAL 3) - list(GET _dep_list 1 _dep1) - list(GET _dep_list 2 _dep2) - if((NOT "${_dep1}" MATCHES "^[0-9][0-9\\.]*$") OR (NOT "${_dep2}" STREQUAL "EXACT")) - set(_match 1) - endif() - else() - set(_match 1) - endif() - - if(_match) - string(APPEND PACKAGE_DEPENDENCIES "find_package(${_dep} \${_${_Name}_FIND_PARTS_QUIET} \${_${_Name}_FIND_PARTS_REQUIRED})\n") - else() - string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n") - endif() - endforeach() - - if(_need_private_deps) - foreach(_dep ${_IBPF_PRIVATE_DEPENDENCIES}) - set(_match 0) - string(REGEX REPLACE "[ \n\t]+" ";" _dep_list "${_dep}") - list(LENGTH _dep_list _len) - if(_len EQUAL 1) - set(_match 0) - elseif(_len EQUAL 2) - list(GET _dep_list 1 _dep1) - if(NOT "${_dep1}" MATCHES "^[0-9][0-9\\.]*$") - set(_match 1) - endif() - elseif(_len EQUAL 3) - list(GET _dep_list 1 _dep1) - list(GET _dep_list 2 _dep2) - if((NOT "${_dep1}" MATCHES "^[0-9][0-9\\.]*$") OR (NOT "${_dep2}" STREQUAL "EXACT")) - set(_match 1) - endif() - else() - set(_match 1) - endif() - - if(_match) - string(APPEND PACKAGE_DEPENDENCIES "find_package(${_dep} \${_${_Name}_FIND_PARTS_QUIET} \${_${_Name}_FIND_PARTS_REQUIRED})\n") - else() - string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n") - endif() - endforeach() - endif() - - else() - - foreach(_dep ${_IBPF_DEPENDENCIES}) - string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n") - endforeach() - - if(_need_private_deps) - foreach(_dep ${_IBPF_PRIVATE_DEPENDENCIES}) - string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n") - endforeach() - endif() - - endif() - - string(APPEND PACKAGE_DEPENDENCIES "\n###############################################################################\n") - endif() - - # Prepare PACKAGE_VERSION variable - set(PACKAGE_VERSION ${_IBPF_VERSION}) - - # Config.cmake (build tree) - foreach(p ${_build_path_vars_suffix}) - if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p}) - set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_BUILD_${p}}") - elseif(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p}) - set(${_IBPF_VARS_PREFIX}_${p} "${BUILD_${_IBPF_VARS_PREFIX}_${p}}") - endif() - endforeach() - configure_package_config_file("${_config_cmake_in}" - "${_IBPF_EXPORT_DESTINATION}/${_config_filename}" - INSTALL_DESTINATION ${_IBPF_EXPORT_DESTINATION} - PATH_VARS ${_build_path_vars} - ${configure_package_config_file_extra_args} - INSTALL_PREFIX ${CMAKE_BINARY_DIR}) - - # Config.cmake (installed) - foreach(p ${_install_path_vars_suffix}) - if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p}) - set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_INSTALL_${p}}") - elseif(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p}) - set(${_IBPF_VARS_PREFIX}_${p} "${INSTALL_${_IBPF_VARS_PREFIX}_${p}}") - endif() - endforeach() - configure_package_config_file("${_config_cmake_in}" - "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${_config_filename}.install" - INSTALL_DESTINATION ${_IBPF_INSTALL_DESTINATION} - PATH_VARS ${_install_path_vars} - ${configure_package_config_file_extra_args}) - install(FILES "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${_config_filename}.install" - DESTINATION ${_IBPF_INSTALL_DESTINATION} - RENAME ${_config_filename} - COMPONENT ${_IBPF_COMPONENT}) - - - # Targets.cmake (build tree) - export(${_export_cmd} - NAMESPACE ${_IBPF_NAMESPACE} - FILE "${_IBPF_EXPORT_DESTINATION}/${_targets_filename}") - - # Targets.cmake (installed) - install(${_install_cmd} - NAMESPACE ${_IBPF_NAMESPACE} - DESTINATION ${_IBPF_INSTALL_DESTINATION} - FILE "${_targets_filename}" - COMPONENT ${_IBPF_COMPONENT}) -endfunction() diff --git a/devices/CMakeLists.txt b/devices/CMakeLists.txt index f7b497e09..ff32ab66b 100644 --- a/devices/CMakeLists.txt +++ b/devices/CMakeLists.txt @@ -1,6 +1,28 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause +find_package(iDynTree) + +add_subdirectory(IWearRemapper) +add_subdirectory(IAnalogSensorToIWear) +add_subdirectory(IFrameTransformToIWear) + +if(ENABLE_Paexo) + add_subdirectory(Paexo) +endif() + +if(ENABLE_XsensSuit) + add_subdirectory(XsensSuit) +endif() + +if(ENABLE_ICub) + add_subdirectory(ICub) +endif() + +if(ENABLE_HapticGlove) + add_subdirectory(HapticGlove) +endif() + add_subdirectory(HumanStateProvider) add_subdirectory(HumanWrenchProvider) add_subdirectory(HumanDynamicsEstimator) diff --git a/devices/FTShoes/CMakeLists.txt b/devices/FTShoes/CMakeLists.txt new file mode 100644 index 000000000..8d32c26a2 --- /dev/null +++ b/devices/FTShoes/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +add_library(FTShoes SHARED + src/FTShoes.cpp + include/FTShoes.h) + +target_include_directories(FTShoes + PUBLIC $ + $) + +target_link_libraries(FTShoes IWear) + +install(TARGETS FTShoes + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + +install( + FILES include/FTShoes.h + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable) diff --git a/devices/FTShoes/include/FTShoes.h b/devices/FTShoes/include/FTShoes.h new file mode 100644 index 000000000..ae4d9492f --- /dev/null +++ b/devices/FTShoes/include/FTShoes.h @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#ifndef WEAR_FTSHOES +#define WEAR_FTSHOES + +#include "IWear/IWear.h" + +namespace wearable { + namespace suit { + class FTShoes; + } +} // namespace wearable + +class wearable::suit::FTShoes final : public wearable::IWear +{ +private: + class Impl; + std::unique_ptr pImpl; + +public: + FTShoes(); + ~FTShoes() override; + + FTShoes(const FTShoes& other) = delete; + FTShoes(FTShoes&& other) = delete; + FTShoes& operator=(const FTShoes& other) = delete; + FTShoes& operator=(FTShoes&& other) = delete; + + // ======= + // GENERIC + // ======= + + wearable::SensorPtr + getSensor(const wearable::sensor::SensorName name) const override; + + wearable::VectorOfSensorPtr + getSensors(const wearable::sensor::SensorType) const override; + + // ============== + // SINGLE SENSORS + // ============== + + virtual wearable::SensorPtr + getAccelerometer(const wearable::sensor::SensorName name) const override; + + virtual wearable::SensorPtr + getForce3DSensor(const wearable::sensor::SensorName name) const override; + + virtual wearable::SensorPtr + getForceTorque6DSensor(const wearable::sensor::SensorName name) const override; + + virtual wearable::SensorPtr + getGyroscope(const wearable::sensor::SensorName name) const override; + + virtual wearable::SensorPtr + getMagnetometer(const wearable::sensor::SensorName name) const override; + + virtual wearable::SensorPtr + getOrientationSensor(const wearable::sensor::SensorName name) const override; + + virtual wearable::SensorPtr + getTemperatureSensor(const wearable::sensor::SensorName name) const override; + + virtual wearable::SensorPtr + getTorque3DSensor(const wearable::sensor::SensorName name) const override; +}; + +#endif // WEAR_FTSHOES diff --git a/devices/FTShoes/src/FTShoes.cpp b/devices/FTShoes/src/FTShoes.cpp new file mode 100644 index 000000000..02a0b6e01 --- /dev/null +++ b/devices/FTShoes/src/FTShoes.cpp @@ -0,0 +1,17 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "FTShoes.h" + +using namespace wear::suit; + +class FTShoes::Impl +{}; + +FTShoes::FTShoes() + : pImpl{new Impl()} +{} + +// Without this destructor here, the linker complains for +// undefined reference to vtable +FTShoes::~FTShoes() = default; diff --git a/devices/HapticGlove/CMakeLists.txt b/devices/HapticGlove/CMakeLists.txt new file mode 100644 index 000000000..b43c11950 --- /dev/null +++ b/devices/HapticGlove/CMakeLists.txt @@ -0,0 +1,44 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(SenseGlove REQUIRED) + +# Compile the plugin by default +yarp_prepare_plugin(hapticGlove TYPE wearable::devices::HapticGlove + INCLUDE include/HapticGlove.h + CATEGORY device + ADVANCED + DEFAULT ON + ) + +yarp_add_plugin(HapticGlove + src/HapticGlove.cpp + src/SenseGloveHelper.cpp + include/HapticGlove.h + include/SenseGloveHelper.hpp + ) + +target_include_directories(HapticGlove PRIVATE + $) + +target_link_libraries(HapticGlove + PUBLIC + YARP::YARP_dev + IWear::IWear + PRIVATE + YARP::YARP_init + SenseGlove + Eigen3::Eigen + ) + +yarp_install(TARGETS HapticGlove + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR} + ) + +set (WEARABLES_XML_FILES conf/HapticGlove.xml) +install(FILES ${WEARABLES_XML_FILES} + DESTINATION ${CMAKE_INSTALL_DATADIR}/${WEARABLES_PROJECT_NAME}) diff --git a/devices/HapticGlove/conf/HapticGlove.xml b/devices/HapticGlove/conf/HapticGlove.xml new file mode 100644 index 000000000..06d15622c --- /dev/null +++ b/devices/HapticGlove/conf/HapticGlove.xml @@ -0,0 +1,78 @@ + + + + + + + + 0.01 + false + ( "l_thumb_oppose", "l_thumb_proximal", "l_thumb_middle", "l_thumb_distal", + "l_index_abduction", "l_index_proximal", "l_index_middle", "l_index_distal", + "l_middle_abduction", "l_middle_proximal", "l_middle_middle", "l_middle_distal", + "l_ring_abduction", "l_ring_proximal", "l_ring_middle", "l_ring_distal", + "l_pinky_abduction", "l_pinky_proximal", "l_pinky_middle", "l_pinky_distal" ) + ( "l_thumb_finger", "l_index_finger", "l_middle_finger", "l_ring_finger", "l_little_finger" ) + l_hand + + + + 0.1 + /WearableData/HapticGlove/LeftHand/data:o + /WearableData/HapticGlove/LeftHand/metadataRpc:o + + + LeftHapticGloveWearableDevice + + + + + + + 0.1 + /WearableData/HapticGlove/LeftHand/Actuators/input:i + + + LeftHapticGloveWearableDevice + + + + + + + + 0.01 + true + ( "r_thumb_oppose", "r_thumb_proximal", "r_thumb_middle", "r_thumb_distal", + "r_index_abduction", "r_index_proximal", "r_index_middle", "r_index_distal", + "r_middle_abduction", "r_middle_proximal", "r_middle_middle", "r_middle_distal", + "r_ring_abduction", "r_ring_proximal", "r_ring_middle", "r_ring_distal", + "r_pinky_abduction", "r_pinky_proximal", "r_pinky_middle", "r_pinky_distal" ) + ( "r_thumb_finger", "r_index_finger", "r_middle_finger", "r_ring_finger", "r_little_finger" ) + r_hand + + + + 0.1 + /WearableData/HapticGlove/RightHand/data:o + /WearableData/HapticGlove/RightHand/metadataRpc:o + + + RightHapticGloveWearableDevice + + + + + + + 0.1 + /WearableData/HapticGlove/RightHand/Actuators/input:i + + + RightHapticGloveWearableDevice + + + + + + diff --git a/devices/HapticGlove/conf/SenseGlove.desktop b/devices/HapticGlove/conf/SenseGlove.desktop new file mode 100644 index 000000000..7367e190f --- /dev/null +++ b/devices/HapticGlove/conf/SenseGlove.desktop @@ -0,0 +1,10 @@ +[Desktop Entry] +Version=1.0 +Type=Application +Name=SenseGlove +Comment=SenseGlove Application to communicate with the SenseGlove HW. +Exec=/home/icub/robot-code/Teleoperation/SenseGlove-API/SenseCom/Linux/SenseCom.x86_64 +Icon=/home/icub/robot-code/Teleoperation/SenseGlove-API/SenseCom/Linux/SenseCom_Data/Resources/UnityPlayer.png +Terminal=false +# copy this file to "~/.local/share/applications/" for finding it in the ubuntu-launcher +# cp SenseGlove.desktop ~/.local/share/applications/ diff --git a/devices/HapticGlove/include/HapticGlove.h b/devices/HapticGlove/include/HapticGlove.h new file mode 100644 index 000000000..d32c892cd --- /dev/null +++ b/devices/HapticGlove/include/HapticGlove.h @@ -0,0 +1,240 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#ifndef HAPTIC_GLOVE_H +#define HAPTIC_GLOVE_H + +#include + +#include +#include +#include +#include +#include + +namespace wearable { + namespace devices { + class HapticGlove; + } // namespace devices +} // namespace wearable + +class wearable::devices::HapticGlove + : public yarp::dev::DeviceDriver + , public yarp::os::PeriodicThread + , public yarp::dev::IPreciselyTimed + , public wearable::IWear +{ +private: + class SenseGloveImpl; + std::unique_ptr m_pImpl; + +public: + HapticGlove(); + ~HapticGlove() override; + + // DeviceDriver + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // PeriodicThread + void run() override; + void threadRelease() 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; + + ElementPtr + getActuator(const actuator::ActuatorName name) const override; + + VectorOfElementPtr + getActuators(const actuator::ActuatorType type) const override; + + // IMPLEMENTED SENSORS + // ------------------- + + SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName name) const override; + SensorPtr + getVirtualJointKinSensor(const sensor::SensorName name) const override; + + // IMPLEMENTED ACTUATORS + // --------------------- + + ElementPtr + getHapticActuator(const actuator::ActuatorName name) const override; + + // UNIMPLEMENTED SENSORS + // --------------------- + + inline SensorPtr + getForce3DSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getTorque3DSensor(const sensor::SensorName /*name*/) const override; + + inline SensorPtr + getForceTorque6DSensor(const sensor::SensorName /*name*/) const override; + + 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 + 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 + 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; + + // UNIMPLEMENTED ACTUATORS + // ----------------------- + + inline ElementPtr + getHeaterActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getMotorActuator(const actuator::ActuatorName) const override; +}; + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getForce3DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getTorque3DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getForceTorque6DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getFreeBodyAccelerationSensor( + const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getMagnetometer(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getOrientationSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getPoseSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getPositionSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getVirtualSphericalJointKinSensor( + const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getAccelerometer(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getEmgSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getGyroscope(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getSkinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::HapticGlove::getTemperatureSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::HapticGlove::getHeaterActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::HapticGlove::getMotorActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +#endif // HAPTIC_GLOVE_H diff --git a/devices/HapticGlove/include/SenseGloveHelper.hpp b/devices/HapticGlove/include/SenseGloveHelper.hpp new file mode 100644 index 000000000..255ac80ac --- /dev/null +++ b/devices/HapticGlove/include/SenseGloveHelper.hpp @@ -0,0 +1,323 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#ifndef SENSE_GLOVE_HELPER_HPP +#define SENSE_GLOVE_HELPER_HPP + +#include + +// std +#include +#include +#include +#include + +// YARP +#include +#include + +// Sense Glove +#include +#include + +/** + * GloveControlHelper is an helper class for controlling the glove. + */ +namespace senseGlove { + class SenseGloveHelper; + enum class ThumperCmd : unsigned int; + const std::string LogPrefix = "senseGlove::SenseGloveHelper::"; + const size_t PoseSize = 7; // [ position , quaternion ] +} // namespace senseGlove + +enum class senseGlove::ThumperCmd : unsigned int +{ + None = 126, + + /// Turn off the thumper effects. + TurnOff = 124, + + /// A 5-second long, constant vibration. + Cue_Game_Over = 118, + + /// A double-click at 100% intensity. + Button_Double_100 = 10, + /// A double click at 60% intensity. + Button_Double_60 = 11, + + /// Simulates an impact of the hand at 100% intensity. + Impact_Thump_100 = 1, + /// Simulates an impact of the hand at 30% intensity. + Impact_Thump_30 = 3, + /// Simulates an sharp impact of the hand at 40% intensity. + /// + Impact_Thump_10 = 6, + + /// A light vibration to cue the user that an object it picked up. + /// 100% intensity. + Object_Grasp_100 = 7, + /// A light vibration to cue the user that an object it picked up. + /// 60% intensity. + Object_Grasp_60 = 8, + /// A light vibration to cue the user that an object it picked up. + /// 30% intensity. + Object_Grasp_30 = 9 +}; + +class senseGlove::SenseGloveHelper +{ + + /// Number of the actuated motors Dofs to produce force feedback to the human + int m_forceFbDof; + + /// Number of the actuated vibro-tactile Dofs to produce vibro tactile + /// feedback to the human + int m_buzzDof; + + /// Number of the links of the human hand model + int m_handNoLinks; + + /// Number of the links of the human hand model used for retrieving the euler + /// angles + int m_handNoLinksForEulerAngles; + + /// Number of the links of the glove + int m_gloveNoLinks; + + /// Number of the sensors of the glove + int m_NoJointSensors; + + /// true if the glove is ready to use, i.e., communication working + bool m_isReady; + + /// true if the glove is the right hand + bool m_isRightHand; + + /// Desired force feedback [N], resistence force between 0-40 N transformed to + /// percentage 0-100 + std::vector m_desiredForceValues; + + /// Desired vibro-tactile feedbacks, percentage 0-100 + std::vector m_desiredBuzzValues; + + /// sensory data of the glove in radians + std::vector m_sensorData; + + /// sensory data of the glove poses + Eigen::MatrixXd m_glovePose; + + /// sensory data of the hand link poses; from thumb to pinky, proximal to + /// distal, pos [x y z] Quat [x y z w] + Eigen::MatrixXd m_handPose; + + /// sensory data of the human hand joints angles; From thumb to pinky, + /// proximal to distal [rad] [Pronation/Supination (x), Flexion/Extension (y), + /// Abduction/Adduction (z)] + Eigen::MatrixXd m_handOrientationEulerAngles; + + /// vector of the human joint names + std::vector m_humanJointNameList; + + /// vector of the human finger names + std::vector m_humanFingerNameList; + + /// the name of the human hand link name + std::string m_humanHandLinkName; + + /// the object to interface with the sense glove sdk + SGCore::SG::SenseGlove m_glove; + + /** + * Setup the communication with the glove + * @return true / false in case of success / failure + */ + bool setupGlove(); + + /** + * Get the human hand joint angles from the sense glove data structure + * @return true / false in case of success / failure + */ + bool getHandJointsAngles(); + +public: + /** + Constructor + **/ + SenseGloveHelper(); + + /** + Destructor + **/ + ~SenseGloveHelper(); + + /** + * Configure the class + * @param config configuration options + * @param rightHand if true, the right hand glove will be configured, + * otherwise left. + * @return true / false in case of success / failure + */ + bool configure(const yarp::os::Searchable& config); + + /** + * Set the desired Force Feedback for all the fingers + * @param desiredValue desired force feedback of all the fingers + * @return true / false in case of success / failure + */ + bool setFingersForceReference(const std::vector& desiredValue); + + /** + * Set the desired vibro-tactile feedback for all the fingers + * @param desiredValue desired vibro-tactile values + * @return true / false in case of success / failure + */ + bool setBuzzMotorsReference(const std::vector& desiredValue); + + /** + * Set the desired vibro-tactile feedback for the palm + * @param desiredValue desired vibro-tactile value of the palm + * @return true / false in case of success / failure + */ + bool setPalmFeedbackThumper(ThumperCmd desiredValue); + + /** + * Get the measured hand link poses values + * @param measuredValue measured joint values + * @return true / false in case of success / failure + */ + bool getHandLinksPose(Eigen::MatrixXd& measuredValue); + + /** + * Get the human hand joint angles + * @param jointAngleList std vector of doubles of human hand joint angles + * @return true / false in case of success / failure + */ + bool getHandJointsAngles(std::vector& jointAngleList); + + /** + * Get the human hand joint angles + * @param measuredValue Eigen matrix of human hand joint angles + * @return true / false in case of success / failure + */ + bool getHandJointsAngles(Eigen::MatrixXd measuredValue); + + /** + * Get the glove link poses + * @param measuredValue Eigen matrix of glove poses + * @return true / false in case of success / failure + */ + bool getGloveLinksPose(Eigen::MatrixXd& measuredValue); + + /** + * Get the fingertip poses based on glove sensory data + * @param fingertipPoses Eigen matrix of glove poses [(pos: x, y, z) , + * (rotation: w, x, y, z )] + * @return true / false in case of success / failure + */ + bool getGloveFingertipLinksPose(std::vector>& fingertipPoses); + + /** + * Get the glove sensory data + * @param measuredValue vector of glove sensory data + * @return true / false in case of success / failure + */ + bool getGloveSensorData(std::vector& measuredValues); + + /** + * Get the glove IMU data + * @param palmLinkPose human palm pose based on glove IMU data with the order + * pos(x y z), quat(w x y z) + * @return true / false in case of success / failure + */ + bool getPalmLinkPose(std::vector& palmLinkPose); + + /** + * Trun off the vibro-tactile feedback + * @return true / false in case of success / failure + */ + bool turnOffBuzzMotors(); + + /** + * Trun off the force feedback + * @return true / false in case of success / failure + */ + bool turnOffForceFeedback(); + + /** + * Trun off the palm thumper feedback + * @return true / false in case of success / failure + */ + bool turnOffPalmFeedbackThumper(); + + /** + * get the number of buzz motors/vibro-tactile feedback motors + * @return number of buzz motors + */ + int getNumOfBuzzMotors() const; + + /** + * get the number of force feedback motors + * @return number of force feedback motors + */ + int getNumOfForceFeedback() const; + + /** + * Get the number of hand links + * @return the number of hand links + */ + int getNumHandLinks() const; + + /** + * Get the number of glove links + * @return the number of glove links + */ + int getNumGloveLinks() const; + + /** + * Get the number of glove sensors + * @return the number of glove sensors + */ + int getNumSensors() const; + + /** + * Check if the glove is connected + * @return true / false in case of connected / disconnected + */ + bool isGloveConnected(); + + /** + * Get the human joint list + * @param jointList the human joint list + * @return true / false in case of success / failure + */ + bool getHumanJointNameList(std::vector& jointList) const; + + /** + * Get the human hand link name + * @param handLinkName the human hand link name + * @return true / false in case of success / failure + */ + bool getHumanHandLinkName(std::string& handLinkName) const; + + /** + * Get the human finger list + * @param fingerList the human joint list + * @return true / false in case of success / failure + */ + bool getHumanFingerNameList(std::vector& fingerList) const; + + /** + * Get the left/right hand + * @return true / false in case of right / left hand + */ + bool isRightHand() const; + + /** + * close the device + * @return true / false in case of connected / disconnected + */ + bool close(); +}; + +#endif diff --git a/devices/HapticGlove/src/HapticGlove.cpp b/devices/HapticGlove/src/HapticGlove.cpp new file mode 100644 index 000000000..b9231f539 --- /dev/null +++ b/devices/HapticGlove/src/HapticGlove.cpp @@ -0,0 +1,759 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +const std::string DeviceName = "HapticGlove"; +const std::string LogPrefix = DeviceName + wearable::Separator; +const size_t PoseSize = 7; + +using namespace wearable; +using namespace wearable::devices; + +struct SenseGloveIMUData +{ + // sensors + std::string humanHandLinkName; + // [pos(x, y, z), quat(w, x, y , z)] + // palm/hand frame wrt to hand inertial frame + std::vector humanPalmLinkPose; + // [pos(x, y, z), quat(w, x, y , z)] + // wrt to hand frame + std::vector> fingertipPoses; + std::vector humanLinkNames; + std::vector> humanLinkPoses; + std::map humanLinkSensorNameIdMap; + + std::vector humanJointNames; + std::vector humanJointValues; + std::map humanJointSensorNameIdMap; + + // actuators + std::vector fingersForceFeedback; + std::vector fingersVibroTactileFeedback; + senseGlove::ThumperCmd palmThumperFeedback; + std::vector fingersHapticFeedback; // [fingersForceFeedback, + // fingersVibroTactileFeedback, + // palmThumperFeedback] + + std::vector humanFingerNames; + std::map + humanHapticActuatorNameIdMap; // First part is Force Feedback, second part + // is Vibrotactile feedback +}; + +class HapticGlove::SenseGloveImpl +{ +public: + mutable std::mutex mutex; + + double period = 0.01; //default 100Hz + + SenseGloveIMUData gloveData; + + WearableName wearableName; + + TimeStamp timeStamp; + + const size_t nFingers = 5; + + // Number of sensors + const int nLinkSensors = 6; // 1 palm (hand) imu + 5 fingertips + + // 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. */ + + // link Sensor + std::string linkSensorPrefix; + class SenseGloveVirtualLinkKinSensor; + std::vector> sensegloveLinkSensorVector; + + // joint sensor + std::string jointSensorPrefix; + class SenseGloveVirtualJointKinSensor; + std::vector> sensegloveJointSensorVector; + + // haptic actuator + std::string hapticActuatorPrefix; + class SenseGloveHapticActuator; + std::vector> sensegloveHapticActuatorVector; + + // Methods + SenseGloveImpl(); + + bool run(); + + bool configure(yarp::os::Searchable& config); + + bool isAvailable(const std::string& name, const std::map& map) + { + if (map.find(name) == map.end()) { + return false; + } + return true; + } + + bool close(); +}; + +HapticGlove::SenseGloveImpl::SenseGloveImpl() {} + +bool HapticGlove::SenseGloveImpl::configure(yarp::os::Searchable& config) +{ + std::lock_guard lock(this->mutex); + + this->pGlove = std::make_unique(); + + // configure the glove device + if (!this->pGlove->configure(config)) { + yError() << LogPrefix << "Unable to initialize the sense glove helper device."; + return false; + } + + if (!this->pGlove->getHumanHandLinkName(this->gloveData.humanHandLinkName)) { + yError() << LogPrefix << "Unable to get human hand link name."; + return false; + } + // Initialize snese glove imu data buffer + this->gloveData.humanPalmLinkPose.resize(PoseSize, 0.0); + if (!this->pGlove->getPalmLinkPose(this->gloveData.humanPalmLinkPose)) { + yError() << LogPrefix << "Unable to get the human palm IMU data."; + return false; + } + + this->gloveData.fingertipPoses.resize(this->nFingers, std::vector(PoseSize)); + if (!this->pGlove->getGloveFingertipLinksPose(this->gloveData.fingertipPoses)) { + yError() << LogPrefix << "Unable to get the human fingertip poses."; + return false; + } + + // get joint names + if (!this->pGlove->getHumanJointNameList(this->gloveData.humanJointNames)) { + yError() << LogPrefix << "Unable to get the human joint names."; + return false; + } + + // get the human joint values + this->gloveData.humanJointValues.resize(this->gloveData.humanJointNames.size(), 0.0); + if (!this->pGlove->getHandJointsAngles(this->gloveData.humanJointValues)) { + yError() << LogPrefix << "Unable to get the human hand joint angles."; + return false; + } + + // get finger names + if (!this->pGlove->getHumanFingerNameList(this->gloveData.humanFingerNames)) { + yError() << LogPrefix << "Unable to get the human finger names."; + return false; + } + + this->gloveData.humanLinkPoses.resize(this->nLinkSensors, std::vector(PoseSize)); + + this->gloveData.fingersForceFeedback.resize(this->nFingers, + 0.0); // 5 actuators + + this->gloveData.fingersVibroTactileFeedback.resize(this->nFingers, + 0.0); // 5 actuators + + this->gloveData.palmThumperFeedback = senseGlove::ThumperCmd::TurnOff; + + // [force feedback, vibrotactile feedback] = nActuators + this->gloveData.fingersHapticFeedback.resize(this->nActuators, 0.0); + + return true; +} + +bool HapticGlove::SenseGloveImpl::run() +{ + yTrace() << "SenseGloveImpl::run()"; + std::lock_guard lock(this->mutex); + + // sensors + this->pGlove->getPalmLinkPose(this->gloveData.humanPalmLinkPose); + + this->pGlove->getHandJointsAngles(this->gloveData.humanJointValues); + + this->pGlove->getGloveFingertipLinksPose(this->gloveData.fingertipPoses); + + // link poses + this->gloveData.humanLinkPoses[0] = this->gloveData.humanPalmLinkPose; + for (size_t i = 0; i < this->nFingers; i++) { + this->gloveData.humanLinkPoses[i + 1] = this->gloveData.fingertipPoses[i]; + } + + // actuators + for (size_t i = 0; i < this->nFingers; i++) { + this->gloveData.fingersForceFeedback[i] = this->gloveData.fingersHapticFeedback[i]; + this->gloveData.fingersVibroTactileFeedback[i] = + this->gloveData.fingersHapticFeedback[i + this->nFingers]; + } + this->gloveData.palmThumperFeedback = static_cast( + round(this->gloveData.fingersHapticFeedback[2 * this->nFingers])); + + this->pGlove->setFingersForceReference(this->gloveData.fingersForceFeedback); + this->pGlove->setBuzzMotorsReference(this->gloveData.fingersVibroTactileFeedback); + this->pGlove->setPalmFeedbackThumper(this->gloveData.palmThumperFeedback); + + return true; +} + +bool HapticGlove::SenseGloveImpl::close() +{ + + if (!this->pGlove->turnOffBuzzMotors()) { + yError() << LogPrefix << "cannot turn off the glove buzz motors."; + return false; + } + + if (!this->pGlove->turnOffForceFeedback()) { + yError() << LogPrefix << "cannot turn off the glove force feedback."; + return false; + } + + if (!this->pGlove->turnOffPalmFeedbackThumper()) { + yError() << LogPrefix << "cannot turn off the glove palm thumper feedback."; + return false; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // wait for 100ms. + + return true; +} +HapticGlove::HapticGlove() + : PeriodicThread(0.01) //default 100Hz + , m_pImpl{std::make_unique()} +{} + +// Destructor +HapticGlove::~HapticGlove() = default; + +bool HapticGlove::open(yarp::os::Searchable& config) +{ + yTrace() << LogPrefix << "HapticGlove::open(yarp::os::Searchable& config)"; + + // ================================== + // Check the configuration parameters + // ================================== + // Period of the this device + if (!(config.check("period") && config.find("period").isFloat64())) { + yInfo() << LogPrefix << "Using default period: " << m_pImpl->period << "s"; + } + else { + m_pImpl->period = config.find("period").asFloat64(); + yInfo() << LogPrefix << "Using the period : " << m_pImpl->period << "s"; + } + setPeriod(m_pImpl->period); + + if (!(config.check("wearableName") && config.find("wearableName").isString())) { + yInfo() << LogPrefix << "Using default wearable name SenseGlove"; + m_pImpl->wearableName = DeviceName; + } + else { + m_pImpl->wearableName = config.find("wearableName").asString(); + yInfo() << LogPrefix << "Using the wearable name " << m_pImpl->wearableName; + } + + // Configure the implementation class + if (!m_pImpl->configure(config)) { + yInfo() << LogPrefix << "Cannot configure the implementation class"; + return false; + } + + // ====================================== + // Sensors ans Actuators Initialization + // ====================================== + + m_pImpl->linkSensorPrefix = getWearableName() + sensor::IVirtualLinkKinSensor::getPrefix(); + + m_pImpl->sensegloveLinkSensorVector.push_back( + std::make_shared( + m_pImpl.get(), m_pImpl->linkSensorPrefix + m_pImpl->gloveData.humanHandLinkName)); + + for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++) { + m_pImpl->sensegloveLinkSensorVector.push_back( + std::make_shared( + m_pImpl.get(), + m_pImpl->linkSensorPrefix + m_pImpl->gloveData.humanFingerNames[i] + + "::fingertip")); + } + + m_pImpl->jointSensorPrefix = getWearableName() + sensor::IVirtualJointKinSensor::getPrefix(); + for (size_t i = 0; i < m_pImpl->gloveData.humanJointNames.size(); i++) { + m_pImpl->sensegloveJointSensorVector.push_back( + std::make_shared( + m_pImpl.get(), m_pImpl->jointSensorPrefix + m_pImpl->gloveData.humanJointNames[i])); + } + + 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( + m_pImpl.get(), + m_pImpl->hapticActuatorPrefix + m_pImpl->gloveData.humanFingerNames[i] + + "::ForceFeedback")); + } + + 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 + m_pImpl->gloveData.humanFingerNames[i] + + "::VibroTactileFeedback")); + } + + m_pImpl->sensegloveHapticActuatorVector.push_back( + std::make_shared( + m_pImpl.get(), + m_pImpl->hapticActuatorPrefix + m_pImpl->gloveData.humanHandLinkName + + "::palmThumper")); + + m_pImpl->gloveData.humanLinkSensorNameIdMap.emplace( + std::make_pair(m_pImpl->linkSensorPrefix + m_pImpl->gloveData.humanHandLinkName, 0)); + + for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++) + m_pImpl->gloveData.humanLinkSensorNameIdMap.emplace(std::make_pair( + m_pImpl->linkSensorPrefix + m_pImpl->gloveData.humanFingerNames[i] + "::fingertip", + i + 1)); // +1 for the human hand link + + for (size_t i = 0; i < m_pImpl->gloveData.humanJointNames.size(); i++) + 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] + + "::ForceFeedback", + 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] + + "::VibroTactileFeedback", + i + m_pImpl->nFingers)); + + m_pImpl->gloveData.humanHapticActuatorNameIdMap.emplace(std::make_pair( + m_pImpl->hapticActuatorPrefix + m_pImpl->gloveData.humanHandLinkName + "::palmThumper", + 2 * m_pImpl->nFingers)); + + yInfo() << LogPrefix << "The device is opened successfully."; + + // Start the PeriodicThread loop + if (!start()) { + yError() << LogPrefix << "Failed to start the period thread."; + return false; + } + + return true; +} + +// =========================================== +// SenseGlove implementation of VirtualLinkKinSensor +// =========================================== + +class HapticGlove::SenseGloveImpl::SenseGloveVirtualLinkKinSensor + : public sensor::IVirtualLinkKinSensor +{ +public: + SenseGloveVirtualLinkKinSensor(HapticGlove::SenseGloveImpl* gloveImplPtr, + const sensor::SensorName& name = {}, + const sensor::SensorStatus& status = sensor::SensorStatus::Ok) + : IVirtualLinkKinSensor(name, status) + , m_gloveImpl(gloveImplPtr) + , m_sensorName(name) + { + assert(m_gloveImpl != nullptr); + } + + ~SenseGloveVirtualLinkKinSensor() override = default; + + bool getLinkAcceleration(Vector3& linear, Vector3& angular) const override + { + + std::lock_guard lock(m_gloveImpl->mutex); + + // we do not handle linear and angular accelerations in the current + // implementation + + linear.fill(0.0); + + angular.fill(0.0); + + return true; + } + + bool getLinkPose(Vector3& position, Quaternion& orientation) const override + { + + std::lock_guard lock(m_gloveImpl->mutex); + + if (!m_gloveImpl->isAvailable(m_sensorName, + m_gloveImpl->gloveData.humanLinkSensorNameIdMap)) { + yError() << LogPrefix << "The sensor name (" << m_sensorName + << ") is not found in the list of link sensors."; + + return false; + } + + size_t id = m_gloveImpl->gloveData.humanLinkSensorNameIdMap[m_sensorName]; + + // we do not handle position in the current implementation + position = {m_gloveImpl->gloveData.humanLinkPoses[id][0], + m_gloveImpl->gloveData.humanLinkPoses[id][1], + m_gloveImpl->gloveData.humanLinkPoses[id][2]}; + + orientation = {m_gloveImpl->gloveData.humanLinkPoses[id][3], + m_gloveImpl->gloveData.humanLinkPoses[id][4], + m_gloveImpl->gloveData.humanLinkPoses[id][5], + m_gloveImpl->gloveData.humanLinkPoses[id][6]}; + + return true; + } + + bool getLinkVelocity(Vector3& linear, Vector3& angular) const override + { + + std::lock_guard lock(m_gloveImpl->mutex); + + // we do not handle linear and angular velocity + + angular.fill(0.0); + + linear.fill(0.0); + + return true; + } + + inline void setStatus(const sensor::SensorStatus& status) { m_status = status; } + +private: + HapticGlove::SenseGloveImpl* m_gloveImpl{nullptr}; + std::string m_sensorName; +}; + +// =========================================== +// SenseGlove implementation of VirtualJointKinSensor +// =========================================== + +class HapticGlove::SenseGloveImpl::SenseGloveVirtualJointKinSensor + : public sensor::IVirtualJointKinSensor +{ +public: + SenseGloveVirtualJointKinSensor(HapticGlove::SenseGloveImpl* gloveImplPtr, + const sensor::SensorName& name = {}, + const sensor::SensorStatus& status = sensor::SensorStatus::Ok) + : IVirtualJointKinSensor(name, status) + , m_gloveImpl(gloveImplPtr) + , m_sensorName(name) + { + assert(m_gloveImpl != nullptr); + } + + ~SenseGloveVirtualJointKinSensor() override = default; + + bool getJointPosition(double& position) const override + { + + std::lock_guard lock(m_gloveImpl->mutex); + + if (!m_gloveImpl->isAvailable(m_sensorName, + m_gloveImpl->gloveData.humanJointSensorNameIdMap)) { + yError() << LogPrefix << "The sensor name (" << m_sensorName + << ") is not found in the list of joint sensors."; + + return false; + } + + position = + m_gloveImpl->gloveData + .humanJointValues[m_gloveImpl->gloveData.humanJointSensorNameIdMap[m_sensorName]]; + + return true; + } + + bool getJointVelocity(double& velocity) const override + { + + std::lock_guard lock(m_gloveImpl->mutex); + + velocity = 0.0; + + return true; + } + + bool getJointAcceleration(double& acceleration) const override + { + + std::lock_guard lock(m_gloveImpl->mutex); + + acceleration = 0.0; + + return true; + } + + inline void setStatus(const sensor::SensorStatus& status) { m_status = status; } + +private: + HapticGlove::SenseGloveImpl* m_gloveImpl{nullptr}; + std::string m_sensorName; +}; + +// =========================================== +// SenseGlove implementation of IHaptic actuator +// =========================================== + +class HapticGlove::SenseGloveImpl::SenseGloveHapticActuator : public wearable::actuator::IHaptic +{ +public: + SenseGloveHapticActuator(HapticGlove::SenseGloveImpl* gloveImplPtr, + const actuator::ActuatorName& name = {}, + const actuator::ActuatorStatus& status = actuator::ActuatorStatus::Ok) + : IHaptic(name, status) + , m_gloveImpl(gloveImplPtr) + , m_actuatorName(name) + { + assert(m_gloveImpl != nullptr); + } + ~SenseGloveHapticActuator() override = default; + + 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; + } + + bool setHapticCommands(const std::vector& forceValue, const std::vector& vibrotactileValue) const override + { + + 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; + } + 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: + HapticGlove::SenseGloveImpl* m_gloveImpl{nullptr}; + std::string m_actuatorName; +}; + +// =========================================== +void HapticGlove::run() +{ + // Get timestamp + m_pImpl->timeStamp.time = yarp::os::Time::now(); + + // to implement + m_pImpl->run(); +} + +bool HapticGlove::close() +{ + yTrace() << LogPrefix << "HapticGlove::close()"; + + if (!m_pImpl->close()) { + yError() << LogPrefix << "Cannot close correctly the sense glove implementation."; + return false; + } + return true; +} + +void HapticGlove::threadRelease() {} + +// ========================= +// IPreciselyTimed interface +// ========================= +yarp::os::Stamp HapticGlove::getLastInputStamp() +{ + // Stamp count should be always zero + std::lock_guard lock(m_pImpl->mutex); + return yarp::os::Stamp(0, m_pImpl->timeStamp.time); +} + +// --------------------------- +// Implement Sensors Methods +// --------------------------- + +wearable::WearableName HapticGlove::getWearableName() const +{ + return m_pImpl->wearableName + wearable::Separator; +} + +wearable::WearStatus HapticGlove::getStatus() const +{ + wearable::WearStatus status = wearable::WearStatus::Ok; + + for (const auto& s : getAllSensors()) { + if (s->getSensorStatus() != sensor::SensorStatus::Ok) { + status = wearable::WearStatus::Error; + // TO CHECK + break; + } + } + // Default return status is Ok + return status; +} + +wearable::TimeStamp HapticGlove::getTimeStamp() const +{ + std::lock_guard lock(m_pImpl->mutex); + return {m_pImpl->timeStamp.time, 0}; +} + +wearable::SensorPtr +HapticGlove::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 +HapticGlove::getSensors(const wearable::sensor::SensorType aType) const +{ + wearable::VectorOfSensorPtr outVec; + switch (aType) { + case sensor::SensorType::VirtualLinkKinSensor: { + outVec.reserve(m_pImpl->sensegloveLinkSensorVector.size()); + for (const auto& sensegloveLinkSensor : m_pImpl->sensegloveLinkSensorVector) + outVec.push_back(static_cast>(sensegloveLinkSensor)); + break; + } + case sensor::SensorType::VirtualJointKinSensor: { + outVec.reserve(m_pImpl->sensegloveJointSensorVector.size()); + for (const auto& sensegloveJointSensor : m_pImpl->sensegloveJointSensorVector) + outVec.push_back(static_cast>(sensegloveJointSensor)); + break; + } + default: { + return {}; + } + } + + return outVec; +} + +wearable::ElementPtr +HapticGlove::getActuator(const wearable::actuator::ActuatorName name) const +{ + wearable::VectorOfElementPtr actuators = getAllActuators(); + + for (const auto& a : actuators) { + if (a->getActuatorName() == name) { + return a; + } + } + yWarning() << LogPrefix << "User specified actuator name <" << name << "> not found"; + return nullptr; +} + +wearable::VectorOfElementPtr +HapticGlove::getActuators(const wearable::actuator::ActuatorType aType) const +{ + wearable::VectorOfElementPtr outVec; + switch (aType) { + case wearable::actuator::ActuatorType::Haptic: { + outVec.reserve(m_pImpl->sensegloveHapticActuatorVector.size()); + for (const auto& hapticActuator : m_pImpl->sensegloveHapticActuatorVector) { + outVec.push_back(static_cast>(hapticActuator)); + } + break; + } + default: { + return {}; + } + } + return outVec; +} + +wearable::SensorPtr +HapticGlove::getVirtualLinkKinSensor(const wearable::sensor::SensorName name) const +{ + + if (!m_pImpl->isAvailable(name, m_pImpl->gloveData.humanLinkSensorNameIdMap)) { + yError() << LogPrefix << "Invalid sensor name " << name; + return nullptr; + } + + // Return a shared point to the required sensor + return static_cast>( + m_pImpl->sensegloveLinkSensorVector.at(m_pImpl->gloveData.humanLinkSensorNameIdMap[name])); +} + +wearable::SensorPtr +HapticGlove::getVirtualJointKinSensor(const wearable::sensor::SensorName name) const +{ + if (!m_pImpl->isAvailable(name, m_pImpl->gloveData.humanJointSensorNameIdMap)) { + yError() << LogPrefix << "Invalid sensor name " << name; + return nullptr; + } + + // Return a shared point to the required sensor + return static_cast>( + m_pImpl->sensegloveJointSensorVector.at( + m_pImpl->gloveData.humanJointSensorNameIdMap[name])); +} + +wearable::ElementPtr +HapticGlove::getHapticActuator(const actuator::ActuatorName name) const +{ + // Check if user-provided name corresponds to an available actuator + if (!m_pImpl->isAvailable(name, m_pImpl->gloveData.humanHapticActuatorNameIdMap)) { + yError() << LogPrefix << "Invalid actuator name " << name; + return nullptr; + } + + return static_cast>( + m_pImpl->sensegloveHapticActuatorVector.at( + m_pImpl->gloveData.humanHapticActuatorNameIdMap[name])); +} diff --git a/devices/HapticGlove/src/SenseGloveHelper.cpp b/devices/HapticGlove/src/SenseGloveHelper.cpp new file mode 100644 index 000000000..b8c6ca47c --- /dev/null +++ b/devices/HapticGlove/src/SenseGloveHelper.cpp @@ -0,0 +1,532 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include +#include + +#include + +using namespace senseGlove; + +SenseGloveHelper::SenseGloveHelper() +{ + yInfo() << LogPrefix << "SenseGloveHelper()"; + + m_isReady = false; + m_forceFbDof = 5; + m_buzzDof = 5; + m_gloveNoLinks = 30; + m_handNoLinks = 20; + m_handNoLinksForEulerAngles = 15; + m_NoJointSensors = 20; + + m_desiredBuzzValues.resize(m_buzzDof, 0); + m_desiredForceValues.resize(m_forceFbDof, 0); + m_glovePose = Eigen::MatrixXd::Zero(m_gloveNoLinks, 7); + m_handPose = Eigen::MatrixXd::Zero(m_handNoLinks, 7); + m_handOrientationEulerAngles = Eigen::MatrixXd::Zero(m_handNoLinksForEulerAngles, 3); +} + +bool SenseGloveHelper::configure(const yarp::os::Searchable& config) +{ + yInfo() << LogPrefix << "configure:: "; + + if (!(config.check("rightHand") && config.find("rightHand").isBool())) { + yInfo() << LogPrefix << "Using default hand Sense Glove: Right hand"; + m_isRightHand = true; + } + else { + m_isRightHand = config.find("rightHand").asBool(); + yInfo() << LogPrefix << "Using the right hand: " << m_isRightHand + << "(if false, using left hand)"; + } + + // get human hand link name + if (!(config.check("hand_link") && config.find("hand_link").isString())) { + yError() << LogPrefix << "Unable to find hand_link in the config file."; + return false; + } + m_humanHandLinkName = config.find("hand_link").asString(); + yInfo() << LogPrefix << "human hand link name: " << m_humanHandLinkName; + + // get human hand joint names + yarp::os::Bottle* jointListYarp; + if (!(config.check("human_joint_list") && config.find("human_joint_list").isList())) { + yError() << LogPrefix << "Unable to find human_joint_list in the config file."; + return false; + } + jointListYarp = config.find("human_joint_list").asList(); + + for (size_t i = 0; i < jointListYarp->size(); i++) { + m_humanJointNameList.push_back(jointListYarp->get(i).asString()); + } + yInfo() << LogPrefix << "human joint names: " << m_humanJointNameList; + + // get human hand finger names + yarp::os::Bottle* fingerListYarp; + if (!(config.check("human_finger_list") && config.find("human_finger_list").isList())) { + yError() << LogPrefix << "Unable to find human_finger_list in the config file."; + return false; + } + fingerListYarp = config.find("human_finger_list").asList(); + + for (size_t i = 0; i < fingerListYarp->size(); i++) { + m_humanFingerNameList.push_back(fingerListYarp->get(i).asString()); + } + yInfo() << LogPrefix << "human finger names: " << m_humanFingerNameList; + + if (!setupGlove()) { + yError() << LogPrefix << "Unable to set up the sense glove."; + return false; + } + + return true; +} + +bool SenseGloveHelper::setupGlove() +{ + yInfo() << LogPrefix << "setupGlove()"; + + if (!SGCore::DeviceList::SenseCommRunning()) // Returns true if SenseComm is + // running. + { + yError() << LogPrefix << "SenseComm is not running. Please run SenseComm, then try again."; + return false; + } + + if (!SGCore::SG::SenseGlove::GetSenseGlove(m_isRightHand, m_glove)) { + yError() << LogPrefix + << "No sense gloves connected to the system. Ensure the USB " + "connection is " + "secure, then try again."; + return false; + } + + SGCore::SG::SG_GloveInfo gloveModel = m_glove.GetGloveModel(); + yInfo() << LogPrefix << "glove model:" << gloveModel.ToString(true); + + return true; +} + +bool SenseGloveHelper::setFingersForceReference(const std::vector& desiredValue) +{ + if (desiredValue.size() != m_forceFbDof) { + yError() << LogPrefix + << "Size of the input desired vecotr and the number of haptic " + "force feedbacks are not equal."; + return false; + } + + for (size_t i = 0; i < m_forceFbDof; i++) { + m_desiredForceValues[i] = + (int) std::round(std::max(0.0, std::min(desiredValue[i], 40.0)) * 100 / 40); + } + + m_glove.SendHaptics(SGCore::Haptics::SG_FFBCmd(m_desiredForceValues)); + + return true; +} + +bool SenseGloveHelper::setBuzzMotorsReference(const std::vector& desiredValue) +{ + if (desiredValue.size() != m_buzzDof) { + yError() << LogPrefix + << "Size of the input desired vector and the number of buzz " + "motors are not equal."; + return false; + } + for (size_t i = 0; i < m_buzzDof; i++) { + m_desiredBuzzValues[i] = (int) std::round(std::max(0.0, std::min(desiredValue[i], 100.0))); + } + m_glove.SendHaptics(SGCore::Haptics::SG_BuzzCmd(m_desiredBuzzValues)); + + return true; +} + +bool SenseGloveHelper::setPalmFeedbackThumper(ThumperCmd desiredValue) +{ + + bool result; + switch (desiredValue) { + case ThumperCmd::None: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::None); + break; + + case ThumperCmd::TurnOff: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::TurnOff); + break; + + case ThumperCmd::Cue_Game_Over: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Cue_Game_Over); + break; + + case ThumperCmd::Button_Double_100: + // result = + // m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Button_Double_100); + yWarning() << LogPrefix + << "The glove may stop working due to overloal, so " + "Button_Double_60 is passed instead of Button_Double_100"; + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Button_Double_60); + break; + + case ThumperCmd::Button_Double_60: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Button_Double_60); + break; + + case ThumperCmd::Impact_Thump_100: + // result = + // m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Impact_Thump_100); + yWarning() << LogPrefix + << "The glove may stop working due to overloal, so " + "Impact_Thump_30 is passed instead of Impact_Thump_100"; + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Impact_Thump_30); + break; + + case ThumperCmd::Impact_Thump_30: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Impact_Thump_30); + break; + + case ThumperCmd::Impact_Thump_10: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Impact_Thump_10); + break; + + case ThumperCmd::Object_Grasp_100: + // result = + // m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Object_Grasp_100); + yWarning() << LogPrefix + << "The glove may stop working due to overloal, so " + "Object_Grasp_60 is passed instead of Object_Grasp_100"; + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Object_Grasp_60); + break; + + case ThumperCmd::Object_Grasp_60: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Object_Grasp_60); + break; + + case ThumperCmd::Object_Grasp_30: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::Object_Grasp_30); + break; + + default: + result = m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::TurnOff); + break; + } + + return result; +} + +bool SenseGloveHelper::getHandLinksPose(Eigen::MatrixXd& measuredValue) +{ + + SGCore::HandProfile profile = SGCore::HandProfile::Default(m_glove.IsRight()); + SGCore::HandPose handPose; + if (!m_glove.GetHandPose(profile, handPose)) { + yWarning() << LogPrefix << "m_glove.GetHandPose method of the glove returns error."; + measuredValue = m_handPose; + return true; // to avoid stopping the device + } + + int count = 0; + // size is 5 (5 Fingers) + for (int i = 0; i < handPose.jointPositions.size(); i++) { + // size is 4 (4 links each finger) + for (int j = 0; j < handPose.jointPositions[i].size(); j++) { + m_handPose(count, 0) = handPose.jointPositions[i][j].x; + m_handPose(count, 1) = handPose.jointPositions[i][j].y; + m_handPose(count, 2) = handPose.jointPositions[i][j].z; + + // wrt to the origin frame + m_handPose(count, 3) = handPose.jointRotations[i][j].w; + m_handPose(count, 4) = handPose.jointRotations[i][j].x; + m_handPose(count, 5) = handPose.jointRotations[i][j].y; + m_handPose(count, 6) = handPose.jointRotations[i][j].z; + count++; + } + } + measuredValue = m_handPose; + + return true; +} + +bool SenseGloveHelper::getHandJointsAngles() +{ + + SGCore::HandProfile profile = SGCore::HandProfile::Default(m_glove.IsRight()); + SGCore::HandPose handPose; + + if (!m_glove.GetHandPose(profile, handPose)) { + yWarning() << LogPrefix << "m_glove.GetHandPose method of the glove returns error."; + return true; + } + + int count = 0; + // size is 5 (5 Fingers) + for (int i = 0; i < handPose.handAngles.size(); i++) { + // size is 3 + for (int j = 0; j < handPose.handAngles[i].size(); j++) { + // Euler representations of all possible hand angles + m_handOrientationEulerAngles(count, 0) = handPose.handAngles[i][j].x; + m_handOrientationEulerAngles(count, 1) = handPose.handAngles[i][j].y; + m_handOrientationEulerAngles(count, 2) = handPose.handAngles[i][j].z; + count++; + } + } + return true; +} + +bool SenseGloveHelper::getHandJointsAngles(std::vector& jointAngleList) +{ + getHandJointsAngles(); + + // if (jointAngleList.size() != m_humanJointNameList.size()) { + // yInfo() << "m_humanJointNameList.size(): " << m_humanJointNameList.size(); + jointAngleList.resize(m_humanJointNameList.size(), 0.0); // 20 + // } + // std::cout << "m_handOrientationEulerAngles\n" + // << m_handOrientationEulerAngles << std::endl; + + // thumb (0:3) + jointAngleList[0] = m_handOrientationEulerAngles(0, 2); + jointAngleList[1] = m_handOrientationEulerAngles(0, 1); + jointAngleList[2] = m_handOrientationEulerAngles(1, 1); + jointAngleList[3] = m_handOrientationEulerAngles(2, 1); + + // index (4:7) + jointAngleList[4] = m_handOrientationEulerAngles(3, 2); + jointAngleList[5] = m_handOrientationEulerAngles(3, 1); + jointAngleList[6] = m_handOrientationEulerAngles(4, 1); + jointAngleList[7] = m_handOrientationEulerAngles(5, 1); + + // middle (8:11) + jointAngleList[8] = m_handOrientationEulerAngles(6, 2); + jointAngleList[9] = m_handOrientationEulerAngles(6, 1); + jointAngleList[10] = m_handOrientationEulerAngles(7, 1); + jointAngleList[11] = m_handOrientationEulerAngles(8, 1); + + // ring (12:15) + jointAngleList[12] = m_handOrientationEulerAngles(9, 2); + jointAngleList[13] = m_handOrientationEulerAngles(9, 1); + jointAngleList[14] = m_handOrientationEulerAngles(10, 1); + jointAngleList[15] = m_handOrientationEulerAngles(11, 1); + + // pinkie (16:19) + jointAngleList[16] = m_handOrientationEulerAngles(12, 2); + jointAngleList[17] = m_handOrientationEulerAngles(12, 1); + jointAngleList[18] = m_handOrientationEulerAngles(13, 1); + jointAngleList[19] = m_handOrientationEulerAngles(14, 1); + + return true; +} + +bool SenseGloveHelper::getHandJointsAngles(Eigen::MatrixXd measuredValue) +{ + measuredValue = m_handOrientationEulerAngles; + return true; +} + +bool SenseGloveHelper::getGloveLinksPose(Eigen::MatrixXd& measuredValue) +{ + SGCore::SG::SG_GlovePose glovePose; + if (!m_glove.GetGlovePose(glovePose)) { + yWarning() << LogPrefix << "m_glove.GetGlovePose return error."; + measuredValue = m_glovePose; + return true; + } + + int count = 0; + // glove no of fingers :5 + for (int i = 0; i < glovePose.jointPositions.size(); i++) { + // glove's finger no of links : 6 + for (int j = 0; j < glovePose.jointPositions[i].size(); j++) { + m_glovePose(count, 0) = glovePose.jointPositions[i][j].x / 1000.0; // mm to meter + m_glovePose(count, 1) = glovePose.jointPositions[i][j].y / 1000.0; // mm to meter + m_glovePose(count, 2) = glovePose.jointPositions[i][j].z / 1000.0; // mm to meter + + // wrt to the origin frame + m_glovePose(count, 3) = glovePose.jointRotations[i][j].w; + m_glovePose(count, 4) = glovePose.jointRotations[i][j].x; + m_glovePose(count, 5) = glovePose.jointRotations[i][j].y; + m_glovePose(count, 6) = glovePose.jointRotations[i][j].z; + count++; + } + } + measuredValue = m_glovePose; + return true; +} + +bool SenseGloveHelper::getGloveFingertipLinksPose(std::vector>& fingertipPoses) +{ + + if (fingertipPoses.size() != m_humanFingerNameList.size()) { + fingertipPoses.resize(m_humanFingerNameList.size(), std::vector(PoseSize)); + } + // avoid iterating on all the elements + if (fingertipPoses[0].size() != PoseSize) { + + fingertipPoses.resize(m_humanFingerNameList.size(), std::vector(PoseSize)); + } + + Eigen::MatrixXd glovePoses; + getGloveLinksPose(glovePoses); // 30X7 + if (glovePoses.rows() != m_gloveNoLinks || glovePoses.cols() != PoseSize) { + yWarning() << LogPrefix << "glovePoses size is not correct:: rows:" << glovePoses.rows() + << " , cols:" << glovePoses.cols(); + return true; // to avoid stoping + } + + for (size_t i = 0; i < m_humanFingerNameList.size(); i++) { + for (size_t j = 0; j < PoseSize; j++) + fingertipPoses[i][j] = glovePoses(i * 6 + 5, + j); // the last value of each finger for the glove + // data is associated with the fingertip + } + + return true; +} + +bool SenseGloveHelper::getGloveSensorData(std::vector& measuredValues) +{ + SGCore::SG::SG_SensorData sensorData; + if (!m_glove.GetSensorData(sensorData)) { + yWarning() << LogPrefix << "m_glove.GetSensorData return error."; + measuredValues = m_sensorData; + return true; + } + m_sensorData = sensorData.GetAngleSequence(); + measuredValues = m_sensorData; + return true; +} + +bool SenseGloveHelper::getPalmLinkPose(std::vector& palmLinkPose) +{ + SGCore::Kinematics::Quat imu; + + if (palmLinkPose.size() != 7) { + palmLinkPose.resize(7, 0.0); + } + + if (!m_glove.GetIMURotation(imu)) { + yWarning() << LogPrefix << "Cannot get glove IMU value"; + return true; // to avoid crashing + } + // position + palmLinkPose[0] = 0.0; + palmLinkPose[1] = 0.0; + palmLinkPose[2] = 0.0; + // orientation: IMU + palmLinkPose[3] = imu.w; + palmLinkPose[4] = imu.x; + palmLinkPose[5] = imu.y; + palmLinkPose[6] = imu.z; + + double norm = 0.0; + for (size_t i = 3; i < palmLinkPose.size(); i++) { + norm += palmLinkPose[i] * palmLinkPose[i]; + } + norm = std::sqrt(norm); + + for (size_t i = 3; i < palmLinkPose.size(); i++) { + palmLinkPose[i] = palmLinkPose[i] / norm; + } + + return true; +} + +bool SenseGloveHelper::isGloveConnected() +{ + return m_glove.IsConnected(); +} + +bool SenseGloveHelper::turnOffBuzzMotors() +{ + m_glove.SendHaptics(SGCore::Haptics::SG_BuzzCmd::off); + return true; +} + +bool SenseGloveHelper::turnOffForceFeedback() +{ + m_glove.SendHaptics(SGCore::Haptics::SG_FFBCmd::off); + return true; +} + +bool SenseGloveHelper::turnOffPalmFeedbackThumper() +{ + m_glove.SendHaptics(SGCore::Haptics::SG_ThumperCmd::TurnOff); + return true; +} + +int SenseGloveHelper::getNumOfBuzzMotors() const +{ + return m_buzzDof; +} + +int SenseGloveHelper::getNumOfForceFeedback() const +{ + return m_forceFbDof; +} + +int SenseGloveHelper::getNumGloveLinks() const +{ + return m_gloveNoLinks; +} + +int SenseGloveHelper::getNumHandLinks() const +{ + return m_handNoLinks; +} + +int SenseGloveHelper::getNumSensors() const +{ + return m_NoJointSensors; +} + +bool SenseGloveHelper::getHumanJointNameList(std::vector& jointList) const +{ + if (m_humanJointNameList.size() == 0) { + yError() << LogPrefix << "m_humanJointNameList vector size is zero."; + return false; + } + + jointList.resize(m_humanJointNameList.size()); + + for (size_t i = 0; i < m_humanJointNameList.size(); i++) + jointList[i] = m_humanJointNameList[i]; + + return true; +} + +bool SenseGloveHelper::getHumanHandLinkName(std::string& handLinkName) const +{ + handLinkName = m_humanHandLinkName; + return true; +} + +bool SenseGloveHelper::getHumanFingerNameList(std::vector& fingerList) const +{ + + if (m_humanFingerNameList.size() == 0) { + yError() << LogPrefix << "m_humanFingerNameList vector size is zero."; + return false; + } + + fingerList.resize(m_humanFingerNameList.size()); + + for (size_t i = 0; i < m_humanFingerNameList.size(); i++) + fingerList[i] = m_humanFingerNameList[i]; + + return true; +} + +SenseGloveHelper::~SenseGloveHelper() {} + +bool SenseGloveHelper::isRightHand() const +{ + return m_isRightHand; +} + +bool SenseGloveHelper::close() +{ + turnOffBuzzMotors(); + turnOffForceFeedback(); + return true; +} diff --git a/devices/HumanControlBoard/CMakeLists.txt b/devices/HumanControlBoard/CMakeLists.txt index c67b7b6d5..c3b07be56 100644 --- a/devices/HumanControlBoard/CMakeLists.txt +++ b/devices/HumanControlBoard/CMakeLists.txt @@ -1,7 +1,6 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause -find_package(IWear REQUIRED) find_package(iDynTree REQUIRED) yarp_prepare_plugin(human_control_board diff --git a/devices/HumanStateProvider/CMakeLists.txt b/devices/HumanStateProvider/CMakeLists.txt index b088b08c3..5313f6439 100644 --- a/devices/HumanStateProvider/CMakeLists.txt +++ b/devices/HumanStateProvider/CMakeLists.txt @@ -1,7 +1,6 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause -find_package(IWear REQUIRED) find_package(iDynTree REQUIRED) find_package(OsqpEigen 0.4.0 REQUIRED) diff --git a/devices/HumanWrenchProvider/CMakeLists.txt b/devices/HumanWrenchProvider/CMakeLists.txt index e40fa9fe5..7042d1a22 100644 --- a/devices/HumanWrenchProvider/CMakeLists.txt +++ b/devices/HumanWrenchProvider/CMakeLists.txt @@ -1,7 +1,6 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause -find_package(IWear REQUIRED) find_package(iDynTree REQUIRED) find_package(Eigen3 REQUIRED) diff --git a/devices/IAnalogSensorToIWear/CMakeLists.txt b/devices/IAnalogSensorToIWear/CMakeLists.txt new file mode 100644 index 000000000..92acff38d --- /dev/null +++ b/devices/IAnalogSensorToIWear/CMakeLists.txt @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(ianalogsensor_to_iwear + TYPE wearable::devices::IAnalogSensorToIWear + INCLUDE include/IAnalogSensorToIWear.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(IAnalogSensorToIWear + src/IAnalogSensorToIWear.cpp + include/IAnalogSensorToIWear.h) + +target_include_directories(IAnalogSensorToIWear PRIVATE + $) + +target_link_libraries(IAnalogSensorToIWear PUBLIC + IWear::IWear YARP::YARP_dev YARP::YARP_init) + +yarp_install( + TARGETS IAnalogSensorToIWear + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) diff --git a/devices/IAnalogSensorToIWear/conf/ianalogsensor_to_iwear.xml b/devices/IAnalogSensorToIWear/conf/ianalogsensor_to_iwear.xml new file mode 100644 index 000000000..629fe43b2 --- /dev/null +++ b/devices/IAnalogSensorToIWear/conf/ianalogsensor_to_iwear.xml @@ -0,0 +1,41 @@ + + + + + 0.5 + fakeSensor + + + + + FakeSensor + FakeWearable + 1 + 0 + TemperatureSensor + + + + FakeAnalogSensor + + + + + + + + + 0.01 + /Example/IWearWrapper/data:o + /Example/IWearWrapper/metadata:o + + + + IAnalogSensorToIWear + + + + + + + diff --git a/devices/IAnalogSensorToIWear/include/IAnalogSensorToIWear.h b/devices/IAnalogSensorToIWear/include/IAnalogSensorToIWear.h new file mode 100644 index 000000000..503c136f2 --- /dev/null +++ b/devices/IAnalogSensorToIWear/include/IAnalogSensorToIWear.h @@ -0,0 +1,172 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IANALOGSENSORTOIWEAR +#define WEARABLE_IANALOGSENSORTOIWEAR + +#include "Wearable/IWear/IWear.h" + +#include +#include +#include +#include + +#include + +namespace wearable { + namespace devices { + class IAnalogSensorToIWear; + } // namespace devices +} // namespace wearable + +class wearable::devices::IAnalogSensorToIWear + : public wearable::IWear + , public yarp::dev::DeviceDriver + , public yarp::dev::IPreciselyTimed + , public yarp::dev::IWrapper + , public yarp::dev::IMultipleWrapper +{ +private: + class Impl; + std::unique_ptr pImpl; + +public: + IAnalogSensorToIWear(); + ~IAnalogSensorToIWear() override; + + IAnalogSensorToIWear(const IAnalogSensorToIWear& other) = delete; + IAnalogSensorToIWear(IAnalogSensorToIWear&& other) = delete; + IAnalogSensorToIWear& operator=(const IAnalogSensorToIWear& other) = delete; + IAnalogSensorToIWear& operator=(IAnalogSensorToIWear&& other) = delete; + + // DeviceDriver + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // IPreciselyTimed + yarp::os::Stamp getLastInputStamp() override; + + // IWrapper interface + bool attach(yarp::dev::PolyDriver* poly) override; + bool detach() override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() override; + + // ===== + // IWEAR + // ===== + + // ------- + // GENERIC + // ------- + + SensorPtr + getSensor(const wearable::sensor::SensorName name) const override; + + VectorOfSensorPtr + getSensors(const wearable::sensor::SensorType type) const override; + + WearableName getWearableName() const override; + WearStatus getStatus() const override; + TimeStamp getTimeStamp() const override; + + // -------------- + // SINGLE SENSORS + // -------------- + + SensorPtr + getAccelerometer(const wearable::sensor::SensorName name) const override; + + SensorPtr + getForce3DSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr + getForceTorque6DSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr + getGyroscope(const wearable::sensor::SensorName name) const override; + + SensorPtr + getMagnetometer(const wearable::sensor::SensorName name) const override; + + SensorPtr + getOrientationSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr + getTemperatureSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr + getTorque3DSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr getEmgSensor(const sensor::SensorName name) const override; + + SensorPtr + getFreeBodyAccelerationSensor(const sensor::SensorName name) const override; + + SensorPtr + getPoseSensor(const sensor::SensorName name) const override; + + SensorPtr + getPositionSensor(const sensor::SensorName name) const override; + + SensorPtr + getSkinSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualJointKinSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualSphericalJointKinSensor(const sensor::SensorName name) const override; + + inline ElementPtr + getActuator(const actuator::ActuatorName name) const override; + + inline VectorOfElementPtr + getActuators(const actuator::ActuatorType type) const override; + + inline ElementPtr + getHapticActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getMotorActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getHeaterActuator(const actuator::ActuatorName) const override; +}; + +inline wearable::ElementPtr +wearable::devices::IAnalogSensorToIWear::getActuator(const actuator::ActuatorName name) const +{ + return nullptr; +} + +inline wearable::VectorOfElementPtr +wearable::devices::IAnalogSensorToIWear::getActuators(const actuator::ActuatorType type) const +{ + return {}; +} + +inline wearable::ElementPtr +wearable::devices::IAnalogSensorToIWear::getHapticActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::IAnalogSensorToIWear::getMotorActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::IAnalogSensorToIWear::getHeaterActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +#endif // WEARABLE_IANALOGSENSORTOIWEAR diff --git a/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp new file mode 100644 index 000000000..aa11c4f7d --- /dev/null +++ b/devices/IAnalogSensorToIWear/src/IAnalogSensorToIWear.cpp @@ -0,0 +1,711 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "IAnalogSensorToIWear.h" + +#include +#include +#include +#include + +#include +#include +#include + +const std::string DeviceName = "IAnalogSensorToIWear"; +const std::string LogPrefix = DeviceName + " :"; + +using namespace wearable; +using namespace wearable::sensor; +using namespace wearable::devices; + +// Class that stores the IAnalogSensor interface and provides utilites to +// map its data to containers compatible with IWear +class IAnalogSensorHandler +{ +public: + yarp::sig::Vector buffer; + yarp::dev::IAnalogSensor* interface = nullptr; + + bool readData() + { + if (!interface) { + yError() << LogPrefix << "Failed to read data. Interface is nullptr."; + return false; + } + + if (interface->read(buffer) != yarp::dev::IAnalogSensor::AS_OK) { + yError() << LogPrefix << "Failed to read data from the IAnalogSensor interface." + << "Sensor read status is" << interface->read(buffer); + return false; + } + + return true; + } + + wearable::sensor::SensorStatus getStatus() + { + if (!interface) { + yError() << LogPrefix << "Failed to get status of IAnalogSensor"; + return SensorStatus::Unknown; + } + + SensorStatus status = SensorStatus::Ok; + + // Logic for combining the status of all channels. + // The tricky part is deciding how to handle the mixed case of timeout and overflow. + // TODO: For now, overflow is stronger. + for (int i = 0; i < interface->getChannels(); ++i) { + switch (interface->getState(i)) { + case yarp::dev::IAnalogSensor::AS_ERROR: + // If even just one sensor is AS_ERROR, return Error + return SensorStatus::Error; + case yarp::dev::IAnalogSensor::AS_OVF: + status = SensorStatus::Overflow; + break; + case yarp::dev::IAnalogSensor::AS_TIMEOUT: + if (status == SensorStatus::Overflow) { + break; + } + status = SensorStatus::Timeout; + break; + case yarp::dev::IAnalogSensor::AS_OK: + // Keep checking other channels + break; + } + } + + return status; + } + + bool getData(double& value, const size_t offset = 0) + { + if (buffer.size() != 1) { + yError() << LogPrefix << "Size mismatch of the data read from IAnalogSensor interface." + << "The buffer was initialized with a size of" << buffer.size() + << "but the wearable sensor exposes" << 1 << "value"; + return false; + } + + value = buffer[offset]; + return true; + } + + bool getData(std::array& array, const size_t offset = 0) + { + if (buffer.size() < 3 + offset) { + yError() << LogPrefix << "Size mismatch of the data read from IAnalogSensor interface." + << "The buffer was initialized with a size of" << buffer.size() + << "but the wearable sensor exposes" << 3 << "values"; + return false; + } + + std::copy(buffer.data() + offset, buffer.data() + offset + 3, array.data()); + return true; + } + + bool getData(std::array& array, const size_t offset = 0) + { + if (buffer.size() < 4 + offset) { + yError() << LogPrefix << "Size mismatch of the data read from IAnalogSensor interface." + << "The buffer was initialized with a size of" << buffer.size() + << "but the wearable sensor exposes" << 4 << "values"; + return false; + } + + 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; + } +}; + +struct ParsedOptions +{ + wearable::WearableName wearableName; + + wearable::sensor::SensorName sensorName; + wearable::sensor::SensorType wearableSensorType; + + size_t numberOfChannels; + size_t channelOffset; + + bool getGroundReactionFT; +}; + +class IAnalogSensorToIWear::Impl +{ +public: + bool firstRun = true; + mutable std::recursive_mutex mutex; + + TimeStamp timestamp; + ParsedOptions options; + + std::unique_ptr network = nullptr; + + wearable::SensorPtr iSensor; + + bool allocateSensor(const wearable::sensor::SensorType type, + const wearable::sensor::SensorName name, + IAnalogSensorHandler handler); +}; + +// ================================ +// WEARABLE SENSORS IMPLEMENTATIONS +// ================================ + +class ForceTorque6DSensor : public IForceTorque6DSensor +{ +public: + unsigned offset = 0; + bool groundReactionFT; + IAnalogSensorHandler handler; + + ForceTorque6DSensor(SensorName name, + IAnalogSensorHandler analogSensorHandler, + SensorStatus status = SensorStatus::Unknown) + : IForceTorque6DSensor(name, status) + , handler(analogSensorHandler) + {} + + void setStatus(const SensorStatus status) { m_status = status; } + + bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const override + { + // Dirty workaround to set the status from a const method and call non-const methods of the + // handler + auto nonConstThis = const_cast(this); + bool dataOk = nonConstThis->handler.readData(); + nonConstThis->setStatus(nonConstThis->handler.getStatus()); + + // 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. + bool ok = dataOk && nonConstThis->handler.getData(force3D, offset) + && nonConstThis->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; + } +}; + +class Force3DSensor : public IForce3DSensor +{ +public: + unsigned offset = 0; + IAnalogSensorHandler handler; + + Force3DSensor(SensorName name, + IAnalogSensorHandler analogSensorHandler, + SensorStatus status = SensorStatus::Unknown) + : IForce3DSensor(name, status) + , handler(analogSensorHandler) + {} + + void setStatus(const SensorStatus status) { m_status = status; } + + bool getForce3D(Vector3& force3D) const override + { + // Dirty workaround to set the status from a const method and call non-const methods of the + // handler + auto nonConstThis = const_cast(this); + bool dataOk = nonConstThis->handler.readData(); + nonConstThis->setStatus(nonConstThis->handler.getStatus()); + + return dataOk && nonConstThis->handler.getData(force3D, offset); + } +}; + +class Torque3DSensor : public ITorque3DSensor +{ +public: + unsigned offset = 0; + IAnalogSensorHandler handler; + + Torque3DSensor(SensorName name, + IAnalogSensorHandler analogSensorHandler, + SensorStatus status = SensorStatus::Unknown) + : ITorque3DSensor(name, status) + , handler(analogSensorHandler) + {} + + void setStatus(const SensorStatus status) { m_status = status; } + + bool getTorque3D(Vector3& torque3D) const override + { + // Dirty workaround to set the status from a const method and call non-const methods of the + // handler + auto nonConstThis = const_cast(this); + bool dataOk = nonConstThis->handler.readData(); + nonConstThis->setStatus(nonConstThis->handler.getStatus()); + + return dataOk && nonConstThis->handler.getData(torque3D, offset); + } +}; + +class TemperatureSensor : public ITemperatureSensor +{ +public: + unsigned offset = 0; + IAnalogSensorHandler handler; + + TemperatureSensor(SensorName name, + IAnalogSensorHandler analogSensorHandler, + SensorStatus status = SensorStatus::Unknown) + : ITemperatureSensor(name, status) + , handler(analogSensorHandler) + {} + + void setStatus(const SensorStatus status) { m_status = status; } + + bool getTemperature(double& temperature) const override + { + // Dirty workaround to set the status from a const method and call non-const methods of the + // handler and call non-const methods of the handler + auto nonConstThis = const_cast(this); + bool dataOk = nonConstThis->handler.readData(); + nonConstThis->setStatus(nonConstThis->handler.getStatus()); + + return dataOk && nonConstThis->handler.getData(temperature, offset); + } +}; + +class SkinSensor : public ISkinSensor +{ +public: + unsigned offset = 0; + IAnalogSensorHandler handler; + + SkinSensor(SensorName name, + IAnalogSensorHandler analogSensorHandler, + SensorStatus status = SensorStatus::Unknown) + : ISkinSensor(name, status) + , handler(analogSensorHandler) + {} + + void setStatus(const SensorStatus status) { m_status = status; } + + bool getPressure(std::vector& pressure) const override + { + // Dirty workaround to set the status from a const method and call non-const methods of the + // handler + auto nonConstThis = const_cast(this); + bool dataOk = nonConstThis->handler.readData(); + nonConstThis->setStatus(nonConstThis->handler.getStatus()); + + return dataOk && nonConstThis->handler.getData(pressure, offset); + } +}; + +// TODO: implement all the other sensors + +// ==================== +// IANALOGSENSORTOIWEAR +// ==================== + +IAnalogSensorToIWear::IAnalogSensorToIWear() + : pImpl{new Impl()} +{} + +// Without this destructor here, the linker complains for +// undefined reference to vtable +IAnalogSensorToIWear::~IAnalogSensorToIWear() = default; + +bool IAnalogSensorToIWear::open(yarp::os::Searchable& config) +{ + // =============================== + // CHECK THE CONFIGURATION OPTIONS + // =============================== + + if (!(config.check("sensorName") && config.find("sensorName").isString())) { + yError() << LogPrefix << "Parameter 'sensorName' missing or invalid"; + return false; + } + + if (!(config.check("wearableName") && config.find("wearableName").isString())) { + yError() << LogPrefix << "Parameter 'wearableName' missing or invalid"; + return false; + } + + if (!(config.check("numberOfChannels") && config.find("numberOfChannels").isInt32())) { + yError() << LogPrefix << "Parameter 'numberOfChannels' missing or invalid"; + return false; + } + + if (!(config.check("channelOffset") && config.find("channelOffset").isInt32())) { + yError() << LogPrefix << "Parameter 'channelOffset' missing or invalid"; + return false; + } + + if (!(config.check("wearableSensorType") && config.find("wearableSensorType").isString())) { + yError() << LogPrefix << "Parameter 'wearableSensorType' missing or invalid"; + return false; + } + + if (!(config.check("getGroundReactionFT") && config.find("getGroundReactionFT").isBool())) { + yError() << LogPrefix << "Parameter 'getGroundReactionFT' missing or invalid"; + return false; + } + + // =============== + // READ PARAMETERS + // =============== + + pImpl->options.sensorName = config.find("sensorName").asString(); + pImpl->options.wearableName = config.find("wearableName").asString(); + pImpl->options.numberOfChannels = config.find("numberOfChannels").asInt32(); + pImpl->options.channelOffset = config.find("channelOffset").asInt32(); + std::string sensorType = config.find("wearableSensorType").asString(); + pImpl->options.getGroundReactionFT = config.find("getGroundReactionFT").asBool(); + pImpl->options.wearableSensorType = wearable::sensor::sensorTypeFromString(sensorType); + + yInfo() << LogPrefix << "*** ===================="; + yInfo() << LogPrefix << "*** Sensor name :" << pImpl->options.sensorName; + yInfo() << LogPrefix << "*** Sensor Type :" << sensorType; + 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 << "*** ===================="; + + // ================================= + // CHECK YARP NETWORK INITIALIZATION + // ================================= + + pImpl->network = std::make_unique(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + yError() << LogPrefix << "YARP server wasn't found active."; + return false; + } + + return true; +} + +bool IAnalogSensorToIWear::close() +{ + detach(); + pImpl->iSensor.reset(); + return true; +} + +yarp::os::Stamp IAnalogSensorToIWear::getLastInputStamp() +{ + std::lock_guard lock(pImpl->mutex); + return yarp::os::Stamp(pImpl->timestamp.sequenceNumber, yarp::os::Time::now()); +} + +bool IAnalogSensorToIWear::Impl::allocateSensor(const wearable::sensor::SensorType type, + const wearable::sensor::SensorName name, + IAnalogSensorHandler handler) +{ + // The sensors are initialized as Ok in order to trigger the first data read. + // If there is any error during the first read, the sensor updates its own status + // that is then propagated to the global IWear status. + switch (type) { + case wearable::sensor::SensorType::Force3DSensor: { + auto sensor = std::make_shared(name, handler, SensorStatus::Ok); + sensor->offset = options.channelOffset; + iSensor = std::dynamic_pointer_cast(sensor); + break; + } + case wearable::sensor::SensorType::ForceTorque6DSensor: { + auto sensor = std::make_shared(name, handler, SensorStatus::Ok); + sensor->offset = options.channelOffset; + sensor->groundReactionFT = options.getGroundReactionFT; + iSensor = std::dynamic_pointer_cast(sensor); + break; + } + case wearable::sensor::SensorType::TemperatureSensor: { + auto sensor = std::make_shared(name, handler, SensorStatus::Ok); + sensor->offset = options.channelOffset; + iSensor = std::dynamic_pointer_cast(sensor); + break; + } + case wearable::sensor::SensorType::Torque3DSensor: { + auto sensor = std::make_shared(name, handler, SensorStatus::Ok); + sensor->offset = options.channelOffset; + iSensor = std::dynamic_pointer_cast(sensor); + break; + } + case wearable::sensor::SensorType::SkinSensor: { + auto sensor = std::make_shared(name, handler, SensorStatus::Ok); + sensor->offset = options.channelOffset; + iSensor = std::dynamic_pointer_cast(sensor); + break; + } + default: + // TODO: implement the remaining sensors + return false; + } + + return true; +} + +bool IAnalogSensorToIWear::attach(yarp::dev::PolyDriver* poly) +{ + IAnalogSensorHandler handler; + handler.buffer.resize(pImpl->options.numberOfChannels); + + if (!poly) { + yError() << LogPrefix << "Passed PolyDriver is nullptr"; + return false; + } + + if (!(poly->view(handler.interface) && handler.interface)) { + yError() << LogPrefix << "Failed to view the IAnalogSensor interface from the PolyDriver"; + return false; + } + + // =================== + // CHECK THE INTERFACE + // =================== + + if (handler.interface->getChannels() == 0) { + yError() << LogPrefix << "The number of channels is 0"; + return false; + } + + if (handler.interface->getChannels() + != pImpl->options.numberOfChannels + pImpl->options.channelOffset) { + yError() << LogPrefix << "The number of sensor channels (" + << handler.interface->getChannels() + << ") is different than the number specified in the options plus the offset (" + << pImpl->options.numberOfChannels + pImpl->options.channelOffset << ")"; + return false; + } + + for (unsigned i = 0; i < handler.interface->getChannels(); ++i) { + if (handler.interface->getState(i) != yarp::dev::IAnalogSensor::AS_OK) { + yError() << LogPrefix << "The status of IAnalogSensor interface for channel" << i + << "is not AS_OK (" << handler.interface->getState(i) << ")"; + return false; + } + } + + if (!pImpl->allocateSensor( + pImpl->options.wearableSensorType, pImpl->options.sensorName, handler)) { + yError() << LogPrefix << "Failed to allocate a new sensor of the specified type"; + return false; + } + + // Notify that the sensor is ready to be used + pImpl->firstRun = false; + + return true; +} + +bool IAnalogSensorToIWear::detach() +{ + return true; +} + +bool IAnalogSensorToIWear::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + if (driverList.size() > 1) { + yError() << LogPrefix << "This wrapper accepts only one attached PolyDriver"; + return false; + } + + const yarp::dev::PolyDriverDescriptor* driver = driverList[0]; + + if (!driver) { + yError() << LogPrefix << "Passed PolyDriverDescriptor is nullptr"; + return false; + } + + return attach(driver->poly); +} + +bool IAnalogSensorToIWear::detachAll() +{ + return detach(); +} + +wearable::SensorPtr IAnalogSensorToIWear::getSensor(const SensorName name) const +{ + // This device can provide only one sensor. Check if the name matches. + if (!(pImpl->iSensor && (pImpl->iSensor->getSensorName() == name))) { + yError() << LogPrefix << "Failed to get sensor" << name; + return nullptr; + } + + return pImpl->iSensor; +} + +wearable::VectorOfSensorPtr +IAnalogSensorToIWear::getSensors(const SensorType type) const +{ + wearable::VectorOfSensorPtr vector; + + if (pImpl->options.wearableSensorType == type) { + vector.push_back(getSensor(pImpl->options.sensorName)); + } + + return vector; +} + +wearable::WearableName IAnalogSensorToIWear::getWearableName() const +{ + return pImpl->options.wearableName + wearable::Separator; +} + +wearable::WearStatus IAnalogSensorToIWear::getStatus() const +{ + // This is necessary if something that uses the exposed IWear interface asks the status + // before IAnalogSensor is attached + if (pImpl->firstRun) { + return WearStatus::WaitingForFirstRead; + } + + if (!pImpl->iSensor) { + yError() << LogPrefix << "The stored ISensor has not been yet allocated"; + return WearStatus::Error; + } + + return pImpl->iSensor->getSensorStatus(); +} + +wearable::TimeStamp IAnalogSensorToIWear::getTimeStamp() const +{ + std::lock_guard lock(pImpl->mutex); + + pImpl->timestamp.sequenceNumber = 0; // Always zero + pImpl->timestamp.time = yarp::os::Time::now(); + + return pImpl->timestamp; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getAccelerometer(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getForce3DSensor(const wearable::sensor::SensorName name) const +{ + 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 +IAnalogSensorToIWear::getForceTorque6DSensor(const wearable::sensor::SensorName name) const +{ + 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 +IAnalogSensorToIWear::getGyroscope(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getMagnetometer(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getOrientationSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getTemperatureSensor(const wearable::sensor::SensorName name) const +{ + 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 +IAnalogSensorToIWear::getTorque3DSensor(const wearable::sensor::SensorName name) const +{ + 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 +IAnalogSensorToIWear::getEmgSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getFreeBodyAccelerationSensor( + const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getPoseSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getPositionSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getSkinSensor(const wearable::sensor::SensorName name) const +{ + 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 +IAnalogSensorToIWear::getVirtualLinkKinSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getVirtualJointKinSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IAnalogSensorToIWear::getVirtualSphericalJointKinSensor( + const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} diff --git a/devices/ICub/CMakeLists.txt b/devices/ICub/CMakeLists.txt new file mode 100644 index 000000000..38d55606c --- /dev/null +++ b/devices/ICub/CMakeLists.txt @@ -0,0 +1,33 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(icub_wearable_device + TYPE wearable::devices::ICub + INCLUDE 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 + IWear::IWear + YARP::YARP_dev + YARP::YARP_init + PRIVATE + iDynTree::idyntree-model + iDynTree::idyntree-high-level) + +yarp_install( + TARGETS ICub + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_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 000000000..9fd2873b9 --- /dev/null +++ b/devices/ICub/conf/ICub.xml @@ -0,0 +1,30 @@ + + + + model.urdf + root_link + + /wholeBodyDynamics/left_arm/endEffectorWrench:o + /wholeBodyDynamics/right_arm/endEffectorWrench:o + /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o + /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o + + + (head torso left_arm right_arm left_leg right_leg) + /icubSim + /ICubWearableRemoteClient + + + + + 0.01 + /ICub/WearableData/data:o + /ICub/WearableData/metadataRpc:o + + + ICubWearableDevice + + + + + diff --git a/devices/ICub/include/ICub.h b/devices/ICub/include/ICub.h new file mode 100644 index 000000000..bdf282313 --- /dev/null +++ b/devices/ICub/include/ICub.h @@ -0,0 +1,245 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_DEVICES_ICUB_H +#define WEARABLE_DEVICES_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; + + SensorPtr + getVirtualJointKinSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualLinkKinSensor(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 + 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 ElementPtr + getActuator(const actuator::ActuatorName name) const override; + + inline VectorOfElementPtr + getActuators(const actuator::ActuatorType type) const override; + + inline ElementPtr + getHapticActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getMotorActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getHeaterActuator(const actuator::ActuatorName) 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::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; +} + +inline wearable::ElementPtr +wearable::devices::ICub::getActuator(const actuator::ActuatorName name) const +{ + return nullptr; +} + +inline wearable::VectorOfElementPtr +wearable::devices::ICub::getActuators(const actuator::ActuatorType type) const +{ + return {}; +} + +inline wearable::ElementPtr +wearable::devices::ICub::getHapticActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::ICub::getMotorActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::ICub::getHeaterActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +#endif // WEARABLE_DEVICES_ICUB_H diff --git a/devices/ICub/src/ICub.cpp b/devices/ICub/src/ICub.cpp new file mode 100644 index 000000000..8f3bc4cd5 --- /dev/null +++ b/devices/ICub/src/ICub.cpp @@ -0,0 +1,1057 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "ICub.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#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; + yarp::os::Network yarpNetwork; + + 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).asFloat64(); + } + + if (this->firstRun) { + this->firstRun = false; + } + } + else { + yWarning() << LogPrefix << "[WrenchPort] read an invalid wrench bottle"; + } + } +}; + +class ICub::ICubImpl +{ +public: + + mutable std::mutex mtx; + + // Generic + const WearableName wearableName = "ICub"; + + //Gravity variable + iDynTree::Vector3 world_gravity; + + // model + iDynTree::Model robotModel; + std::string floatingBaseName; + + // kindyn object + iDynTree::KinDynComputations kinDynComputations; + + bool updatedLinkQuantities; + void computeLinkQuantities(); + + size_t nSensors; + std::vector sensorNames; + + wearable::TimeStamp timeStamp; + + // FT Sensor + class ICubForceTorque6DSensor; + + std::vector ftSensorNames; + std::vector ftSensorPortNames; + std::vector wrenchPortsVector; + std::map> ftSensorsMap; + + // Joint Sensor + class ICubVirtualJointKinSensor; + + std::vector jointSensorNames; + std::map> jointSensorsMap; + + // Vector of motor control boards + yarp::os::Property options; + std::vector remoteControlBoardsVec; + std::vector controlBoardNamesVec; + std::vector controlBoardJointsVec; + + yarp::dev::IEncoders* iEncoders = nullptr; + yarp::dev::IAxisInfo* iAxisInfo = nullptr; + + // Virtual Link Sensor + class ICubVirtualLinkKinSensor; + + std::vector linkSensorNames; + std::map> linkSensorsMap; + + // Joint variables + iDynTree::VectorDynSize jointPositionsVec; + iDynTree::VectorDynSize jointVelocitiesVec; + iDynTree::VectorDynSize jointAccelerationsVec; + + // Link variables + std::map linkPositionMap; + std::map linkOrientationMap; + std::map linkLinearVelocityMap; + std::map linkAngularVelocityMap; + std::map linkLinearAccelerationMap; + std::map linkAngularAccelerationMap; +}; + +// ========================================== +// 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 + for (size_t n = 0; n < icubImpl->ftSensorNames.size(); n++) { + if (this->m_name.find(icubImpl->ftSensorNames.at(n))) { + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); + break; + } + } + } + + ~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 + for (size_t n = 0; n < icubImpl->ftSensorNames.size(); n++) { + if (this->m_name.find(icubImpl->ftSensorNames.at(n))) { + std::lock_guard lck(icubImpl->wrenchPortsVector.at(n)->mtx); + wrench = icubImpl->wrenchPortsVector.at(n)->wrenchValues; + icubImpl->timeStamp.time = yarp::os::Time::now(); + break; + } + } + + // 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 implementation of ICubVirtualJointKinSensor +// ================================================ +class ICub::ICubImpl::ICubVirtualJointKinSensor : public wearable::sensor::IVirtualJointKinSensor +{ +public: + ICub::ICubImpl* icubImpl = nullptr; + + yarp::dev::IEncoders* jointIEncoders = nullptr; + int jointIndex; + + // Joint Variables + double jointPos; + double jointVel; + double jointAcc; + + // ------------------------ + // Constructor / Destructor + // ------------------------ + ICubVirtualJointKinSensor( + ICub::ICubImpl* impl, + const wearable::sensor::SensorName name = {}, + const wearable::sensor::SensorStatus status = + wearable::sensor::SensorStatus::WaitingForFirstRead) // Default sensor status + : IVirtualJointKinSensor(name, status) + , icubImpl(impl) + { + // Get the joint index corresponding to the sensor and the encoders pointer + size_t boardCount = 0; + yarp::dev::IEncoders* m_iEncoders = nullptr; + yarp::dev::IAxisInfo* m_iAxisInfo = nullptr; + + for (const auto& controlBoard : icubImpl->controlBoardNamesVec) { + // Get encoder interface + if (!icubImpl->remoteControlBoardsVec.at(boardCount)->view(m_iEncoders) + || !m_iEncoders) { + yError() << LogPrefix << "Failed to view the IEncoder interface from the " + << controlBoard << " remote control board device"; + } + + // Get axis info interface + if (!icubImpl->remoteControlBoardsVec.at(boardCount)->view(m_iAxisInfo) + || !m_iAxisInfo) { + yError() << LogPrefix << "Failed to view the IAxisInfo interface from the " + << controlBoard << " remote control board device"; + } + + // Get joint axes from encoder interface + int nJoints; + m_iEncoders->getAxes(&nJoints); + + // Get axes names + for (int j = 0; j < nJoints; j++) { + std::string axisName; + m_iAxisInfo->getAxisName(j, axisName); + // Check if the joint name is equal to sensor name + if (this->m_name + == icubImpl->wearableName + wearable::Separator + + sensor::IVirtualJointKinSensor::getPrefix() + axisName) { + this->jointIndex = j; + this->jointIEncoders = m_iEncoders; + break; + } + } + + boardCount++; + } + + // Set the sensor status Ok after receiving the first data from joint encoder + if (this->jointIEncoders->getEncoder(this->jointIndex, &this->jointPos) + && this->jointIEncoders->getEncoderSpeed(this->jointIndex, &this->jointVel) + && this->jointIEncoders->getEncoderAcceleration(this->jointIndex, &this->jointAcc)) { + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); + } + } + + ~ICubVirtualJointKinSensor() override = default; + + void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } + + // ==================================== + // ICubVirtualJointKinSensor interfaces + // ==================================== + // ICub joint quantities are in radians + bool getJointPosition(double& position) const override + { + assert(icubImpl != nullptr); + + // Reading joint position + if (!this->jointIEncoders->getEncoder(this->jointIndex, &position)) { + return false; + } + + return true; + } + + bool getJointVelocity(double& velocity) const override + { + assert(icubImpl != nullptr); + + // Reading joint velocity + if (!this->jointIEncoders->getEncoderSpeed(this->jointIndex, &velocity)) { + return false; + } + + return true; + } + + bool getJointAcceleration(double& acceleration) const override + { + assert(icubImpl != nullptr); + + // Reading joint acceleration + if (!this->jointIEncoders->getEncoderAcceleration(this->jointIndex, &acceleration)) { + return false; + } + + return true; + } +}; + +void ICub::ICubImpl::computeLinkQuantities() +{ + size_t boardCount = 0; + int jointIndex = 0; + yarp::dev::IEncoders* iEncoders = nullptr; + yarp::dev::IAxisInfo* iAxisInfo = nullptr; + + // Clear joint buffers + this->jointPositionsVec.zero(); + this->jointVelocitiesVec.zero(); + this->jointAccelerationsVec.zero(); + + for (const auto& controlBoard : this->controlBoardNamesVec) { + // Get encoder interface + if (!this->remoteControlBoardsVec.at(boardCount)->view(iEncoders) + || !iEncoders) { + yError() << LogPrefix << "Failed to view the IEncoder interface from the " + << controlBoard << " remote control board device"; + } + + // Get axis info interface + if (!this->remoteControlBoardsVec.at(boardCount)->view(iAxisInfo) + || !iAxisInfo) { + yError() << LogPrefix << "Failed to view the IAxisInfo interface from the " + << controlBoard << " remote control board device"; + } + + // Get joint axes from encoder interface + int nJoints; + iEncoders->getAxes(&nJoints); + + // Get axes names + for (int j = 0; j < nJoints; j++) { + std::string axisName; + iAxisInfo->getAxisName(j, axisName); + + // Store the joint quantities + double position; + double velocity; + double acceleration; + + // Get joint quantities + iEncoders->getEncoder(j, &position); + iEncoders->getEncoderSpeed(j, &velocity); + iEncoders->getEncoderAcceleration(j, &acceleration); + + if (this->robotModel.getJointIndex(axisName) != iDynTree::JOINT_INVALID_INDEX) { + + // Update the joint buffers + this->jointPositionsVec.setVal(jointIndex, position*(M_PI/180)); + this->jointVelocitiesVec.setVal(jointIndex, velocity*(M_PI/180)); + this->jointAccelerationsVec.setVal(jointIndex, acceleration*(M_PI/180)); + + jointIndex++; + + } + + } + + boardCount++; + } + + // Set floating base + this->kinDynComputations.setFloatingBase(this->floatingBaseName); + + // Set robot state + this->kinDynComputations.setRobotState(iDynTree::Transform::Identity(), + this->jointPositionsVec, + iDynTree::Twist::Zero(), + this->jointVelocitiesVec, + this->world_gravity); + + // Set frame velocity representation + // TODO: Double check this representation + this->kinDynComputations.setFrameVelocityRepresentation(iDynTree::INERTIAL_FIXED_REPRESENTATION); + + // Clear link variables + this->linkPositionMap.clear(); + this->linkOrientationMap.clear(); + this->linkLinearVelocityMap.clear(); + this->linkAngularVelocityMap.clear(); + + { + std::lock_guard lock(this->mtx); + for (size_t l = 0; l < this->robotModel.getNrOfLinks(); l++) + { + // Get link name + std::string linkName = this->robotModel.getFrameName(l); + // Get link pose + iDynTree::Transform linkTransform; + linkTransform = this->kinDynComputations.getRelativeTransform(this->floatingBaseName, + linkName); + + iDynTree::Position position = linkTransform.getPosition(); + iDynTree::Vector4 orientation = linkTransform.getRotation().asQuaternion(); // format: w x y z + + wearable::Vector3 linkPosition; + linkPosition.at(0) = position.getVal(0); + linkPosition.at(1) = position.getVal(1); + linkPosition.at(2) = position.getVal(2); + + this->linkPositionMap.emplace(linkName, linkPosition); + + wearable::Quaternion linkOrientation; + //TODO: Double check quaternion handling + linkOrientation.at(0) = orientation.getVal(0); + linkOrientation.at(1) = orientation.getVal(1); + linkOrientation.at(2) = orientation.getVal(2); + linkOrientation.at(3) = orientation.getVal(3); + + this->linkOrientationMap.emplace(linkName, linkOrientation); + + // Get link velocity + iDynTree::Twist linkVelocity; + linkVelocity = this->kinDynComputations.getFrameVel(linkName); + + iDynTree::Vector3 linearVelocity = linkVelocity.getLinearVec3(); + iDynTree::Vector3 angularVelocity = linkVelocity.getAngularVec3(); + + wearable::Vector3 linkLinearVelocity; + linkLinearVelocity.at(0) = linearVelocity.getVal(0); + linkLinearVelocity.at(1) = linearVelocity.getVal(1); + linkLinearVelocity.at(2) = linearVelocity.getVal(2); + + wearable::Vector3 linkAngularVelocity; + linkAngularVelocity.at(0) = angularVelocity.getVal(0); + linkAngularVelocity.at(1) = angularVelocity.getVal(1); + linkAngularVelocity.at(2) = angularVelocity.getVal(2); + + this->linkLinearVelocityMap.emplace(linkName, linkLinearVelocity); + this->linkAngularVelocityMap.emplace(linkName, linkAngularVelocity); + + // Get link acceleration + //TODO + wearable::Vector3 linkLinearAcceleration; + wearable::Vector3 linkAngularAcceleration; + + linkLinearAcceleration.fill(0.0); + linkAngularAcceleration.fill(0.0); + + this->linkLinearAccelerationMap.emplace(linkName, linkLinearAcceleration); + this->linkAngularAccelerationMap.emplace(linkName, linkAngularAcceleration); + } + } + +} + +// =========================================== +// ICub implementation of VirtualLinkKinsensor +// =========================================== + +class ICub::ICubImpl::ICubVirtualLinkKinSensor : public wearable::sensor::IVirtualLinkKinSensor +{ +public: + ICub::ICubImpl* icubImpl = nullptr; + + // Constructor + ICubVirtualLinkKinSensor( + ICub::ICubImpl* impl, + const wearable::sensor::SensorName name = {}, + const wearable::sensor::SensorStatus status = + wearable::sensor::SensorStatus::WaitingForFirstRead) // Default sensor status + : IVirtualLinkKinSensor(name, status) + , icubImpl(impl) + { + //TODO: Check if any other sensor checks can be used + auto nonConstThis = const_cast(this); + nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); + } + + // Destructor + ~ICubVirtualLinkKinSensor() override = default; + + // ------------------------------- + // IVirtualLinkKinSensor interface + // ------------------------------- + + bool getLinkPose(Vector3& position, Quaternion& orientation) const override + { + if (icubImpl->linkSensorsMap.find(this->m_name) + == icubImpl->linkSensorsMap.end()) { + yError() << LogPrefix << "Sensor" << this->m_name << "NOT found"; + position.fill(0.0); + orientation.fill(0.0); + return false; + } + + // Call link quantities computation + //TODO: This call has to be handled differently + icubImpl->computeLinkQuantities(); + + // Get link name + size_t found = this->m_name.find_last_of(":"); + std::string linkName = this->m_name.substr(found+1); + + { + // Get link position + std::lock_guard lock(icubImpl->mtx); + if (icubImpl->linkPositionMap.find(linkName) == icubImpl->linkPositionMap.end()) { + yError() << LogPrefix << "Link " << linkName << "not found in link position map"; + position.fill(0.0); + return false; + } + + auto linkPos = icubImpl->linkPositionMap.find(linkName); + position = linkPos->second; + + // Get link orientation + if (icubImpl->linkOrientationMap.find(linkName) == icubImpl->linkOrientationMap.end()) { + yError() << LogPrefix << "Link " << linkName << "not found in link orientation map"; + orientation.fill(0.0); + return false; + } + + auto linkOri = icubImpl->linkOrientationMap.find(linkName); + orientation = linkOri->second; + } + + return true; + } + + bool getLinkVelocity(Vector3& linear, Vector3& angular) const override + { + if (icubImpl->linkSensorsMap.find(this->m_name) + == icubImpl->linkSensorsMap.end()) { + yError() << LogPrefix << "Sensor" << this->m_name << "NOT found"; + linear.fill(0.0); + angular.fill(0.0); + return false; + } + + // Call link quantities computation + icubImpl->computeLinkQuantities(); + + // Get link name + size_t found = this->m_name.find_last_of(":"); + std::string linkName = this->m_name.substr(found+1); + + { + std::lock_guard lock(icubImpl->mtx); + if (icubImpl->linkLinearVelocityMap.find(linkName) == icubImpl->linkLinearVelocityMap.end()) { + yError() << LogPrefix << "Link " << linkName << "not found in link linear velocity map"; + linear.fill(0.0); + return false; + } + + auto linearVel = icubImpl->linkLinearVelocityMap.find(linkName); + linear = linearVel->second; + + if (icubImpl->linkAngularVelocityMap.find(linkName) == icubImpl->linkAngularVelocityMap.end()) { + yError() << LogPrefix << "Link " << linkName << "not found in link angular velocity map"; + angular.fill(0.0); + return false; + } + + auto angularVel = icubImpl->linkAngularVelocityMap.find(linkName); + angular = angularVel->second; + } + + return true; + } + + bool getLinkAcceleration(Vector3& linear, Vector3& angular) const override + { + if (icubImpl->linkSensorsMap.find(this->m_name) + == icubImpl->linkSensorsMap.end()) { + yError() << LogPrefix << "Sensor" << this->m_name << "NOT found"; + linear.fill(0.0); + angular.fill(0.0); + return false; + } + + //TODO: Do a dummy implementation later + linear.fill(0.0); + angular.fill(0.0); + return true; + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } +}; + +// ========================================== +// 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"; + + // Set gravity vector + pImpl->world_gravity.zero(); + pImpl->world_gravity.setVal(2, -9.81); + + // =============================== + // CHECK THE CONFIGURATION OPTIONS + // =============================== + + if (!(config.check("urdf") && config.find("urdf").isString())) { + yError() << LogPrefix << "urdf option not found or not valid"; + return false; + } + + if (!(config.check("floatingBase") && config.find("floatingBase").isString())) { + yError() << LogPrefix << "floatingBase option not found or not valid"; + return false; + } + + yarp::os::Bottle wbdHandsFTSensorsGroup = config.findGroup("ft-sensors"); + if (wbdHandsFTSensorsGroup.isNull()) { + yError() << LogPrefix << "REQUIRED parameter NOT found"; + return false; + } + + yarp::os::Bottle icubJointSensorGroup = config.findGroup("joint-sensors"); + if (icubJointSensorGroup.isNull()) { + yError() << LogPrefix << "REQUIRED parameter NOT found"; + return false; + } + + if (!icubJointSensorGroup.check("controlBoardsList")) { + yError() << LogPrefix << "REQUIRED parameter NOT found"; + return false; + } + + if (!icubJointSensorGroup.check("remotePrefix") || !icubJointSensorGroup.check("localPrefix")) { + yError() << LogPrefix << "REQUIRED parameter or NOT found"; + return false; + } + + // ========================== + // INITIALIZE THE ROBOT MODEL + // ========================== + + const std::string urdfFileName = config.find("urdf").asString(); + + auto& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + std::string urdfFilePath = rf.findFile(urdfFileName); + if (urdfFilePath.empty()) { + yError() << LogPrefix << "Failed to find file" << config.find("urdf").asString(); + return false; + } + + iDynTree::ModelLoader modelLoader; + if (!modelLoader.loadModelFromFile(urdfFilePath) || !modelLoader.isValid()) { + yError() << LogPrefix << "Failed to load model" << urdfFilePath; + return false; + } + + pImpl->floatingBaseName = config.find("floatingBase").asString(); + + // Get the model from the loader + pImpl->robotModel = modelLoader.model(); + + // Set the model to kindyn object + pImpl->kinDynComputations.loadRobotModel(pImpl->robotModel); + + // Initialize joint quantities size + pImpl->jointPositionsVec.resize(pImpl->robotModel.getNrOfDOFs()); + pImpl->jointVelocitiesVec.resize(pImpl->robotModel.getNrOfDOFs()); + pImpl->jointAccelerationsVec.resize(pImpl->robotModel.getNrOfDOFs()); + + // ====================================== + // PARSE FT SENSORS CONFIGURATION OPTIONS + // ====================================== + + pImpl->nSensors = + wbdHandsFTSensorsGroup.size() - 1; // Set number of sensors equal to number of ft sensors + + for (size_t n = 1; n <= pImpl->nSensors; n++) { + yarp::os::Bottle* param = wbdHandsFTSensorsGroup.get(n).asList(); + pImpl->ftSensorNames.push_back(param->get(0).asString()); + pImpl->sensorNames.push_back(param->get(0).asString()); + pImpl->ftSensorPortNames.push_back(param->get(1).asString()); + } + + // ========================= + // FT Sensors Initialization + // ========================= + + std::string ft6dPrefix = + getWearableName() + wearable::Separator + sensor::IForceTorque6DSensor::getPrefix(); + + for (size_t n = 0; n < pImpl->nSensors; n++) { + + WrenchPort* wrenchPort = new WrenchPort; + + if (!(wrenchPort->open("/ICub" + pImpl->ftSensorPortNames.at(n) + ":i") + && yarp::os::Network::connect(pImpl->ftSensorPortNames.at(n), + wrenchPort->getName().c_str()))) { + yError() << LogPrefix << "Failed to open " << wrenchPort->getName().c_str() + << " port, " + "or connect to " + << pImpl->ftSensorPortNames.at(n); + return false; + } + + // Enable the callback of the ports + wrenchPort->useCallback(); + + // Wait to receive first data + while (wrenchPort->firstRun) { + yarp::os::Time::delay(1); + } + + // Push to wrench ports vector + pImpl->wrenchPortsVector.push_back(wrenchPort); + + // Create the new ft6d sensors + auto ft6d = std::make_shared( + pImpl.get(), ft6dPrefix + pImpl->ftSensorNames[n]); + + pImpl->ftSensorsMap.emplace(ft6dPrefix + pImpl->ftSensorNames[n], + SensorPtr{ft6d}); + } + + yInfo() << "check"; + + // ========================================= + // PARSE JOINT SENSORS CONFIGURATION OPTIONS + // ========================================= + + // Get control board list + int controlBoardsListIndex; + bool controlBoardsListExists = false; + for (size_t i = 0; i < icubJointSensorGroup.size(); i++) { + if (icubJointSensorGroup.get(i).isList()) { + yarp::os::Bottle* param = icubJointSensorGroup.get(i).asList(); + if (param->get(0).asString() == "controlBoardsList") { + controlBoardsListIndex = i; + controlBoardsListExists = true; + } + } + } + + if (!controlBoardsListExists) { + yError() << LogPrefix << " param is not a list"; + return false; + } + + yarp::os::Bottle* controlBoardsListParam = + icubJointSensorGroup.get(controlBoardsListIndex).asList(); + yarp::os::Bottle* controlBoardList = controlBoardsListParam->get(1).asList(); + + std::string remotePrefix = + icubJointSensorGroup.check("remotePrefix", yarp::os::Value(false)).asString(); + std::string localPrefix = + icubJointSensorGroup.check("localPrefix", yarp::os::Value(false)).asString(); + + yInfo() << LogPrefix + << "Control boards list : " << controlBoardList->toString().c_str(); + yInfo() << LogPrefix << "Remote control board prefix : " << remotePrefix; + yInfo() << LogPrefix << "Local control board prefix : " << localPrefix; + + // ============================ + // Joint Sensors Initialization + // ============================ + + // Resize control board variables + pImpl->controlBoardNamesVec.resize(controlBoardList->size()); + pImpl->controlBoardJointsVec.resize(controlBoardList->size()); + pImpl->remoteControlBoardsVec.resize(controlBoardList->size()); + + // Set control board names + for (unsigned index = 0; index < controlBoardList->size(); index++) { + std::string controlBoardName = controlBoardList->get(index).asString(); + pImpl->controlBoardNamesVec.at(index) = controlBoardName; + } + + // Initialize remote control boards + size_t boardCount = 0; + for (const auto& controlBoard : pImpl->controlBoardNamesVec) { + pImpl->options.put("device", "remote_controlboard"); + pImpl->options.put("remote", remotePrefix + "/" + controlBoard); + pImpl->options.put("local", localPrefix + "/" + controlBoard); + + // Open remote control board + pImpl->remoteControlBoardsVec.at(boardCount) = new yarp::dev::PolyDriver; + pImpl->remoteControlBoardsVec.at(boardCount)->open(pImpl->options); + + if (!pImpl->remoteControlBoardsVec.at(boardCount)->isValid()) { + yError() << LogPrefix << "Failed to open the remote control board device for " + << controlBoard << " part"; + return false; + } + + // Get encoder interface + if (!pImpl->remoteControlBoardsVec.at(boardCount)->view(pImpl->iEncoders) + || !pImpl->iEncoders) { + yError() << LogPrefix << "Failed to view the IEncoder interface from the " + << controlBoard << " remote control board device"; + return false; + } + + // Get axis info interface + if (!pImpl->remoteControlBoardsVec.at(boardCount)->view(pImpl->iAxisInfo) + || !pImpl->iAxisInfo) { + yError() << LogPrefix << "Failed to view the IAxisInfo interface from the " + << controlBoard << " remote control board device"; + return false; + } + + // Get joint axes from encoder interface + int remoteControlBoardJoints; + pImpl->iEncoders->getAxes(&remoteControlBoardJoints); + + // Get axes names and pad to sensor names + for (int j = 0; j < remoteControlBoardJoints; j++) { + std::string axisName; + pImpl->iAxisInfo->getAxisName(j, axisName); + pImpl->jointSensorNames.push_back(axisName); + pImpl->sensorNames.push_back(axisName); + } + + // Update control board joints vector + pImpl->controlBoardJointsVec.at(boardCount) = remoteControlBoardJoints; + + // Update the number of sensors + pImpl->nSensors = pImpl->nSensors + remoteControlBoardJoints; + + pImpl->options.clear(); + boardCount++; + } + + // Get joint sensor prefix + std::string jointSensorPrefix = + getWearableName() + wearable::Separator + sensor::IVirtualJointKinSensor::getPrefix(); + + for (size_t s = 0; s < pImpl->jointSensorNames.size(); s++) { + // Create the new joint sensors + auto jointsensor = std::make_shared( + pImpl.get(), jointSensorPrefix + pImpl->jointSensorNames[s]); + + pImpl->jointSensorsMap.emplace( + jointSensorPrefix + pImpl->jointSensorNames[s], + SensorPtr{jointsensor}); + } + + // ============================ + // Link Sensors Initialization + // ============================ + + // Set the size of joint variables + pImpl->jointPositionsVec.resize(pImpl->robotModel.getNrOfDOFs()); + pImpl->jointVelocitiesVec.resize(pImpl->robotModel.getNrOfDOFs()); + + // Update the number of sensors + pImpl->nSensors = pImpl->nSensors + pImpl->robotModel.getNrOfLinks(); + + // Get joint sensor prefix + std::string linkSensorPrefix = + getWearableName() + wearable::Separator + sensor::IVirtualLinkKinSensor::getPrefix(); + + for (size_t l = 0; l < pImpl->robotModel.getNrOfLinks(); l++) { + //create the new link sensors + auto linksensor = std::make_shared( + pImpl.get(), linkSensorPrefix + pImpl->robotModel.getLinkName(l)); + + pImpl->linkSensorsMap.emplace( + linkSensorPrefix + pImpl->robotModel.getLinkName(l), + SensorPtr{linksensor}); + } + + // Initialize link quantities flag + pImpl->updatedLinkQuantities = false; + + 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); +} + +// --------------------------- +// Implement 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; + outVec.reserve(pImpl->nSensors); + switch (type) { + case sensor::SensorType::ForceTorque6DSensor: { + for (const auto& ft6d : pImpl->ftSensorsMap) { + outVec.push_back(static_cast>(ft6d.second)); + } + + break; + } + case sensor::SensorType::VirtualJointKinSensor: { + for (const auto& jointSensor : pImpl->jointSensorsMap) { + outVec.push_back(static_cast>(jointSensor.second)); + } + + break; + } + case sensor::SensorType::VirtualLinkKinSensor: { + for (const auto& linkSensor : pImpl->linkSensorsMap) { + outVec.push_back(static_cast>(linkSensor.second)); + } + + break; + } + default: { + return {}; + } + } + + return outVec; +} + +// ----------- +// FT6D Sensor +// ----------- + +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 dynamic_cast&>( + *pImpl->ftSensorsMap.at(name)); +} + +// ------------ +// JOINT Sensor +// ------------ + +wearable::SensorPtr +ICub::getVirtualJointKinSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->jointSensorsMap.find(name) == pImpl->jointSensorsMap.end()) { + yError() << LogPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared point to the required sensor + return dynamic_cast&>( + *pImpl->jointSensorsMap.at(name)); +} + +// ------------ +// LINK Sensor +// ------------ + +wearable::SensorPtr +ICub::getVirtualLinkKinSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->linkSensorsMap.find(name) == pImpl->linkSensorsMap.end()) { + yError() << LogPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared point to the required sensor + return dynamic_cast&>( + *pImpl->linkSensorsMap.at(name)); +} diff --git a/devices/IFrameTransformToIWear/CMakeLists.txt b/devices/IFrameTransformToIWear/CMakeLists.txt new file mode 100644 index 000000000..7c5593da9 --- /dev/null +++ b/devices/IFrameTransformToIWear/CMakeLists.txt @@ -0,0 +1,31 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(iframetransform_to_iwear + TYPE wearable::devices::IFrameTransformToIWear + INCLUDE include/IFrameTransformToIWear.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(IFrameTransformToIWear + src/IFrameTransformToIWear.cpp + include/IFrameTransformToIWear.h) + +target_include_directories(IFrameTransformToIWear PRIVATE + $) + +target_link_libraries(IFrameTransformToIWear PUBLIC + IWear::IWear YARP::YARP_dev YARP::YARP_init) + +yarp_install( + TARGETS IFrameTransformToIWear + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) + +set (WEARABLES_XML_FILES conf/iframetransform_to_iwear.xml) +install(FILES ${WEARABLES_XML_FILES} + DESTINATION ${CMAKE_INSTALL_DATADIR}/${WEARABLES_PROJECT_NAME}) diff --git a/devices/IFrameTransformToIWear/conf/iframetransform_to_iwear.xml b/devices/IFrameTransformToIWear/conf/iframetransform_to_iwear.xml new file mode 100644 index 000000000..817c3bd6a --- /dev/null +++ b/devices/IFrameTransformToIWear/conf/iframetransform_to_iwear.xml @@ -0,0 +1,43 @@ + + + + + + 0.01 + ftc_yarp_only.xml + /IFrameTransformToIWear/tf + /IFrameTransformToIWear/tf/local_rpc + + + + + TransformServer + ground + PoseSensor + (ground /iCub/root_link_fake) + + + + TransformClient + + + + + + + + + 0.01 + /Wearable/FrameTransform/data:o + /Example/IWearWrapper/metadata:o + + + + IFrameTransformToIWear + + + + + + + diff --git a/devices/IFrameTransformToIWear/include/IFrameTransformToIWear.h b/devices/IFrameTransformToIWear/include/IFrameTransformToIWear.h new file mode 100644 index 000000000..4113216e9 --- /dev/null +++ b/devices/IFrameTransformToIWear/include/IFrameTransformToIWear.h @@ -0,0 +1,172 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IFRAMETRANSFORMTOIWEAR +#define WEARABLE_IFRAMETRANSFORMTOIWEAR + +#include "Wearable/IWear/IWear.h" + +#include +#include +#include +#include + +#include + +namespace wearable { + namespace devices { + class IFrameTransformToIWear; + } // namespace devices +} // namespace wearable + +class wearable::devices::IFrameTransformToIWear + : public wearable::IWear + , public yarp::dev::DeviceDriver + , public yarp::dev::IPreciselyTimed + , public yarp::dev::IWrapper + , public yarp::dev::IMultipleWrapper +{ +private: + class Impl; + std::unique_ptr pImpl; + +public: + IFrameTransformToIWear(); + ~IFrameTransformToIWear() override; + + IFrameTransformToIWear(const IFrameTransformToIWear& other) = delete; + IFrameTransformToIWear(IFrameTransformToIWear&& other) = delete; + IFrameTransformToIWear& operator=(const IFrameTransformToIWear& other) = delete; + IFrameTransformToIWear& operator=(IFrameTransformToIWear&& other) = delete; + + // DeviceDriver + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // IPreciselyTimed + yarp::os::Stamp getLastInputStamp() override; + + // IWrapper interface + bool attach(yarp::dev::PolyDriver* poly) override; + bool detach() override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() override; + + // ===== + // IWEAR + // ===== + + // ------- + // GENERIC + // ------- + + SensorPtr + getSensor(const wearable::sensor::SensorName name) const override; + + VectorOfSensorPtr + getSensors(const wearable::sensor::SensorType type) const override; + + WearableName getWearableName() const override; + WearStatus getStatus() const override; + TimeStamp getTimeStamp() const override; + + // -------------- + // SINGLE SENSORS + // -------------- + + SensorPtr + getAccelerometer(const wearable::sensor::SensorName name) const override; + + SensorPtr + getForce3DSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr + getForceTorque6DSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr + getGyroscope(const wearable::sensor::SensorName name) const override; + + SensorPtr + getMagnetometer(const wearable::sensor::SensorName name) const override; + + SensorPtr + getOrientationSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr + getTemperatureSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr + getTorque3DSensor(const wearable::sensor::SensorName name) const override; + + SensorPtr getEmgSensor(const sensor::SensorName name) const override; + + SensorPtr + getFreeBodyAccelerationSensor(const sensor::SensorName name) const override; + + SensorPtr + getPoseSensor(const sensor::SensorName name) const override; + + SensorPtr + getPositionSensor(const sensor::SensorName name) const override; + + SensorPtr + getSkinSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualJointKinSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualSphericalJointKinSensor(const sensor::SensorName name) const override; + + inline ElementPtr + getActuator(const actuator::ActuatorName name) const override; + + inline VectorOfElementPtr + getActuators(const actuator::ActuatorType type) const override; + + inline ElementPtr + getHapticActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getMotorActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getHeaterActuator(const actuator::ActuatorName) const override; +}; + +inline wearable::ElementPtr +wearable::devices::IFrameTransformToIWear::getActuator(const actuator::ActuatorName name) const +{ + return nullptr; +} + +inline wearable::VectorOfElementPtr +wearable::devices::IFrameTransformToIWear::getActuators(const actuator::ActuatorType type) const +{ + return {}; +} + +inline wearable::ElementPtr +wearable::devices::IFrameTransformToIWear::getHapticActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::IFrameTransformToIWear::getMotorActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::IFrameTransformToIWear::getHeaterActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +#endif // WEARABLE_IFRAMETRANSFORMTOIWEAR diff --git a/devices/IFrameTransformToIWear/src/IFrameTransformToIWear.cpp b/devices/IFrameTransformToIWear/src/IFrameTransformToIWear.cpp new file mode 100644 index 000000000..0a60a958d --- /dev/null +++ b/devices/IFrameTransformToIWear/src/IFrameTransformToIWear.cpp @@ -0,0 +1,483 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "IFrameTransformToIWear.h" + +#include +#include +#include + +#include +#include +#include +#include + +const std::string DeviceName = "IFrameTransformToIWear"; +const std::string LogPrefix = DeviceName + " :"; + +using namespace wearable; +using namespace wearable::sensor; +using namespace wearable::devices; + +// Class that provides each frame information, and utilites +// to map its data to containers compatible with IWear +class IFrameTransformHandler +{ +public: + IFrameTransformHandler(const std::string& targetFrame, + const std::string& rootFrame, + yarp::dev::IFrameTransform* frameTransformInterface) + : m_targetFrame(targetFrame) + , m_rootFrame(rootFrame) + , m_frameTransformInterface(frameTransformInterface) + , m_matrixBuffer(4, 4){}; + + wearable::sensor::SensorStatus getStatus() + { + if (m_frameTransformInterface->canTransform(m_targetFrame, m_rootFrame)) { + return SensorStatus::Ok; + } + else { + return SensorStatus::Error; + } + } + + bool getTransform(wearable::Quaternion& quaternion, wearable::Vector3& position) + { + + if (!m_frameTransformInterface->getTransform(m_targetFrame, m_rootFrame, m_matrixBuffer)) { + return false; + } + + position[0] = m_matrixBuffer(0, 3); + position[1] = m_matrixBuffer(1, 3); + position[2] = m_matrixBuffer(2, 3); + + for (size_t r = 0; r < 3; r++) { + for (size_t c = 0; c < 3; c++) { + m_rotationMatrixBuffer[r][c] = m_matrixBuffer(r, c); + } + } + + quaternion = wearable::utils::rotationMatrixToQuaternion(m_rotationMatrixBuffer); + return true; + } + +private: + yarp::dev::IFrameTransform* m_frameTransformInterface = nullptr; + std::string m_rootFrame, m_targetFrame; + yarp::sig::Matrix m_matrixBuffer; + wearable::Matrix3 m_rotationMatrixBuffer; +}; + +class IFrameTransformToIWear::Impl +{ +public: + bool firstRun = true; + mutable std::recursive_mutex mutex; + + TimeStamp timestamp; + struct + { + wearable::WearableName wearableName; + wearable::sensor::SensorType wearableSensorType; + std::string rootFrameID; + std::vector frameIDs; + } options; + + std::unique_ptr network = nullptr; + + yarp::dev::IFrameTransform* frameTransformInterface = nullptr; + + template + using SensorsMap = std::map>; + + // Sensors stored for exposing wearable::IWear + class PoseSensor; + SensorsMap poseSensorsMap; + + template + bool isSensorAvailable(const std::string& name, const SensorsMap& map) + { + if (map.find(name) == map.end()) { + return false; + } + return true; + } +}; + +// ================================ +// WEARABLE SENSORS IMPLEMENTATIONS +// ================================ + +class IFrameTransformToIWear::Impl::PoseSensor : public wearable::sensor::IPoseSensor +{ +public: + // ------------------------ + // Constructor / Destructor + // ------------------------ + PoseSensor( + IFrameTransformHandler* frameTransformHandler, + const wearable::sensor::SensorName& aName, + IFrameTransformToIWear::Impl* impl, + const wearable::sensor::SensorStatus& aStatus = wearable::sensor::SensorStatus::Unknown) + : IPoseSensor(aName, aStatus) + , m_frameTransformHandler(frameTransformHandler) + , m_frameTransformToIWearImpl(impl) + {} + + ~PoseSensor() override = default; + + // ------------------------- + // IOrientationSensor interface + // ------------------------- + bool getPose(Quaternion& orientation, Vector3& position) const override + { + std::lock_guard lock(m_frameTransformToIWearImpl->mutex); + return this->m_frameTransformHandler->getTransform(orientation, position); + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } + +private: + IFrameTransformHandler* m_frameTransformHandler = nullptr; + IFrameTransformToIWear::Impl* m_frameTransformToIWearImpl = nullptr; +}; + +// TODO: implement other sensors + +// ====================== +// IFrameTransformToIWear +// ====================== + +IFrameTransformToIWear::IFrameTransformToIWear() + : pImpl{std::make_unique()} +{} + +// Without this destructor here, the linker complains for +// undefined reference to vtable +IFrameTransformToIWear::~IFrameTransformToIWear() = default; + +bool IFrameTransformToIWear::open(yarp::os::Searchable& config) +{ + // =============================== + // CHECK THE CONFIGURATION OPTIONS + // =============================== + + if (!(config.check("wearableName") && config.find("wearableName").isString())) { + yError() << LogPrefix << "Parameter 'wearableName' missing or invalid"; + return false; + } + + if (!(config.check("rootFrameID") && config.find("rootFrameID").isString())) { + yError() << LogPrefix << "Parameter 'rootFrameID' missing or invalid"; + return false; + } + + if (!(config.check("wearableSensorType") && config.find("wearableSensorType").isString())) { + yError() << LogPrefix << "Parameter 'wearableSensorType' missing or invalid"; + return false; + } + + if (!(config.check("frameIDs") && config.find("frameIDs").isList())) { + yError() << LogPrefix << "Parameter 'frameIDs' missing or invalid"; + return false; + } + + // =============== + // READ PARAMETERS + // =============== + + pImpl->options.wearableName = config.find("wearableName").asString(); + pImpl->options.rootFrameID = config.find("rootFrameID").asString(); + std::string sensorType = config.find("wearableSensorType").asString(); + pImpl->options.wearableSensorType = wearable::sensor::sensorTypeFromString(sensorType); + + auto frameIDsBottle = config.find("frameIDs").asList(); + for (size_t it = 0; it < frameIDsBottle->size(); it++) { + pImpl->options.frameIDs.push_back(frameIDsBottle->get(it).asString()); + } + + yInfo() << LogPrefix << "*** ===================="; + yInfo() << LogPrefix << "*** Wearable name :" << pImpl->options.wearableName; + yInfo() << LogPrefix << "*** Sensor Type :" << sensorType; + yInfo() << LogPrefix << "*** Root Frame ID :" << pImpl->options.rootFrameID; + yInfo() << LogPrefix << "*** Selected Frame IDs :"; + for (auto ID : pImpl->options.frameIDs) { + yInfo() << LogPrefix << "*** " << ID; + } + yInfo() << LogPrefix << "*** ===================="; + + // ================================= + // CHECK YARP NETWORK INITIALIZATION + // ================================= + + pImpl->network = std::make_unique(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + yError() << LogPrefix << "YARP server wasn't found active."; + return false; + } + + return true; +} + +bool IFrameTransformToIWear::close() +{ + detach(); + return true; +} + +yarp::os::Stamp IFrameTransformToIWear::getLastInputStamp() +{ + std::lock_guard lock(pImpl->mutex); + return yarp::os::Stamp(pImpl->timestamp.sequenceNumber, yarp::os::Time::now()); +} + +bool IFrameTransformToIWear::attach(yarp::dev::PolyDriver* poly) +{ + if (!poly) { + yError() << LogPrefix << "Passed PolyDriver is nullptr"; + return false; + } + + if (!(poly->view(pImpl->frameTransformInterface) && pImpl->frameTransformInterface)) { + yError() << LogPrefix << "Failed to view the IFrameTransform interface from the PolyDriver"; + return false; + } + + // =========== + // ADD SENSORS + // =========== + + // sensor prefixes + auto poseSensPrefix = getWearableName() + sensor::IPoseSensor::getPrefix(); + auto virtualLinkSensPrefix = getWearableName() + sensor::IVirtualLinkKinSensor::getPrefix(); + + if (!pImpl->frameTransformInterface->frameExists(pImpl->options.rootFrameID)) { + yError() << LogPrefix << "No transform found for the root frame ( " + << pImpl->options.rootFrameID << " )"; + return false; + } + + for (auto ID : pImpl->options.frameIDs) { + + if (!pImpl->frameTransformInterface->frameExists(ID)) { + yError() << LogPrefix << "No transform found for the frame ( " << ID << " )"; + return false; + } + + // The sensors are initialized as Ok in order to trigger the first data read. + // If there is any error during the first read, the sensor updates its own status + // that is then propagated to the global IWear status. + switch (pImpl->options.wearableSensorType) { + case wearable::sensor::SensorType::PoseSensor: { + auto poseSensorName = poseSensPrefix + ID; + auto poseSensor = std::make_shared( + new IFrameTransformHandler( + ID, pImpl->options.rootFrameID, pImpl->frameTransformInterface), + poseSensorName, + pImpl.get(), + wearable::sensor::SensorStatus::Ok); + pImpl->poseSensorsMap.emplace(poseSensorName, poseSensor); + break; + } + default: + // TODO: implement the remaining sensors + yError() << LogPrefix << "Selected wearableSensorType is not valid."; + return false; + } + } + + // Notify that the sensor is ready to be used + pImpl->firstRun = false; + + return true; +} + +bool IFrameTransformToIWear::detach() +{ + return true; +} + +bool IFrameTransformToIWear::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + if (driverList.size() > 1) { + yError() << LogPrefix << "This wrapper accepts only one attached PolyDriver"; + return false; + } + + const yarp::dev::PolyDriverDescriptor* driver = driverList[0]; + + if (!driver) { + yError() << LogPrefix << "Passed PolyDriverDescriptor is nullptr"; + return false; + } + + return attach(driver->poly); +} + +bool IFrameTransformToIWear::detachAll() +{ + return detach(); +} + +wearable::SensorPtr IFrameTransformToIWear::getSensor(const 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 +IFrameTransformToIWear::getSensors(const SensorType type) const +{ + wearable::VectorOfSensorPtr allSensors; + switch (type) { + case wearable::sensor::SensorType::PoseSensor: { + allSensors.reserve(pImpl->poseSensorsMap.size()); + for (const auto& poseSens : pImpl->poseSensorsMap) { + allSensors.push_back(static_cast>(poseSens.second)); + } + break; + } + default: { + yWarning() << LogPrefix << "Selected sensor type (" << static_cast(type) + << ") is not supported by IFrameTransformToIWear."; + return {}; + } + } + return allSensors; +} + +wearable::WearableName IFrameTransformToIWear::getWearableName() const +{ + return pImpl->options.wearableName + wearable::Separator; +} + +wearable::WearStatus IFrameTransformToIWear::getStatus() const +{ + // TODO + return wearable::WearStatus::Ok; +} + +wearable::TimeStamp IFrameTransformToIWear::getTimeStamp() const +{ + std::lock_guard lock(pImpl->mutex); + + pImpl->timestamp.sequenceNumber = 0; // Always zero + pImpl->timestamp.time = yarp::os::Time::now(); + + return pImpl->timestamp; +} + +wearable::SensorPtr +IFrameTransformToIWear::getAccelerometer(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getForce3DSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getForceTorque6DSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getGyroscope(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getMagnetometer(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getOrientationSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getTemperatureSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getTorque3DSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getEmgSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getFreeBodyAccelerationSensor( + const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getPoseSensor(const wearable::sensor::SensorName name) const +{ + if (!pImpl->isSensorAvailable(name, pImpl->poseSensorsMap)) { + yError() << LogPrefix << "Invalid sensor name."; + return nullptr; + } + + return static_cast>( + pImpl->poseSensorsMap.at(static_cast(name))); +} + +wearable::SensorPtr +IFrameTransformToIWear::getPositionSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getSkinSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getVirtualLinkKinSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getVirtualJointKinSensor(const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} + +wearable::SensorPtr +IFrameTransformToIWear::getVirtualSphericalJointKinSensor( + const wearable::sensor::SensorName /*name*/) const +{ + return nullptr; +} diff --git a/devices/IWearRemapper/CMakeLists.txt b/devices/IWearRemapper/CMakeLists.txt new file mode 100644 index 000000000..6288a5bf0 --- /dev/null +++ b/devices/IWearRemapper/CMakeLists.txt @@ -0,0 +1,32 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(iwear_remapper + TYPE wearable::devices::IWearRemapper + INCLUDE include/IWearRemapper.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(IWearRemapper + src/IWearRemapper.cpp + include/IWearRemapper.h) + +target_include_directories(IWearRemapper PUBLIC + $) + +target_link_libraries(IWearRemapper PUBLIC + IWear::IWear + Wearable::WearableData + Wearable::SensorsImpl + YARP::YARP_dev + YARP::YARP_os + YARP::YARP_init) + +yarp_install( + TARGETS IWearRemapper + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) diff --git a/devices/IWearRemapper/conf/IWearRemapper.ini b/devices/IWearRemapper/conf/IWearRemapper.ini new file mode 100644 index 000000000..8e8b9d0cd --- /dev/null +++ b/devices/IWearRemapper/conf/IWearRemapper.ini @@ -0,0 +1,2 @@ +iwearPorts ("/iwear/ftShoes:o", "/iwear/xsens:o") +portPrefix /iwearRemapper diff --git a/devices/IWearRemapper/include/IWearRemapper.h b/devices/IWearRemapper/include/IWearRemapper.h new file mode 100644 index 000000000..195aecdd2 --- /dev/null +++ b/devices/IWearRemapper/include/IWearRemapper.h @@ -0,0 +1,164 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IWEARREMAPPER_H +#define IWEARREMAPPER_H + +#include "Wearable/IWear/IWear.h" + +#include +#include +#include +#include +#include +#include + +#include + +namespace wearable { + namespace msg { + class WearableData; + } + namespace devices { + class IWearRemapper; + } +} // namespace wearable + +class wearable::devices::IWearRemapper + : public yarp::dev::DeviceDriver + , public wearable::IWear + , public yarp::os::TypedReaderCallback + , public yarp::os::PeriodicThread + , public yarp::dev::IMultipleWrapper + , public yarp::dev::IPreciselyTimed +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + IWearRemapper(); + ~IWearRemapper() override; + + // DeviceDriver interface + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // PeriodicThread + void run() override; + void threadRelease() override; + + // TypedReaderCallback + void onRead(msg::WearableData& wearData, const yarp::os::TypedReader& typedReader) override; + + // PreciselyTimed interface + yarp::os::Stamp getLastInputStamp() override; + + // IWear interface + 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 type) const override; + + SensorPtr + getAccelerometer(const sensor::SensorName /*name*/) const override; + + SensorPtr + getEmgSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getForce3DSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getForceTorque6DSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getGyroscope(const sensor::SensorName /*name*/) const override; + + SensorPtr + getMagnetometer(const sensor::SensorName /*name*/) const override; + + SensorPtr + getOrientationSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getPoseSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getPositionSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getSkinSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getTemperatureSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getTorque3DSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getVirtualJointKinSensor(const sensor::SensorName /*name*/) const override; + + SensorPtr + getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const override; + + inline ElementPtr + getActuator(const actuator::ActuatorName name) const override; + + inline VectorOfElementPtr + getActuators(const actuator::ActuatorType type) const override; + + inline ElementPtr + getHapticActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getMotorActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getHeaterActuator(const actuator::ActuatorName) const override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() override; +}; + +inline wearable::ElementPtr +wearable::devices::IWearRemapper::getActuator(const actuator::ActuatorName name) const +{ + return nullptr; +} + +inline wearable::VectorOfElementPtr +wearable::devices::IWearRemapper::getActuators(const actuator::ActuatorType type) const +{ + return {}; +} + +inline wearable::ElementPtr +wearable::devices::IWearRemapper::getHapticActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::IWearRemapper::getMotorActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::IWearRemapper::getHeaterActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +#endif // IWEARREMAPPER_H diff --git a/devices/IWearRemapper/src/IWearRemapper.cpp b/devices/IWearRemapper/src/IWearRemapper.cpp new file mode 100644 index 000000000..2c7d80b08 --- /dev/null +++ b/devices/IWearRemapper/src/IWearRemapper.cpp @@ -0,0 +1,1373 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "IWearRemapper.h" +#include "Wearable/IWear/IWear.h" +#include "Wearable/IWear/Sensors/impl/SensorsImpl.h" +#include "thrift/WearableData.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +const std::string WrapperName = "IWearRemapper"; +const std::string logPrefix = WrapperName + " :"; + +using namespace wearable; +using namespace wearable::devices; + +// ============== +// IMPL AND UTILS +// ============== + +class IWearRemapper::impl +{ +public: + yarp::os::Network network; + TimeStamp timestamp; + bool firstRun = true; + bool terminationCall = false; + bool inputDataPorts = false; + + bool allowDynamicData = true; + + // Flag to wait for first data received + bool waitForAttachAll = false; + + mutable std::mutex mutex; + + msg::WearableData wearableData; + std::vector>> inputPortsWearData; + std::vector firstInputReceived; //flag to check that at least a first message from the inputs port was received + + // Sensors stored for exposing wearable::IWear + std::unordered_map> accelerometers; + std::unordered_map> emgSensors; + std::unordered_map> force3DSensors; + std::unordered_map> forceTorque6DSensors; + std::unordered_map> + freeBodyAccelerationSensors; + std::unordered_map> gyroscopes; + std::unordered_map> magnetometers; + std::unordered_map> orientationSensors; + std::unordered_map> poseSensors; + std::unordered_map> positionSensors; + std::unordered_map> skinSensors; + std::unordered_map> temperatureSensors; + std::unordered_map> torque3DSensors; + std::unordered_map> + virtualLinkKinSensors; + std::unordered_map> + virtualJointKinSensors; + std::unordered_map> + virtualSphericalJointKinSensors; + + bool updateData(msg::WearableData& receivedWearData, bool create); + + template + SensorPtr + getSensor(const sensor::SensorName name, + const sensor::SensorType type, + const std::unordered_map>& storage) const; + + template + SensorPtr + getOrCreateSensor(const sensor::SensorName name, + const sensor::SensorType type, + std::unordered_map>& storage, + bool create); + + SensorPtr + getAccelerometer(const sensor::SensorName /*name*/) const; + + SensorPtr + getEmgSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getForce3DSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getForceTorque6DSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getGyroscope(const sensor::SensorName /*name*/) const; + + SensorPtr + getMagnetometer(const sensor::SensorName /*name*/) const; + + SensorPtr + getOrientationSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getPoseSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getPositionSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getSkinSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getTemperatureSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getTorque3DSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getVirtualJointKinSensor(const sensor::SensorName /*name*/) const; + + SensorPtr + getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const; +}; + +// ============== +// IWEAR REMAPPER +// ============== + +IWearRemapper::IWearRemapper() + : PeriodicThread(1) + , pImpl{new impl()} +{} + +IWearRemapper::~IWearRemapper() = default; + +bool IWearRemapper::open(yarp::os::Searchable& config) +{ + // ===================== + // CHECK THE INPUT PORTS + // ===================== + + // wait for attachAll + if (config.check("waitForAttachAll")) { + if (!config.find("waitForAttachAll").isBool()) { + yError() << logPrefix << "waitForAttachAll option is not a bool"; + return false; + } + pImpl->waitForAttachAll = config.find("waitForAttachAll").asBool(); + } + + if (!config.check("allowDynamicData") || !config.find("allowDynamicData").isBool() ) + { + yInfo() << logPrefix << "Cannot find a suitable allowDynamicData parameter, using default value."; + } + else + { + pImpl->allowDynamicData = config.find("allowDynamicData").asBool(); + } + yInfo() << logPrefix << "Using allowDynamicData parameter:"<allowDynamicData; + + pImpl->inputDataPorts = config.check("wearableDataPorts"); + + if (pImpl->inputDataPorts) { + + // Check that wearableDataPorts option is a list + if (!config.find("wearableDataPorts").isList()) { + yError() << logPrefix << "wearableDataPorts option is not a list"; + return false; + } + + yarp::os::Bottle* inputDataPortsNamesList = config.find("wearableDataPorts").asList(); + + if (inputDataPortsNamesList->size() == 0) { + pImpl->inputDataPorts = false; + } + else { + for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) { + if (!inputDataPortsNamesList->get(i).isString()) { + yError() << logPrefix << i << "th entry of wearableDataPorts list is not a string"; + return false; + } + } + + std::vector localNames; + if (config.check("wearableDataLocals")) { + if (!config.find("wearableDataLocals").isList()) { + yError() << logPrefix << "wearableDataLocals option found, but it is not a list."; + return false; + } + yarp::os::Bottle* localDataPortsNamesList = + config.find("wearableDataLocals").asList(); + if (localDataPortsNamesList->size() != inputDataPortsNamesList->size()) { + yError() << logPrefix + << "wearableDataLocals and wearableDataPorts have different sizes."; + return false; + } + for (unsigned i = 0; i < localDataPortsNamesList->size(); ++i) { + if (!localDataPortsNamesList->get(i).isString()) { + yError() << logPrefix << i + << "th entry of wearableDataLocals list is not a string."; + return false; + } + localNames.push_back(localDataPortsNamesList->get(i).asString()); + } + } + else { + yInfo() << logPrefix + << "wearableDataLocals option not found, using temporary port names for the local ports."; + for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) { + localNames.push_back("..."); // use temporary port names + } + } + + // =============================== + // PARSE THE CONFIGURATION OPTIONS + // =============================== + + // Convert list to vector + std::vector inputDataPortsNamesVector; + for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) { + inputDataPortsNamesVector.emplace_back(inputDataPortsNamesList->get(i).asString()); + } + + yInfo() << logPrefix << "*** ========================"; + for (unsigned i = 0; i < inputDataPortsNamesVector.size(); ++i) { + yInfo() << logPrefix << "*** Wearable Data Port" << i + 1 << " :" + << inputDataPortsNamesVector[i]; + } + + yInfo() << logPrefix << "*** ========================"; + + // Carrier optional configuration + std::string carrier = ""; + if (config.check("carrier")) { + carrier = config.find("carrier").asString(); + } + + // ========================== + // CONFIGURE INPUT DATA PORTS + // ========================== + yDebug() << logPrefix << "Configuring input data ports"; + + for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) { + pImpl->inputPortsWearData.emplace_back(new yarp::os::BufferedPort()); + pImpl->inputPortsWearData.back()->useCallback(*this); + + if (!pImpl->inputPortsWearData.back()->open(localNames[i])) { + yError() << logPrefix << "Failed to open local input port"; + return false; + } + pImpl->firstInputReceived.push_back(false); + } + + // ================ + // OPEN INPUT PORTS + // ================ + yDebug() << logPrefix << "Opening input ports"; + + // Initialize the network + pImpl->network = yarp::os::Network(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + yError() << logPrefix << "YARP server wasn't found active."; + return false; + } + + for (unsigned i = 0; i < inputDataPortsNamesList->size(); ++i) { + if (!yarp::os::Network::connect(inputDataPortsNamesVector[i], + pImpl->inputPortsWearData[i]->getName(), + carrier)) { + yError() << logPrefix << "Failed to connect " << inputDataPortsNamesVector[i] + << " with " << pImpl->inputPortsWearData[i]->getName(); + return false; + } + } + + // If it not necessary to wait for the attachAll start the callbacks + // We use callbacks on the input ports, the loop is a no-op + if (!pImpl->waitForAttachAll) { + start(); + } + + } + } + + + yDebug() << logPrefix << "Opened correctly"; + return true; +} + +void IWearRemapper::threadRelease() {} + +bool IWearRemapper::close() +{ + pImpl->terminationCall = true; + + while (isRunning()) { + stop(); + } + + return true; +} + +void IWearRemapper::run() +{ + if (getStatus() == WearStatus::Error) + askToStop(); + return; +} + +const std::unordered_map MapSensorStatus = { + {msg::SensorStatus::OK, sensor::SensorStatus::Ok}, + {msg::SensorStatus::ERROR, sensor::SensorStatus::Error}, + {msg::SensorStatus::DATA_OVERFLOW, sensor::SensorStatus::Overflow}, + {msg::SensorStatus::CALIBRATING, sensor::SensorStatus::Calibrating}, + {msg::SensorStatus::TIMEOUT, sensor::SensorStatus::Timeout}, + {msg::SensorStatus::WAITING_FOR_FIRST_READ, sensor::SensorStatus::WaitingForFirstRead}, + {msg::SensorStatus::UNKNOWN, sensor::SensorStatus::Unknown}, +}; + + + + +bool IWearRemapper::impl::updateData(msg::WearableData& receivedWearData, bool create) +{ + for (auto& accelerometersMap : receivedWearData.accelerometers) { + const std::string& inputSensorName = accelerometersMap.first; + const auto& wearDataInputSensor = accelerometersMap.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::Accelerometer, accelerometers, create); + + if (!isensor) { + yError() << logPrefix << "Failed to get Accelerometer" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer( + {wearDataInputSensor.data.x, wearDataInputSensor.data.y, wearDataInputSensor.data.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.emgSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::EmgSensor, emgSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get EmgSensor" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer(wearDataInputSensor.data.value, wearDataInputSensor.data.normalization); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.force3DSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::Force3DSensor, force3DSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get Force3DSensor" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer( + {wearDataInputSensor.data.x, wearDataInputSensor.data.y, wearDataInputSensor.data.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.forceTorque6DSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::ForceTorque6DSensor, forceTorque6DSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get ForceTorque6DSensor" << inputSensorName; + return false; + } + const auto* constSensor = + static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer({wearDataInputSensor.data.force.x, + wearDataInputSensor.data.force.y, + wearDataInputSensor.data.force.z}, + {wearDataInputSensor.data.torque.x, + wearDataInputSensor.data.torque.y, + wearDataInputSensor.data.torque.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.freeBodyAccelerationSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::FreeBodyAccelerationSensor, freeBodyAccelerationSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get FreeBodyAccelerationSensor" << inputSensorName; + return false; + } + const auto* constSensor = + static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer( + {wearDataInputSensor.data.x, wearDataInputSensor.data.y, wearDataInputSensor.data.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.gyroscopes) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::Gyroscope, gyroscopes, create); + + if (!isensor) { + yError() << logPrefix << "Failed to get Gyroscope" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer( + {wearDataInputSensor.data.x, wearDataInputSensor.data.y, wearDataInputSensor.data.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.magnetometers) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::Magnetometer, magnetometers, create); + if (!isensor) { + yError() << logPrefix << "Failed to get Magnetometer" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer( + {wearDataInputSensor.data.x, wearDataInputSensor.data.y, wearDataInputSensor.data.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.orientationSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::OrientationSensor, orientationSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get OrientationSensor" << inputSensorName; + return false; + } + const auto* constSensor = + static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer({wearDataInputSensor.data.w, + wearDataInputSensor.data.x, + wearDataInputSensor.data.y, + wearDataInputSensor.data.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.poseSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::PoseSensor, poseSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get PoseSensor" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer({wearDataInputSensor.data.orientation.w, + wearDataInputSensor.data.orientation.x, + wearDataInputSensor.data.orientation.y, + wearDataInputSensor.data.orientation.z}, + {wearDataInputSensor.data.position.x, + wearDataInputSensor.data.position.y, + wearDataInputSensor.data.position.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.positionSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::PositionSensor, positionSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get PositionSensor" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer( + {wearDataInputSensor.data.x, wearDataInputSensor.data.y, wearDataInputSensor.data.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.skinSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + // Create the sensor if it does not exist + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::SkinSensor, skinSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get SkinSensor" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + + //Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer(wearDataInputSensor.data); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.temperatureSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::TemperatureSensor, temperatureSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get TemperatureSensor" << inputSensorName; + return false; + } + const auto* constSensor = + static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer(wearDataInputSensor.data); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.torque3DSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::Torque3DSensor, torque3DSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get Torque3DSensor" << inputSensorName; + return false; + } + const auto* constSensor = static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer( + {wearDataInputSensor.data.x, wearDataInputSensor.data.y, wearDataInputSensor.data.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.virtualLinkKinSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::VirtualLinkKinSensor, virtualLinkKinSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get VirtualLinkKinSensor" << inputSensorName; + return false; + } + const auto* constSensor = + static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer({wearDataInputSensor.data.linearAcceleration.x, + wearDataInputSensor.data.linearAcceleration.y, + wearDataInputSensor.data.linearAcceleration.z}, + {wearDataInputSensor.data.angularAcceleration.x, + wearDataInputSensor.data.angularAcceleration.y, + wearDataInputSensor.data.angularAcceleration.z}, + {wearDataInputSensor.data.linearVelocity.x, + wearDataInputSensor.data.linearVelocity.y, + wearDataInputSensor.data.linearVelocity.z}, + {wearDataInputSensor.data.angularVelocity.x, + wearDataInputSensor.data.angularVelocity.y, + wearDataInputSensor.data.angularVelocity.z}, + {wearDataInputSensor.data.position.x, + wearDataInputSensor.data.position.y, + wearDataInputSensor.data.position.z}, + {wearDataInputSensor.data.orientation.w, + wearDataInputSensor.data.orientation.x, + wearDataInputSensor.data.orientation.y, + wearDataInputSensor.data.orientation.z}); + + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + for (auto& s : receivedWearData.virtualJointKinSensors) { + const auto& inputSensorName = s.first; + const auto& wearDataInputSensor = s.second; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::VirtualJointKinSensor, virtualJointKinSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get VirtualJointKinSensor" << inputSensorName; + return false; + } + const auto* constSensor = + static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer({wearDataInputSensor.data.position}, + {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; + + // ==================== + // EXPOSE THE INTERFACE + // ==================== + auto isensor = getOrCreateSensor( + inputSensorName, sensor::SensorType::VirtualSphericalJointKinSensor, virtualSphericalJointKinSensors, create); + if (!isensor) { + yError() << logPrefix << "Failed to get VirtualSphericalJointKinSensor" + << inputSensorName; + return false; + } + const auto* constSensor = + static_cast(isensor.get()); + auto* sensor = const_cast(constSensor); + // Copy its data to the buffer used for exposing the IWear interface + sensor->setBuffer({wearDataInputSensor.data.angle.r, + wearDataInputSensor.data.angle.p, + wearDataInputSensor.data.angle.y}, + {wearDataInputSensor.data.velocity.x, + wearDataInputSensor.data.velocity.y, + wearDataInputSensor.data.velocity.z}, + {wearDataInputSensor.data.acceleration.x, + wearDataInputSensor.data.acceleration.y, + wearDataInputSensor.data.acceleration.z}); + // Set the status + sensor->setStatus(MapSensorStatus.at(wearDataInputSensor.info.status)); + } + + return true; +} + +void IWearRemapper::onRead(msg::WearableData& wearData, const yarp::os::TypedReader& typedReader) +{ + if (pImpl->terminationCall) { + return; + } + + bool dataUpdated = true; + if(pImpl->firstRun || pImpl->allowDynamicData) + { + // locked version + std::lock_guard lock(pImpl->mutex); + dataUpdated = pImpl->updateData(wearData, true); + } + else + { + // non-locked version + dataUpdated = pImpl->updateData(wearData, false); + } + + if(!dataUpdated) + { + askToStop(); + } + + // Update the timestamp + { + std::lock_guard lock(pImpl->mutex); + pImpl->timestamp.sequenceNumber++; + pImpl->timestamp.time = yarp::os::Time::now(); + + // This is used to handle the overall status of IWear + if (pImpl->firstRun) { + // check if all ports were read + bool allRead = true; + for(int i = 0; iinputPortsWearData.size(); i++) + { + if(pImpl->inputPortsWearData[i]->getName()==typedReader.getName()) + { + pImpl->firstInputReceived[i] = true; + } + else if(!pImpl->firstInputReceived[i]) + { + allRead = false; + } + } + + if(allRead) + { + pImpl->firstRun = false; + } + } + } +} + +yarp::os::Stamp IWearRemapper::getLastInputStamp() +{ + // Stamp count should be always zero + return yarp::os::Stamp(0, getTimeStamp().time); +} + +WearableName IWearRemapper::getWearableName() const +{ + return WrapperName + wearable::Separator; +} + +WearStatus IWearRemapper::getStatus() const +{ + if (pImpl->firstRun) { + return WearStatus::WaitingForFirstRead; + } + + // Logic for combining the status of all the sensors. + // The tricky part is deciding how to handle the mixed case of timeout and overflow. + // The WaitingingForFirstRead is not considered since the data is supposed not to be streamed. + // TODO: For now, overflow is stronger. + WearStatus status = WearStatus::Ok; + auto sensorsList = getAllSensors(); + + { + std::lock_guard lock(pImpl->mutex); + + for (const auto& s : sensorsList) { + switch (s->getSensorStatus()) { + case sensor::SensorStatus::Overflow: + yWarning() << logPrefix << "type (" << static_cast(s->getSensorType()) + << ") sensor [" << s->getSensorName() << "] status is (" + << static_cast(s->getSensorStatus()) << ")"; + status = WearStatus::Overflow; + break; + case sensor::SensorStatus::Timeout: + yWarning() << logPrefix << "type (" << static_cast(s->getSensorType()) + << ") sensor [" << s->getSensorName() << "] status is (" + << static_cast(s->getSensorStatus()) << ")"; + if (status == WearStatus::Overflow) { + break; + } + status = WearStatus::Timeout; + break; + case sensor::SensorStatus::Ok: + // Keep checking other sensors + break; + default: + // If even just one sensor is Error, Unknown, or + // any other state return error + yError() << logPrefix << "type (" << static_cast(s->getSensorType()) + << ") sensor [" << s->getSensorName() << "] status is (" + << static_cast(s->getSensorStatus()) << ")"; + return WearStatus::Error; + } + } + } + + return status; +} + +TimeStamp IWearRemapper::getTimeStamp() const +{ + std::lock_guard lock(pImpl->mutex); + return pImpl->timestamp; +} + +SensorPtr IWearRemapper::getSensor(const sensor::SensorName name) const +{ + auto sensorsList = getAllSensors(); + + { + std::lock_guard lock(pImpl->mutex); + for (const auto& s : sensorsList) { + if (s->getSensorName() == name) { + return s; + } + } + } + + return nullptr; +} + +bool IWearRemapper::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + for(int p=0; ppoly->view(iWear)) { + yError() << logPrefix << "Failed to view the IWear interface from the PolyDriver."; + return false; + } + + for (const auto& sensor : iWear->getAccelerometers()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->accelerometers.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getEmgSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->emgSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getForce3DSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->force3DSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getForceTorque6DSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->forceTorque6DSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getFreeBodyAccelerationSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->freeBodyAccelerationSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getGyroscopes()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->gyroscopes.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getMagnetometers()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->magnetometers.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getOrientationSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->orientationSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getPoseSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->poseSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getPositionSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->positionSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getSkinSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->skinSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getTemperatureSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->temperatureSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getTorque3DSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->torque3DSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getVirtualLinkKinSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->virtualLinkKinSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getVirtualJointKinSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->virtualJointKinSensors.emplace(sensor->getSensorName(), newSensor); + } + for (const auto& sensor : iWear->getVirtualSphericalJointKinSensors()) { + const auto* constSensor = static_cast(sensor.get()); + auto* newSensor = const_cast(constSensor); + newSensor->setStatus(sensor->getSensorStatus()); + pImpl->virtualSphericalJointKinSensors.emplace(sensor->getSensorName(), newSensor); + } + + } + + // If there are not input ports there is no need to wait for the first data and to start the no-op loop + if (!pImpl->inputDataPorts) { + pImpl->firstRun = false; + return true; + } + else { + // If it is wating for the attach all, the loop can now be started + if (pImpl->waitForAttachAll) { + start(); + } + } + + return true; +} + +bool IWearRemapper::detachAll() +{ + return true; +} + +VectorOfSensorPtr +IWearRemapper::getSensors(const sensor::SensorType type) const +{ + VectorOfSensorPtr sensors; + + if(pImpl->firstRun) + { + return sensors; + } + if(pImpl->allowDynamicData) + { + pImpl->mutex.lock(); + } + + switch (type) { + case sensor::SensorType::Accelerometer: + for (const auto& s : pImpl->accelerometers) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::EmgSensor: + for (const auto& s : pImpl->emgSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::Force3DSensor: + for (const auto& s : pImpl->force3DSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::ForceTorque6DSensor: + for (const auto& s : pImpl->forceTorque6DSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::FreeBodyAccelerationSensor: + for (const auto& s : pImpl->freeBodyAccelerationSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::Gyroscope: + for (const auto& s : pImpl->gyroscopes) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::Magnetometer: + for (const auto& s : pImpl->magnetometers) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::OrientationSensor: + for (const auto& s : pImpl->orientationSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::PoseSensor: + for (const auto& s : pImpl->poseSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::PositionSensor: + for (const auto& s : pImpl->positionSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::SkinSensor: + for (const auto& s : pImpl->skinSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::TemperatureSensor: + for (const auto& s : pImpl->temperatureSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::Torque3DSensor: + for (const auto& s : pImpl->torque3DSensors) { + sensors.push_back(s.second); + } + break; + case sensor::SensorType::VirtualLinkKinSensor: + for (const auto& s : pImpl->virtualLinkKinSensors) { + 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); + } + break; + case sensor::SensorType::Invalid: + yWarning() << logPrefix << "Requested Invalid sensor type"; + break; + } + + if(pImpl->allowDynamicData) + { + pImpl->mutex.unlock(); + } + + return sensors; +} + +template +SensorPtr +IWearRemapper::impl::getSensor(const sensor::SensorName name, + const sensor::SensorType type, + const std::unordered_map>& storage) const +{ + SensorPtr ptr = nullptr; + if(allowDynamicData) + { + mutex.lock(); + } + + if (storage.find(name) != storage.end()) { + ptr = storage.at(name); + } + + if(allowDynamicData) + { + mutex.unlock(); + } + + return ptr; +} + +template +SensorPtr +IWearRemapper::impl::getOrCreateSensor(const sensor::SensorName name, + const sensor::SensorType type, + std::unordered_map>& storage, + bool create) +{ + bool found = storage.find(name)!=storage.end(); + + if(!found && !create) + { + return nullptr; + } + + if (!found && create) { + const auto newSensor = + std::make_shared(name, wearable::sensor::SensorStatus::Unknown); + storage.emplace(name, newSensor); + } + + return storage[name]; +} + +wearable::SensorPtr +IWearRemapper::impl::getAccelerometer(const sensor::SensorName name) const +{ + return getSensor( + name, wearable::sensor::SensorType::Accelerometer, accelerometers); +} + +wearable::SensorPtr +IWearRemapper::impl::getEmgSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::EmgSensor, emgSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getForce3DSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::Force3DSensor, force3DSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getForceTorque6DSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::ForceTorque6DSensor, forceTorque6DSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getFreeBodyAccelerationSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::FreeBodyAccelerationSensor, freeBodyAccelerationSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getGyroscope(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::Gyroscope, gyroscopes); +} + +wearable::SensorPtr +IWearRemapper::impl::getMagnetometer(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::Magnetometer, magnetometers); +} + +wearable::SensorPtr +IWearRemapper::impl::getOrientationSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::OrientationSensor, orientationSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getPoseSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::PoseSensor, poseSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getPositionSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::PositionSensor, positionSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getSkinSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::SkinSensor, skinSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getTemperatureSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::TemperatureSensor, temperatureSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getTorque3DSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::Torque3DSensor, torque3DSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getVirtualLinkKinSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::VirtualLinkKinSensor, virtualLinkKinSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getVirtualJointKinSensor(const sensor::SensorName name) const +{ + return getSensor( + name, sensor::SensorType::VirtualJointKinSensor, virtualJointKinSensors); +} + +wearable::SensorPtr +IWearRemapper::impl::getVirtualSphericalJointKinSensor(const sensor::SensorName name) const +{ + return getSensor( + name, + sensor::SensorType::VirtualSphericalJointKinSensor, + virtualSphericalJointKinSensors); +} + + +// IWear Interface + +wearable::SensorPtr +IWearRemapper::getAccelerometer(const sensor::SensorName name) const +{ + return pImpl->getAccelerometer(name); +} + +wearable::SensorPtr +IWearRemapper::getEmgSensor(const sensor::SensorName name) const +{ + return pImpl->getEmgSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getForce3DSensor(const sensor::SensorName name) const +{ + return pImpl->getForce3DSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getForceTorque6DSensor(const sensor::SensorName name) const +{ + return pImpl->getForceTorque6DSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getFreeBodyAccelerationSensor(const sensor::SensorName name) const +{ + return pImpl->getFreeBodyAccelerationSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getGyroscope(const sensor::SensorName name) const +{ + return pImpl->getGyroscope(name); +} + +wearable::SensorPtr +IWearRemapper::getMagnetometer(const sensor::SensorName name) const +{ + return pImpl->getMagnetometer(name); +} + +wearable::SensorPtr +IWearRemapper::getOrientationSensor(const sensor::SensorName name) const +{ + return pImpl->getOrientationSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getPoseSensor(const sensor::SensorName name) const +{ + return pImpl->getPoseSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getPositionSensor(const sensor::SensorName name) const +{ + return pImpl->getPositionSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getSkinSensor(const sensor::SensorName name) const +{ + return pImpl->getSkinSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getTemperatureSensor(const sensor::SensorName name) const +{ + return pImpl->getTemperatureSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getTorque3DSensor(const sensor::SensorName name) const +{ + return pImpl->getTorque3DSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getVirtualLinkKinSensor(const sensor::SensorName name) const +{ + return pImpl->getVirtualLinkKinSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getVirtualJointKinSensor(const sensor::SensorName name) const +{ + return pImpl->getVirtualJointKinSensor(name); +} + +wearable::SensorPtr +IWearRemapper::getVirtualSphericalJointKinSensor(const sensor::SensorName name) const +{ + return pImpl->getVirtualSphericalJointKinSensor(name); +} diff --git a/devices/Paexo/CMakeLists.txt b/devices/Paexo/CMakeLists.txt new file mode 100644 index 000000000..ab141b43e --- /dev/null +++ b/devices/Paexo/CMakeLists.txt @@ -0,0 +1,50 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +# Flag to enable iFeelDriver in Paexo wearable device +option(ENABLE_PAEXO_USE_iFEELDriver "Flag that enables building Paexo wearable device with iFeelDriver" OFF) + +if (ENABLE_PAEXO_USE_iFEELDriver) + + ## Find iFeelDriver + find_package(iFeelDriver REQUIRED) + +endif() + +# Generate cmake config.file +configure_file ( + "${CMAKE_CURRENT_SOURCE_DIR}/conf/paexo-cmake-config.h.in" + "${CMAKE_CURRENT_SOURCE_DIR}/include/paexo-cmake-config.h" + @ONLY) + +# Compile the plugin by default +yarp_prepare_plugin(paexo TYPE wearable::devices::Paexo + INCLUDE include/Paexo.h + CATEGORY device + ADVANCED + DEFAULT ON + ) + +yarp_add_plugin(Paexo src/Paexo.cpp include/Paexo.h) + +target_include_directories(Paexo PRIVATE + $) + +target_link_libraries(Paexo + PUBLIC + YARP::YARP_dev + YARP::YARP_init + Wearable::IWear + ) + +if (ENABLE_PAEXO_USE_iFEELDriver) + target_link_libraries(Paexo PUBLIC iFeelDriver::iFeelDriver) + message(STATUS "Paexo: Compiling with iFeelDriver") +endif() + +yarp_install(TARGETS Paexo + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR} + ) diff --git a/devices/Paexo/conf/Paexo.xml b/devices/Paexo/conf/Paexo.xml new file mode 100644 index 000000000..c08d93864 --- /dev/null +++ b/devices/Paexo/conf/Paexo.xml @@ -0,0 +1,112 @@ + + + + + + + + 0 + + /dev/ttyACM0 + 115200 + 0 + 0 + 0 + 50 + 0 + none + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 8 + 1 + 10 + 13 + + + + + 0.01 + /wearable/paexo + + + /dev/ttyACM1 + 1000 + + 5 + (2) + + + + SerialportDevice + + + + + + + 0.01 + /Paexo/WearableData/data:o + /Paexo/WearableData/metadataRpc:o + + + PaexoWearableDevice + + + + + + + 0.01 + /Paexo/WearableActuatorsCommand/input:i + + + PaexoWearableDevice + + + + + + + 0.01 + (yarp) + true + false + false + false + true + true + true + false + true + false + false + false + false + false + false + false + + true + test_iwear_logger + /path/to/some/folder/ + 100000 + true + 120.0 + 300 + true + + + + PaexoWearableDevice + + + + + + diff --git a/devices/Paexo/conf/paexo-cmake-config.h.in b/devices/Paexo/conf/paexo-cmake-config.h.in new file mode 100644 index 000000000..6288a3845 --- /dev/null +++ b/devices/Paexo/conf/paexo-cmake-config.h.in @@ -0,0 +1 @@ +#cmakedefine ENABLE_PAEXO_USE_iFEELDriver diff --git a/devices/Paexo/conf/paexo-validation-motor-control.sh b/devices/Paexo/conf/paexo-validation-motor-control.sh new file mode 100755 index 000000000..8cd959d68 --- /dev/null +++ b/devices/Paexo/conf/paexo-validation-motor-control.sh @@ -0,0 +1,24 @@ +#!/bin/bash +# set n to 1 +min_motor_pos=30 +max_motor_pos=50 +step_size=2 + +## Paexo initialization +echo "start" | yarp rpc /wearable/paexo/rpc:i +sleep 0.5 +echo "en_bc_data" | yarp rpc /wearable/paexo/rpc:i +sleep 0.5 +echo "init:r" | yarp rpc /wearable/paexo/rpc:i + +# Set max position as initial motor position +current_pos=${max_motor_pos} + +# continue until $n equals 5 +while [ ${min_motor_pos} -le ${current_pos} ] +do + read -s -n1 key + #sleep 0.1 + echo "move:r:${current_pos}" | yarp rpc /wearable/paexo/rpc:i + current_pos=$((current_pos - step_size)) +done diff --git a/devices/Paexo/include/Paexo.h b/devices/Paexo/include/Paexo.h new file mode 100644 index 000000000..5fb354649 --- /dev/null +++ b/devices/Paexo/include/Paexo.h @@ -0,0 +1,254 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_PAEXO_H +#define WEARABLE_PAEXO_H + +#include "Wearable/IWear/IWear.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include "paexo-cmake-config.h" + +namespace wearable { + namespace devices { + class Paexo; + } // namespace devices +} // namespace wearable + +class wearable::devices::Paexo : + public yarp::dev::DeviceDriver, + public yarp::os::PeriodicThread, + public yarp::dev::IWrapper, + public yarp::dev::IMultipleWrapper, + public yarp::dev::IPreciselyTimed, + public wearable::IWear +{ + +private: + class PaexoImpl; + std::unique_ptr pImpl; + +public: + Paexo(); + ~Paexo() override; + + // DeviceDriver + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // PeriodicThread + void run() override; + void threadRelease() override; + + // IWrapper interface + bool attach(yarp::dev::PolyDriver* poly) override; + bool detach() override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() 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; + + ElementPtr getActuator(const actuator::ActuatorName name) const override; + + VectorOfElementPtr getActuators(const actuator::ActuatorType type) const override; + + // IMPLEMENTED SENSORS + // ------------------- + + SensorPtr + getVirtualJointKinSensor(const sensor::SensorName name) const override; + + SensorPtr + getForce3DSensor(const sensor::SensorName name) const override; + + SensorPtr + getTorque3DSensor(const sensor::SensorName name) const override; + +#ifdef ENABLE_PAEXO_USE_iFEELDriver + + SensorPtr + getForceTorque6DSensor(const sensor::SensorName name) const override; + +#else + inline SensorPtr + getForceTorque6DSensor(const sensor::SensorName /*name*/) const override; + +#endif + // IMPLEMENTED ACTUATORS + // --------------------- + + ElementPtr + getMotorActuator(const actuator::ActuatorName) const override; + + // UNIMPLEMENTED SENSORS + // --------------------- + + inline SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const override; + + 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 + 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 + 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; + + // UNIMPLEMENTED ACTUATORS + // ----------------------- + + inline ElementPtr + getHapticActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getHeaterActuator(const actuator::ActuatorName) const override; + +}; + +#ifndef ENABLE_PAEXO_USE_iFEELDriver +inline wearable::SensorPtr +wearable::devices::Paexo::getForceTorque6DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} +#endif + +inline wearable::SensorPtr +wearable::devices::Paexo::getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getMagnetometer(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getOrientationSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getPoseSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getPositionSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getAccelerometer(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getEmgSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getGyroscope(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getSkinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::Paexo::getTemperatureSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::Paexo::getHapticActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::Paexo::getHeaterActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +#endif // WEARABLE_PAEXO_H diff --git a/devices/Paexo/src/Paexo.cpp b/devices/Paexo/src/Paexo.cpp new file mode 100644 index 000000000..e58ddb226 --- /dev/null +++ b/devices/Paexo/src/Paexo.cpp @@ -0,0 +1,1076 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "Paexo.h" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#ifdef ENABLE_PAEXO_USE_iFEELDriver +#include +#endif + +const std::string DeviceName = "Paexo"; +const std::string LogPrefix = DeviceName + wearable::Separator; +double period = 0.01; + +using namespace wearable; +using namespace wearable::devices; + +using YarpBufferedPort = yarp::os::BufferedPort; + +const std::string EOL = "\n"; // EOL character +const int MAX_LINE_LENGTH = 5000; // Maximum line length to read from serial port + +struct PaexoData +{ + bool updated; + double angle; + double force; + double leverarm; +}; + +class Paexo::PaexoImpl +{ +public: + std::unique_ptr network = nullptr; + + mutable std::mutex mutex; + yarp::dev::ISerialDevice* iSerialDevice = nullptr; + + wearable::TimeStamp timeStamp; + + std::string portsPrefix; + yarp::os::BufferedPort dataPort; + + // RPC related + class CmdParser; + std::unique_ptr cmdPro; + yarp::os::RpcServer rpcPort; + + // Paexo data buffer + PaexoData paexoData; + + // Wearable variables + wearable::WearableName wearableName; + + // Joint Sensor + std::string jointSensorPrefix; + const std::string jointSensorName = "Angle"; + class PaexoVirtualJointKinSensor; + SensorPtr paexoJointSensor; + + // 3D Force Sensor + std::string forceSensorPrefix; + const std::string forceSensorName = "SupportForce"; + class PaexoForce3DSensor; + SensorPtr paexoForceSensor; + + // 3D Torque Sensor + std::string torqueSensorPrefix; + const std::string torqueSensorName = "LeverArm"; + class PaexoTorque3DSensor; + SensorPtr paexoTorqueSensor; + + // 6D Force-Torque Sensors + std::string ftSensorPrefix; + const std::string ftSensorName = "ArmForceTorque"; + class PaexoForceTorque6DSensor; + SensorPtr paexoLeftArmFTSensor; + SensorPtr paexoRightArmFTSensor; + + // Motor Actuator + std::string motorActuatorPrefix; + const std::string motorActuatorName = "Motor"; + class PaexoMotorActuator; + ElementPtr paexoLeftMotorActuator; + ElementPtr paexoRightMotorActuator; + + // Motor actuator yarp port control + class PaexoMotorControlPort; + std::unique_ptr paexoLeftMotorControlPort; + std::unique_ptr paexoRightMotorControlPort; + + // Helper function to split wearable element name + // TODO: To be moved to wearables utilities + inline std::vector split(const std::string& s, const std::string& delimiter) + { + std::size_t pos_start = 0, pos_end, delim_len = delimiter.length(); + std::string token; + std::vector res; + + while ((pos_end = s.find(delimiter, pos_start)) != std::string::npos) { + token = s.substr(pos_start, pos_end - pos_start); + pos_start = pos_end + delim_len; + res.push_back(token); + } + + res.push_back(s.substr(pos_start)); + return res; + } + + inline std::string getValidYarpName(const std::string& actuatorName) + { + std::string portName; + + // Get valid port name from actuator name + const auto vecStr = split(actuatorName, wearable::Separator); + + for (const auto& str : vecStr) { + if (portName.empty()) { + portName = str; + } + else { + portName = portName + '/' + str; + } + } + + portName = "/" + portName + ":i"; + + return portName; + } + + // Number of sensors + const int nSensors = 3; // Hardcoded for Paexo + + // Numbe of actuators + const int nActuators = 1; // Hardcoded for Paexo + + // First data flag + bool firstDataRead; + + // constructor + PaexoImpl(); + +#ifdef ENABLE_PAEXO_USE_iFEELDriver + // iFeelDriver + bool useiFeelDriver; + iFeel::SerialConfig serial; // serial port configuraton + iFeel::iFeelSystemConfig ifeelConfig; // iFeelDriver configuration + iFeel::iFeelSystemData ifeelData; // iFeel sensors data + std::unique_ptr ifeelDriver{nullptr}; +#endif +}; + +class Paexo::PaexoImpl::CmdParser : public yarp::os::PortReader +{ + +public: + std::string cmdString; + bool cmdUpdated = false; + bool data_broadcast = false; + bool measurement_status = false; + + bool read(yarp::os::ConnectionReader& connection) override + { + yarp::os::Bottle command, response; + if (command.read(connection) && !cmdUpdated) { + cmdString = command.toString(); + + if (*cmdString.begin() == '"' && *(cmdString.end() - 1) == '"') { + cmdString = cmdString.substr(1, cmdString.size() - 2); + } + + response.addString("Entered commands is " + cmdString); + + // TODO: This check can be better if status returns the measurement and broadcast + // information Check for measurement related command + if (cmdString == "start") { + measurement_status = true; + } + else if (cmdString == "stop") { + measurement_status = false; + } + + // Check for data boardcast related command + if (cmdString == "en_bc_data") { + data_broadcast = true; + } + else if (cmdString == "di_bc_data") { + data_broadcast = false; + } + + cmdString.append(EOL); + cmdUpdated = true; + + yarp::os::ConnectionWriter* reply = connection.getWriter(); + + if (reply != NULL) { + response.write(*reply); + } + else + return false; + } + + return true; + } +}; + +Paexo::PaexoImpl::PaexoImpl() + : cmdPro(new CmdParser()) +{} + +// Default constructor +Paexo::Paexo() + : PeriodicThread(period) + , pImpl{new PaexoImpl()} +{} + +// Destructor +Paexo::~Paexo() = default; + +class Paexo::PaexoImpl::PaexoMotorControlPort : public YarpBufferedPort +{ +public: + std::string portName; + ElementPtr paexoMotorActuator = nullptr; + + void onRead(yarp::os::Bottle& motorCommand) override; + + // Constructor + PaexoMotorControlPort(const std::string& name, + ElementPtr& actuator) + { + portName = name; + // Set the actutor + if (actuator == nullptr) { + throw std::runtime_error(LogPrefix + "Actuator passing error for motor control port " + + portName); + } + + paexoMotorActuator = actuator; + } +}; + +bool Paexo::open(yarp::os::Searchable& config) +{ + // ================================== + // Check the configuration parameters + // ================================== + + // Period of the this device + if (!(config.check("period") && config.find("period").isFloat64())) { + yInfo() << LogPrefix << "Using default period: " << period << "s"; + } + else { + period = config.find("period").asFloat64(); + yInfo() << LogPrefix << "Using the period : " << period << "s"; + } + + // Get port prefix name + if (!(config.check("portsPrefixName") && config.find("portsPrefixName").isString())) { + yInfo() << LogPrefix << "Using default port prefix /wearable/paexo"; + } + else { + pImpl->portsPrefix = config.find("portsPrefixName").asString(); + yInfo() << LogPrefix << "Using the ports prefix " << pImpl->portsPrefix; + } + + if (!(config.check("wearableName") && config.find("wearableName").isString())) { + yInfo() << LogPrefix << "Using default wearable name Paexo"; + pImpl->wearableName = DeviceName; + } + else { + pImpl->wearableName = config.find("wearableName").asString(); + yInfo() << LogPrefix << "Using the wearable name " << pImpl->wearableName; + } + + // ========================================== + // Check iFeelDriver configuration parameters + // ========================================== + + yarp::os::Bottle& ifeelDriverGroup = config.findGroup("iFeelDriver"); + +#ifdef ENABLE_PAEXO_USE_iFEELDriver + pImpl->useiFeelDriver = false; + if (!ifeelDriverGroup.isNull()) { + yarp::os::Bottle& ifeelDriverSerialGroup = ifeelDriverGroup.findGroup("serial-config"); + + if (ifeelDriverSerialGroup.isNull()) { + yError() << LogPrefix + << "iFeelDriver serial-config is not provided, cannot configure iFeelDriver " + "to be used with Paexo"; + return false; + } + + // Configure iFeelDriver serial device + if (!ifeelDriverSerialGroup.check("serial-port-name")) { + yError() << LogPrefix << "REQUIRED parameter NOT found."; + return false; + } + pImpl->serial.name = ifeelDriverSerialGroup.find("serial-port-name").asString(); + yInfo() << LogPrefix << "iFeelDriver configuring through the serial port " + << pImpl->serial.name; + + // Serial timeout + uint32_t timeoutms; + if (!ifeelDriverSerialGroup.check("serial-timeout-ms")) { + timeoutms = 100; + yWarning() << LogPrefix + << "Optional parameter NOT found. Setting default value:" + << timeoutms << " ms."; + } + else { + timeoutms = + static_cast(ifeelDriverSerialGroup.find("serial-timeout-ms").asInt32()); + } + pImpl->serial.tout = serial::Timeout::simpleTimeout(timeoutms); + + // TODO: Some of the serial ports can be passed through configuration file + pImpl->serial.size = 256; + pImpl->serial.sleep_us = 100; + pImpl->serial.baudrate = 9600; + pImpl->serial.eol = END_OF_LINE_STR; + + // iFeelDriver configuration parameters + if (!ifeelDriverGroup.check("ifeeldriver-config-timeout-s")) { + pImpl->ifeelConfig.ConfigurationDuration = std::chrono::seconds(1); + yWarning() << LogPrefix + << "Optional parameter NOT found. Using " + "default value of 1 second"; + } + else { + pImpl->ifeelConfig.ConfigurationDuration = static_cast( + ifeelDriverGroup.find("ifeeldriver-config-timeout-s").asInt32()); + } + + // iFeelDriver nodes configuration list + if (!(ifeelDriverGroup.check("ifeeldriver-config-nodes") + && ifeelDriverGroup.find("ifeeldriver-config-nodes").isList())) { + yError() << LogPrefix + << "Required parameter NOT Found or is not a valid " + "list of NodeIDs"; + return false; + } + + yarp::os::Bottle* nodeList = ifeelDriverGroup.find("ifeeldriver-config-nodes").asList(); + + // NOTE: Assuming only one FTShoeNode is configured through iFeelDriver + assert(nodeList->size() == 1 && nodeList->get(i).asInt32() == 2); + + for (size_t i = 0; i < nodeList->size(); i++) { + pImpl->ifeelConfig.ConfigurationNodes.push_back(nodeList->get(i).asInt32()); + } + + pImpl->ifeelDriver = + std::make_unique(pImpl->serial, pImpl->ifeelConfig); + + // initialize iFeel driver + if (!pImpl->ifeelDriver->init()) { + yError() << LogPrefix << "Failed to initialize iFeelDriver."; + return false; + } + + yInfo() << LogPrefix << "iFeelDriver is configured to be used with Paexo "; + } + else { + yWarning() << LogPrefix << "iFeelDriver is not configured to be used with Paexo "; + } + +#endif + + // =================== + // Ports configuration + // =================== + if (!pImpl->dataPort.open(pImpl->portsPrefix + ":o")) { + yError() << LogPrefix << "Failed to open data port " << pImpl->portsPrefix + ":o"; + return false; + } + + if (!pImpl->rpcPort.open(pImpl->portsPrefix + "/rpc:i")) { + yError() << LogPrefix << "Failed to open rpc port " << pImpl->portsPrefix + "/rpc:i"; + return false; + } + + // Set rpc port reader + pImpl->rpcPort.setReader(*pImpl->cmdPro); + + // Intialize wearable sensors + pImpl->jointSensorPrefix = getWearableName() + sensor::IVirtualJointKinSensor::getPrefix(); + pImpl->paexoJointSensor = SensorPtr{ + std::make_shared( + pImpl.get(), pImpl->jointSensorPrefix + pImpl->jointSensorName)}; + + pImpl->forceSensorPrefix = getWearableName() + sensor::IForce3DSensor::getPrefix(); + pImpl->paexoForceSensor = + SensorPtr{std::make_shared( + pImpl.get(), pImpl->forceSensorPrefix + pImpl->forceSensorName)}; + + pImpl->torqueSensorPrefix = getWearableName() + sensor::ITorque3DSensor::getPrefix(); + pImpl->paexoTorqueSensor = + SensorPtr{std::make_shared( + pImpl.get(), pImpl->torqueSensorPrefix + pImpl->torqueSensorName)}; + +#ifdef ENABLE_PAEXO_USE_iFEELDriver + pImpl->ftSensorPrefix = getWearableName() + sensor::IForceTorque6DSensor::getPrefix(); + pImpl->paexoLeftArmFTSensor = SensorPtr{ + std::make_shared( + pImpl.get(), pImpl->ftSensorPrefix + "Left" + pImpl->ftSensorName)}; + pImpl->paexoRightArmFTSensor = SensorPtr{ + std::make_shared( + pImpl.get(), pImpl->ftSensorPrefix + "Right" + pImpl->ftSensorName)}; +#endif + + // Initialize wearable actuators for left and right motor + pImpl->motorActuatorPrefix = getWearableName() + actuator::IMotor::getPrefix(); + const std::string leftActuatorName = + pImpl->motorActuatorPrefix + "Left" + pImpl->motorActuatorName; + pImpl->paexoLeftMotorActuator = + std::make_shared(pImpl.get(), leftActuatorName); + + const std::string rightActuatorName = + pImpl->motorActuatorPrefix + "Right" + pImpl->motorActuatorName; + pImpl->paexoRightMotorActuator = + std::make_shared(pImpl.get(), rightActuatorName); + + // Initialize yarp control ports + + // Check yarp network initialization + pImpl->network = std::make_unique(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + yError() << LogPrefix << "YARP server wasn't found active."; + return false; + } + + yInfo() << LogPrefix << "Initiailizing PaexoMotorControlPort for " << leftActuatorName; + + const std::string leftActuatorPortName = pImpl->getValidYarpName(leftActuatorName); + pImpl->paexoLeftMotorControlPort = std::make_unique( + leftActuatorPortName, pImpl->paexoLeftMotorActuator); + pImpl->paexoLeftMotorControlPort->useCallback(); + + if (!pImpl->paexoLeftMotorControlPort->open(leftActuatorPortName)) { + yError() << LogPrefix << "Failed to open paexo motor control port " << leftActuatorPortName; + return false; + } + + yInfo() << LogPrefix << "Initiailizing PaexoMotorControlPort for " << rightActuatorName; + + const std::string rightActuatorPortName = pImpl->getValidYarpName(rightActuatorName); + pImpl->paexoRightMotorControlPort = std::make_unique( + rightActuatorPortName, pImpl->paexoRightMotorActuator); + pImpl->paexoRightMotorControlPort->useCallback(); + + if (!pImpl->paexoRightMotorControlPort->open(rightActuatorPortName)) { + yError() << LogPrefix << "Failed to open paexo motor control port " + << rightActuatorPortName; + return false; + } + + // Initialize paexo data buffer + pImpl->paexoData.angle = 0.0; + pImpl->paexoData.force = 0.0; + pImpl->paexoData.leverarm = 0.0; + pImpl->paexoData.updated = false; + + // Initialize first data flag + pImpl->firstDataRead = false; + + return true; +} + +// ============================================= +// Paexo implementation of VirtualJointKinSensor +// ============================================= +class Paexo::PaexoImpl::PaexoVirtualJointKinSensor : public wearable::sensor::IVirtualJointKinSensor +{ +public: + Paexo::PaexoImpl* paexoImpl = nullptr; + + PaexoVirtualJointKinSensor( + Paexo::PaexoImpl* impl, + const wearable::sensor::SensorName name = {}, + const wearable::sensor::SensorStatus status = + wearable::sensor::SensorStatus::Ok) // Default sensors status is set Ok + : IVirtualJointKinSensor(name, status) + , paexoImpl(impl) + { + // TODO: Initialization + } + + ~PaexoVirtualJointKinSensor() override = default; + + void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } + + bool getJointPosition(double& position) const override + { + assert(paexoImpl != nullptr); + + std::lock_guard lock(paexoImpl->mutex); + position = paexoImpl->paexoData.angle; + return true; + } + + bool getJointVelocity(double& velocity) const override + { + assert(paexoImpl != nullptr); + + velocity = 0.0; + + return true; + } + + bool getJointAcceleration(double& acceleration) const override + { + assert(paexoImpl != nullptr); + + acceleration = 0.0; + + return true; + } +}; + +// ===================================== +// Paexo implementation of Force3DSensor +// ===================================== +class Paexo::PaexoImpl::PaexoForce3DSensor : public wearable::sensor::IForce3DSensor +{ +public: + Paexo::PaexoImpl* paexoImpl = nullptr; + + PaexoForce3DSensor(Paexo::PaexoImpl* impl, + const wearable::sensor::SensorName name = {}, + const wearable::sensor::SensorStatus status = + wearable::sensor::SensorStatus::Ok) // Default sensors status is set Ok + : IForce3DSensor(name, status) + , paexoImpl(impl) + { + // TODO: Initialization + } + + ~PaexoForce3DSensor() override = default; + + void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } + + bool getForce3D(Vector3& force) const override + { + assert(paexoImpl != nullptr); + + std::lock_guard lock(paexoImpl->mutex); + force[0] = paexoImpl->paexoData.force; + force[1] = 0.0; + force[2] = 0.0; + return true; + } +}; + +// ====================================== +// Paexo implementation of Torque3DSensor +// ====================================== +class Paexo::PaexoImpl::PaexoTorque3DSensor : public wearable::sensor::ITorque3DSensor +{ +public: + Paexo::PaexoImpl* paexoImpl = nullptr; + + PaexoTorque3DSensor(Paexo::PaexoImpl* impl, + const wearable::sensor::SensorName name = {}, + const wearable::sensor::SensorStatus status = + wearable::sensor::SensorStatus::Ok) // Default sensors status is set Ok + : ITorque3DSensor(name, status) + , paexoImpl(impl) + { + // TODO: Initialization + } + + ~PaexoTorque3DSensor() override = default; + + void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } + + bool getTorque3D(Vector3& torque) const override + { + assert(paexoImpl != nullptr); + + std::lock_guard lock(paexoImpl->mutex); + torque[0] = paexoImpl->paexoData.leverarm; + torque[1] = 0.0; + torque[2] = 0.0; + return true; + } +}; + +// =========================================== +// Paexo implementation of ForceTorque6DSensor +// =========================================== +#ifdef ENABLE_PAEXO_USE_iFEELDriver +class Paexo::PaexoImpl::PaexoForceTorque6DSensor : public wearable::sensor::IForceTorque6DSensor +{ +public: + Paexo::PaexoImpl* paexoImpl = nullptr; + + PaexoForceTorque6DSensor( + Paexo::PaexoImpl* impl, + const wearable::sensor::SensorName name = {}, + const wearable::sensor::SensorStatus status = + wearable::sensor::SensorStatus::Ok) // Default sensor status is Ok, to be updated using + // node status from iFeelDriver + : IForceTorque6DSensor(name, status) + , paexoImpl(impl) + { + // TODO: Initialization + } + + ~PaexoForceTorque6DSensor() override = default; + + void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } + + bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const override + { + assert(paexoImpl != nullptr); + + std::lock_guard lock(paexoImpl->mutex); + + // Get FT Data from front or back FT using iFeelDriver data + // TODO: Cleanup ftData buffer handling + paexoImpl->ifeelData = paexoImpl->ifeelDriver->getData(); + + auto it = paexoImpl->ifeelData.FTShoeNodes.begin(); + iFeel::Utils::DataTypes::NodeID nodeID = it->first; + + if (this->getSensorName().find("Left") != std::string::npos) { + iFeel::ForceTorque ftData = paexoImpl->ifeelData.FTShoeNodes[nodeID]->getFrontFT(); + + force3D[0] = ftData.x; + force3D[1] = ftData.y; + force3D[2] = ftData.z; + + torque3D[0] = ftData.tx; + torque3D[1] = ftData.ty; + torque3D[2] = ftData.tz; + } + + if (this->getSensorName().find("Right") != std::string::npos) { + iFeel::ForceTorque ftData = paexoImpl->ifeelData.FTShoeNodes[nodeID]->getBackFT(); + + force3D[0] = ftData.x; + force3D[1] = ftData.y; + force3D[2] = ftData.z; + + torque3D[0] = ftData.tx; + torque3D[1] = ftData.ty; + torque3D[2] = ftData.tz; + } + + return true; + } +}; + +#endif + +// ======================================= +// Paexo implementation of Motor actutator +// ======================================= +class Paexo::PaexoImpl::PaexoMotorActuator : public wearable::actuator::IMotor +{ +public: + Paexo::PaexoImpl* paexoImpl = nullptr; + + PaexoMotorActuator( + Paexo::PaexoImpl* impl, + const wearable::actuator::ActuatorName name = {}, + const wearable::actuator::ActuatorStatus status = + wearable::actuator::ActuatorStatus::Ok) // Default actuator status is set ok + : IMotor(name, status) + , paexoImpl(impl) + { + // TODO: Initialization + } + + bool setMotorPosition(double& value) const override + { + // Prepare the move command + std::string motorCommand = {}; + + if (this->getActuatorName().find("Left") != std::string::npos) { + motorCommand = "move:l:" + std::to_string(value); + } + + if (this->getActuatorName().find("Right") != std::string::npos) { + motorCommand = "move:r:" + std::to_string(value); + } + + motorCommand += EOL; + + char c[motorCommand.length() + 1]; + std::strcpy(c, motorCommand.c_str()); + + // Set the commanded value to the serial write + // TODO: Check for serial write failure + paexoImpl->iSerialDevice->flush(); + paexoImpl->iSerialDevice->send(c, motorCommand.length()); + + return true; + } +}; + +void Paexo::PaexoImpl::PaexoMotorControlPort::onRead(yarp::os::Bottle& motorCommand) +{ + // TODO: Check if mutex is needed + // NOTE: Assuming the associated port received a vector of one double as motor command + assert(paexoMotorActuator != nullptr); + // yInfo() << LogPrefix << "Data received on " << portName << motorCommand.toString().c_str(); + + double cmd = motorCommand.get(0).asFloat64(); + paexoMotorActuator.get()->setMotorPosition(cmd); +} + +void Paexo::run() +{ + // Send commands to BLE central serial port + { + std::lock_guard lock(pImpl->mutex); + if (pImpl->cmdPro->cmdUpdated) { + + int s = pImpl->cmdPro->cmdString.length(); + char c[s + 1]; + std::strcpy(c, pImpl->cmdPro->cmdString.c_str()); + if (pImpl->iSerialDevice->send(c, s)) { + pImpl->cmdPro->cmdUpdated = false; + } + } + } + + char msg[MAX_LINE_LENGTH]; + int size = pImpl->iSerialDevice->receiveLine(msg, MAX_LINE_LENGTH); + + if (size > 1) { + + // Get timestamp + pImpl->timeStamp.time = yarp::os::Time::now(); + + // Check if the first char is a digit, if it is the received message is broadcast + // information + if (isdigit(msg[0]) + && (pImpl->cmdPro->measurement_status && pImpl->cmdPro->data_broadcast)) { + + // Prepare yarp bottle with serial message and write to yarp port + yarp::os::Bottle& bc_data = pImpl->dataPort.prepare(); + bc_data.clear(); + bc_data.fromString(msg); + + pImpl->dataPort.write(); + + // Add baroadcast data to buffer + std::lock_guard lock(pImpl->mutex); + pImpl->paexoData.angle = bc_data.get(0).asFloat64(); + pImpl->paexoData.force = bc_data.get(1).asFloat64(); + pImpl->paexoData.leverarm = bc_data.get(2).asFloat64(); + pImpl->paexoData.updated = true; + } + else if (!isdigit(msg[0])) { + // yInfo() << LogPrefix << msg; + } + } + + if (!pImpl->firstDataRead && pImpl->paexoData.updated) { + + // Set sensor status + // TODO: Check if the sensor status can be modified from here + pImpl->firstDataRead = true; + } +} + +bool Paexo::close() +{ + detach(); + return true; +} + +bool Paexo::attach(yarp::dev::PolyDriver* poly) +{ + if (!poly) { + yError() << LogPrefix << "Passed PolyDriver is a nullptr"; + return false; + } + + if (pImpl->iSerialDevice || !poly->view(pImpl->iSerialDevice) || !pImpl->iSerialDevice) { + yError() + << LogPrefix + << "Failed to view the ISerialDevice interface from the attached polydriver device"; + return false; + } + else { + yInfo() << LogPrefix << "ISerialDevice interface viewed correctly"; + } + + // TODO: Check if the ISerialDevice interface is configured correctly + // I do not see any method to check this + + // Start the PeriodicThread loop + if (!start()) { + yError() << LogPrefix << "Failed to start the period thread."; + return false; + } + + yInfo() << LogPrefix << "attach() successful"; + return true; +} + +bool Paexo::detach() +{ + while (yarp::os::PeriodicThread::isRunning()) { + yarp::os::PeriodicThread::stop(); + } + + pImpl->iSerialDevice = nullptr; + return true; +} + +bool Paexo::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + // A single serial device will be streaming data from all the sensors from the FTShoes + if (driverList.size() > 1) { + yError() << LogPrefix << "This wrapper accepts only one attached yarp Serial device"; + return false; + } + + const yarp::dev::PolyDriverDescriptor* driver = driverList[0]; + + if (!driver) { + yError() << LogPrefix << "Passed PolyDriverDescriptor is nullptr"; + return false; + } + + return attach(driver->poly); +} + +bool Paexo::detachAll() +{ + return detach(); +} + +void Paexo::threadRelease() {} + +// ========================= +// IPreciselyTimed interface +// ========================= +yarp::os::Stamp Paexo::getLastInputStamp() +{ + // Stamp count should be always zero + std::lock_guard lock(pImpl->mutex); + return yarp::os::Stamp(0, pImpl->timeStamp.time); +} + +// --------------------------- +// Implement Sensors Methods +// --------------------------- + +wearable::WearableName Paexo::getWearableName() const +{ + return pImpl->wearableName + wearable::Separator; +} + +wearable::WearStatus Paexo::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 Paexo::getTimeStamp() const +{ + std::lock_guard lock(pImpl->mutex); + return {pImpl->timeStamp.time, 0}; +} + +wearable::SensorPtr +Paexo::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 +Paexo::getSensors(const wearable::sensor::SensorType aType) const +{ + wearable::VectorOfSensorPtr outVec; + outVec.reserve(pImpl->nSensors); + switch (aType) { + case sensor::SensorType::VirtualJointKinSensor: { + outVec.push_back(static_cast>(pImpl->paexoJointSensor)); + break; + } + case sensor::SensorType::Force3DSensor: { + outVec.push_back(static_cast>(pImpl->paexoForceSensor)); + break; + } + case sensor::SensorType::Torque3DSensor: { + outVec.push_back(static_cast>(pImpl->paexoTorqueSensor)); + break; + } + case sensor::SensorType::ForceTorque6DSensor: { +#ifdef ENABLE_PAEXO_USE_iFEELDriver + outVec.push_back(static_cast>(pImpl->paexoLeftArmFTSensor)); + outVec.push_back(static_cast>(pImpl->paexoRightArmFTSensor)); +#endif + break; + } + default: { + return {}; + } + } + + return outVec; +} + +wearable::ElementPtr +Paexo::getActuator(const wearable::actuator::ActuatorName name) const +{ + wearable::VectorOfElementPtr actuators = getAllActuators(); + + for (const auto& a : actuators) { + if (a->getActuatorName() == name) { + return a; + } + } + yWarning() << LogPrefix << "User specified actuator name <" << name << "> not found"; + return nullptr; +} + +wearable::VectorOfElementPtr +Paexo::getActuators(const wearable::actuator::ActuatorType aType) const +{ + wearable::VectorOfElementPtr outVec; + outVec.reserve(pImpl->nActuators); + + switch (aType) { + case wearable::actuator::ActuatorType::Motor: { + outVec.push_back( + static_cast>(pImpl->paexoLeftMotorActuator)); + outVec.push_back( + static_cast>(pImpl->paexoRightMotorActuator)); + break; + } + default: { + return {}; + } + } + + return outVec; +} + +// ------------ +// JOINT Sensor +// ------------ +wearable::SensorPtr +Paexo::getVirtualJointKinSensor(const wearable::sensor::SensorName name) const +{ + + // Check if user-provided name corresponds to an available sensor + if (name == pImpl->jointSensorPrefix + pImpl->jointSensorName) { + yError() << LogPrefix << "Invalid sensor name " << name; + return nullptr; + } + + // Return a shared point to the required sensor + return dynamic_cast&>( + *pImpl->paexoJointSensor); +} + +// ------------ +// FORCE Sensor +// ------------ +wearable::SensorPtr +Paexo::getForce3DSensor(const wearable::sensor::SensorName name) const +{ + + // Check if user-provided name corresponds to an available sensor + if (name == pImpl->forceSensorPrefix + pImpl->forceSensorName) { + yError() << LogPrefix << "Invalid sensor name " << name; + return nullptr; + } + + // Return a shared point to the required sensor + return dynamic_cast&>( + *pImpl->paexoForceSensor); +} + +// ------------- +// TORQUE Sensor +// ------------- +wearable::SensorPtr +Paexo::getTorque3DSensor(const wearable::sensor::SensorName name) const +{ + + // Check if user-provided name corresponds to an available sensor + if (name == pImpl->torqueSensorPrefix + pImpl->torqueSensorName) { + yError() << LogPrefix << "Invalid sensor name " << name; + return nullptr; + } + + // Return a shared point to the required sensor + return dynamic_cast&>( + *pImpl->paexoTorqueSensor); +} + +// ------------------- +// FORCE TORQUE Sensor +// ------------------- +#ifdef ENABLE_PAEXO_USE_iFEELDriver +wearable::SensorPtr +wearable::devices::Paexo::getForceTorque6DSensor(const sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (name == pImpl->ftSensorPrefix + "Left" + pImpl->ftSensorName + || name == pImpl->ftSensorPrefix + "Right" + pImpl->ftSensorName) { + yError() << LogPrefix << "Invalid sensor name " << name; + return nullptr; + } + + // Return a shared point to the required sensor + // TODO: Return corresponding left and right sensors + wearable::SensorPtr ftSensor; + if (name.find("Left")) { + ftSensor = dynamic_cast&>( + *pImpl->paexoLeftArmFTSensor); + } + else if (name.find("Right")) { + ftSensor = dynamic_cast&>( + *pImpl->paexoRightArmFTSensor); + } + return ftSensor; +} +#endif + +// --------------- +// MOTORO Actuator +// --------------- +wearable::ElementPtr +Paexo::getMotorActuator(const actuator::ActuatorName name) const +{ + // Check if user-provided name corresponds to an available actuator + if (name == pImpl->motorActuatorPrefix + pImpl->motorActuatorName) { + yError() << LogPrefix << "Invalid actuator name " << name; + return nullptr; + } + + // Return a shared point to the required actuator + return dynamic_cast&>( + *pImpl->paexoLeftMotorActuator); + return dynamic_cast&>( + *pImpl->paexoRightMotorActuator); +} diff --git a/devices/Paexo/test-application/CMakeLists.txt b/devices/Paexo/test-application/CMakeLists.txt new file mode 100644 index 000000000..7e3e4ac78 --- /dev/null +++ b/devices/Paexo/test-application/CMakeLists.txt @@ -0,0 +1,20 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +cmake_minimum_required(VERSION 3.5) +project(PaexoTestApplication) + +find_package(IWear REQUIRED) +find_package(WearableActuators REQUIRED) +find_package(YARP COMPONENTS os REQUIRED) + +add_executable(${PROJECT_NAME} paexo-test-application.cpp) + +target_link_libraries(${PROJECT_NAME} PUBLIC + IWear::IWear + WearableActuators::WearableActuators + YARP::YARP_os + YARP::YARP_init) + + diff --git a/devices/Paexo/test-application/paexo-test-application.cpp b/devices/Paexo/test-application/paexo-test-application.cpp new file mode 100644 index 000000000..16f32aac6 --- /dev/null +++ b/devices/Paexo/test-application/paexo-test-application.cpp @@ -0,0 +1,78 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include +#include +#include +#include +#include +#include + +#include + +#define MIN_MOTOR_POSITION 30 +#define MAX_MOTOR_POSITION 50 +#define MOTOR_STEP_SIZE 2 //Could go as low as 0.2 +using namespace yarp::os; +using namespace wearable; +using YarpBufferedPort = yarp::os::BufferedPort; + +int main() { + + yarp::os::Network::init(); + + std::string inPort = "/Paexo/WearableActuatorsCommand/input:i"; + std::string outPort = "/Paexo/WearableActuatorsCommand/output:o"; + + std::vector PaexoActuators = {"Paexo::motor::LeftMotor", "Paexo::motor::RightMotor"}; + + std::vector PaexoActuatorPortNames = {"/Paexo/motor/LeftMotor"}; + std::vector> PaexoActuatorPorts; + + // Yarp buffered ports for motor control + for (const auto& portName : PaexoActuatorPortNames) + { + std::unique_ptr port = std::make_unique(); + + if (!port->open(portName + ":o")) + { + yError() << "Failed to open port " << (portName + ":o"); + return -1; + } + + if (!yarp::os::Network::connect(portName + ":o", portName + ":i")) + { + yError() << "Failed to connect ports " << (portName + ":o") << " and " << (portName + ":i"); + return -1; + } + + PaexoActuatorPorts.push_back(std::move(port)); + } + + + while (true) { + + for (const auto& port : PaexoActuatorPorts) + { + double current_motor_position = MIN_MOTOR_POSITION; + + while (current_motor_position < MAX_MOTOR_POSITION) + { + yarp::os::Bottle& cmd = port->prepare(); + + cmd.clear(); + + yInfo() << "Sending motor position " << current_motor_position; + cmd.addDouble(current_motor_position); + + port->write(); + + Time::delay(0.5); + + current_motor_position += MOTOR_STEP_SIZE; + } + } + } + + return 0; +} diff --git a/devices/RobotPositionController/CMakeLists.txt b/devices/RobotPositionController/CMakeLists.txt index 3d406b346..baef3b5d2 100644 --- a/devices/RobotPositionController/CMakeLists.txt +++ b/devices/RobotPositionController/CMakeLists.txt @@ -1,7 +1,6 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause -find_package(IWear REQUIRED) find_package(iDynTree REQUIRED) find_package(ICUB REQUIRED) diff --git a/devices/XsensSuit/CMakeLists.txt b/devices/XsensSuit/CMakeLists.txt new file mode 100644 index 000000000..f9f87c4c4 --- /dev/null +++ b/devices/XsensSuit/CMakeLists.txt @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(xsens_suit + TYPE wearable::devices::XsensSuit + INCLUDE include/XsensSuit.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(XsensSuit + src/XsensSuit.cpp + include/XsensSuit.h) + +target_include_directories(XsensSuit PRIVATE + $) + +target_link_libraries(XsensSuit PUBLIC + IWear::IWear IXsensMVNControl XSensMVN YARP::YARP_dev YARP::YARP_init) + +yarp_install( + TARGETS XsensSuit + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) diff --git a/devices/XsensSuit/conf/XsensSuit.xml b/devices/XsensSuit/conf/XsensSuit.xml new file mode 100644 index 000000000..4176f51bf --- /dev/null +++ b/devices/XsensSuit/conf/XsensSuit.xml @@ -0,0 +1,43 @@ + + + + + "C:\Program Files\Xsens\MVN SDK 2018.0.3\SDK Files\rundeps" + + + + + FullBody + + + singleLevel + + + Npose + + + acceptable + + 60 + + + 120 + + + 0.07 + 1.71 + 1.71 + 0.26 + 0.87 + 0.25 + 0.50 + 0.34 + 0.02 + + + + false + true + true + + diff --git a/devices/XsensSuit/include/XsensSuit.h b/devices/XsensSuit/include/XsensSuit.h new file mode 100644 index 000000000..907e611ac --- /dev/null +++ b/devices/XsensSuit/include/XsensSuit.h @@ -0,0 +1,237 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef XSENSSUIT_H +#define XSENSSUIT_H + +#include "IXsensMVNControl.h" +#include "Wearable/IWear/IWear.h" + +#include +#include + +#include + +namespace wearable { + namespace devices { + class XsensSuit; + } // namespace devices +} // namespace wearable + +class wearable::devices::XsensSuit final + : public yarp::dev::DeviceDriver + , public yarp::dev::IPreciselyTimed + , public xsensmvn::IXsensMVNControl + , public wearable::IWear +{ +private: + class XsensSuitImpl; + std::unique_ptr pImpl; + +public: + XsensSuit(); + ~XsensSuit() override; + + XsensSuit(const XsensSuit& other) = delete; + XsensSuit(XsensSuit&& other) = delete; + XsensSuit& operator=(const XsensSuit& other) = delete; + XsensSuit& operator=(XsensSuit&& other) = delete; + + // ============= + // DEVICE_DRIVER + // ============= + + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // ================ + // IPRECISELY_TIMED + // ================ + + yarp::os::Stamp getLastInputStamp() override; + + // ================== + // IXSENS_MVN_CONTROL + // ================== + + bool setBodyDimensions(const std::map& dimensions) override; + bool getBodyDimensions(std::map& dimensions) const override; + bool getBodyDimension(const std::string bodyName, double& dimension) const override; + + // Calibration methods + bool calibrate(const std::string& calibrationType = {}) override; + bool abortCalibration() override; + + // Acquisition methods + bool startAcquisition() override; + bool stopAcquisition() 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 + getFreeBodyAccelerationSensor(const sensor::SensorName name) const override; + + SensorPtr + getMagnetometer(const sensor::SensorName name) const override; + + SensorPtr + getOrientationSensor(const sensor::SensorName name) const override; + + SensorPtr + getPoseSensor(const sensor::SensorName name) const override; + + SensorPtr + getPositionSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName name) const override; + + SensorPtr + getVirtualSphericalJointKinSensor(const sensor::SensorName name) const override; + + // NOT IMPLEMENTED SENSORS + // ----------------------- + + 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 + getForceTorque6DSensor(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 SensorPtr + getVirtualJointKinSensor(const sensor::SensorName /*name*/) const override; + + inline ElementPtr + getActuator(const actuator::ActuatorName name) const override; + + inline VectorOfElementPtr + getActuators(const actuator::ActuatorType type) const override; + + inline ElementPtr + getHapticActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getMotorActuator(const actuator::ActuatorName) const override; + + inline ElementPtr + getHeaterActuator(const actuator::ActuatorName) const override; +}; + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getAccelerometer(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getEmgSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getForce3DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getForceTorque6DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getGyroscope(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getSkinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getTemperatureSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getTorque3DSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::SensorPtr +wearable::devices::XsensSuit::getVirtualJointKinSensor(const sensor::SensorName /*name*/) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::XsensSuit::getActuator(const actuator::ActuatorName name) const +{ + return nullptr; +} + +inline wearable::VectorOfElementPtr +wearable::devices::XsensSuit::getActuators(const actuator::ActuatorType type) const +{ + return {}; +} + +inline wearable::ElementPtr +wearable::devices::XsensSuit::getHapticActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::XsensSuit::getMotorActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +inline wearable::ElementPtr +wearable::devices::XsensSuit::getHeaterActuator(const actuator::ActuatorName) const +{ + return nullptr; +} + +#endif // XSENSSUIT_H diff --git a/devices/XsensSuit/src/XsensSuit.cpp b/devices/XsensSuit/src/XsensSuit.cpp new file mode 100644 index 000000000..aed1d6289 --- /dev/null +++ b/devices/XsensSuit/src/XsensSuit.cpp @@ -0,0 +1,1236 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "XsensSuit.h" +#include "XSensMVNDriver.h" + +#include +#include + +#include +#include +#include + +using namespace wearable::devices; + +const std::string logPrefix = "XsensSuit :"; + +class XsensSuit::XsensSuitImpl +{ +public: + class XsensFreeBodyAccelerationSensor; + class XsensPositionSensor; + class XsensOrientationSensor; + class XsensPoseSensor; + class XsensMagnetometer; + class XsensVirtualLinkKinSensor; + class XsensVirtualSphericalJointKinSensor; + + std::unique_ptr driver; + XsensSuitImpl() + : driver(nullptr) + {} + + const std::map calibrationQualities{ + {"Unknown", xsensmvn::CalibrationQuality::UNKNOWN}, + {"Good", xsensmvn::CalibrationQuality::GOOD}, + {"Acceptable", xsensmvn::CalibrationQuality::ACCEPTABLE}, + {"Poor", xsensmvn::CalibrationQuality::POOR}, + {"Failed", xsensmvn::CalibrationQuality::FAILED}}; + + const std::map driverToSensorStatusMap{ + {xsensmvn::DriverStatus::Disconnected, sensor::SensorStatus::Error}, + {xsensmvn::DriverStatus::Unknown, sensor::SensorStatus::Unknown}, + {xsensmvn::DriverStatus::Recording, sensor::SensorStatus::Ok}, + {xsensmvn::DriverStatus::Calibrating, sensor::SensorStatus::Calibrating}, + {xsensmvn::DriverStatus::CalibratedAndReadyToRecord, + sensor::SensorStatus::WaitingForFirstRead}, + {xsensmvn::DriverStatus::Connected, sensor::SensorStatus::WaitingForFirstRead}, + {xsensmvn::DriverStatus::Scanning, sensor::SensorStatus::Error}}; + + template + struct driverToDeviceSensors + { + std::shared_ptr xsSensor; + size_t driverIndex; + }; + + std::map> + freeBodyAccerlerationSensorsMap; + std::map> positionSensorsMap; + std::map> orientationSensorsMap; + std::map> poseSensorsMap; + std::map> magnetometersMap; + std::map> + virtualLinkKinSensorsMap; + std::map> + virtualSphericalJointKinSensorsMap; + + std::unique_ptr network = nullptr; + + // ------------------------ + // Custom utility functions + // ------------------------ + void setAllSensorStates(wearable::sensor::SensorStatus aStatus); + + const WearableName wearableName = "XsensSuit::"; +}; + +// =================================================== +// Xsens implementation of IFreeBodyAccelerationSensor +// =================================================== +class XsensSuit::XsensSuitImpl::XsensFreeBodyAccelerationSensor + : public wearable::sensor::IFreeBodyAccelerationSensor +{ +public: + // ------------------------ + // Constructor / Destructor + // ------------------------ + XsensFreeBodyAccelerationSensor( + XsensSuit::XsensSuitImpl* xsSuitImpl, + const wearable::sensor::SensorName aName = {}, + const wearable::sensor::SensorStatus aStatus = wearable::sensor::SensorStatus::Unknown) + : IFreeBodyAccelerationSensor(aName, aStatus) + , m_suitImpl(xsSuitImpl) + {} + + ~XsensFreeBodyAccelerationSensor() override = default; + + // ------------------------------------- + // IFreeBodyAccelerationSensor interface + // ------------------------------------- + bool getFreeBodyAcceleration(wearable::Vector3& fba) const override + { + if (m_suitImpl->freeBodyAccerlerationSensorsMap.find(this->m_name) + == m_suitImpl->freeBodyAccerlerationSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + fba.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const xsensmvn::SensorData sensorData = m_suitImpl->driver->getSensorDataSample().data.at( + m_suitImpl->freeBodyAccerlerationSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + sensorData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + sensorData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + fba = sensorData.freeBodyAcceleration; + return true; + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } + +private: + // --------- + // Variables + // --------- + const XsensSuit::XsensSuitImpl* m_suitImpl = nullptr; +}; + +// ======================================= +// Xsens implementation of IPositionSensor +// ======================================= +class XsensSuit::XsensSuitImpl::XsensPositionSensor : public wearable::sensor::IPositionSensor +{ +public: + // ------------------------ + // Constructor / Destructor + // ------------------------ + XsensPositionSensor( + XsensSuit::XsensSuitImpl* xsSuitImpl, + const wearable::sensor::SensorName aName = {}, + const wearable::sensor::SensorStatus aStatus = wearable::sensor::SensorStatus::Unknown) + : IPositionSensor(aName, aStatus) + , m_suitImpl(xsSuitImpl) + {} + + ~XsensPositionSensor() override = default; + + // ------------------------- + // IPositionSensor interface + // ------------------------- + bool getPosition(wearable::Vector3& pos) const override + { + if (m_suitImpl->positionSensorsMap.find(this->m_name) + == m_suitImpl->positionSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + pos.fill(0.0); + return false; + } + // Retrieve data sample directly form XSens Driver + const xsensmvn::SensorData sensorData = m_suitImpl->driver->getSensorDataSample().data.at( + m_suitImpl->positionSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + sensorData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + sensorData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + pos = sensorData.position; + return true; + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } + +private: + XsensSuit::XsensSuitImpl* m_suitImpl = nullptr; +}; + +// ========================================== +// Xsens implementation of IOrientationSensor +// ========================================== +class XsensSuit::XsensSuitImpl::XsensOrientationSensor : public wearable::sensor::IOrientationSensor +{ +public: + // ------------------------ + // Constructor / Destructor + // ------------------------ + XsensOrientationSensor( + XsensSuit::XsensSuitImpl* xsSuitImpl, + const wearable::sensor::SensorName aName = {}, + const wearable::sensor::SensorStatus aStatus = wearable::sensor::SensorStatus::Unknown) + : IOrientationSensor(aName, aStatus) + , m_suitImpl(xsSuitImpl) + {} + + ~XsensOrientationSensor() override = default; + + // ------------------------- + // IOrientationSensor interface + // ------------------------- + bool getOrientationAsQuaternion(wearable::Quaternion& quat) const override + { + if (m_suitImpl->orientationSensorsMap.find(this->m_name) + == m_suitImpl->orientationSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + quat.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const xsensmvn::SensorData sensorData = m_suitImpl->driver->getSensorDataSample().data.at( + m_suitImpl->orientationSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + sensorData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + sensorData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + quat = sensorData.orientation; + return true; + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } + +private: + XsensSuit::XsensSuitImpl* m_suitImpl = nullptr; +}; + +// =================================== +// Xsens implementation of IPoseSensor +// =================================== +class XsensSuit::XsensSuitImpl::XsensPoseSensor : public wearable::sensor::IPoseSensor +{ +public: + // ------------------------ + // Constructor / Destructor + // ------------------------ + XsensPoseSensor( + XsensSuit::XsensSuitImpl* xsSuitImpl, + const wearable::sensor::SensorName aName = {}, + const wearable::sensor::SensorStatus aStatus = wearable::sensor::SensorStatus::Unknown) + : IPoseSensor(aName, aStatus) + , m_suitImpl(xsSuitImpl) + {} + + ~XsensPoseSensor() override = default; + + // ------------------------- + // IOrientationSensor interface + // ------------------------- + bool getPose(Quaternion& orientation, Vector3& position) const override + { + if (m_suitImpl->poseSensorsMap.find(this->m_name) == m_suitImpl->poseSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + orientation.fill(0.0); + position.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const xsensmvn::SensorData sensorData = m_suitImpl->driver->getSensorDataSample().data.at( + m_suitImpl->poseSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + sensorData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + sensorData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + orientation = sensorData.orientation; + position = sensorData.position; + return true; + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } + +private: + XsensSuit::XsensSuitImpl* m_suitImpl = nullptr; +}; + +// ===================================== +// Xsens implementation of IMagnetometer +// ===================================== +class XsensSuit::XsensSuitImpl::XsensMagnetometer : public wearable::sensor::IMagnetometer +{ +public: + // ------------------------ + // Constructor / Destructor + // ------------------------ + XsensMagnetometer( + XsensSuit::XsensSuitImpl* xsSuitImpl, + const wearable::sensor::SensorName aName = {}, + const wearable::sensor::SensorStatus aStatus = wearable::sensor::SensorStatus::Unknown) + : IMagnetometer(aName, aStatus) + , m_suitImpl(xsSuitImpl) + {} + + ~XsensMagnetometer() override = default; + + // ------------------------- + // IMagnetometer interface + // ------------------------- + bool getMagneticField(wearable::Vector3& mf) const override + { + if (m_suitImpl->magnetometersMap.find(this->m_name) == m_suitImpl->magnetometersMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + mf.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const xsensmvn::SensorData sensorData = m_suitImpl->driver->getSensorDataSample().data.at( + m_suitImpl->magnetometersMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + sensorData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + sensorData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + mf = sensorData.magneticField; + return true; + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } + +private: + XsensSuit::XsensSuitImpl* m_suitImpl = nullptr; +}; + +// ============================================= +// Xsens implementation of IVirtualLinkKinsensor +// ============================================= +class XsensSuit::XsensSuitImpl::XsensVirtualLinkKinSensor + : public wearable::sensor::IVirtualLinkKinSensor +{ +public: + // ------------------------ + // Constructor / Destructor + // ------------------------ + XsensVirtualLinkKinSensor( + XsensSuit::XsensSuitImpl* xsSuitImpl, + const wearable::sensor::SensorName aName = {}, + const wearable::sensor::SensorStatus aStatus = wearable::sensor::SensorStatus::Unknown) + : IVirtualLinkKinSensor(aName, aStatus) + , m_suitImpl(xsSuitImpl) + {} + + ~XsensVirtualLinkKinSensor() override = default; + + // ------------------------------- + // IVirtualLinkKinSensor interface + // ------------------------------- + bool getLinkAcceleration(Vector3& linear, Vector3& angular) const override + { + if (m_suitImpl->virtualLinkKinSensorsMap.find(this->m_name) + == m_suitImpl->virtualLinkKinSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + linear.fill(0.0); + angular.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const xsensmvn::LinkData linkData = m_suitImpl->driver->getLinkDataSample().data.at( + m_suitImpl->virtualLinkKinSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + linkData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + linkData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + linear = linkData.linearAcceleration; + angular = linkData.angularAcceleration; + return true; + } + + bool getLinkPose(Vector3& position, Quaternion& orientation) const override + { + if (m_suitImpl->virtualLinkKinSensorsMap.find(this->m_name) + == m_suitImpl->virtualLinkKinSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + position.fill(0.0); + orientation.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const auto linkData = m_suitImpl->driver->getLinkDataSample().data.at( + m_suitImpl->virtualLinkKinSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + linkData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + linkData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + position = linkData.position; + orientation = linkData.orientation; + return true; + } + + bool getLinkVelocity(Vector3& linear, Vector3& angular) const override + { + if (m_suitImpl->virtualLinkKinSensorsMap.find(this->m_name) + == m_suitImpl->virtualLinkKinSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + linear.fill(0.0); + angular.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const auto linkData = m_suitImpl->driver->getLinkDataSample().data.at( + m_suitImpl->virtualLinkKinSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + linkData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + linkData.name + << "Wearable Sensor name " << this->m_name; + return false; + } + + // Fill argument with retrieved data + linear = linkData.linearVelocity; + angular = linkData.angularVelocity; + return true; + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } + +private: + XsensSuit::XsensSuitImpl* m_suitImpl = nullptr; +}; + +// ============================================== +// Xsens implementation of IVirtualJointKinSensor +// ============================================== +class XsensSuit::XsensSuitImpl::XsensVirtualSphericalJointKinSensor + : public wearable::sensor::IVirtualSphericalJointKinSensor +{ +public: + // ------------------------ + // Constructor / Destructor + // ------------------------ + XsensVirtualSphericalJointKinSensor( + XsensSuit::XsensSuitImpl* xsSuitImpl, + const wearable::sensor::SensorName aName = {}, + const wearable::sensor::SensorStatus aStatus = wearable::sensor::SensorStatus::Unknown) + : IVirtualSphericalJointKinSensor(aName, aStatus) + , m_suitImpl(xsSuitImpl) + {} + + ~XsensVirtualSphericalJointKinSensor() override = default; + + // ----------------------------------------- + // IVirtualSphericalJointKinSensor interface + // ----------------------------------------- + bool getJointAnglesAsRPY(Vector3& angleAsRPY) const override + { + if (m_suitImpl->virtualSphericalJointKinSensorsMap.find(this->m_name) + == m_suitImpl->virtualSphericalJointKinSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + angleAsRPY.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const xsensmvn::JointData jointData = m_suitImpl->driver->getJointDataSample().data.at( + m_suitImpl->virtualSphericalJointKinSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + jointData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + jointData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + angleAsRPY = jointData.angles; + return true; + } + + bool getJointVelocities(Vector3& velocities) const override + { + if (m_suitImpl->virtualSphericalJointKinSensorsMap.find(this->m_name) + == m_suitImpl->virtualSphericalJointKinSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + velocities.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const auto jointData = m_suitImpl->driver->getJointDataSample().data.at( + m_suitImpl->virtualSphericalJointKinSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + jointData.name) { + yError() << logPrefix << "Names mismatch Driver name:" << prefix + jointData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + velocities = jointData.velocities; + return true; + } + + bool getJointAccelerations(Vector3& accelerations) const override + { + if (m_suitImpl->virtualSphericalJointKinSensorsMap.find(this->m_name) + == m_suitImpl->virtualSphericalJointKinSensorsMap.end()) { + yError() << logPrefix << "Sensor" << this->m_name << "NOT found"; + accelerations.fill(0.0); + return false; + } + + // Retrieve data sample directly form XSens Driver + const auto jointData = m_suitImpl->driver->getJointDataSample().data.at( + m_suitImpl->virtualSphericalJointKinSensorsMap.at(this->m_name).driverIndex); + + // TODO: This should be guaranteed, runtime check should be removed + // Check if suit sensor and driver data sample have the same name + auto prefix = m_suitImpl->wearableName + this->getPrefix(); + if (this->m_name != prefix + jointData.name) { + yError() << logPrefix << "Names mismatch Driver name " << prefix + jointData.name + << "Wearable Sensor name" << this->m_name; + return false; + } + + // Fill argument with retrieved data + accelerations = jointData.accelerations; + return true; + } + + // ------------------------ + // Custom utility functions + // ------------------------ + inline void setStatus(const wearable::sensor::SensorStatus aStatus) { m_status = aStatus; } + +private: + XsensSuit::XsensSuitImpl* m_suitImpl = nullptr; +}; + +// ========================================== +// XsensSuit utility functions implementation +// ========================================== +void XsensSuit::XsensSuitImpl::setAllSensorStates(wearable::sensor::SensorStatus aStatus) +{ + for (const auto& fbas : freeBodyAccerlerationSensorsMap) { + fbas.second.xsSensor->setStatus(aStatus); + } + for (const auto& ps : positionSensorsMap) { + ps.second.xsSensor->setStatus(aStatus); + } + for (const auto& os : orientationSensorsMap) { + os.second.xsSensor->setStatus(aStatus); + } + for (const auto& ps : poseSensorsMap) { + ps.second.xsSensor->setStatus(aStatus); + } + for (const auto& m : magnetometersMap) { + m.second.xsSensor->setStatus(aStatus); + } + for (const auto& vlks : virtualLinkKinSensorsMap) { + vlks.second.xsSensor->setStatus(aStatus); + } + for (const auto& vsjks : virtualSphericalJointKinSensorsMap) { + vsjks.second.xsSensor->setStatus(aStatus); + } +} + +// ================================================= +// XsensSuit constructor / destructor implementation +// ================================================= +XsensSuit::XsensSuit() + : pImpl{new XsensSuitImpl()} +{} + +XsensSuit::~XsensSuit() = default; + +// ====================== +// DeviceDriver interface +// ====================== +bool XsensSuit::open(yarp::os::Searchable& config) +{ + yInfo() << logPrefix << " Starting to configure"; + // Read from config file the Xsens rundeps folder + if (!config.check("xsens-rundeps-dir")) { + yError() << logPrefix << "REQUIRED parameter NOT found"; + return false; + } + const std::string rundepsFolder = config.find("xsens-rundeps-dir").asString(); + + // Read from config file the Xsens suit configuration to use. + if (!config.check("suit-config")) { + yError() << logPrefix << "REQUIRED parameter NOT found"; + return false; + } + const std::string suitConfiguration = config.find("suit-config").asString(); + + // Read from config file the Xsens acquisition scenario to use. + // Since it is optional, if not found it is safe to use an empty string + std::string acquisitionScenario; + if (!config.check("acquisition-scenario")) { + yWarning() << logPrefix << "OPTIONAL parameter NOT found"; + acquisitionScenario = ""; + } + else { + acquisitionScenario = config.find("acquisition-scenario").asString(); + } + + // Read from config file the calibration routine to be used as default. + // Since it is optional, if not found it is safe to use an empty string + std::string defaultCalibrationType = "Npose"; + if (!config.check("default-calibration-type")) { + yWarning() << logPrefix << "OPTIONAL parameter NOT found"; + yInfo() << logPrefix << "Using Npose as default"; + } + else { + defaultCalibrationType = config.find("default-calibration-type").asString(); + } + + // Read from config file the minimum required calibration quality. + // If not provided using POOR + xsensmvn::CalibrationQuality minCalibrationQualityRequired; + if (!config.check("minimum-calibration-quality-required")) { + yWarning() << logPrefix + << "OPTIONAL parameter NOT found"; + yWarning() << logPrefix << "Using POOR as minimum required calibration quality"; + minCalibrationQualityRequired = pImpl->calibrationQualities.at("Poor"); + } + else { + std::string tmpLabel = config.find("minimum-calibration-quality-required").asString(); + if (pImpl->calibrationQualities.find(tmpLabel) != pImpl->calibrationQualities.end()) { + minCalibrationQualityRequired = pImpl->calibrationQualities.at(tmpLabel); + } + else { + yWarning() << logPrefix + << "OPTIONAL parameter INVALID"; + yWarning() << logPrefix << "Using POOR as minimum required calibration quality"; + minCalibrationQualityRequired = pImpl->calibrationQualities.at("Poor"); + } + } + + // Read from config file the scan-for-suit timeout. + // If not provided ENABLING endless scan mode + int scanTimeout = -1; + if (!config.check("scan-timeout")) { + yWarning() << logPrefix << "OPTIONAL parameter NOT found"; + yWarning() << logPrefix << "Endless scan mode ENABLED"; + } + else { + scanTimeout = config.find("scan-timeout").asInt32(); + } + + // Read from config file the sampling-rate. + // If not provided USING highest available one + int samplingRate = -1; + if (!config.check("sampling-rate")) { + yWarning() << logPrefix << "OPTIONAL parameter NOT found"; + yWarning() << logPrefix << "Using highest supported sampling rate"; + } + else { + samplingRate = config.find("sampling-rate").asInt32(); + } + + // Get subject-specific body dimensions from the configuration file and push them to + // subjectBodyDimensions + xsensmvn::bodyDimensions subjectBodyDimensions; + yarp::os::Bottle bodyDimensionSet = config.findGroup("body-dimensions", ""); + if (bodyDimensionSet.isNull()) { + yWarning() << logPrefix << "OPTIONAL parameter group NOT found"; + yWarning() << logPrefix + << "USING default body dimensions, this may affect estimation quality"; + } + else { + for (size_t i = 1; i < bodyDimensionSet.size(); ++i) { + if (bodyDimensionSet.get(i).asList()->get(1).isFloat64() + && bodyDimensionSet.get(i).asList()->get(1).asFloat64() != -1) { + subjectBodyDimensions.insert({bodyDimensionSet.get(i).asList()->get(0).asString(), + bodyDimensionSet.get(i).asList()->get(1).asFloat64()}); + } + } + } + + // Read from config file the selected output stream configuration. + // If not provided USING default configuration, Joints: OFF, Links: ON, Sensors: ON + xsensmvn::DriverDataStreamConfig outputStreamConfig; + yarp::os::Bottle streamGroup = config.findGroup("output-stream-configuration", ""); + if (streamGroup.isNull()) { + yWarning() << logPrefix + << "OPTIONAL parameters group NOT found"; + yWarning() << logPrefix + << "USING default configuration, Joints: OFF, Links: ON, Sensors: ON"; + } + outputStreamConfig.enableJointData = + streamGroup.check("enable-joint-data", yarp::os::Value(false)).asBool(); + outputStreamConfig.enableLinkData = + streamGroup.check("enable-link-data", yarp::os::Value(true)).asBool(); + outputStreamConfig.enableSensorData = + streamGroup.check("enable-sensor-data", yarp::os::Value(true)).asBool(); + + // Check for mvn recording flag + bool saveMVNRecording; + if (!config.check("saveMVNRecording")) { + yWarning() << logPrefix + << "OPTIONAL parameter NOT found, setting it to false."; + saveMVNRecording = false; + } + else { + saveMVNRecording = config.find("saveMVNRecording").asBool(); + yInfo() << logPrefix << " parameter set to " << saveMVNRecording; + } + + // Check for saving current calibration flag + bool saveCurrentCalibration; + if (!config.check("saveCurrentCalibration")) { + yWarning() << logPrefix + << "OPTIONAL parameter NOT found, setting it to false."; + saveCurrentCalibration = false; + } + else { + saveCurrentCalibration = config.find("saveCurrentCalibration").asBool(); + yInfo() << logPrefix << " parameter set to " + << saveCurrentCalibration; + } + + xsensmvn::DriverConfiguration driverConfig{rundepsFolder, + suitConfiguration, + acquisitionScenario, + defaultCalibrationType, + minCalibrationQualityRequired, + scanTimeout, + samplingRate, + subjectBodyDimensions, + outputStreamConfig, + saveMVNRecording, + saveCurrentCalibration}; + + pImpl->driver.reset(new xsensmvn::XSensMVNDriver(driverConfig)); + + if (!pImpl->driver->configureAndConnect()) { + yError() << logPrefix << "Unable to configure the driver and connect to the suit"; + return false; + } + + std::string fbasPrefix = getWearableName() + sensor::IFreeBodyAccelerationSensor::getPrefix(); + std::string posPrefix = getWearableName() + sensor::IPositionSensor::getPrefix(); + std::string orientPrefix = getWearableName() + sensor::IOrientationSensor::getPrefix(); + std::string posePrefix = getWearableName() + sensor::IPoseSensor::getPrefix(); + std::string magPrefix = getWearableName() + sensor::IMagnetometer::getPrefix(); + std::string vlksPrefix = getWearableName() + sensor::IVirtualLinkKinSensor::getPrefix(); + std::string vsjksPrefix = + getWearableName() + sensor::IVirtualSphericalJointKinSensor::getPrefix(); + + if (pImpl->driver->getDriverConfiguration().dataStreamConfiguration.enableSensorData) { + // Get the names of the sensors from the driver + std::vector sensorNames = pImpl->driver->getSuitSensorLabels(); + + for (size_t s = 0; s < sensorNames.size(); ++s) { + // Create the new sensors + auto fba = std::make_shared( + pImpl.get(), fbasPrefix + sensorNames[s]); + auto pos = std::make_shared( + pImpl.get(), posPrefix + sensorNames[s]); + auto orient = std::make_shared( + pImpl.get(), orientPrefix + sensorNames[s]); + auto pose = std::make_shared( + pImpl.get(), posePrefix + sensorNames[s]); + auto mag = std::make_shared( + pImpl.get(), magPrefix + sensorNames[s]); + + pImpl->freeBodyAccerlerationSensorsMap.emplace( + fbasPrefix + sensorNames[s], + XsensSuitImpl::driverToDeviceSensors< + XsensSuitImpl::XsensFreeBodyAccelerationSensor>{fba, s}); + + pImpl->positionSensorsMap.emplace( + posPrefix + sensorNames[s], + XsensSuitImpl::driverToDeviceSensors{pos, s}); + + pImpl->orientationSensorsMap.emplace( + orientPrefix + sensorNames[s], + XsensSuitImpl::driverToDeviceSensors{orient, + s}); + + pImpl->poseSensorsMap.emplace( + posePrefix + sensorNames[s], + XsensSuitImpl::driverToDeviceSensors{pose, s}); + + pImpl->magnetometersMap.emplace( + magPrefix + sensorNames[s], + XsensSuitImpl::driverToDeviceSensors{mag, s}); + } + } + + if (pImpl->driver->getDriverConfiguration().dataStreamConfiguration.enableLinkData) { + // Get the names of the links from the driver + std::vector linkNames = pImpl->driver->getSuitLinkLabels(); + + for (size_t s = 0; s < linkNames.size(); ++s) { + // Create the new sensor + auto sensor = std::make_shared( + pImpl.get(), vlksPrefix + linkNames[s]); + // Insert it in the output structure + pImpl->virtualLinkKinSensorsMap.emplace( + vlksPrefix + linkNames[s], + XsensSuitImpl::driverToDeviceSensors{ + sensor, s}); + } + } + + if (pImpl->driver->getDriverConfiguration().dataStreamConfiguration.enableJointData) { + // Get the names of the joints from the driver + std::vector jointNames = pImpl->driver->getSuitJointLabels(); + + for (size_t s = 0; s < jointNames.size(); ++s) { + // Create the new sensor + auto sensor = std::make_shared( + pImpl.get(), vsjksPrefix + jointNames[s]); + // Insert it in the output structure + pImpl->virtualSphericalJointKinSensorsMap.emplace( + vsjksPrefix + jointNames[s], + XsensSuitImpl::driverToDeviceSensors< + XsensSuitImpl::XsensVirtualSphericalJointKinSensor>{sensor, s}); + } + } + + // ================================= + // CHECK YARP NETWORK INITIALIZATION + // ================================= + + pImpl->network = std::make_unique(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + yError() << logPrefix << "YARP server wasn't found active."; + return false; + } + + return true; +} + +bool XsensSuit::close() +{ + pImpl->driver->stopAcquisition(); + pImpl->driver->terminate(); + return true; +} + +// ========================= +// IPreciselyTimed interface +// ========================= +yarp::os::Stamp XsensSuit::getLastInputStamp() +{ + // Stamp count should be always zero + return yarp::os::Stamp(0, pImpl->driver->getTimeStamps().systemTime); +} + +// ========================== +// IXsensMVNControl interface +// ========================== + +bool XsensSuit::setBodyDimensions(const std::map& dimensions) +{ + if (!pImpl->driver) { + return false; + } + + return pImpl->driver->setBodyDimensions(dimensions); +} + +bool XsensSuit::getBodyDimensions(std::map& dimensions) const +{ + if (!pImpl->driver) { + return false; + } + + return pImpl->driver->getBodyDimensions(dimensions); +} + +bool XsensSuit::getBodyDimension(const std::string bodyName, double& dimension) const +{ + if (!pImpl->driver) { + return false; + } + + return pImpl->driver->getBodyDimension(bodyName, dimension); +} + +// Calibration methods +bool XsensSuit::calibrate(const std::string& calibrationType) +{ + + if (!pImpl->driver) { + return false; + } + + pImpl->setAllSensorStates(sensor::SensorStatus::Calibrating); + + bool calibrationSuccess = pImpl->driver->calibrate(calibrationType); + + if (calibrationSuccess) { + pImpl->setAllSensorStates(sensor::SensorStatus::WaitingForFirstRead); + } + else { + pImpl->setAllSensorStates(sensor::SensorStatus::Error); + } + + return calibrationSuccess; +} + +bool XsensSuit::abortCalibration() +{ + if (!pImpl->driver) { + return false; + } + + bool abortCalibrationSuccess = pImpl->driver->abortCalibration(); + + if (abortCalibrationSuccess) { + pImpl->setAllSensorStates(sensor::SensorStatus::Unknown); + } + else { + pImpl->setAllSensorStates(sensor::SensorStatus::Error); + } + + return abortCalibrationSuccess; +} + +// Acquisition methods +bool XsensSuit::startAcquisition() +{ + if (!pImpl->driver) { + return false; + } + + bool startAcquisitionSuccess = pImpl->driver->startAcquisition(); + + if (startAcquisitionSuccess) { + pImpl->setAllSensorStates(sensor::SensorStatus::Ok); + } + else { + pImpl->setAllSensorStates(sensor::SensorStatus::WaitingForFirstRead); + } + + return startAcquisitionSuccess; +} + +bool XsensSuit::stopAcquisition() +{ + if (!pImpl->driver) { + return false; + } + + bool stopAcquisitionSuccess = pImpl->driver->stopAcquisition(); + if (stopAcquisitionSuccess) { + pImpl->setAllSensorStates(sensor::SensorStatus::WaitingForFirstRead); + } + else { + pImpl->setAllSensorStates(sensor::SensorStatus::Ok); + } + + return stopAcquisitionSuccess; +} + +// =============== +// IWear interface +// =============== + +// --------------- +// Generic Methods +// --------------- + +wearable::WearableName XsensSuit::getWearableName() const +{ + return pImpl->wearableName; +} + +wearable::WearStatus XsensSuit::getStatus() const +{ + return pImpl->driverToSensorStatusMap.at(pImpl->driver->getStatus()); +} + +wearable::TimeStamp XsensSuit::getTimeStamp() const +{ + // Stamp count should be always zero + return {pImpl->driver->getTimeStamps().systemTime, 0}; +} + +// --------------------------- +// Implemented Sensors Methods +// --------------------------- + +wearable::SensorPtr +XsensSuit::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 +XsensSuit::getSensors(const wearable::sensor::SensorType aType) const +{ + wearable::VectorOfSensorPtr outVec; + switch (aType) { + case sensor::SensorType::FreeBodyAccelerationSensor: { + outVec.reserve(pImpl->freeBodyAccerlerationSensorsMap.size()); + for (const auto& fbas : pImpl->freeBodyAccerlerationSensorsMap) { + outVec.push_back( + static_cast>(fbas.second.xsSensor)); + } + break; + } + case sensor::SensorType::PositionSensor: { + outVec.reserve(pImpl->positionSensorsMap.size()); + for (const auto& ps : pImpl->positionSensorsMap) { + outVec.push_back(static_cast>(ps.second.xsSensor)); + } + break; + } + case sensor::SensorType::OrientationSensor: { + outVec.reserve(pImpl->orientationSensorsMap.size()); + for (const auto& os : pImpl->orientationSensorsMap) { + outVec.push_back(static_cast>(os.second.xsSensor)); + } + break; + } + case sensor::SensorType::PoseSensor: { + outVec.reserve(pImpl->poseSensorsMap.size()); + for (const auto& ps : pImpl->poseSensorsMap) { + outVec.push_back(static_cast>(ps.second.xsSensor)); + } + break; + } + case sensor::SensorType::Magnetometer: { + outVec.reserve(pImpl->magnetometersMap.size()); + for (const auto& m : pImpl->magnetometersMap) { + outVec.push_back(static_cast>(m.second.xsSensor)); + } + break; + } + case sensor::SensorType::VirtualLinkKinSensor: { + outVec.reserve(pImpl->virtualLinkKinSensorsMap.size()); + for (const auto& vlks : pImpl->virtualLinkKinSensorsMap) { + outVec.push_back( + static_cast>(vlks.second.xsSensor)); + } + break; + } + case sensor::SensorType::VirtualSphericalJointKinSensor: { + outVec.reserve(pImpl->virtualSphericalJointKinSensorsMap.size()); + for (const auto& vsjks : pImpl->virtualSphericalJointKinSensorsMap) { + outVec.push_back( + static_cast>(vsjks.second.xsSensor)); + } + + break; + } + default: { + yWarning() << logPrefix << "Selected sensor type (" << static_cast(aType) + << ") is not supported by XsensSuit"; + return {}; + } + } + + return outVec; +} + +wearable::SensorPtr +XsensSuit::getFreeBodyAccelerationSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->freeBodyAccerlerationSensorsMap.find(static_cast(name)) + == pImpl->freeBodyAccerlerationSensorsMap.end()) { + yError() << logPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared pointer to the required sensor + return static_cast>( + pImpl->freeBodyAccerlerationSensorsMap.at(static_cast(name)).xsSensor); +} + +wearable::SensorPtr +XsensSuit::getPositionSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->positionSensorsMap.find(static_cast(name)) + == pImpl->positionSensorsMap.end()) { + yError() << logPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared pointer to the required sensor + return static_cast>( + pImpl->positionSensorsMap.at(static_cast(name)).xsSensor); +} + +wearable::SensorPtr +XsensSuit::getOrientationSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->orientationSensorsMap.find(static_cast(name)) + == pImpl->orientationSensorsMap.end()) { + yError() << logPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared pointer to the required sensor + return static_cast>( + pImpl->orientationSensorsMap.at(static_cast(name)).xsSensor); +} + +wearable::SensorPtr +XsensSuit::getPoseSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->poseSensorsMap.find(static_cast(name)) == pImpl->poseSensorsMap.end()) { + yError() << logPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared pointer to the required sensor + return static_cast>( + pImpl->poseSensorsMap.at(static_cast(name)).xsSensor); +} + +wearable::SensorPtr +XsensSuit::getMagnetometer(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->magnetometersMap.find(static_cast(name)) + == pImpl->magnetometersMap.end()) { + yError() << logPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared pointer to the required sensor + return static_cast>( + pImpl->magnetometersMap.at(static_cast(name)).xsSensor); +} + +wearable::SensorPtr +XsensSuit::getVirtualLinkKinSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->virtualLinkKinSensorsMap.find(static_cast(name)) + == pImpl->virtualLinkKinSensorsMap.end()) { + yError() << logPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared pointer to the required sensor + return static_cast>( + pImpl->virtualLinkKinSensorsMap.at(static_cast(name)).xsSensor); +} + +wearable::SensorPtr +XsensSuit::getVirtualSphericalJointKinSensor(const wearable::sensor::SensorName name) const +{ + // Check if user-provided name corresponds to an available sensor + if (pImpl->virtualSphericalJointKinSensorsMap.find(static_cast(name)) + == pImpl->virtualSphericalJointKinSensorsMap.end()) { + yError() << logPrefix << "Invalid sensor name"; + return nullptr; + } + + // Return a shared pointer to the required sensor + return static_cast>( + pImpl->virtualSphericalJointKinSensorsMap.at(static_cast(name)).xsSensor); +} diff --git a/doc/How-to-compile-and-run-HapticGlove.md b/doc/How-to-compile-and-run-HapticGlove.md new file mode 100644 index 000000000..2faab7f7f --- /dev/null +++ b/doc/How-to-compile-and-run-HapticGlove.md @@ -0,0 +1,70 @@ +## HapticGlove + +Here we are exposing the [SenseGlove SDK](https://github.com/Adjuvo/SenseGlove-API) as a wearable device. This device expose the interfaces in order to stream hand motion data and setting the haptic feedback to the glove through the YARP netwrok. + +The following interfaces are exposed for each hand/glove: + +- human hand joint anlges (computed by SenseGlove SDK) `[20 DoF (each finger 4 DoF), rad]` +- glove fingertip poses attached to the human fingertip wrt human frame `5 pose vectors []` +- glove hand pose (using the hand IMU data) wrt human hand inertial frame`Pose []` +- hand fingertip vibrotactile feedbacks `[5 values: 0-100]` +- hand fingertip forceFeedback feedbacks `[5 values: 0-40 N]` +- hand palm vibrotactile feedback `[1 value: `[`Enumerated in "senseGlove::ThumperCmd"`](./include/SenseGloveHelper.hpp)`]` + + +### Dependencies + +- Sense Glove SDK +``` +git clone https://github.com/Adjuvo/SenseGlove-API +cd SenseGlove-API +git checkout baad587d4e165d7bfafd7f0c8ee6a1ce8ad651d6 +``` +In Linux machine add following environment variable: + +``` +export SenseGlove_DIR= +export PATH=${PATH}:${SenseGlove_DIR}/Core/SGCoreCpp/lib/linux/ +``` +In Windows machine following environment variable: + +``` +set SenseGlove_DIR= +set PATH=${PATH}:${SenseGlove_DIR}/Core/SGCoreCpp/lib/win/ +``` + +### BUILD + +Enable the option `ENABLE_HapticGlove` in Wearables when you are building it. Install the repository as well. + +### RUN + + +If you have installed correctly the Wearabes repository, and you have set the environment varaibles as mentioned in [Superbuild](https://github.com/robotology/robotology-superbuild) +you can run the HapticGlove wearable device by the following command: + + +- Run the following SenseGlove communication executable: + - Linux: run `SenseCom.x86_64` located in `/SenseCom/Linux` + - Windows: run `SenseCom` located in `/SenseCom/Win` + +- Then run: +``` +yarprobotinterface --config HapticGlove.xml +``` + +To check and visualize the the Sense Glove frames. + +In order to build the frame visualizer, enable `ENABLE_FrameVisualizer` option in Wearables. + +- You can run the following wearable frame visualizer module: +``` +IWearFrameVisualizerModule --from HapticGloveFramesVisualizationConfig.ini +``` + +**N.B. when `SenseCom.x86_64` runs the glove colors in GUI should be blue. If it is not, try to do:** +``` +sudo adduser $USER dialout +``` + +**N.B. In order to add `SenseCom.x86_64` executable to the list of application, follow the instructions in `config/SenseGlove.desktop`** diff --git a/doc/How-to-run-FTshoes.md b/doc/How-to-run-FTshoes.md new file mode 100644 index 000000000..4f4d0a3bb --- /dev/null +++ b/doc/How-to-run-FTshoes.md @@ -0,0 +1,28 @@ +# How to run FTShoes as Wearable Device + +This page describes how to run the [FTShoes](https://github.com/robotology/forcetorque-yarp-devices/tree/master/ftShoe) as a wearable device source. Further information on how to install and use the shoes can be found at https://github.com/robotology/forcetorque-yarp-devices/tree/master/ftShoe. + +The configuration file to run the FTShoes as wearable device is [`FTShoesWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/FTShoesWearableDevice.xml), and it can be launched with: +``` +yarprobotinterface --config FTShoesWearableDevice.xml +``` +The two shoes can also be launched separately using [`FTShoeLeftWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/FTShoeLeftWearableDevice.xml) and [`FTShoeRightWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/FTShoeRightWearableDevice.xml). + +### Before running +Before running the device make sure that: +- [`yarpserver`](https://www.yarp.it/yarpserver.html) is running +- The shoes are connected and recognized by the laptop +- The can address of the FT sensors in the shoes is the same of the configuration file (e.g. `ftShoe_Left_Front` -> ` 0x01 `) +- There is not another device running that is using the port names. In case multiple pairs of shoes have to run simultaneously, it is necessary to differentiate the name of the ports like in [`FTShoesWearableDevice_2.xml`](https://github.com/robotology/wearables/blob/master/app/xml/FTShoesWearableDevice_2.xml). +- The `inSituMatrices` contained in the configuration file correspond to the shoes you are using. + + +#### Calibration +The FT data streamed by the shoes may be characterized by an offset. The procedure for removing the offset is described at https://github.com/robotology/forcetorque-yarp-devices/tree/master/ftShoe#how-to-use-the-ftshoes. + +#### Data +If the device is running correctly, the stream of wearable data can be read with: +``` +yarp read ... /FTShoeLeft/WearableData/data:o +yarp read ... /FTShoeRight/WearableData/data:o +``` diff --git a/doc/How-to-run-XsensSuit.md b/doc/How-to-run-XsensSuit.md new file mode 100644 index 000000000..4b89d2897 --- /dev/null +++ b/doc/How-to-run-XsensSuit.md @@ -0,0 +1,48 @@ +# How to run Xsens suit as Wearable Device + +This page describes how to run the [Xsens suit](https://www.xsens.com/motion-capture) as a wearable device source. Further information on how to install can be found at https://github.com/robotology/human-dynamics-estimation/wiki/Set-up-Machine-for-running-HDE#xsens-only-for-windows, while for more information on the Xsens suit device tutorials are available at https://tutorial.xsens.com/. + +The configuration file to run the XsensSuit as wearable device is [`XsensSuitWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/XsensSuitWearableDevice.xml), and it can be launched with: +``` +yarprobotinterface --config XsensSuitWearableDevice.xml +``` +Once the device is started, it will start searching for the suit until it finds all the sensor. You can check the output of the device to follow the status of the search. It may be required to move the sensor in order to let them be discovered. + +### Before running +Before running the device make sure that: +- [`yarpserver`](https://www.yarp.it/yarpserver.html) is running +- The router/receiver of the suit is connected to the laptop, the suit is powered on, and the pendrive with the licence is instered on the laptop. +- There is not another device running that is using the port names. In case multiple suits have to run simultaneously, it is necessary to differentiate the name of the ports. +- In the [configuration file](https://github.com/robotology/wearables/blob/master/app/xml/XsensSuitWearableDevice.xml), the following parameters are set propery: + - `xsens-rundeps-dir`: Folder where XsensMVN runtime dependencies are stored. + - `default-calibration-type`: Calibration type to be used (available values: `NposeWalk`, `TposeWalk`, `Npose`, `Tpose`). + - `minimum-calibration-quality-required`: Minimum calibration quality to be considered good enought to be applied and allow the acquisition to start (available values: `Poor`, `Acceptable`, `Good`). + - `body-dimensions`: Subject specific body dimensions. +- The subject is wearing the suit correctly + +#### Calibration +In order to acquire data from the Xsens suit it is required to perform a calibration procedure, the type of procedure is set through the configuration file (`default-calibration-type`). +The calibration is started sending the following command: +``` +yarp rpc /XsensSuit/Control/rpc:i +calibrate +``` +The device will give as output the indication for performing the calibration, and the final quality of the calibration. + +In case the calibration does not meet the minimum quality requirement, new calibration can be performed with the same command. In case a new calibration is required after the acquisition is started, it is necessary to stop the acquisition befor performing a new calibration: +``` +yarp rpc /XsensSuit/Control/rpc:i +stopAcquisition +calibrate +``` + +#### Data +Once the calibration procedure is completed with the required minimum quality level, the data acquisition is started sending the following command: +``` +yarp rpc /XsensSuit/Control/rpc:i +startAcquisition +``` +If the device is running correctly, the stream of wearable data can be read with: +``` +yarp read ... /XsensSuit/WearableData/data:o +``` \ No newline at end of file diff --git a/doc/How-to-run-iCub-as-wearable-source.md b/doc/How-to-run-iCub-as-wearable-source.md new file mode 100644 index 000000000..55a5fb8a9 --- /dev/null +++ b/doc/How-to-run-iCub-as-wearable-source.md @@ -0,0 +1,45 @@ +# How to run iCub as Wearable Device + +This page describes how to run the [`iCub`](http://www.icub.org/) humanoid robot as a wearable device source, by exposing its sensors measurement and joints state. + + +The configuration file to run `iCub` as wearable device is [`ICubWearableDevice.xml`](https://github.com/robotology/wearables/blob/master/app/xml/ICubWearableDevice.xml), and it can be launched with: +``` +yarprobotinterface --config ICubWearableDevice.xml +``` + + +#### Before running +Before running the device make sure that: +- [`yarpserver`](https://www.yarp.it/yarpserver.html) is running +- The `YARP_ROBOT_NAME` is set to the proper robot name. +- The robot is currently running (either [Gazebo simulator](https://github.com/robotology/icub-gazebo) or real robot making sure it is runned with the [proper configuration for whole-body-dynamics](https://github.com/robotology/whole-body-controllers/blob/master/doc/How-to-setup-the-robot-for-wbc-experiments.md)) +- If the robot is running in simulation, the `whole-body-dynamics` estimatior has to be launched + ``` + yarprobotinterface --config launch-wholebodydynamics.xml + ``` + When the real robot is running, `whole-body-dynamics` estimatior should be running if the robot is launched with the [proper configuration](https://github.com/robotology/whole-body-controllers/blob/master/doc/How-to-setup-the-robot-for-wbc-experiments.md). + To verify if the estimator is running, you can check whether data are streamed for the end-effectors to be used as sensors (e.g. `/wholeBodyDynamics/right_arm/endEffectorWrench:o`). +- In the [configuration file](https://github.com/robotology/wearables/blob/master/app/xml/ICubWearableDevice.xml), the following parameters are set properly: + - `ft-sensors`: wrenche measurements that have to be exposed as wearable data. Those measurements should be provided by `whole-body-dynamics` estimator, or directly from sensors measurement. + - `joint-sensors`: joints for which the state has to be exposed as wearable data. + + +#### Calibration +The estimated wrench data computed by `whole-body-dynamics` may be characterized by an offset due to ft sensors meaasurement. +If using the real-robot, the following command can be sent to remove the offset (while the robot is lifted from the ground): +``` +yarp rpc /wholeBodyDynamics/rpc +calib all 300 +``` +If instead simulated robot is used, the command is the following (while the robot is on the ground): +``` +yarp rpc /wholeBodyDynamics/rpc +resetOffset all 300 +``` + +#### Data +If the device is running correctly, the stream of wearable data can be read with: +``` +/ICub/WearableData/data:o +``` \ No newline at end of file diff --git a/doc/README.md b/doc/README.md new file mode 100644 index 000000000..04264633e --- /dev/null +++ b/doc/README.md @@ -0,0 +1,11 @@ +## Documentation + +Guidelines on how to run and use wearable sensor source devices. + +### Available documentation + +- [How-to-run-FTshoes](How-to-run-FTshoes.md) + +- [How-to-run-XsensSuit](How-to-run-XsensSuit.md) + +- [How-to-run-HapticGlove](How-to-compile-and-run-HapticGlove.md) diff --git a/impl/CMakeLists.txt b/impl/CMakeLists.txt new file mode 100644 index 000000000..9d0c08ade --- /dev/null +++ b/impl/CMakeLists.txt @@ -0,0 +1,5 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +add_subdirectory(SensorsImpl) diff --git a/impl/SensorsImpl/CMakeLists.txt b/impl/SensorsImpl/CMakeLists.txt new file mode 100644 index 000000000..c3fb9590f --- /dev/null +++ b/impl/SensorsImpl/CMakeLists.txt @@ -0,0 +1,20 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +add_library(SensorsImpl + SensorsImpl.cpp + include/Wearable/IWear/Sensors/impl/SensorsImpl.h) +add_library(Wearable::SensorsImpl ALIAS SensorsImpl) + +target_include_directories(SensorsImpl PUBLIC + $) + +target_link_libraries(SensorsImpl PUBLIC IWear::IWear) + +install( + TARGETS SensorsImpl + EXPORT SensorsImpl + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/impl/SensorsImpl/SensorsImpl.cpp b/impl/SensorsImpl/SensorsImpl.cpp new file mode 100644 index 000000000..29860ca5b --- /dev/null +++ b/impl/SensorsImpl/SensorsImpl.cpp @@ -0,0 +1,436 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "Wearable/IWear/Sensors/impl/SensorsImpl.h" + +using namespace wearable::sensor::impl; + +// ============= +// Accelerometer +// ============= + +Accelerometer::Accelerometer(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : wearable::sensor::IAccelerometer(n, s) +{} + +bool Accelerometer::getLinearAcceleration(wearable::Vector3& linearAcceleration) const +{ + std::lock_guard lock(m_mutex); + linearAcceleration = m_buffer; + return true; +} + +void Accelerometer::setBuffer(const wearable::Vector3& data) +{ + std::lock_guard lock(m_mutex); + m_buffer = data; +} + +// ========= +// EmgSensor +// ========= + +EmgSensor::EmgSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : wearable::sensor::IEmgSensor(n, s) +{} + +bool EmgSensor::getEmgSignal(double& emgSignal) const +{ + std::lock_guard lock(m_mutex); + emgSignal = m_value; + return true; +} + +bool EmgSensor::getNormalizationValue(double& normalizationValue) const +{ + std::lock_guard lock(m_mutex); + normalizationValue = m_normalization; + return true; +} + +void EmgSensor::setBuffer(const double value, const double normalization) +{ + std::lock_guard lock(m_mutex); + m_value = value; + m_normalization = normalization; +} + +// ============= +// Force3DSensor +// ============= + +Force3DSensor::Force3DSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : wearable::sensor::IForce3DSensor(n, s) +{} + +bool Force3DSensor::getForce3D(wearable::Vector3& force) const +{ + std::lock_guard lock(m_mutex); + force = m_buffer; + return true; +} + +void Force3DSensor::setBuffer(const wearable::Vector3& data) +{ + std::lock_guard lock(m_mutex); + m_buffer = data; +} + +// =================== +// ForceTorque6DSensor +// =================== + +ForceTorque6DSensor::ForceTorque6DSensor(wearable::sensor::SensorName n, + wearable::sensor::SensorStatus s) + : wearable::sensor::IForceTorque6DSensor(n, s) +{} + +bool ForceTorque6DSensor::getForceTorque6D(wearable::Vector3& force3D, + wearable::Vector3& torque3D) const +{ + std::lock_guard lock(m_mutex); + force3D = m_force; + torque3D = m_torque; + return true; +} + +void ForceTorque6DSensor::setBuffer(const wearable::Vector3& force, const wearable::Vector3& torque) +{ + std::lock_guard lock(m_mutex); + m_force = force; + m_torque = torque; +} + +// ========================== +// FreeBodyAccelerationSensor +// ========================== + +FreeBodyAccelerationSensor::FreeBodyAccelerationSensor(wearable::sensor::SensorName n, + wearable::sensor::SensorStatus s) + : wearable::sensor::IFreeBodyAccelerationSensor(n, s) +{} + +bool FreeBodyAccelerationSensor::getFreeBodyAcceleration( + wearable::Vector3& freeBodyAcceleration) const +{ + std::lock_guard lock(m_mutex); + freeBodyAcceleration = m_buffer; + return true; +} + +void FreeBodyAccelerationSensor::setBuffer(const wearable::Vector3& data) +{ + std::lock_guard lock(m_mutex); + m_buffer = data; +} + +// ========= +// Gyroscope +// ========= + +Gyroscope::Gyroscope(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : wearable::sensor::IGyroscope(n, s) +{} + +bool Gyroscope::getAngularRate(wearable::Vector3& angularRate) const +{ + std::lock_guard lock(m_mutex); + angularRate = m_buffer; + return true; +} + +void Gyroscope::setBuffer(const wearable::Vector3& data) +{ + std::lock_guard lock(m_mutex); + m_buffer = data; +} + +// ============ +// Magnetometer +// ============ + +Magnetometer::Magnetometer(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : wearable::sensor::IMagnetometer(n, s) +{} + +bool Magnetometer::getMagneticField(wearable::Vector3& magneticField) const +{ + std::lock_guard lock(m_mutex); + magneticField = m_buffer; + return true; +} + +void Magnetometer::setBuffer(const wearable::Vector3& data) +{ + std::lock_guard lock(m_mutex); + m_buffer = data; +} + +// ================= +// OrientationSensor +// ================= + +OrientationSensor::OrientationSensor(wearable::sensor::SensorName n, + wearable::sensor::SensorStatus s) + : wearable::sensor::IOrientationSensor(n, s) +{} + +bool OrientationSensor::getOrientationAsQuaternion(wearable::Quaternion& orientation) const +{ + std::lock_guard lock(m_mutex); + orientation = m_buffer; + return true; +} + +void OrientationSensor::setBuffer(const wearable::Quaternion& data) +{ + std::lock_guard lock(m_mutex); + m_buffer = data; +} + +// ========== +// PoseSensor +// ========== + +PoseSensor::PoseSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : wearable::sensor::IPoseSensor(n, s) +{} + +bool PoseSensor::getPose(wearable::Quaternion& orientation, wearable::Vector3& position) const +{ + std::lock_guard lock(m_mutex); + orientation = m_orientation; + position = m_position; + return true; +} + +void PoseSensor::setBuffer(const wearable::Quaternion& orientation, + const wearable::Vector3& position) +{ + std::lock_guard lock(m_mutex); + m_orientation = orientation; + m_position = position; +} + +// ============== +// PositionSensor +// ============== + +PositionSensor::PositionSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : wearable::sensor::IPositionSensor(n, s) +{} + +bool PositionSensor::getPosition(wearable::Vector3& position) const +{ + std::lock_guard lock(m_mutex); + position = m_buffer; + return true; +} + +void PositionSensor::setBuffer(const wearable::Vector3& data) +{ + std::lock_guard lock(m_mutex); + m_buffer = data; +} + +// ========== +// SkinSensor +// ========== + +SkinSensor::SkinSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : 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 +// ================= + +TemperatureSensor::TemperatureSensor(wearable::sensor::SensorName n, + wearable::sensor::SensorStatus s) + : wearable::sensor::ITemperatureSensor(n, s) +{} + +bool TemperatureSensor::getTemperature(double& temperature) const +{ + std::lock_guard lock(m_mutex); + temperature = m_value; + return true; +} + +void TemperatureSensor::setBuffer(const double value) +{ + std::lock_guard lock(m_mutex); + m_value = value; +} + +// ============== +// Torque3DSensor +// ============== + +Torque3DSensor::Torque3DSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s) + : wearable::sensor::ITorque3DSensor(n, s) +{} + +bool Torque3DSensor::getTorque3D(wearable::Vector3& torque) const +{ + std::lock_guard lock(m_mutex); + torque = m_buffer; + return true; +} + +void Torque3DSensor::setBuffer(const wearable::Vector3& data) +{ + std::lock_guard lock(m_mutex); + m_buffer = data; +} + +// ==================== +// VirtualLinkKinSensor +// ==================== + +VirtualLinkKinSensor::VirtualLinkKinSensor(wearable::sensor::SensorName n, + wearable::sensor::SensorStatus s) + : wearable::sensor::IVirtualLinkKinSensor(n, s) +{} + +bool VirtualLinkKinSensor::getLinkAcceleration(wearable::Vector3& linear, + wearable::Vector3& angular) const +{ + std::lock_guard lock(m_mutex); + linear = m_linearAcc; + angular = m_angularAcc; + return true; +} + +//#include +bool VirtualLinkKinSensor::getLinkPose(wearable::Vector3& position, + wearable::Quaternion& orientation) const +{ + std::lock_guard lock(m_mutex); + position = m_position; + orientation = m_orientation; + + // std::cerr << "sensorImpl" << orientation[0] << orientation[1] << orientation[2] + // << orientation[3] << m_name << std::endl; + + return true; +} + +bool VirtualLinkKinSensor::getLinkVelocity(wearable::Vector3& linear, + wearable::Vector3& angular) const +{ + std::lock_guard lock(m_mutex); + linear = m_linearVel; + angular = m_angularVel; + return true; +} + +void VirtualLinkKinSensor::setBuffer(const wearable::Vector3& linearAcc, + const wearable::Vector3& angularAcc, + const wearable::Vector3& linearVel, + const wearable::Vector3& angularVel, + const wearable::Vector3& position, + const wearable::Quaternion& orientation) +{ + std::lock_guard lock(m_mutex); + m_linearAcc = linearAcc; + m_angularAcc = angularAcc; + m_linearVel = linearVel; + m_angularVel = angularVel; + m_position = position; + m_orientation = orientation; +} + +// ============================== +// VirtualJointKinSensor +// ============================== + +VirtualJointKinSensor::VirtualJointKinSensor(wearable::sensor::SensorName n, + wearable::sensor::SensorStatus s) + : wearable::sensor::IVirtualJointKinSensor(n, s) +{} + +bool VirtualJointKinSensor::getJointPosition(double& position) const +{ + std::lock_guard lock(m_mutex); + position = m_position; + return true; +} + +bool VirtualJointKinSensor::getJointVelocity(double& velocity) const +{ + std::lock_guard lock(m_mutex); + velocity = m_velocity; + return true; +} + +bool VirtualJointKinSensor::getJointAcceleration(double& acceleration) const +{ + std::lock_guard lock(m_mutex); + acceleration = m_acceleration; + return true; +} + +void VirtualJointKinSensor::setBuffer(const double& position, + const double& velocity, + const double& acceleration) +{ + std::lock_guard lock(m_mutex); + m_position = position; + m_velocity = velocity; + m_acceleration = acceleration; +} + + +// ============================== +// VirtualSphericalJointKinSensor +// ============================== + +VirtualSphericalJointKinSensor::VirtualSphericalJointKinSensor(wearable::sensor::SensorName n, + wearable::sensor::SensorStatus s) + : wearable::sensor::IVirtualSphericalJointKinSensor(n, s) +{} + +bool VirtualSphericalJointKinSensor::getJointAnglesAsRPY(wearable::Vector3& angleAsRPY) const +{ + std::lock_guard lock(m_mutex); + angleAsRPY = m_angleAsRPY; + return true; +} + +bool VirtualSphericalJointKinSensor::getJointVelocities(wearable::Vector3& velocities) const +{ + std::lock_guard lock(m_mutex); + velocities = m_velocities; + return true; +} + +bool VirtualSphericalJointKinSensor::getJointAccelerations(wearable::Vector3& accelerations) const +{ + std::lock_guard lock(m_mutex); + accelerations = m_accelerations; + return true; +} + +void VirtualSphericalJointKinSensor::setBuffer(const wearable::Vector3& angleAsRPY, + const wearable::Vector3& velocities, + const wearable::Vector3& accelerations) +{ + std::lock_guard lock(m_mutex); + m_angleAsRPY = angleAsRPY; + m_velocities = velocities; + m_accelerations = accelerations; +} diff --git a/impl/SensorsImpl/include/Wearable/IWear/Sensors/impl/SensorsImpl.h b/impl/SensorsImpl/include/Wearable/IWear/Sensors/impl/SensorsImpl.h new file mode 100644 index 000000000..6f4a350b3 --- /dev/null +++ b/impl/SensorsImpl/include/Wearable/IWear/Sensors/impl/SensorsImpl.h @@ -0,0 +1,325 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef SENSORSIMPL_H +#define SENSORSIMPL_H + +#include "Wearable/IWear/IWear.h" +#include +#include + +namespace wearable { + namespace sensor { + namespace impl { + class Accelerometer; + class EmgSensor; + class Force3DSensor; + class ForceTorque6DSensor; + class FreeBodyAccelerationSensor; + class Gyroscope; + class Magnetometer; + class OrientationSensor; + class PoseSensor; + class PositionSensor; + class SkinSensor; + class TemperatureSensor; + class Torque3DSensor; + class VirtualLinkKinSensor; + class VirtualJointKinSensor; + class VirtualSphericalJointKinSensor; + } // namespace impl + } // namespace sensor +} // namespace wearable + +class wearable::sensor::impl::Accelerometer : public wearable::sensor::IAccelerometer +{ +public: + wearable::Vector3 m_buffer; + mutable std::mutex m_mutex; + + Accelerometer(wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~Accelerometer() override = default; + + bool getLinearAcceleration(wearable::Vector3& linearAcceleration) const override; + + void setBuffer(const wearable::Vector3& data); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::EmgSensor : public wearable::sensor::IEmgSensor +{ +public: + double m_value; + double m_normalization; + mutable std::mutex m_mutex; + + EmgSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s); + ~EmgSensor() override = default; + + bool getEmgSignal(double& emgSignal) const override; + bool getNormalizationValue(double& normalizationValue) const override; + + void setBuffer(const double value, const double normalization); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::Force3DSensor : public wearable::sensor::IForce3DSensor +{ +public: + wearable::Vector3 m_buffer; + mutable std::mutex m_mutex; + + Force3DSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s); + ~Force3DSensor() override = default; + + bool getForce3D(wearable::Vector3& force) const override; + + void setBuffer(const wearable::Vector3& data); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::ForceTorque6DSensor : public wearable::sensor::IForceTorque6DSensor +{ +public: + wearable::Vector3 m_force; + wearable::Vector3 m_torque; + mutable std::mutex m_mutex; + + ForceTorque6DSensor(wearable::sensor::SensorName n, wearable::sensor::SensorStatus s); + ~ForceTorque6DSensor() override = default; + + bool getForceTorque6D(wearable::Vector3& force3D, wearable::Vector3& torque3D) const override; + + void setBuffer(const wearable::Vector3& force, const wearable::Vector3& torque); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::FreeBodyAccelerationSensor + : public wearable::sensor::IFreeBodyAccelerationSensor +{ +public: + wearable::Vector3 m_buffer; + mutable std::mutex m_mutex; + + FreeBodyAccelerationSensor( + wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~FreeBodyAccelerationSensor() override = default; + + bool getFreeBodyAcceleration(wearable::Vector3& freeBodyAcceleration) const override; + + void setBuffer(const wearable::Vector3& data); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::Gyroscope : public wearable::sensor::IGyroscope +{ +public: + wearable::Vector3 m_buffer; + mutable std::mutex m_mutex; + + Gyroscope(wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~Gyroscope() override = default; + + bool getAngularRate(wearable::Vector3& angularRate) const override; + + void setBuffer(const wearable::Vector3& data); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::Magnetometer : public wearable::sensor::IMagnetometer +{ +public: + wearable::Vector3 m_buffer; + mutable std::mutex m_mutex; + + Magnetometer(wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~Magnetometer() override = default; + + bool getMagneticField(wearable::Vector3& magneticField) const override; + + void setBuffer(const wearable::Vector3& data); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::OrientationSensor : public wearable::sensor::IOrientationSensor +{ +public: + wearable::Quaternion m_buffer; + mutable std::mutex m_mutex; + + OrientationSensor(wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~OrientationSensor() override = default; + + bool getOrientationAsQuaternion(wearable::Quaternion& orientation) const override; + + void setBuffer(const wearable::Quaternion& data); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::PoseSensor : public wearable::sensor::IPoseSensor +{ +public: + wearable::Vector3 m_position; + wearable::Quaternion m_orientation; + mutable std::mutex m_mutex; + + PoseSensor(wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~PoseSensor() override = default; + + bool getPose(wearable::Quaternion& orientation, wearable::Vector3& position) const override; + + void setBuffer(const wearable::Quaternion& orientation, const wearable::Vector3& position); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::PositionSensor : public wearable::sensor::IPositionSensor +{ +public: + wearable::Vector3 m_buffer; + mutable std::mutex m_mutex; + + PositionSensor(wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~PositionSensor() override = default; + + bool getPosition(wearable::Vector3& position) const override; + + void setBuffer(const wearable::Vector3& data); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +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; } +}; + +class wearable::sensor::impl::TemperatureSensor : public wearable::sensor::ITemperatureSensor +{ +public: + mutable std::mutex m_mutex; + double m_value; + + TemperatureSensor(wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~TemperatureSensor() override = default; + + bool getTemperature(double& temperature) const override; + + void setBuffer(const double value); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::Torque3DSensor : public wearable::sensor::ITorque3DSensor +{ +public: + wearable::Vector3 m_buffer; + mutable std::mutex m_mutex; + + Torque3DSensor(wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~Torque3DSensor() override = default; + + bool getTorque3D(wearable::Vector3& torque) const override; + + void setBuffer(const wearable::Vector3& data); + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::VirtualLinkKinSensor : public wearable::sensor::IVirtualLinkKinSensor +{ +public: + wearable::Vector3 m_linearAcc; + wearable::Vector3 m_angularAcc; + wearable::Vector3 m_linearVel; + wearable::Vector3 m_angularVel; + wearable::Vector3 m_position; + wearable::Quaternion m_orientation; + mutable std::mutex m_mutex; + + VirtualLinkKinSensor( + wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~VirtualLinkKinSensor() override = default; + + bool getLinkAcceleration(wearable::Vector3& linear, wearable::Vector3& angular) const override; + bool getLinkPose(wearable::Vector3& position, wearable::Quaternion& orientation) const override; + bool getLinkVelocity(wearable::Vector3& linear, wearable::Vector3& angular) const override; + + void setBuffer(const wearable::Vector3& linearAcc, + const wearable::Vector3& angularAcc, + const wearable::Vector3& linearVel, + const wearable::Vector3& angularVel, + const wearable::Vector3& position, + const wearable::Quaternion& orientation); + + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::VirtualJointKinSensor + : public wearable::sensor::IVirtualJointKinSensor +{ +public: + double m_position; + double m_velocity; + double m_acceleration; + mutable std::mutex m_mutex; + + VirtualJointKinSensor( + wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~VirtualJointKinSensor() override = default; + + bool getJointPosition(double& position) const override; + bool getJointVelocity(double& velocity) const override; + bool getJointAcceleration(double& acceleration) const override; + + void setBuffer(const double& position, + const double& velocity, + const double& acceleration); + + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +class wearable::sensor::impl::VirtualSphericalJointKinSensor + : public wearable::sensor::IVirtualSphericalJointKinSensor +{ +public: + wearable::Vector3 m_angleAsRPY; + wearable::Vector3 m_velocities; + wearable::Vector3 m_accelerations; + mutable std::mutex m_mutex; + + VirtualSphericalJointKinSensor( + wearable::sensor::SensorName n = {}, + wearable::sensor::SensorStatus s = wearable::sensor::SensorStatus::Unknown); + ~VirtualSphericalJointKinSensor() override = default; + + bool getJointAnglesAsRPY(wearable::Vector3& angleAsRPY) const override; + bool getJointVelocities(wearable::Vector3& velocities) const override; + bool getJointAccelerations(wearable::Vector3& accelerations) const override; + + void setBuffer(const wearable::Vector3& angleAsRPY, + const wearable::Vector3& velocities, + const wearable::Vector3& accelerations); + + inline void setStatus(const wearable::sensor::SensorStatus status) { m_status = status; } +}; + +#endif // SENSORSIMPL_H diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index ab671c883..430bc92d4 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -1,6 +1,8 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause +add_subdirectory(IWear) +add_subdirectory(IXsensMVNControl) add_subdirectory(IHumanState) add_subdirectory(IHumanWrench) add_subdirectory(IHumanDynamics) diff --git a/interfaces/IWear/CMakeLists.txt b/interfaces/IWear/CMakeLists.txt new file mode 100644 index 000000000..8c2654b6c --- /dev/null +++ b/interfaces/IWear/CMakeLists.txt @@ -0,0 +1,67 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +cmake_minimum_required(VERSION 3.5) +project(IWear LANGUAGES CXX VERSION ${PROJECT_VERSION}) + +include(GNUInstallDirs) + +# =============== +# IWEAR INTERFACE +# =============== + +# Policy to suppress An interface source of target "IWear" has a relative path warning +# Policy CMP0076 is not set: target_sources() command converts relative pathsto absolute. +cmake_policy(SET CMP0076 OLD) + +# Group files with the correct path for the target_sources command +file(GLOB_RECURSE IWEAR_HEADERS_BUILD include/*.h) +file(GLOB_RECURSE IWEAR_HEADERS_INSTALL + RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} + *.h) + +set(IWEAR_HEADERS + $ + $) + +add_library(${PROJECT_NAME} INTERFACE) +add_library(IWear::IWear ALIAS IWear) +target_sources(${PROJECT_NAME} INTERFACE ${IWEAR_HEADERS}) + +target_include_directories(${PROJECT_NAME} INTERFACE + $ + $) + +install( + TARGETS ${PROJECT_NAME} + EXPORT IWear) + +if(MSVC) + # Import definitions from standard cmath + target_compile_definitions(${PROJECT_NAME} INTERFACE "_USE_MATH_DEFINES") +endif() + +# Install all the directories preserving their structure +install( + FILES include/Wearable/IWear/Common.h + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) +install( + FILES include/Wearable/IWear/IWear.h + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) +install( + FILES include/Wearable/IWear/Utils.h + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) +install( + DIRECTORY include/Wearable/IWear/Sensors + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) +install( + DIRECTORY include/Wearable/IWear/Actuators + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/Wearable/IWear) + +# Generate the PackageConfig.cmake file +install_basic_package_files(IWear + VERSION ${PROJECT_VERSION} + COMPATIBILITY AnyNewerVersion + EXPORT IWear + NO_CHECK_REQUIRED_COMPONENTS_MACRO) diff --git a/interfaces/IWear/include/Wearable/IWear/Actuators/IActuator.h b/interfaces/IWear/include/Wearable/IWear/Actuators/IActuator.h new file mode 100644 index 000000000..82be4fd8c --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Actuators/IActuator.h @@ -0,0 +1,80 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IACTUATOR_H +#define WEARABLE_IACTUATOR_H + +#include "Wearable/IWear/Common.h" + +#include + +namespace wearable { + + namespace actuator { + + using ActuatorName = std::string; + + enum class ActuatorType + { + Haptic = 0, + Motor, + Heater, + Invalid + }; + + enum class ActuatorStatus + { + Error = 0, + Ok, + Unknown, + }; + + class IActuator; + } // namespace actuator +} // namespace wearable + +class wearable::actuator::IActuator : public wearable::IWearableDevice +{ +protected: + ActuatorName m_name; + ActuatorType m_type; + std::atomic m_status; + +public: + IActuator(ActuatorName aName = {}, ActuatorStatus aStatus = ActuatorStatus::Unknown) + : m_name{aName} + , m_status{aStatus} + { + m_wearable_element_type = ElementType::WearableActuator; + } + + virtual ~IActuator() = default; + + inline ElementType getWearableElementType() const; + + inline ActuatorName getActuatorName() const; + inline ActuatorType getActuatorType() const; + inline ActuatorStatus getActuatorStatus() const; +}; + +inline wearable::ElementType wearable::actuator::IActuator::getWearableElementType() const +{ + return m_wearable_element_type; +} + +inline wearable::actuator::ActuatorName wearable::actuator::IActuator::getActuatorName() const +{ + return m_name; +} + +inline wearable::actuator::ActuatorType wearable::actuator::IActuator::getActuatorType() const +{ + return m_type; +} + +inline wearable::actuator::ActuatorStatus wearable::actuator::IActuator::getActuatorStatus() const +{ + return m_status; +} + +#endif // WEARABLE_IACTUATOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h b/interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h new file mode 100644 index 000000000..2d5fcc751 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h @@ -0,0 +1,39 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IHAPTIC_H +#define WEARABLE_IHAPTIC_H + +#include "Wearable/IWear/Actuators/IActuator.h" + +namespace wearable { + namespace actuator { + class IHaptic; + } // namespace actuator +} // namespace wearable + +class wearable::actuator::IHaptic : public wearable::actuator::IActuator +{ +public: + IHaptic(ActuatorName aName = {}, + ActuatorStatus aStatus = ActuatorStatus::Unknown) + : IActuator(aName, aStatus) + { + m_type = ActuatorType::Haptic; + } + + virtual ~IHaptic() = default; + + 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() +{ + return "haptic" + wearable::Separator; +} + +#endif // WEARABLE_IHAPTIC_H diff --git a/interfaces/IWear/include/Wearable/IWear/Actuators/IHeater.h b/interfaces/IWear/include/Wearable/IWear/Actuators/IHeater.h new file mode 100644 index 000000000..b296096fc --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Actuators/IHeater.h @@ -0,0 +1,37 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IHEATER_H +#define WEARABLE_IHEATER_H + +#include "Wearable/IWear/Actuators/IActuator.h" + +namespace wearable { + namespace actuator { + class IHeater; + } // namespace actuator +} // namespace wearable + +class wearable::actuator::IHeater : public wearable::actuator::IActuator +{ +public: + IHeater(ActuatorName aName = {}, + ActuatorStatus aStatus = ActuatorStatus::Unknown) + : IActuator(aName, aStatus) + { + m_type = ActuatorType::Heater; + } + + virtual ~IHeater() = default; + + inline static const std::string getPrefix(); + + //TODO: Update set and get methods +}; + +inline const std::string wearable::actuator::IHeater::getPrefix() +{ + return "heater" + wearable::Separator; +} + +#endif // WEARABLE_IHEATER_H diff --git a/interfaces/IWear/include/Wearable/IWear/Actuators/IMotor.h b/interfaces/IWear/include/Wearable/IWear/Actuators/IMotor.h new file mode 100644 index 000000000..ee8e00ec3 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Actuators/IMotor.h @@ -0,0 +1,37 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IMOTOR_H +#define WEARABLE_IMOTOR_H + +#include "Wearable/IWear/Actuators/IActuator.h" + +namespace wearable { + namespace actuator { + class IMotor; + } // namespace actuator +} // namespace wearable + +class wearable::actuator::IMotor : public wearable::actuator::IActuator +{ +public: + IMotor(ActuatorName aName = {}, + ActuatorStatus aStatus = ActuatorStatus::Unknown) + : IActuator(aName, aStatus) + { + m_type = ActuatorType::Motor; + } + + virtual ~IMotor() = default; + + inline static const std::string getPrefix(); + + virtual bool setMotorPosition(double& value) const = 0; //TODO: Double check if rads or deg +}; + +inline const std::string wearable::actuator::IMotor::getPrefix() +{ + return "motor" + wearable::Separator; +} + +#endif // WEARABLE_IMOTOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Common.h b/interfaces/IWear/include/Wearable/IWear/Common.h new file mode 100644 index 000000000..393ddd8b1 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Common.h @@ -0,0 +1,41 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_COMMON_H +#define WEARABLE_COMMON_H + +#ifndef wError +#include +#define wError std::cerr +#endif + +#ifndef wWarning +#include +#define wWarning std::cout +#endif + +#include + +namespace wearable { + + const std::string Separator = "::"; + + enum class ElementType + { + WearableSensor = 0, + WearableActuator, + }; + + class IWearableDevice; + +} // namespace wearable + +class wearable::IWearableDevice +{ +protected: + ElementType m_wearable_element_type; + + virtual ElementType getWearableElementType() const = 0; +}; + +#endif // WEARABLE_COMMON_H diff --git a/interfaces/IWear/include/Wearable/IWear/IWear.h b/interfaces/IWear/include/Wearable/IWear/IWear.h new file mode 100644 index 000000000..72e9311c2 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/IWear.h @@ -0,0 +1,575 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IWEAR_H +#define WEARABLE_IWEAR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +#include "Wearable/IWear/Sensors/IAccelerometer.h" +#include "Wearable/IWear/Sensors/IEmgSensor.h" +#include "Wearable/IWear/Sensors/IForce3DSensor.h" +#include "Wearable/IWear/Sensors/IForceTorque6DSensor.h" +#include "Wearable/IWear/Sensors/IFreeBodyAccelerationSensor.h" +#include "Wearable/IWear/Sensors/IGyroscope.h" +#include "Wearable/IWear/Sensors/IMagnetometer.h" +#include "Wearable/IWear/Sensors/IOrientationSensor.h" +#include "Wearable/IWear/Sensors/IPoseSensor.h" +#include "Wearable/IWear/Sensors/IPositionSensor.h" +#include "Wearable/IWear/Sensors/ISkinSensor.h" +#include "Wearable/IWear/Sensors/ITemperatureSensor.h" +#include "Wearable/IWear/Sensors/ITorque3DSensor.h" +#include "Wearable/IWear/Sensors/IVirtualJointKinSensor.h" +#include "Wearable/IWear/Sensors/IVirtualLinkKinSensor.h" +#include "Wearable/IWear/Sensors/IVirtualSphericalJointKinSensor.h" + +#include "Wearable/IWear/Actuators/IActuator.h" + +#include "Wearable/IWear/Actuators/IHaptic.h" +#include "Wearable/IWear/Actuators/IHeater.h" +#include "Wearable/IWear/Actuators/IMotor.h" + +#include +#include +#include + +namespace wearable { + using WearableName = std::string; + using WearStatus = sensor::SensorStatus; + + // Wearable sensors generic variables + template + using SensorVector = std::vector; + + template + using SensorPtr = std::shared_ptr; + + template + using VectorOfSensorPtr = SensorVector>; + + using VectorOfSensorNames = SensorVector; + + // Wearable element (referes to either sensors or actuator) generic variables + template + using ElementVector = std::vector; + + template + using ElementPtr = std::shared_ptr; + + template + using VectorOfElementPtr = ElementVector>; + + using VectorOfActuatorNames = ElementVector; + + struct TimeStamp + { + double time = 0; + size_t sequenceNumber = 0; + }; + + // Vector with all the valid sensor types + // (actuator::SensorType::Invalid is not included in the list) + const std::vector AllSensorTypes = { + sensor::SensorType::Accelerometer, + sensor::SensorType::EmgSensor, + sensor::SensorType::Force3DSensor, + sensor::SensorType::ForceTorque6DSensor, + sensor::SensorType::FreeBodyAccelerationSensor, + sensor::SensorType::Gyroscope, + sensor::SensorType::Magnetometer, + sensor::SensorType::OrientationSensor, + sensor::SensorType::PoseSensor, + sensor::SensorType::PositionSensor, + sensor::SensorType::SkinSensor, + sensor::SensorType::TemperatureSensor, + sensor::SensorType::Torque3DSensor, + sensor::SensorType::VirtualLinkKinSensor, + sensor::SensorType::VirtualJointKinSensor, + sensor::SensorType::VirtualSphericalJointKinSensor, + }; + + // Vector with all the valid actuator types + // (actuator::ActuatorType::Invalid is not included in the list) + const std::vector AllActuatorTypes = { + actuator::ActuatorType::Haptic, + actuator::ActuatorType::Motor, + actuator::ActuatorType::Heater, + }; + + class IWear; +} // namespace wearable + +class wearable::IWear +{ +private: + template + static VectorOfSensorPtr + castVectorOfSensorPtr(const VectorOfSensorPtr& iSensors); + + template // Element, Type + static VectorOfElementPtr + castVectorOfElementPtr(const VectorOfElementPtr& iElement); + +public: + virtual ~IWear() = default; + + // =============== + // GENERIC METHODS + // =============== + + virtual WearableName getWearableName() const = 0; + virtual WearStatus getStatus() const = 0; + virtual TimeStamp getTimeStamp() const = 0; + + virtual SensorPtr getSensor(const sensor::SensorName name) const = 0; + virtual VectorOfSensorPtr + getSensors(const sensor::SensorType type) const = 0; + + // NOTE: Templatized virtual functions are not allowed + virtual ElementPtr + getActuator(const actuator::ActuatorName name) const = 0; + virtual VectorOfElementPtr + getActuators(const actuator::ActuatorType type) const = 0; + + // ============== + // SINGLE SENSORS + // ============== + + virtual SensorPtr + getAccelerometer(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getEmgSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getForce3DSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getForceTorque6DSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getFreeBodyAccelerationSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getGyroscope(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getMagnetometer(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getOrientationSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getPoseSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getPositionSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getSkinSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getTemperatureSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getTorque3DSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getVirtualLinkKinSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getVirtualJointKinSensor(const sensor::SensorName /*name*/) const = 0; + + virtual SensorPtr + getVirtualSphericalJointKinSensor(const sensor::SensorName /*name*/) const = 0; + + // ================= + // GENERIC UTILITIES + // ================= + + inline VectorOfSensorPtr getAllSensors() const; + + inline VectorOfSensorNames getSensorNames(const sensor::SensorType type) const; + + inline VectorOfSensorNames getAllSensorNames() const; + + // ================ + // SENSOR UTILITIES + // ================ + + inline VectorOfSensorPtr getAccelerometers() const; + + inline VectorOfSensorPtr getEmgSensors() const; + + inline VectorOfSensorPtr getForce3DSensors() const; + + inline VectorOfSensorPtr getForceTorque6DSensors() const; + + inline VectorOfSensorPtr + getFreeBodyAccelerationSensors() const; + + inline VectorOfSensorPtr getGyroscopes() const; + + inline VectorOfSensorPtr getMagnetometers() const; + + inline VectorOfSensorPtr getOrientationSensors() const; + + inline VectorOfSensorPtr getPoseSensors() const; + + inline VectorOfSensorPtr getPositionSensors() const; + + inline VectorOfSensorPtr getSkinSensors() const; + + inline VectorOfSensorPtr getTemperatureSensors() const; + + inline VectorOfSensorPtr getTorque3DSensors() const; + + inline VectorOfSensorPtr getVirtualLinkKinSensors() const; + + inline VectorOfSensorPtr + getVirtualJointKinSensors() const; + + inline VectorOfSensorPtr + getVirtualSphericalJointKinSensors() const; + + // =============== + // SINGLE ACTUATOR + // =============== + + virtual ElementPtr + getHapticActuator(const actuator::ActuatorName) const = 0; + + virtual ElementPtr + getMotorActuator(const actuator::ActuatorName) const = 0; + + virtual ElementPtr + getHeaterActuator(const actuator::ActuatorName) const = 0; + + // ========================== + // GENERIC ACTUATOR UTILITIES + // ========================== + + inline VectorOfElementPtr getAllActuators() const; + + inline VectorOfActuatorNames getActuatorNames(const actuator::ActuatorType type) const; + + inline VectorOfActuatorNames getAllActuatorNames() const; + + // =================== + // ACTUATORS UTILITIES + // =================== + + inline VectorOfElementPtr getHapticActuators() const; + + inline VectorOfElementPtr getMotorActuators() const; + + inline VectorOfElementPtr getHeaterActuators() const; + + // ======================== + // GENERIC DEVICE UTILITIES + // ======================== + + // TODO: The following methods can replace sensor generic utilities + // TODO: Two template names are redundant, check how to use one + // template + // inline VectorOfElementPtr getAllElements() const; +}; + +// ============================================ +// IMPLEMENTATION OF INLINE AND PRIVATE METHODS +// ============================================ + +template +wearable::VectorOfSensorPtr +wearable::IWear::castVectorOfSensorPtr(const VectorOfSensorPtr& iSensors) +{ + VectorOfSensorPtr sensors; + sensors.reserve(iSensors.size()); + + for (const auto& iSensor : iSensors) { + wearable::SensorPtr castSensor = std::dynamic_pointer_cast(iSensor); + + if (!castSensor) { + wError << "Failed to cast sensor"; + return {}; + } + + sensors.push_back(castSensor); + } + + return sensors; +} + +template +wearable::VectorOfElementPtr +wearable::IWear::castVectorOfElementPtr(const VectorOfElementPtr& iElements) +{ + VectorOfElementPtr elements; + elements.reserve(iElements.size()); + + for (const auto& iElement : iElements) { + wearable::ElementPtr castElement = std::dynamic_pointer_cast(iElement); + + if (!castElement) { + + if (castElement->getWearableElementType() == wearable::ElementType::WearableSensor) { + wError << "Failed to cast wearable sensor element"; + } + else if (castElement->getWearableElementType() + == wearable::ElementType::WearableActuator) { + wError << "Failed to case wearable actuator element"; + } + } + + elements.push_back(castElement); + } + + return elements; +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getAllSensors() const +{ + VectorOfSensorPtr allSensors; + + for (const auto& sensorType : AllSensorTypes) { + VectorOfSensorPtr tmp; + tmp = getSensors(sensorType); + allSensors.insert(allSensors.end(), tmp.begin(), tmp.end()); + } + + return allSensors; +} + +inline wearable::VectorOfElementPtr +wearable::IWear::getAllActuators() const +{ + VectorOfElementPtr allActuators; + + for (const auto& actuatorType : AllActuatorTypes) { + VectorOfElementPtr tmp; + tmp = getActuators(actuatorType); + allActuators.insert(allActuators.end(), tmp.begin(), tmp.end()); + } + + return allActuators; +} + +// template +// inline wearable::VectorOfElementPtr wearable::IWear::getAllElements() const +//{ +// wearable::VectorOfElementPtr allElements; + +// if (allElements.at(0)->getWearableElementType() == wearable::ElementType::WearableSensor) +// { +// for (const auto& type : wearable::AllSensorTypes) +// { +// wearable::VectorOfElementPtr tmp; +// tmp = getSensors(type); + +// allElements.insert(allElements.end(), tmp.begin(), tmp.end()); +// } + +// } + +// if (allElements.at(0)->getWearableElementType() == wearable::ElementType::WearableActuator) +// { +// for (const auto& type : wearable::AllActuatorTypes) +// { +// wearable::VectorOfElementPtr tmp; +// tmp = getActuators(type); + +// allElements.insert(allDevices.end(), tmp.begin(), tmp.end()); +// } +// } + +// return allElements; +//} + +inline wearable::VectorOfSensorNames +wearable::IWear::getSensorNames(const sensor::SensorType type) const +{ + const wearable::VectorOfSensorPtr sensors = getSensors(type); + + VectorOfSensorNames sensorNames; + sensorNames.reserve(sensors.size()); + + for (const auto& s : sensors) { + sensorNames.push_back(s->getSensorName()); + } + return sensorNames; +} + +inline wearable::VectorOfActuatorNames +wearable::IWear::getActuatorNames(const actuator::ActuatorType type) const +{ + const wearable::VectorOfElementPtr actuators = getActuators(type); + + VectorOfActuatorNames actuatorNames; + actuatorNames.reserve(actuators.size()); + + for (const auto& a : actuators) { + actuatorNames.push_back(a->getActuatorName()); + } + + return actuatorNames; +} + +inline wearable::VectorOfSensorNames wearable::IWear::getAllSensorNames() const +{ + const VectorOfSensorPtr sensors = getAllSensors(); + + VectorOfSensorNames sensorNames; + sensorNames.reserve(sensors.size()); + + for (const auto& s : sensors) { + sensorNames.push_back(s->getSensorName()); + } + + return sensorNames; +} + +inline wearable::VectorOfActuatorNames wearable::IWear::getAllActuatorNames() const +{ + const VectorOfElementPtr actuators = getAllActuators(); + + VectorOfActuatorNames actuatorNames; + actuatorNames.reserve(actuators.size()); + + for (const auto& a : actuators) { + actuatorNames.push_back(a->getActuatorName()); + } + + return actuatorNames; +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getAccelerometers() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::Accelerometer)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getEmgSensors() const +{ + return castVectorOfSensorPtr(getSensors(sensor::SensorType::EmgSensor)); +} + +wearable::VectorOfSensorPtr +wearable::IWear::getForce3DSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::Force3DSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getForceTorque6DSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::ForceTorque6DSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getFreeBodyAccelerationSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::FreeBodyAccelerationSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getGyroscopes() const +{ + return castVectorOfSensorPtr(getSensors(sensor::SensorType::Gyroscope)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getMagnetometers() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::Magnetometer)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getOrientationSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::OrientationSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getPoseSensors() const +{ + return castVectorOfSensorPtr(getSensors(sensor::SensorType::PoseSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getPositionSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::PositionSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getSkinSensors() const +{ + return castVectorOfSensorPtr(getSensors(sensor::SensorType::SkinSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getTemperatureSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::TemperatureSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getTorque3DSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::Torque3DSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getVirtualLinkKinSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::VirtualLinkKinSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getVirtualJointKinSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::VirtualJointKinSensor)); +} + +inline wearable::VectorOfSensorPtr +wearable::IWear::getVirtualSphericalJointKinSensors() const +{ + return castVectorOfSensorPtr( + getSensors(sensor::SensorType::VirtualSphericalJointKinSensor)); +} + +inline wearable::VectorOfElementPtr +wearable::IWear::getHapticActuators() const +{ + return castVectorOfElementPtr( + getActuators(actuator::ActuatorType::Haptic)); +} + +inline wearable::VectorOfElementPtr +wearable::IWear::getMotorActuators() const +{ + return castVectorOfElementPtr( + getActuators(actuator::ActuatorType::Motor)); +} + +inline wearable::VectorOfElementPtr +wearable::IWear::getHeaterActuators() const +{ + return castVectorOfElementPtr( + getActuators(actuator::ActuatorType::Heater)); +} + +#endif // WEARABLE_IWEAR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IAccelerometer.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IAccelerometer.h new file mode 100644 index 000000000..4a7a69a46 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IAccelerometer.h @@ -0,0 +1,35 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IACCELEROMETER_H +#define WEARABLE_IACCELEROMETER_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IAccelerometer; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IAccelerometer : public wearable::sensor::ISensor +{ +public: + IAccelerometer(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::Accelerometer; + } + + virtual ~IAccelerometer() = default; + + virtual bool getLinearAcceleration(Vector3& linearAcceleration) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IAccelerometer::getPrefix() +{ + return "acc" + wearable::Separator; +} +#endif // WEARABLE_IACCELEROMETER_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IEmgSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IEmgSensor.h new file mode 100644 index 000000000..fd9375e24 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IEmgSensor.h @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IEMG_SENSOR_H +#define WEARABLE_IEMG_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IEmgSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IEmgSensor : public wearable::sensor::ISensor +{ +public: + IEmgSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::EmgSensor; + } + + virtual ~IEmgSensor() = default; + + virtual bool getEmgSignal(double& emgSignal) const = 0; + virtual bool getNormalizationValue(double& normalizationValue) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IEmgSensor::getPrefix() +{ + return "emg" + wearable::Separator; +} +#endif // WEARABLE_IEMG_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IForce3DSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IForce3DSensor.h new file mode 100644 index 000000000..d8d183aca --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IForce3DSensor.h @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IFORCE_3D_SENSOR_H +#define WEARABLE_IFORCE_3D_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IForce3DSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IForce3DSensor : public wearable::sensor::ISensor +{ +public: + IForce3DSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::Force3DSensor; + } + + virtual ~IForce3DSensor() = default; + + virtual bool getForce3D(Vector3& force) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IForce3DSensor::getPrefix() +{ + return "f3D" + wearable::Separator; +} + +#endif // WEARABLE_IFORCE_3D_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IForceTorque6DSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IForceTorque6DSensor.h new file mode 100644 index 000000000..9ac2b8d16 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IForceTorque6DSensor.h @@ -0,0 +1,61 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IFORCE_TORQUE_6D_SENSOR_H +#define WEARABLE_IFORCE_TORQUE_6D_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IForceTorque6DSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IForceTorque6DSensor : public wearable::sensor::ISensor +{ +public: + IForceTorque6DSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::ForceTorque6DSensor; + } + + virtual ~IForceTorque6DSensor() = default; + + virtual bool getForceTorque6D(Vector3& force3D, Vector3& torque3D) const = 0; + + inline bool getForceTorque6D(Vector6& forceTorque6D) const; + inline bool getForceTorque3DForce(Vector3& force3D) const; + inline bool getForceTorque3DTorque(Vector3& torque3D) const; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IForceTorque6DSensor::getPrefix() +{ + return "ft6D" + wearable::Separator; +} +bool wearable::sensor::IForceTorque6DSensor::getForceTorque6D( + wearable::Vector6& forceTorque6D) const +{ + wearable::Vector3 force3D, torque3D; + const bool ok = getForceTorque6D(force3D, torque3D); + forceTorque6D = {force3D[0], force3D[1], force3D[2], torque3D[0], torque3D[1], torque3D[2]}; + return ok; +} + +bool wearable::sensor::IForceTorque6DSensor::getForceTorque3DForce(wearable::Vector3& force3D) const +{ + wearable::Vector3 dummy; + return getForceTorque6D(force3D, dummy); +} + +bool wearable::sensor::IForceTorque6DSensor::getForceTorque3DTorque( + wearable::Vector3& torque3D) const +{ + wearable::Vector3 dummy; + return getForceTorque6D(dummy, torque3D); +} + +#endif // WEARABLE_IFORCE_TORQUE_6D_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IFreeBodyAccelerationSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IFreeBodyAccelerationSensor.h new file mode 100644 index 000000000..f03b6997a --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IFreeBodyAccelerationSensor.h @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IFREE_BODY_ACCELERATION_SENSOR_H +#define WEARABLE_IFREE_BODY_ACCELERATION_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IFreeBodyAccelerationSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IFreeBodyAccelerationSensor : public wearable::sensor::ISensor +{ +public: + IFreeBodyAccelerationSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::FreeBodyAccelerationSensor; + } + + virtual ~IFreeBodyAccelerationSensor() = default; + + virtual bool getFreeBodyAcceleration(Vector3& freeBodyAcceleration) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IFreeBodyAccelerationSensor::getPrefix() +{ + return "fbAcc" + wearable::Separator; +} + +#endif // WEARABLE_IFREE_BODY_ACCELERATION_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IGyroscope.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IGyroscope.h new file mode 100644 index 000000000..96a304809 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IGyroscope.h @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IGYROSCOPE_H +#define WEARABLE_IGYROSCOPE_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IGyroscope; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IGyroscope : public wearable::sensor::ISensor +{ +public: + IGyroscope(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::Gyroscope; + } + + virtual ~IGyroscope() = default; + + virtual bool getAngularRate(Vector3& angularRate) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IGyroscope::getPrefix() +{ + return "gyro" + wearable::Separator; +} + +#endif // WEARABLE_IGYROSCOPE_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IMagnetometer.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IMagnetometer.h new file mode 100644 index 000000000..ef3471358 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IMagnetometer.h @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IMAGNETOMETER_H +#define WEARABLE_IMAGNETOMETER_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IMagnetometer; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IMagnetometer : public wearable::sensor::ISensor +{ +public: + IMagnetometer(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::Magnetometer; + } + + virtual ~IMagnetometer() = default; + + virtual bool getMagneticField(Vector3& magneticField) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IMagnetometer::getPrefix() +{ + return "mag" + wearable::Separator; +} + +#endif // WEARABLE_IMAGNETOMETER_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IOrientationSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IOrientationSensor.h new file mode 100644 index 000000000..777f05a03 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IOrientationSensor.h @@ -0,0 +1,60 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IORIENTATION_SENSOR_H +#define WEARABLE_IORIENTATION_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" +#include "Wearable/IWear/Utils.h" + +namespace wearable { + namespace sensor { + class IOrientationSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IOrientationSensor : public wearable::sensor::ISensor +{ +public: + IOrientationSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::OrientationSensor; + } + + virtual ~IOrientationSensor() = default; + virtual bool getOrientationAsQuaternion(Quaternion& orientation) const = 0; + + inline bool getOrientationAsRPY(Vector3& orientation) const; + inline bool getOrientationAsRotationMatrix(Matrix3& orientation) const; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IOrientationSensor::getPrefix() +{ + return "orient" + wearable::Separator; +} + +inline bool wearable::sensor::IOrientationSensor::getOrientationAsRPY(Vector3& orientation) const +{ + Quaternion quat; + if (!getOrientationAsQuaternion(quat)) { + return false; + } + orientation = utils::quaternionToRPY(quat); + return true; +}; + +inline bool +wearable::sensor::IOrientationSensor::getOrientationAsRotationMatrix(Matrix3& orientation) const +{ + Quaternion quat; + if (!getOrientationAsQuaternion(quat)) { + return false; + } + orientation = utils::quaternionToRotationMatrix(quat); + return true; +}; + +#endif // WEARABLE_IORIENTATION_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IPoseSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IPoseSensor.h new file mode 100644 index 000000000..1d4a13747 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IPoseSensor.h @@ -0,0 +1,95 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IPOSE_SENSOR_H +#define WEARABLE_IPOSE_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" +#include "Wearable/IWear/Utils.h" + +namespace wearable { + namespace sensor { + class IPoseSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IPoseSensor : public wearable::sensor::ISensor +{ +public: + IPoseSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::PoseSensor; + } + + virtual ~IPoseSensor() = default; + + virtual bool getPose(Quaternion& orientation, Vector3& position) const = 0; + + inline bool getPose(Vector7& pose) const; + inline bool getPoseOrientationAsQuaternion(Quaternion& orientation) const; + inline bool getPosePosition(Vector3& position) const; + inline bool getPoseOrientationAsRotationMatrix(Matrix3& orientation) const; + inline bool getPoseOrientationAsRPY(Vector3& orientation) const; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IPoseSensor::getPrefix() +{ + return "pose" + wearable::Separator; +} + +inline bool wearable::sensor::IPoseSensor::getPose(Vector7& pose) const +{ + Quaternion orientation; + Vector3 position; + if (!getPose(orientation, position)) { + return false; + } + + pose[0] = orientation[0]; + pose[1] = orientation[1]; + pose[2] = orientation[2]; + pose[3] = orientation[3]; + pose[4] = position[0]; + pose[5] = position[1]; + pose[6] = position[2]; + + return true; +} + +bool wearable::sensor::IPoseSensor::getPoseOrientationAsQuaternion(Quaternion& orientation) const +{ + Vector3 dummy; + return getPose(orientation, dummy); +} + +inline bool wearable::sensor::IPoseSensor::getPosePosition(Vector3& position) const +{ + Quaternion dummy; + return getPose(dummy, position); +} + +inline bool +wearable::sensor::IPoseSensor::getPoseOrientationAsRotationMatrix(Matrix3& orientation) const +{ + Quaternion quat; + if (!getPoseOrientationAsQuaternion(quat)) { + return false; + } + orientation = utils::quaternionToRotationMatrix(quat); + return true; +}; + +inline bool wearable::sensor::IPoseSensor::getPoseOrientationAsRPY(Vector3& orientation) const +{ + Quaternion quat; + if (!getPoseOrientationAsQuaternion(quat)) { + return false; + } + orientation = utils::quaternionToRPY(quat); + return true; +}; + +#endif // WEARABLE_IPOSE_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IPositionSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IPositionSensor.h new file mode 100644 index 000000000..70aab7194 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IPositionSensor.h @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IPOSITION_SENSOR_H +#define WEARABLE_IPOSITION_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IPositionSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IPositionSensor : public wearable::sensor::ISensor +{ +public: + IPositionSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::PositionSensor; + } + + virtual ~IPositionSensor() = default; + + virtual bool getPosition(Vector3& position) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IPositionSensor::getPrefix() +{ + return "pos" + wearable::Separator; +} + +#endif // WEARABLE_IPOSITION_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/ISensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/ISensor.h new file mode 100644 index 000000000..a2405b9ee --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/ISensor.h @@ -0,0 +1,143 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_ISENSOR_H +#define WEARABLE_ISENSOR_H + +#include "Wearable/IWear/Common.h" + +#include +#include + +namespace wearable { + + using Vector3 = std::array; + using Vector6 = std::array; + using Vector7 = std::array; + using Matrix3 = std::array; // TODO: think about it, this force RowMajor + using Quaternion = std::array; + + namespace sensor { + using SensorName = std::string; + + enum class SensorStatus + { + Error = 0, + Ok, + Calibrating, + Overflow, + Timeout, + Unknown, + WaitingForFirstRead, + }; + + enum class SensorType + { + Accelerometer = 0, + EmgSensor, + Force3DSensor, + ForceTorque6DSensor, + FreeBodyAccelerationSensor, + Gyroscope, + Magnetometer, + OrientationSensor, + PoseSensor, + PositionSensor, + SkinSensor, + TemperatureSensor, + Torque3DSensor, + VirtualLinkKinSensor, + VirtualJointKinSensor, + VirtualSphericalJointKinSensor, + Invalid + }; + + inline SensorType sensorTypeFromString(std::string sensorTypeString) + { + if (sensorTypeString == "Accelerometer") + return SensorType::Accelerometer; + else if (sensorTypeString == "EmgSensor") + return SensorType::EmgSensor; + else if (sensorTypeString == "Force3DSensor") + return SensorType::Force3DSensor; + else if (sensorTypeString == "ForceTorque6DSensor") + return SensorType::ForceTorque6DSensor; + else if (sensorTypeString == "FreeBodyAccelerationSensor") + return SensorType::FreeBodyAccelerationSensor; + else if (sensorTypeString == "Gyroscope") + return SensorType::Gyroscope; + else if (sensorTypeString == "Magnetometer") + return SensorType::Magnetometer; + else if (sensorTypeString == "OrientationSensor") + return SensorType::OrientationSensor; + else if (sensorTypeString == "PoseSensor") + return SensorType::PoseSensor; + else if (sensorTypeString == "PositionSensor") + return SensorType::PositionSensor; + else if (sensorTypeString == "SkinSensor") + return SensorType::SkinSensor; + else if (sensorTypeString == "TemperatureSensor") + return SensorType::TemperatureSensor; + else if (sensorTypeString == "Torque3DSensor") + return SensorType::Torque3DSensor; + else if (sensorTypeString == "VirtualLinkKinSensor") + return SensorType::VirtualLinkKinSensor; + else if (sensorTypeString == "VirtualJointKinSensor") + return SensorType::VirtualJointKinSensor; + else if (sensorTypeString == "VirtualSphericalJointKinSensor") + return SensorType::VirtualSphericalJointKinSensor; + else { + return SensorType::Invalid; + } + } + + class ISensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::ISensor : public wearable::IWearableDevice +{ +protected: + SensorName m_name; + SensorType m_type; + std::atomic m_status; + +public: + ISensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : m_name{aName} + , m_status{aStatus} + { + m_wearable_element_type = ElementType::WearableSensor; + } + + virtual ~ISensor() = default; + + inline ElementType getWearableElementType() const; + + // TODO: timestamp? sequence number? + inline SensorName getSensorName() const; + inline SensorStatus getSensorStatus() const; + inline SensorType getSensorType() const; +}; + +inline wearable::ElementType wearable::sensor::ISensor::getWearableElementType() const +{ + return m_wearable_element_type; +} + +inline wearable::sensor::SensorName wearable::sensor::ISensor::getSensorName() const +{ + return m_name; +} + +inline wearable::sensor::SensorType wearable::sensor::ISensor::getSensorType() const +{ + return m_type; +} + +inline wearable::sensor::SensorStatus wearable::sensor::ISensor::getSensorStatus() const +{ + return m_status; +} + +#endif // WEARABLE_ISENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/ISkinSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/ISkinSensor.h new file mode 100644 index 000000000..7ab0b0166 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/ISkinSensor.h @@ -0,0 +1,37 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_ISKIN_SENSOR_H +#define WEARABLE_ISKIN_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" +#include + +namespace wearable { + namespace sensor { + class ISkinSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::ISkinSensor : public wearable::sensor::ISensor +{ +public: + ISkinSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::SkinSensor; + } + + virtual ~ISkinSensor() = default; + + virtual bool getPressure(std::vector& pressure) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::ISkinSensor::getPrefix() +{ + return "skin" + wearable::Separator; +} + +#endif // WEARABLE_ISKIN_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/ITemperatureSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/ITemperatureSensor.h new file mode 100644 index 000000000..ec6be8d90 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/ITemperatureSensor.h @@ -0,0 +1,35 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_ITEMPERATURE_SENSOR_H +#define WEARABLE_ITEMPERATURE_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class ITemperatureSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::ITemperatureSensor : public wearable::sensor::ISensor +{ +public: + ITemperatureSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::TemperatureSensor; + } + + virtual ~ITemperatureSensor() = default; + + virtual bool getTemperature(double& temperature) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::ITemperatureSensor::getPrefix() +{ + return "temp" + wearable::Separator; +} +#endif // WEARABLE_ITEMPERATURE_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/ITorque3DSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/ITorque3DSensor.h new file mode 100644 index 000000000..2c999ef5e --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/ITorque3DSensor.h @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_ITORQUE_3D_SENSOR_H +#define WEARABLE_ITORQUE_3D_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class ITorque3DSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::ITorque3DSensor : public wearable::sensor::ISensor +{ +public: + ITorque3DSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::Torque3DSensor; + } + + virtual ~ITorque3DSensor() = default; + + virtual bool getTorque3D(Vector3& torque) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::ITorque3DSensor::getPrefix() +{ + return "t3D" + wearable::Separator; +} + +#endif // WEARABLE_ITORQUE_3D_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualJointKinSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualJointKinSensor.h new file mode 100644 index 000000000..8d0f2ad1f --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualJointKinSensor.h @@ -0,0 +1,38 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IVIRTUAL_JOINT_KIN_SENSOR_H +#define WEARABLE_IVIRTUAL_JOINT_KIN_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IVirtualJointKinSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IVirtualJointKinSensor : public wearable::sensor::ISensor +{ +public: + IVirtualJointKinSensor(SensorName aName = {}, + SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::VirtualJointKinSensor; + } + + virtual bool getJointPosition(double& position) const = 0; + virtual bool getJointVelocity(double& velocity) const = 0; + virtual bool getJointAcceleration(double& acceleration) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IVirtualJointKinSensor::getPrefix() +{ + return "vJoint" + wearable::Separator; +} + + +#endif // WEARABLE_IVIRTUAL_JOINT_KIN_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualLinkKinSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualLinkKinSensor.h new file mode 100644 index 000000000..6318b5d48 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualLinkKinSensor.h @@ -0,0 +1,88 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IVIRTUAL_LINK_KIN_SENSOR_H +#define WEARABLE_IVIRTUAL_LINK_KIN_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IVirtualLinkKinSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IVirtualLinkKinSensor : public wearable::sensor::ISensor +{ +public: + IVirtualLinkKinSensor(SensorName aName = {}, SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::VirtualLinkKinSensor; + } + + virtual ~IVirtualLinkKinSensor() = default; + + // 6D quantities + virtual bool getLinkAcceleration(Vector3& linear, Vector3& angular) const = 0; + virtual bool getLinkPose(Vector3& position, Quaternion& orientation) const = 0; + virtual bool getLinkVelocity(Vector3& linear, Vector3& angular) const = 0; + + // 3D quantities + inline bool getLinkAngularAcceleration(Vector3& angularAcceleration) const; + inline bool getLinkAngularVelocity(Vector3& angularVelocity) const; + inline bool getLinkLinearAcceleration(Vector3& linearAcceleration) const; + inline bool getLinkLinearVelocity(Vector3& linearVelocity) const; + inline bool getLinkOrientation(Quaternion& orientation) const; + inline bool getLinkPosition(Vector3& position) const; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IVirtualLinkKinSensor::getPrefix() +{ + return "vLink" + wearable::Separator; +} + +inline bool wearable::sensor::IVirtualLinkKinSensor::getLinkAngularAcceleration( + Vector3& angularAcceleration) const +{ + Vector3 dummy; + return getLinkAcceleration(dummy, angularAcceleration); +} + +inline bool wearable::sensor::IVirtualLinkKinSensor::getLinkLinearAcceleration( + Vector3& linearAcceleration) const +{ + Vector3 dummy; + return getLinkAcceleration(linearAcceleration, dummy); +} + +inline bool +wearable::sensor::IVirtualLinkKinSensor::getLinkAngularVelocity(Vector3& angularVelocity) const +{ + Vector3 dummy; + return getLinkVelocity(dummy, angularVelocity); +} + +inline bool +wearable::sensor::IVirtualLinkKinSensor::getLinkLinearVelocity(Vector3& linearVelocity) const +{ + Vector3 dummy; + return getLinkVelocity(linearVelocity, dummy); +} + +inline bool +wearable::sensor::IVirtualLinkKinSensor::getLinkOrientation(Quaternion& orientation) const +{ + Vector3 dummy; + return getLinkPose(dummy, orientation); +} + +inline bool wearable::sensor::IVirtualLinkKinSensor::getLinkPosition(Vector3& position) const +{ + Quaternion dummy; + return getLinkPose(position, dummy); +} + +#endif // WEARABLE_IVIRTUAL_LINK_KIN_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualSphericalJointKinSensor.h b/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualSphericalJointKinSensor.h new file mode 100644 index 000000000..2bb92f4e7 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Sensors/IVirtualSphericalJointKinSensor.h @@ -0,0 +1,40 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_IVIRTUAL_SPHERICAL_JOINT_KIN_SENSOR_H +#define WEARABLE_IVIRTUAL_SPHERICAL_JOINT_KIN_SENSOR_H + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace sensor { + class IVirtualSphericalJointKinSensor; + } // namespace sensor +} // namespace wearable + +class wearable::sensor::IVirtualSphericalJointKinSensor : public wearable::sensor::ISensor +{ +public: + IVirtualSphericalJointKinSensor(SensorName aName = {}, + SensorStatus aStatus = SensorStatus::Unknown) + : ISensor(aName, aStatus) + { + m_type = SensorType::VirtualSphericalJointKinSensor; + } + + virtual ~IVirtualSphericalJointKinSensor() = default; + + // CONVENTION: RPY are defined as rotations about X-Y-Z wrt fixed frame + virtual bool getJointAnglesAsRPY(Vector3& angleAsRPY) const = 0; + virtual bool getJointVelocities(Vector3& velocities) const = 0; + virtual bool getJointAccelerations(Vector3& accelerations) const = 0; + + inline static const std::string getPrefix(); +}; + +inline const std::string wearable::sensor::IVirtualSphericalJointKinSensor::getPrefix() +{ + return "vSJoint" + wearable::Separator; +} + +#endif // WEARABLE_IVIRTUAL_SPHERICAL_JOINT_KIN_SENSOR_H diff --git a/interfaces/IWear/include/Wearable/IWear/Utils.h b/interfaces/IWear/include/Wearable/IWear/Utils.h new file mode 100644 index 000000000..a42840231 --- /dev/null +++ b/interfaces/IWear/include/Wearable/IWear/Utils.h @@ -0,0 +1,260 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WEARABLE_UTILS_H +#define WEARABLE_UTILS_H + +#include + +#include "Wearable/IWear/Sensors/ISensor.h" + +namespace wearable { + namespace utils { + inline Quaternion normalizeQuaternion(const Quaternion& quat); + inline Vector3 quaternionToRPY(const Quaternion& quat); + inline Matrix3 quaternionToRotationMatrix(const Quaternion& quat); + inline Quaternion rotationMatrixToQuaternion(const Matrix3& rotMat); + inline Vector3 rotationMatrixToRPY(const Matrix3& rotMat); + inline Quaternion RPYToQuaternion(const Vector3& rpy); + inline Matrix3 RPYToRotationMatrix(const Vector3& rpy); + } // namespace utils +} // namespace wearable + +inline wearable::Quaternion wearable::utils::normalizeQuaternion(const Quaternion& quat) +{ + double norm = + std::sqrt(quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]); + + Quaternion normalQuat(quat); + + if (norm != 1.0) { + Quaternion normalQuat; + for (auto& qi : normalQuat) { + qi /= norm; + } + } + return normalQuat; +} + +inline wearable::Vector3 wearable::utils::quaternionToRPY(const Quaternion& quat) +{ + Quaternion q = normalizeQuaternion(quat); + + double qw = q[0]; + double qx = q[1]; + double qy = q[2]; + double qz = q[3]; + + Vector3 rpy; + + // roll (x-axis rotation) + double sinr = 2.0 * (qw * qx + qy * qz); + double cosr = 1.0 - 2.0 * (qx * qx + qy * qy); + rpy[0] = std::atan2(sinr, cosr); + + // pitch (y-axis rotation) + double sinp = 2.0 * (qw * qy - qx * qz); + if (std::abs(sinp) >= 1.0) { + rpy[1] = std::copysign(M_PI / 2, sinp); + } + else { + rpy[1] = std::asin(sinp); + } + + // yaw (z-axis rotation) + double siny = 2.0 * (qw * qz + qx * qy); + double cosy = 1.0 - 2.0 * (qy * qy + qz * qz); + rpy[2] = std::atan2(siny, cosy); + + return rpy; +}; + +inline wearable::Matrix3 wearable::utils::quaternionToRotationMatrix(const Quaternion& quat) +{ + Quaternion q = normalizeQuaternion(quat); + + double qw = q[0]; + double qx = q[1]; + double qy = q[2]; + double qz = q[3]; + + Matrix3 rotMat; + + // first row + rotMat[0][0] = 1.0 - 2.0 * qy * qy - 2.0 * qz * qz; + rotMat[0][1] = 2.0 * qx * qy - 2.0 * qz * qw; + rotMat[0][2] = 2.0 * qx * qz + 2.0 * qy * qw; + + // second row + rotMat[1][0] = 2.0 * qx * qy + 2.0 * qz * qw; + rotMat[1][1] = 1.0 - 2.0 * qx * qx - 2.0 * qz * qz; + rotMat[1][2] = 2.0 * qy * qz - 2.0 * qx * qw; + + // third row + rotMat[2][0] = 2.0 * qx + qz - 2.0 * qy * qw; + rotMat[2][1] = 2.0 * qy * qz + 2 * qx * qw; + rotMat[2][2] = 1.0 - 2.0 * qx * qx - 2.0 * qy * qy; + + return rotMat; +}; + +inline wearable::Quaternion wearable::utils::rotationMatrixToQuaternion(const Matrix3& rotMat) +{ + + // Taken from "Contributions to the automatic control of aerial vehicles" + // PhD thesis of "Minh Duc HUA" + // INRIA Sophia Antipolis + // Equation 3.9 (page 101) + + // Diagonal elements used only to find the maximum + // the furthest value from zero + // we use this value as denominator to find the other elements + double qw = (rotMat[0][0] + rotMat[1][1] + rotMat[2][2] + 1.0); + double qx = (rotMat[0][0] - rotMat[1][1] - rotMat[2][2] + 1.0); + double qy = (-rotMat[0][0] + rotMat[1][1] - rotMat[2][2] + 1.0); + double qz = (-rotMat[0][0] - rotMat[1][1] + rotMat[2][2] + 1.0); + + if (qw < 0.0) { + qw = 0.0; + } + if (qx < 0.0) { + qx = 0.0; + } + if (qy < 0.0) { + qy = 0.0; + } + if (qz < 0.0) { + qz = 0.0; + } + + if (qw >= qx && qw >= qy && qw >= qz) { + qw = std::sqrt(qw); + qx = (rotMat[2][1] - rotMat[1][2]) / (2.0 * qw); + qy = (rotMat[0][2] - rotMat[2][0]) / (2.0 * qw); + qz = (rotMat[1][0] - rotMat[0][1]) / (2.0 * qw); + qw /= 2.0; + } + else if (qx >= qw && qx >= qy && qx >= qz) { + qx = std::sqrt(qx); + qw = (rotMat[2][1] - rotMat[1][2]) / (2.0 * qx); + qy = (rotMat[1][0] + rotMat[0][1]) / (2.0 * qx); + qz = (rotMat[2][0] + rotMat[0][2]) / (2.0 * qx); + qx /= 2.0; + } + else if (qy >= qw && qy >= qx && qy >= qz) { + qy = std::sqrt(qy); + qw = (rotMat[0][2] - rotMat[2][0]) / (2.0 * qy); + qx = (rotMat[1][0] + rotMat[0][1]) / (2.0 * qy); + qz = (rotMat[1][2] + rotMat[2][1]) / (2.0 * qy); + qy /= 2.0; + } + else if (qz >= qw && qz >= qx && qz >= qy) { + qz = std::sqrt(qz); + qw = (rotMat[1][0] - rotMat[0][1]) / (2.0 * qz); + qx = (rotMat[2][0] + rotMat[0][2]) / (2.0 * qz); + qy = (rotMat[1][2] + rotMat[2][1]) / (2.0 * qz); + qz /= 2.0; + } + + // Here we impose that the leftmost nonzero element of the quaternion is positive + double eps = 1e-7; + double sign = 1.0; + if (qw > eps || qw < -eps) { + sign = qw > 0 ? 1.0 : -1.0; + } + else if (qx > eps || qx < -eps) { + sign = qx > 0 ? 1.0 : -1.0; + } + else if (qy > eps || qy < -eps) { + sign = qy > 0 ? 1.0 : -1.0; + } + else if (qz > eps || qz < -eps) { + sign = qz > 0 ? 1.0 : -1.0; + } + + Quaternion quat; + quat[0] = qw / sign; + quat[1] = qx / sign; + quat[2] = qy / sign; + quat[3] = qz / sign; + + // return normalized quaternion + return normalizeQuaternion(quat); +}; + +inline wearable::Vector3 wearable::utils::rotationMatrixToRPY(const wearable::Matrix3& rotMat) +{ + + Vector3 rpy; + + if (rotMat[2][0] < 1.0) { + if (rotMat[2][0] > -1.0) { + rpy[0] = std::atan2(rotMat[2][1], rotMat[2][2]); + rpy[1] = std::asin(-rotMat[2][0]); + rpy[2] = std::atan2(rotMat[1][0], rotMat[0][0]); + } + else { + // Not a unique solution + rpy[0] = 0.0; + rpy[1] = M_PI / 2.0; + rpy[2] = -std::atan2(-rotMat[1][2], rotMat[1][1]); + } + } + else { + // Not a unique solution + rpy[0] = 0.0; + rpy[1] = -M_PI / 2.0; + rpy[2] = std::atan2(-rotMat[1][2], rotMat[1][1]); + } + return rpy; +}; + +inline wearable::Quaternion wearable::utils::RPYToQuaternion(const Vector3& rpy) +{ + double cr = std::cos(rpy[0] * 0.5); + double sr = std::sin(rpy[0] * 0.5); + double cp = std::cos(rpy[1] * 0.5); + double sp = std::sin(rpy[1] * 0.5); + double cy = std::cos(rpy[2] * 0.5); + double sy = std::sin(rpy[2] * 0.5); + + Quaternion quat; + + quat[0] = cr * cp * cy + sr * sp * sy; + quat[1] = sr * cp * cy - cr * sp * sy; + quat[2] = cr * sp * cy + sr * cp * sy; + quat[3] = cr * cp * sy - sr * sp * cy; + + return quat; +}; + +inline wearable::Matrix3 wearable::utils::RPYToRotationMatrix(const Vector3& rpy) +{ + double cr = std::cos(rpy[0]); + double sr = std::sin(rpy[0]); + double cp = std::cos(rpy[1]); + double sp = std::sin(rpy[1]); + double cy = std::cos(rpy[2]); + double sy = std::sin(rpy[2]); + + Matrix3 rotMat; + + // first row + rotMat[0][0] = cp * cy; + rotMat[0][1] = sr * sp * cy - cr * sy; + rotMat[0][2] = sr * sy + cr * sp * cy; + + // second row + rotMat[1][0] = cp * sy; + rotMat[1][1] = cr * cy + sr * sp * sy; + rotMat[1][2] = cr * sp * sy - sr * cy; + + // third row + rotMat[2][0] = -sp; + rotMat[2][1] = sr * cp; + rotMat[2][2] = cr * cp; + + return rotMat; +}; + +#endif // WEARABLE_UTILS_H diff --git a/interfaces/IWearableTargets/CMakeLists.txt b/interfaces/IWearableTargets/CMakeLists.txt index 2472f3162..84a5102ba 100644 --- a/interfaces/IWearableTargets/CMakeLists.txt +++ b/interfaces/IWearableTargets/CMakeLists.txt @@ -4,7 +4,6 @@ add_library(IWearableTargets INTERFACE) find_package(iDynTree REQUIRED) -find_package(IWear REQUIRED) target_sources(IWearableTargets INTERFACE $ diff --git a/interfaces/IXsensMVNControl/CMakeLists.txt b/interfaces/IXsensMVNControl/CMakeLists.txt new file mode 100644 index 000000000..6995598e7 --- /dev/null +++ b/interfaces/IXsensMVNControl/CMakeLists.txt @@ -0,0 +1,12 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +# Group files with the correct path for the target_sources command +file(GLOB_RECURSE IXSENS_MVN_CONTROL_HEADERS *.h) + +add_library(IXsensMVNControl INTERFACE) +target_sources(IXsensMVNControl INTERFACE ${IXSENS_MVN_CONTROL_HEADERS}) +target_include_directories(IXsensMVNControl INTERFACE + $ + ) diff --git a/interfaces/IXsensMVNControl/IXsensMVNControl.h b/interfaces/IXsensMVNControl/IXsensMVNControl.h new file mode 100644 index 000000000..27eb86b64 --- /dev/null +++ b/interfaces/IXsensMVNControl/IXsensMVNControl.h @@ -0,0 +1,42 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IXSENS_MVN_CONTROL_H +#define IXSENS_MVN_CONTROL_H + +#include +#include + +namespace xsensmvn { + class IXsensMVNControl; +} + +class xsensmvn::IXsensMVNControl +{ +public: + /* -------------------------- * + * Construtors / Destructors * + * -------------------------- */ + + IXsensMVNControl() = default; + virtual ~IXsensMVNControl() = default; + + /* ---------- * + * Functions * + * ---------- */ + + // Body dimensions set/get + virtual bool setBodyDimensions(const std::map& dimensions) = 0; + virtual bool getBodyDimensions(std::map& dimensions) const = 0; + virtual bool getBodyDimension(const std::string bodyName, double& dimension) const = 0; + + // Calibration methods + virtual bool calibrate(const std::string& calibrationType = {}) = 0; + virtual bool abortCalibration() = 0; + + // Acquisition methods + virtual bool startAcquisition() = 0; + virtual bool stopAcquisition() = 0; +}; + +#endif // IXSENS_MVN_CONTROL_H diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt index 179402d2a..9c6e341b3 100644 --- a/modules/CMakeLists.txt +++ b/modules/CMakeLists.txt @@ -1,6 +1,10 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause +if(ENABLE_FrameVisualizer) + add_subdirectory(IWearFrameVisualizer) +endif() + if(HUMANSTATEPROVIDER_ENABLE_VISUALIZER) add_subdirectory(HumanStateVisualizer) endif() diff --git a/modules/IWearFrameVisualizer/CMakeLists.txt b/modules/IWearFrameVisualizer/CMakeLists.txt new file mode 100644 index 000000000..ce8b7b7b7 --- /dev/null +++ b/modules/IWearFrameVisualizer/CMakeLists.txt @@ -0,0 +1,44 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +# List the subdirectory +# http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() + + +set(EXE_TARGET_NAME IWearFrameVisualizerModule) + +find_package(YARP REQUIRED) +find_package(iDynTree REQUIRED) + +add_executable(${EXE_TARGET_NAME} src/main.cpp) + +target_link_libraries(${EXE_TARGET_NAME} PUBLIC + IWear + YARP::YARP_os + YARP::YARP_sig + YARP::YARP_dev + YARP::YARP_math + iDynTree::idyntree-visualization + ) + +install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) + +file(GLOB TARGET_INI_FILES conf/*.ini) + +install(FILES ${TARGET_INI_FILES} + DESTINATION ${CMAKE_INSTALL_DATADIR}/${WEARABLES_PROJECT_NAME}) + + + + diff --git a/modules/IWearFrameVisualizer/conf/HapticGloveFramesVisualizationConfig.ini b/modules/IWearFrameVisualizer/conf/HapticGloveFramesVisualizationConfig.ini new file mode 100644 index 000000000..7f88f7570 --- /dev/null +++ b/modules/IWearFrameVisualizer/conf/HapticGloveFramesVisualizationConfig.ini @@ -0,0 +1,41 @@ + +wearable_data_ports ( "/WearableData/HapticGlove/LeftHand/data:o", "/WearableData/HapticGlove/RightHand/data:o") + +# the list of fixed frames to add and to visualize +new_fixed_frames ("InertialFrame", "LeftHandRootFrame", "RightHandRootFrame") + +# the list of frames from the wearable data links +link_names_wearables ( "HapticGlove::vLink::l_hand", + "HapticGlove::vLink::l_thumb_finger::fingertip" , + "HapticGlove::vLink::l_index_finger::fingertip" , + "HapticGlove::vLink::l_middle_finger::fingertip", + "HapticGlove::vLink::l_ring_finger::fingertip", + "HapticGlove::vLink::l_little_finger::fingertip", + "HapticGlove::vLink::r_hand", + "HapticGlove::vLink::r_thumb_finger::fingertip" , + "HapticGlove::vLink::r_index_finger::fingertip" , + "HapticGlove::vLink::r_middle_finger::fingertip", + "HapticGlove::vLink::r_ring_finger::fingertip", + "HapticGlove::vLink::r_little_finger::fingertip") + +# position and quaternion should be given in case of new fixed frames. for wearable data they are streamed, so the fields should be empty +# ( child, parent, frame Size , position, quaternion (rotation)) +# The root frame (inertial frame has no parent, and has zero and identity position and rotation) +# To find the fixed frame rotation quaternion, check this website: https://www.andre-gaschler.com/rotationconverter/ +# position (x, y, z) Quaternion (w, x, y, z) +frames_map ( ( "InertialFrame", "", 1.0 ,(), () ), + ( "LeftHandRootFrame", "InertialFrame", 0.5 , (0, 0.5 , 0 ), ( 0.7071068, 0, 0, -0.7071068 ) ), + ( "HapticGlove::vLink::l_hand", "LeftHandRootFrame", 0.5 , (), () ), + ( "HapticGlove::vLink::l_thumb_finger::fingertip" , "HapticGlove::vLink::l_hand", 0.2 , (), () ), + ( "HapticGlove::vLink::l_index_finger::fingertip" , "HapticGlove::vLink::l_hand", 0.2 , (), () ), + ( "HapticGlove::vLink::l_middle_finger::fingertip", "HapticGlove::vLink::l_hand", 0.2 , (), () ), + ( "HapticGlove::vLink::l_ring_finger::fingertip", "HapticGlove::vLink::l_hand", 0.2 , (), () ), + ( "HapticGlove::vLink::l_little_finger::fingertip", "HapticGlove::vLink::l_hand", 0.2 , (), () ), + ( "RightHandRootFrame", "InertialFrame", 0.5 , (0, -0.5 , 0 ), ( 0.7071068, 0, 0, 0.7071068 ) ), + ( "HapticGlove::vLink::r_hand", "RightHandRootFrame", 0.5 , (), () ), + ( "HapticGlove::vLink::r_thumb_finger::fingertip" , "HapticGlove::vLink::r_hand", 0.2 , (), () ), + ( "HapticGlove::vLink::r_index_finger::fingertip" , "HapticGlove::vLink::r_hand", 0.2 , (), () ), + ( "HapticGlove::vLink::r_middle_finger::fingertip", "HapticGlove::vLink::r_hand", 0.2 , (), () ), + ( "HapticGlove::vLink::r_ring_finger::fingertip", "HapticGlove::vLink::r_hand", 0.2 , (), () ), + ( "HapticGlove::vLink::r_little_finger::fingertip", "HapticGlove::vLink::r_hand", 0.2 , (), () ) ) + diff --git a/modules/IWearFrameVisualizer/src/main.cpp b/modules/IWearFrameVisualizer/src/main.cpp new file mode 100644 index 000000000..cab3e8de4 --- /dev/null +++ b/modules/IWearFrameVisualizer/src/main.cpp @@ -0,0 +1,345 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +using namespace yarp::os; + +std::atomic isClosing{false}; + +void my_handler(int) +{ + isClosing = true; +} + +#ifdef WIN32 + +#include + +BOOL WINAPI CtrlHandler(DWORD fdwCtrlType) +{ + switch (fdwCtrlType) { + // Handle the CTRL-C signal. + case CTRL_C_EVENT: + case CTRL_CLOSE_EVENT: + case CTRL_SHUTDOWN_EVENT: + my_handler(0); + return TRUE; + + // Handle all other events + default: + return FALSE; + } +} +#endif + +void handleSigInt() +{ +#ifdef WIN32 + SetConsoleCtrlHandler(CtrlHandler, TRUE); +#else + struct sigaction action; + memset(&action, 0, sizeof(action)); + action.sa_handler = &my_handler; + sigaction(SIGINT, &action, NULL); + sigaction(SIGTERM, &action, NULL); + sigaction(SIGABRT, &action, NULL); +#endif +} + +struct FrameViewer +{ + std::shared_ptr parent{nullptr}; + std::string name; + size_t vizIndex; + iDynTree::Transform transform; +}; + +int main(int argc, char* argv[]) +{ + std::string portNameIn = "WearableData:I"; + std::string portName = "WearableData"; + std::string namePrefix = "/WearableViewer"; + std::string logPrefix = "WearableDataVisualizer"; + + // prepare and configure the resource finder + yarp::os::ResourceFinder& config = yarp::os::ResourceFinder::getResourceFinderSingleton(); + + config.setDefaultConfigFile("HapticGloveFramesVisualizationConfig.ini"); + + config.configure(argc, argv); + + // ========================= + // Start the visualizer + // ========================= + iDynTree::Visualizer visualizer; + + bool ok = visualizer.init(); + + visualizer.camera().animator()->enableMouseControl(); + + if (!ok) { + std::cerr << "Failed to initialize the visualizer." << std::endl; + return EXIT_FAILURE; + } + // ========================= + // Open the Drivers + // ========================= + yarp::dev::PolyDriver tfDriver, iWearDriver; + wearable::IWear* iWear; + + yarp::os::Property wearOptions, tfClientCfg; + + // Iwear Remapper + yarp::os::Value* wearableDataPort; + if (!config.check("wearable_data_ports", wearableDataPort)) { + yError() << logPrefix << "Unable to find wearable_data_ports into config file."; + return EXIT_FAILURE; + } + + wearOptions.put("device", "iwear_remapper"); + wearOptions.put("wearableDataPorts", wearableDataPort); + + if (!iWearDriver.open(wearOptions)) { + yError() << "Unable to open polydriver with the following options:" + << wearOptions.toString(); + return EXIT_FAILURE; + } + + if (!iWearDriver.view(iWear) || iWear == nullptr) { + yError() << "Unable to view Wearable Remapper interface."; + return EXIT_FAILURE; + } + + // ========================= + // Check the wearable driver + // ========================= + while (iWear->getStatus() == wearable::WearStatus::WaitingForFirstRead) { + yInfo() << logPrefix << "IWear interface waiting for first data. Waiting..."; + yarp::os::Time::delay(0.1); + } + + if (iWear->getStatus() != wearable::WearStatus::Ok) { + yError() << logPrefix << "The status of the attached IWear interface is not ok (" + << static_cast(iWear->getStatus()) << ")"; + return EXIT_FAILURE; + } + + // ========================= + // create the map of link names and wearble sensors + // ========================= + std::vector linkNameList; + yarp::os::Bottle* linkListYarp; + if (!(config.check("link_names_wearables") && config.find("link_names_wearables").isList())) { + yError() << logPrefix << "Unable to find link_names_wearables in the config file."; + return EXIT_FAILURE; + } + linkListYarp = config.find("link_names_wearables").asList(); + + for (size_t i = 0; i < linkListYarp->size(); i++) { + linkNameList.push_back(linkListYarp->get(i).asString()); + } + + std::map> + linkSensorsMap; + + for (auto linkName : linkNameList) { + auto sensor = iWear->getVirtualLinkKinSensor(linkName); + if (!sensor) { + yError() << logPrefix << "Failed to find sensor associated to link" << linkName + << "from the IWear interface"; + return EXIT_FAILURE; + } + linkSensorsMap[linkName] = sensor; + yInfo() << logPrefix + << "sensor name to add the sensors vector: " << sensor->getSensorName(); + } + yInfo() << logPrefix << "linkSensorsMap.size(): " << linkSensorsMap.size(); + + // ========================= + // Get the fixed frames + // ========================= + std::vector newFramesList; + yarp::os::Bottle* newFramesListYarp; + if (!(config.check("new_fixed_frames") && config.find("new_fixed_frames").isList())) { + yError() << logPrefix << "Unable to find new_fixed_frames in the config file."; + return EXIT_FAILURE; + } + newFramesListYarp = config.find("new_fixed_frames").asList(); + + for (size_t i = 0; i < linkListYarp->size(); i++) { + newFramesList.push_back(newFramesListYarp->get(i).asString()); + } + + // ========================= + // Get the frames map + // ========================= + std::vector FramesMapList; + yarp::os::Bottle* FramesMapYarp; + if (!(config.check("frames_map") && config.find("frames_map").isList())) { + yError() << logPrefix << "Unable to find frames_map in the config file."; + return EXIT_FAILURE; + } + FramesMapYarp = config.find("frames_map").asList(); + + // ========================= + // Make Frame Viewers + // ========================= + std::vector> frames; + + // create all the frame viwers + for (size_t i = 0; i < FramesMapYarp->size(); i++) { + std::shared_ptr frame = std::make_shared(); + yarp::os::Bottle* frameMapYarp = FramesMapYarp->get(i).asList(); + if (frameMapYarp->size() != 5) { + yError() << logPrefix << "the map does not have expected size. frameMap: (" + << frameMapYarp->toString() << ") , expected size: " << 5 + << " , actual size: " << frameMapYarp->size(); + return EXIT_FAILURE; + } + frame->name = frameMapYarp->get(0).asString(); + frame->vizIndex = visualizer.frames().addFrame(iDynTree::Transform::Identity(), + frameMapYarp->get(2).asFloat64()); + // visualizer.frames() + // .getFrameLabel(frame->vizIndex) + // ->setText(frame->name); // to be merged + frames.push_back(frame); + } + + // check for the parent frame + for (size_t i = 0; i < FramesMapYarp->size(); i++) { + yarp::os::Bottle* frameMapYarp = FramesMapYarp->get(i).asList(); + if (frameMapYarp->size() != 5) { + yError() << logPrefix << "the map does not have expected size. frameMap: (" + << frameMapYarp->toString() << ") , expected size: " << 5 + << " , actual size: " << frameMapYarp->size(); + return EXIT_FAILURE; + } + + std::string parentName = frameMapYarp->get(1).asString(); + std::string Name = frameMapYarp->get(0).asString(); + if (frames[i]->name != Name) { + yError() << logPrefix + << "the frame viewer name is not equal to the one from the " + "frame map list. Frame Viewer Name: " + << frames[i]->name << " , yarp map name: " << Name; + return EXIT_FAILURE; + } + + for (auto& frame : frames) { + if (frame->name == parentName) { + frames[i]->parent = frame; + break; + } + } + + // check for the fixed transformations + + iDynTree::Transform transformation; + transformation.setPosition(iDynTree::Position::Zero()); + transformation.setRotation(iDynTree::Rotation::Identity()); // x is front (north), z is up. + + yarp::os::Bottle* positionList = frameMapYarp->get(3).asList(); + yarp::os::Bottle* quatList = frameMapYarp->get(4).asList(); + if (positionList->size() == 3) { + iDynTree::Position position(positionList->get(0).asFloat64(), + positionList->get(1).asFloat64(), + positionList->get(2).asFloat64()); + transformation.setPosition(position); + } + if (quatList->size() == 4) { + iDynTree::Vector4 quat; + quat[0] = quatList->get(0).asFloat64(); + quat[1] = quatList->get(1).asFloat64(); + quat[2] = quatList->get(2).asFloat64(); + quat[3] = quatList->get(3).asFloat64(); + + // normalize the quaternion + double norm = 0; + for (size_t j = 0; j < 4; j++) + norm += quat[j] * quat[j]; + + norm = std::sqrt(norm); + + for (size_t j = 0; j < 4; j++) + quat[j] = quat[j] / norm; + + iDynTree::Rotation rotation; + rotation.fromQuaternion(quat); //(real: w, imaginary: x y z) + transformation.setRotation(rotation); + } + frames[i]->transform = transformation; + } + + // print frames info + yInfo() << "Frames information:"; + for (auto& frame : frames) { + yInfo() << "name: " << frame->name; + if (frame->parent) + yInfo() << "parent: " << frame->parent->name; + + yInfo() << "initial transformation from the parent: \n" << frame->transform.toString(); + } + yInfo() << "================="; + yInfo() << "=== Running ==="; + yInfo() << "================="; + + // ========================= + // Visualization loop + // ========================= + + iDynTree::Transform frameTransform; + iDynTree::Rotation frameRotation; + iDynTree::Vector4 quat; + + while (visualizer.run() && !isClosing) { + + for (auto& frame : frames) { + if (frame->parent) { // if not it is inertial frame + + // link + auto sensor = linkSensorsMap[frame->name]; + if (sensor) { + wearable::Quaternion orientation; + wearable::Vector3 position; + if (sensor->getSensorStatus() != wearable::WearStatus::Ok) { + yWarning() + << "senor status is not OK, sensor name:" << sensor->getSensorName(); + break; + } + sensor->getLinkPose(position, orientation); + iDynTree::Position framePosition(position[0], position[1], position[2]); + quat[0] = orientation[0]; + quat[1] = orientation[1]; + quat[2] = orientation[2]; + quat[3] = orientation[3]; + frameRotation.fromQuaternion(quat); //(real: w, imaginary: x y z) + frameTransform.setPosition(framePosition); + frameTransform.setRotation(frameRotation); + frame->transform = frame->parent->transform * frameTransform; + } + visualizer.frames().updateFrame(frame->vizIndex, frame->transform); + } + } + visualizer.draw(); + } + + return EXIT_SUCCESS; +} diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index d122b0aee..55d181576 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -1,6 +1,143 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause +# TODO: these libraries are enforced to be static. In order to let the devices +# find them on Windows from the build directory, their Release / Debug +# folder should be added to the PATH. However, this does not work as expected. + +# ============ +# WearableData +# ============ + +yarp_add_idl(WEARABLEDATA_FILES thrift/WearableData.thrift) + +add_library(WearableData ${WEARABLEDATA_FILES} thrift/WearableData.thrift) +add_library(Wearable::WearableData ALIAS WearableData) +target_link_libraries(WearableData YARP::YARP_os) + +# Extract the include directory from the files names +foreach(file ${WEARABLEDATA_FILES}) + STRING(REGEX MATCH ".+\\.h?h$" file ${file}) + if(file) + get_filename_component(include_dir ${file} DIRECTORY) + list(APPEND WEARABLEDATA_INCLUDE_DIRS ${include_dir}) + list(REMOVE_DUPLICATES WEARABLEDATA_INCLUDE_DIRS) + endif() +endforeach() + +foreach(dir ${WEARABLEDATA_INCLUDE_DIRS}) + get_filename_component(parent_dir_name ${dir} NAME) + if(${parent_dir_name} STREQUAL thrift) + list(REMOVE_ITEM WEARABLEDATA_INCLUDE_DIRS ${dir}) + get_filename_component(parent_dir_path ${dir} DIRECTORY) + list(APPEND WEARABLEDATA_INCLUDE_DIRS ${parent_dir_path}) + endif() +endforeach() + +# Setup the include directories +target_include_directories(WearableData PUBLIC + $) + +install(TARGETS WearableData + EXPORT WearableData + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) + +install_basic_package_files(WearableData + VERSION ${PROJECT_VERSION} + COMPATIBILITY AnyNewerVersion + EXPORT WearableData + NO_CHECK_REQUIRED_COMPONENTS_MACRO) + +install(FILES ${WEARABLEDATA_FILES} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/thrift) + +# ================= +# WearableActuators +# ================= + +yarp_add_idl(WEARABLEACTUATORS_FILES thrift/WearableActuators.thrift) + +add_library(WearableActuators ${WEARABLEACTUATORS_FILES} thrift/WearableActuators.thrift) +add_library(Wearable::WearableActuators ALIAS WearableActuators) +target_link_libraries(WearableActuators YARP::YARP_os) + +# Extract the include directory from the files names +foreach(file ${WEARABLEACTUATORS_FILES}) + STRING(REGEX MATCH ".+\\.h?h$" file ${file}) + if(file) + get_filename_component(include_dir ${file} DIRECTORY) + list(APPEND WEARABLEACTUATORS_INCLUDE_DIRS ${include_dir}) + list(REMOVE_DUPLICATES WEARABLEACTUATORS_INCLUDE_DIRS) + endif() +endforeach() + +foreach(dir ${WEARABLEACTUATORS_INCLUDE_DIRS}) + get_filename_component(parent_dir_name ${dir} NAME) + if(${parent_dir_name} STREQUAL thrift) + list(REMOVE_ITEM WEARABLEACTUATORS_INCLUDE_DIRS ${dir}) + get_filename_component(parent_dir_path ${dir} DIRECTORY) + list(APPEND WEARABLEACTUATORS_INCLUDE_DIRS ${parent_dir_path}) + endif() +endforeach() + +# Setup the include directories +target_include_directories(WearableActuators PUBLIC + $ + $) + +install(TARGETS WearableActuators + EXPORT WearableActuators + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) + +install_basic_package_files(WearableActuators + VERSION ${PROJECT_VERSION} + COMPATIBILITY AnyNewerVersion + EXPORT WearableActuators + NO_CHECK_REQUIRED_COMPONENTS_MACRO) + +install(FILES ${WEARABLEACTUATORS_FILES} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/thrift) + +# ======================= +# XsensSuitControlService +# ======================= + +yarp_add_idl(XSENSSUITCONTROL thrift/XsensSuitControlService.thrift) + +add_library(XsensSuitControl ${XSENSSUITCONTROL} thrift/XsensSuitControlService.thrift) +target_link_libraries(XsensSuitControl YARP::YARP_os) + +# Extract the include directory from the files names +foreach(file ${XSENSSUITCONTROL}) + STRING(REGEX MATCH ".+\\.h?h$" file ${file}) + if(file) + get_filename_component(include_dir ${file} DIRECTORY) + list(APPEND XSENSSUITCONTROL_INCLUDE_DIRS ${include_dir}) + list(REMOVE_DUPLICATES XSENSSUITCONTROL_INCLUDE_DIRS) + endif() +endforeach() + +foreach(dir ${XSENSSUITCONTROL_INCLUDE_DIRS}) + get_filename_component(parent_dir_name ${dir} NAME) + if(${parent_dir_name} STREQUAL thrift) + list(REMOVE_ITEM XSENSSUITCONTROL_INCLUDE_DIRS ${dir}) + get_filename_component(parent_dir_path ${dir} DIRECTORY) + list(APPEND XSENSSUITCONTROL_INCLUDE_DIRS ${parent_dir_path}) + endif() +endforeach() + +# Setup the include directories +target_include_directories(XsensSuitControl PUBLIC + $) + +install(TARGETS XsensSuitControl + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) + add_subdirectory(yarp) add_subdirectory(ros) - diff --git a/msgs/thrift/WearableActuators.thrift b/msgs/thrift/WearableActuators.thrift new file mode 100644 index 000000000..701abf2cc --- /dev/null +++ b/msgs/thrift/WearableActuators.thrift @@ -0,0 +1,39 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +namespace yarp wearable.msg + +// ================== +// Actuators metadata +// ================== + +enum ActuatorType { + HAPTIC, + MOTOR, + HEATER, +} + +struct ActuatorInfo { + 1: string name; + 2: ActuatorType type; +} + +// ========================== +// Actuator Command data type +// ========================== + +struct WearableActuatorCommand { + 1: ActuatorInfo info; + 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/msgs/thrift/WearableData.thrift b/msgs/thrift/WearableData.thrift new file mode 100644 index 000000000..00a9fa174 --- /dev/null +++ b/msgs/thrift/WearableData.thrift @@ -0,0 +1,194 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +namespace yarp wearable.msg + +// ================ +// Sensors metadata +// ================ + +enum SensorStatus { + ERROR, + OK, + CALIBRATING, + DATA_OVERFLOW, + TIMEOUT, + UNKNOWN, + WAITING_FOR_FIRST_READ, +} + +struct SensorInfo{ + 1: string name; + 2: SensorStatus status = SensorStatus.UNKNOWN; +} + +// ===================== +// Elementary data types +// ===================== + +struct VectorXYZ { + 1: double x; + 2: double y; + 3: double z; +} + +struct VectorRPY { + 1: double r; + 2: double p; + 3: double y; +} + +struct QuaternionWXYZ { + 1: double w; + 2: double x; + 3: double y; + 4: double z; +} + +// ========================== +// Composed sensor data types +// ========================== + +struct ForceTorque6DSensorData { + 1: VectorXYZ force; + 2: VectorXYZ torque; +} + +struct PoseSensorData { + 1: QuaternionWXYZ orientation; + 2: VectorXYZ position; +} + +struct VirtualLinkKinSensorData { + 1: QuaternionWXYZ orientation; + 2: VectorXYZ position; + 3: VectorXYZ linearVelocity; + 4: VectorXYZ angularVelocity; + 5: VectorXYZ linearAcceleration; + 6: VectorXYZ angularAcceleration; +} + +struct VirtualJointKinSensorData { + 1: double position; + 2: double velocity; + 3: double acceleration; +} + +struct VirtualSphericalJointKinSensorData { + 1: VectorRPY angle; + 2: VectorXYZ velocity; + 3: VectorXYZ acceleration; +} + +struct EmgData { + 1: double value; + 2: double normalization; +} + +// ================= +// Sensor structures +// ================= + +struct Accelerometer { + 1: SensorInfo info; + 2: VectorXYZ data; +} + +struct EmgSensor { + 1: SensorInfo info; + 2: EmgData data; +} + +struct FreeBodyAccelerationSensor { + 1: SensorInfo info; + 2: VectorXYZ data; +} + +struct Force3DSensor { + 1: SensorInfo info; + 2: VectorXYZ data; +} + +struct ForceTorque6DSensor { + 1: SensorInfo info; + 2: ForceTorque6DSensorData data; +} + +struct Gyroscope { + 1: SensorInfo info; + 2: VectorXYZ data; +} + +struct Magnetometer { + 1: SensorInfo info; + 2: VectorXYZ data; +} + +struct OrientationSensor { + 1: SensorInfo info; + 2: QuaternionWXYZ data; +} + +struct PoseSensor { + 1: SensorInfo info; + 2: PoseSensorData data; +} + +struct PositionSensor { + 1: SensorInfo info; + 2: VectorXYZ data; +} + +struct SkinSensor { + 1: SensorInfo info; + 2: list data; +} + +struct TemperatureSensor { + 1: SensorInfo info; + 2: double data; +} + +struct Torque3DSensor { + 1: SensorInfo info; + 2: VectorXYZ data; +} + +struct VirtualLinkKinSensor { + 1: SensorInfo info; + 2: VirtualLinkKinSensorData data; +} + +struct VirtualJointKinSensor { + 1: SensorInfo info; + 2: VirtualJointKinSensorData data; +} + +struct VirtualSphericalJointKinSensor { + 1: SensorInfo info; + 2: VirtualSphericalJointKinSensorData data; +} + +// ======================== +// Complete WearData struct +// ======================== + +struct WearableData { +1: required string producerName; +2: optional map accelerometers; +3: optional map emgSensors; +4: optional map force3DSensors; +5: optional map forceTorque6DSensors; +6: optional map freeBodyAccelerationSensors; +7: optional map gyroscopes; +8: optional map magnetometers; +9: optional map orientationSensors; +10: optional map poseSensors; +11: optional map positionSensors; +12: optional map skinSensors; +13: optional map temperatureSensors; +14: optional map torque3DSensors; +15: optional map virtualLinkKinSensors; +16: optional map virtualJointKinSensors; +17: optional map virtualSphericalJointKinSensors; +} diff --git a/msgs/thrift/XsensSuitControlService.thrift b/msgs/thrift/XsensSuitControlService.thrift new file mode 100644 index 000000000..6ee86e6d3 --- /dev/null +++ b/msgs/thrift/XsensSuitControlService.thrift @@ -0,0 +1,45 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +namespace yarp wearable.msg + +/*struct FrameReference { + 1: string frameReference; + 2: string frameName; +}*/ + +/** + * Methods definition for the XsensDriver Wrapper service + */ +service XsensSuitControlService { + + /** + * Calibrate the Xsens device with the default calibration procedure + * @return true if the calibration is successful, false otherwise + */ + bool calibrate(); + + /** + * Calibrate the Xsens device with the calibration procedure identified + * by the specified parameter + * + * @param calibrationType name of the calibration to execute + * @return true if the calibration is successful, false otherwise + */ + bool calibrateWithType(1: string calibrationType); + + /** + * Abort the calibration procedure + */ + bool abortCalibration(); + + /** + * Start acquiring data from the Xsens suit + */ + bool startAcquisition(); + + /** + * Stop acquiring data from the suit + */ + bool stopAcquisition(); +} diff --git a/publishers/XsensTFPublisher/CMakeLists.txt b/publishers/XsensTFPublisher/CMakeLists.txt index 32cdaa863..928037890 100644 --- a/publishers/XsensTFPublisher/CMakeLists.txt +++ b/publishers/XsensTFPublisher/CMakeLists.txt @@ -2,7 +2,6 @@ # SPDX-License-Identifier: BSD-3-Clause find_package(YARP COMPONENTS rosmsg REQUIRED) -find_package(IWear REQUIRED) find_package(iDynTree REQUIRED) yarp_prepare_plugin(xsens_tf_publisher diff --git a/wrappers/CMakeLists.txt b/wrappers/CMakeLists.txt new file mode 100644 index 000000000..5351cebb9 --- /dev/null +++ b/wrappers/CMakeLists.txt @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +add_subdirectory(IWear) +add_subdirectory(IWearActuators) +add_subdirectory(IXsensMVNControl) + +if(ENABLE_Logger) + add_subdirectory(IWearLogger) +endif() + + diff --git a/wrappers/IWear/CMakeLists.txt b/wrappers/IWear/CMakeLists.txt new file mode 100644 index 000000000..aabce610c --- /dev/null +++ b/wrappers/IWear/CMakeLists.txt @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(iwear_wrapper + TYPE wearable::wrappers::IWearWrapper + INCLUDE include/IWearWrapper.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(IWearWrapper + src/IWearWrapper.cpp + include/IWearWrapper.h) + +target_include_directories(IWearWrapper PRIVATE + $) + +target_link_libraries(IWearWrapper PUBLIC + IWear WearableData YARP::YARP_dev) + +yarp_install( + TARGETS IWearWrapper + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) diff --git a/wrappers/IWear/include/IWearWrapper.h b/wrappers/IWear/include/IWearWrapper.h new file mode 100644 index 000000000..1f56f5247 --- /dev/null +++ b/wrappers/IWear/include/IWearWrapper.h @@ -0,0 +1,50 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IWEARWRAPPER_H +#define IWEARWRAPPER_H + +#include +#include +#include +#include +#include + +namespace wearable { + namespace wrappers { + class IWearWrapper; + } +} // namespace wearable + +class wearable::wrappers::IWearWrapper + : public yarp::dev::DeviceDriver + , public yarp::dev::IWrapper + , public yarp::dev::IMultipleWrapper + , public yarp::os::PeriodicThread +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + IWearWrapper(); + ~IWearWrapper() override; + + // PeriodicThread + void run() override; + void threadRelease() override; + + // DeviceDriver interface + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // IWrapper interface + bool attach(yarp::dev::PolyDriver* poly) override; + bool detach() override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() override; +}; + +#endif // IWEARWRAPPER_H diff --git a/wrappers/IWear/src/IWearWrapper.cpp b/wrappers/IWear/src/IWearWrapper.cpp new file mode 100644 index 000000000..e211d249d --- /dev/null +++ b/wrappers/IWear/src/IWearWrapper.cpp @@ -0,0 +1,544 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "IWearWrapper.h" +#include "Wearable/IWear/IWear.h" +#include "thrift/WearableData.h" + +#include +#include +#include + +const std::string WrapperName = "IWearWrapper"; +const std::string logPrefix = WrapperName + " :"; +constexpr double DefaultPeriod = 0.01; + +using namespace wearable; +using namespace wearable::wrappers; + +class IWearWrapper::impl +{ +public: + yarp::os::BufferedPort dataPort; + + std::string dataPortName; + + bool firstRun = true; + size_t waitingFirstReadCounter = 1; + + wearable::IWear* iWear = nullptr; + yarp::dev::IPreciselyTimed* iPreciselyTimed = nullptr; + + wearable::VectorOfSensorPtr accelerometers; + wearable::VectorOfSensorPtr emgSensors; + wearable::VectorOfSensorPtr force3DSensors; + wearable::VectorOfSensorPtr forceTorque6DSensors; + wearable::VectorOfSensorPtr + freeBodyAccelerationSensors; + wearable::VectorOfSensorPtr gyroscopes; + wearable::VectorOfSensorPtr magnetometers; + wearable::VectorOfSensorPtr orientationSensors; + wearable::VectorOfSensorPtr poseSensors; + wearable::VectorOfSensorPtr positionSensors; + wearable::VectorOfSensorPtr skinSensors; + wearable::VectorOfSensorPtr temperatureSensors; + wearable::VectorOfSensorPtr torque3DSensors; + wearable::VectorOfSensorPtr + virtualLinkKinSensors; + wearable::VectorOfSensorPtr + virtualJointKinSensors; + wearable::VectorOfSensorPtr + virtualSphericalJointKinSensors; + + // std::vector help(const std::string& functionName = "--all") override; // TODO +}; + +IWearWrapper::IWearWrapper() + : yarp::os::PeriodicThread(DefaultPeriod) + , pImpl{new impl()} +{} + +IWearWrapper::~IWearWrapper() +{ + detachAll(); + close(); +} + +// ======= +// Helpers +// ======= + +const std::map mapSensorStatus = { + {sensor::SensorStatus::Ok, msg::SensorStatus::OK}, + {sensor::SensorStatus::Error, msg::SensorStatus::ERROR}, + {sensor::SensorStatus::Overflow, msg::SensorStatus::DATA_OVERFLOW}, + {sensor::SensorStatus::Calibrating, msg::SensorStatus::CALIBRATING}, + {sensor::SensorStatus::Timeout, msg::SensorStatus::TIMEOUT}, + {sensor::SensorStatus::WaitingForFirstRead, msg::SensorStatus::WAITING_FOR_FIRST_READ}, + {sensor::SensorStatus::Unknown, msg::SensorStatus::UNKNOWN}, +}; + +msg::SensorInfo generateSensorStatus(const wearable::sensor::ISensor* sensor) +{ + return {sensor->getSensorName(), mapSensorStatus.at(sensor->getSensorStatus())}; +} + +msg::VectorXYZ vector3ToVectorXYZ(const wearable::Vector3& input) +{ + return {input[0], input[1], input[2]}; +} + +msg::VectorRPY vector3ToVectorRPY(const wearable::Vector3& input) +{ + return {input[0], input[1], input[2]}; +} + +msg::QuaternionWXYZ convertQuaternion(const wearable::Quaternion& input) +{ + return {input[0], input[1], input[2], input[3]}; +} + +// ======================== +// PeriodicThread interface +// ======================== + +void IWearWrapper::run() +{ + if (!pImpl->iWear) { + yError() << logPrefix << "The IWear pointer is null in the driver loop."; + askToStop(); + return; + } + + if (!pImpl->iPreciselyTimed) { + yError() << logPrefix << "The IPreciselyTimed pointer is null in the driver loop."; + askToStop(); + return; + } + + while (pImpl->iWear->getStatus() == WearStatus::Calibrating + || pImpl->iWear->getStatus() == WearStatus::WaitingForFirstRead) { + if (pImpl->waitingFirstReadCounter++ % 1000 == 0) { + pImpl->waitingFirstReadCounter = 1; + yInfo() << logPrefix << "IWear interface waiting for first data. Waiting..."; + } + return; + } + + if (pImpl->iWear->getStatus() == WearStatus::Error + || pImpl->iWear->getStatus() == WearStatus::Unknown) { + yError() << logPrefix << "The status of the IWear interface is not Ok (" + << static_cast(pImpl->iWear->getStatus()) << ")"; + askToStop(); + return; + } + + // case status is TIMEOUT or DATA_OVERFLOW + if (pImpl->iWear->getStatus() != WearStatus::Ok) { + yWarning() << logPrefix << "The status of the IWear interface is not Ok (" + << static_cast(pImpl->iWear->getStatus()) << ")"; + } + + if (pImpl->firstRun) { + pImpl->firstRun = false; + + pImpl->accelerometers = pImpl->iWear->getAccelerometers(); + pImpl->emgSensors = pImpl->iWear->getEmgSensors(); + pImpl->force3DSensors = pImpl->iWear->getForce3DSensors(); + pImpl->forceTorque6DSensors = pImpl->iWear->getForceTorque6DSensors(); + pImpl->freeBodyAccelerationSensors = pImpl->iWear->getFreeBodyAccelerationSensors(); + pImpl->gyroscopes = pImpl->iWear->getGyroscopes(); + pImpl->magnetometers = pImpl->iWear->getMagnetometers(); + pImpl->orientationSensors = pImpl->iWear->getOrientationSensors(); + pImpl->poseSensors = pImpl->iWear->getPoseSensors(); + pImpl->positionSensors = pImpl->iWear->getPositionSensors(); + pImpl->skinSensors = pImpl->iWear->getSkinSensors(); + pImpl->temperatureSensors = pImpl->iWear->getTemperatureSensors(); + pImpl->torque3DSensors = pImpl->iWear->getTorque3DSensors(); + pImpl->virtualLinkKinSensors = pImpl->iWear->getVirtualLinkKinSensors(); + pImpl->virtualJointKinSensors = pImpl->iWear->getVirtualJointKinSensors(); + pImpl->virtualSphericalJointKinSensors = pImpl->iWear->getVirtualSphericalJointKinSensors(); + } + + msg::WearableData& data = pImpl->dataPort.prepare(); + data.producerName = pImpl->iWear->getWearableName(); + + yarp::os::Stamp timestamp = pImpl->iPreciselyTimed->getLastInputStamp(); + pImpl->dataPort.setEnvelope(timestamp); + + { + for (const auto& sensor : pImpl->accelerometers) { + wearable::Vector3 vector3; + if (!sensor->getLinearAcceleration(vector3)) { + yWarning() << logPrefix << "[Accelerometers] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.accelerometers[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + vector3ToVectorXYZ(vector3)}; + } + } + { + for (const auto& sensor : pImpl->emgSensors) { + double value, normalization; + // double normalizationValue; + if (!sensor->getEmgSignal(value) || !sensor->getNormalizationValue(normalization)) { + yWarning() << logPrefix << "[EmgSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + data.emgSensors[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + {value, normalization}}; + } + } + { + for (const auto& sensor : pImpl->force3DSensors) { + wearable::Vector3 vector3; + if (!sensor->getForce3D(vector3)) { + yWarning() << logPrefix << "[Force3DSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.force3DSensors[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + vector3ToVectorXYZ(vector3)}; + } + } + { + for (const auto& sensor : pImpl->forceTorque6DSensors) { + wearable::Vector6 vector6; + if (!sensor->getForceTorque6D(vector6)) { + yWarning() << logPrefix << "[ForceTorque6DSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.forceTorque6DSensors[sensor->getSensorName()] = { + generateSensorStatus(sensor.get()), + {{vector6[0], vector6[1], vector6[2]}, {vector6[3], vector6[4], vector6[5]}}}; + } + } + { + for (const auto& sensor : pImpl->freeBodyAccelerationSensors) { + wearable::Vector3 vector3; + if (!sensor->getFreeBodyAcceleration(vector3)) { + yWarning() << logPrefix << "[FreeBodyAccelerationSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.freeBodyAccelerationSensors[sensor->getSensorName()] = { + generateSensorStatus(sensor.get()), vector3ToVectorXYZ(vector3)}; + } + } + { + for (const auto& sensor : pImpl->gyroscopes) { + wearable::Vector3 vector3; + if (!sensor->getAngularRate(vector3)) { + yWarning() << logPrefix << "[Gyroscopes] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.gyroscopes[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + vector3ToVectorXYZ(vector3)}; + } + } + { + for (const auto& sensor : pImpl->magnetometers) { + wearable::Vector3 vector3; + if (!sensor->getMagneticField(vector3)) { + yWarning() << logPrefix << "[Magnetometers] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.magnetometers[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + vector3ToVectorXYZ(vector3)}; + } + } + { + for (const auto& sensor : pImpl->orientationSensors) { + wearable::Quaternion quaternion; + if (!sensor->getOrientationAsQuaternion(quaternion)) { + yWarning() << logPrefix << "[OrientationSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.orientationSensors[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + convertQuaternion(quaternion)}; + } + } + { + for (const auto& sensor : pImpl->poseSensors) { + wearable::Vector3 vector3; + wearable::Quaternion quaternion; + if (!sensor->getPose(quaternion, vector3)) { + yWarning() << logPrefix << "[PoseSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.poseSensors[sensor->getSensorName()] = { + generateSensorStatus(sensor.get()), + {convertQuaternion(quaternion), vector3ToVectorXYZ(vector3)}}; + } + } + { + for (const auto& sensor : pImpl->positionSensors) { + wearable::Vector3 vector3; + if (!sensor->getPosition(vector3)) { + yWarning() << logPrefix << "[PositionSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.positionSensors[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + vector3ToVectorXYZ(vector3)}; + } + } + { + for (const auto& sensor : pImpl->skinSensors) { + std::vector pressureVector; + if (!sensor->getPressure(pressureVector)) { + yWarning() << logPrefix << "[SkinSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.skinSensors[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + pressureVector}; + } + } + { + for (const auto& sensor : pImpl->temperatureSensors) { + double value; + if (!sensor->getTemperature(value)) { + yWarning() << logPrefix << "[TemperatureSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + } + + data.temperatureSensors[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + value}; + } + } + { + for (const auto& sensor : pImpl->torque3DSensors) { + wearable::Vector3 vector3; + if (!sensor->getTorque3D(vector3)) { + yWarning() << logPrefix << "[Torque3DSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.torque3DSensors[sensor->getSensorName()] = {generateSensorStatus(sensor.get()), + vector3ToVectorXYZ(vector3)}; + } + } + { + for (const auto& sensor : pImpl->virtualLinkKinSensors) { + wearable::Vector3 linearAcc; + wearable::Vector3 angularAcc; + wearable::Vector3 linearVel; + wearable::Vector3 angularVel; + wearable::Vector3 position; + wearable::Quaternion orientation; + if (!sensor->getLinkAcceleration(linearAcc, angularAcc) + || !sensor->getLinkPose(position, orientation) + || !sensor->getLinkVelocity(linearVel, angularVel)) { + yWarning() << logPrefix << "[VirtualLinkKinSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + + data.virtualLinkKinSensors[sensor->getSensorName()] = { + generateSensorStatus(sensor.get()), + {convertQuaternion(orientation), + vector3ToVectorXYZ(position), + vector3ToVectorXYZ(linearVel), + vector3ToVectorXYZ(angularVel), + vector3ToVectorXYZ(linearAcc), + vector3ToVectorXYZ(angularAcc)}}; + } + } + { + for (const auto& sensor : pImpl->virtualJointKinSensors) { + double jointPos; + double jointVel; + double jointAcc; + if (!sensor->getJointPosition(jointPos) || !sensor->getJointVelocity(jointVel) + || !sensor->getJointAcceleration(jointAcc)) { + yWarning() << logPrefix << "[VirtualJointKinSensors] " + << "Failed to read data" + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + data.virtualJointKinSensors[sensor->getSensorName()] = { + generateSensorStatus(sensor.get()), + {jointPos, + jointVel, + jointAcc}}; + } + } + { + for (const auto& sensor : pImpl->virtualSphericalJointKinSensors) { + wearable::Vector3 jointAngles; + wearable::Vector3 jointVel; + wearable::Vector3 jointAcc; + if (!sensor->getJointAnglesAsRPY(jointAngles) || !sensor->getJointVelocities(jointVel) + || !sensor->getJointAccelerations(jointAcc)) { + yWarning() << logPrefix << "[VirtualSphericalJointKinSensors] " + << "Failed to read data, " + << "sensor status is " + << static_cast(sensor->getSensorStatus()); + continue; + } + data.virtualSphericalJointKinSensors[sensor->getSensorName()] = { + generateSensorStatus(sensor.get()), + {vector3ToVectorRPY(jointAngles), + vector3ToVectorXYZ(jointVel), + vector3ToVectorXYZ(jointAcc)}}; + } + } + + // Stream the data though the port + pImpl->dataPort.write(); +} + +// ====================== +// DeviceDriver interface +// ====================== + +bool IWearWrapper::open(yarp::os::Searchable& config) +{ + if (!config.check("dataPortName") || !config.find("dataPortName").isString()) { + yError() << logPrefix << "dataPortName parameter not found"; + return false; + } + + if (!config.check("period")) { + yInfo() << logPrefix << "Using default period: " << DefaultPeriod << "s"; + } + + pImpl->dataPortName = config.find("dataPortName").asString(); + + const double period = config.check("period", yarp::os::Value(DefaultPeriod)).asFloat64(); + setPeriod(period); + + return true; +} + +bool IWearWrapper::close() +{ + pImpl->dataPort.close(); + return true; +} + +// ================== +// IWrapper interface +// ================== + +bool IWearWrapper::attach(yarp::dev::PolyDriver* poly) +{ + if (!poly) { + yError() << logPrefix << "Passed PolyDriver is nullptr."; + return false; + } + + if (pImpl->iWear || !poly->view(pImpl->iWear) || !pImpl->iWear) { + yError() << logPrefix << "Failed to view the IWear interface from the PolyDriver."; + return false; + } + + if (pImpl->iPreciselyTimed || !poly->view(pImpl->iPreciselyTimed) || !pImpl->iPreciselyTimed) { + yError() << logPrefix + << "Failed to view the IPreciselyTimed interface from the PolyDriver."; + return false; + } + + // Open the port for streaming data + pImpl->dataPort.open(pImpl->dataPortName); + + // Start the PeriodicThread loop + if (!start()) { + yError() << logPrefix << "Failed to start the loop."; + return false; + } + + yDebug() << logPrefix << "attach() successful"; + return true; +} + +void IWearWrapper::threadRelease() +{ + +} + +bool IWearWrapper::detach() +{ + while (isRunning()) { + stop(); + } + + pImpl->iWear = nullptr; + pImpl->iPreciselyTimed = nullptr; + + return true; +} + +// ========================== +// IMultipleWrapper interface +// ========================== + +bool IWearWrapper::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + if (driverList.size() > 1) { + yError() << logPrefix << "This wrapper accepts only one attached PolyDriver."; + return false; + } + + const yarp::dev::PolyDriverDescriptor* driver = driverList[0]; + + if (!driver) { + yError() << logPrefix << "Passed PolyDriverDescriptor is nullptr."; + return false; + } + + return attach(driver->poly); +} + +bool IWearWrapper::detachAll() +{ + return detach(); +} diff --git a/wrappers/IWearActuators/CMakeLists.txt b/wrappers/IWearActuators/CMakeLists.txt new file mode 100644 index 000000000..e4d9b01d8 --- /dev/null +++ b/wrappers/IWearActuators/CMakeLists.txt @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(iwearactuators_wrapper + TYPE wearable::wrappers::IWearActuatorsWrapper + INCLUDE include/IWearActuatorsWrapper.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(IWearActuatorsWrapper + src/IWearActuatorsWrapper.cpp + include/IWearActuatorsWrapper.h) + +target_include_directories(IWearActuatorsWrapper PRIVATE + $) + +target_link_libraries(IWearActuatorsWrapper PUBLIC + IWear WearableActuators YARP::YARP_dev YARP::YARP_os YARP::YARP_init) + +yarp_install( + TARGETS IWearActuatorsWrapper + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) diff --git a/wrappers/IWearActuators/include/IWearActuatorsWrapper.h b/wrappers/IWearActuators/include/IWearActuatorsWrapper.h new file mode 100644 index 000000000..bff00e4c3 --- /dev/null +++ b/wrappers/IWearActuators/include/IWearActuatorsWrapper.h @@ -0,0 +1,60 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IWEACTUATORSWRAPPERS_H +#define IWEACTUATORSWRAPPERS_H + +#include +#include +#include +#include +#include +#include + +#include "thrift/WearableActuatorCommand.h" +#include "thrift/GloveActuatorCommand.h" + +namespace wearable { + namespace wrappers { + class IWearActuatorsWrapper; + } +} // namespace wearable + +class wearable::wrappers::IWearActuatorsWrapper + : public yarp::dev::DeviceDriver + , public yarp::dev::IWrapper + , public yarp::dev::IMultipleWrapper + , public yarp::os::PeriodicThread + , public yarp::os::TypedReaderCallback + , public yarp::os::TypedReaderCallback +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + IWearActuatorsWrapper(); + ~IWearActuatorsWrapper() override; + + // DeviceDriver interface + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // TypedReaderCallback + void onRead(msg::WearableActuatorCommand& wearableActuatorCommand) override; + void onRead(msg::GloveActuatorCommand& gloveActuatorCommand) override; + + // PeriodicThread + void run() override; + void threadRelease() override; + + // IWrapper interface + bool attach(yarp::dev::PolyDriver* poly) override; + bool detach() override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() override; +}; + +#endif // IWEACTUATORSWRAPPERS_H diff --git a/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp b/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp new file mode 100644 index 000000000..1fb2c23ee --- /dev/null +++ b/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp @@ -0,0 +1,291 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "IWearActuatorsWrapper.h" + +#include "Wearable/IWear/IWear.h" + +#include +#include + +#include + +using namespace wearable; +using namespace wearable::wrappers; + +const std::string WrapperName = "IWearActuatorsWrapper"; +const std::string LogPrefix = WrapperName + wearable::Separator; +constexpr double DefaultPeriod = 0.01; + +class IWearActuatorsWrapper::impl : public wearable::msg::WearableActuatorCommand +{ +public: + 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; +}; + +IWearActuatorsWrapper::IWearActuatorsWrapper() + : yarp::os::PeriodicThread(DefaultPeriod) + , pImpl{new impl()} +{} + +IWearActuatorsWrapper::~IWearActuatorsWrapper() +{ + detachAll(); + close(); +} + +void IWearActuatorsWrapper::run() +{ + //TODO: Send the values of the actuators state ? +} + +// ====================== +// DeviceDriver interface +// ====================== + +bool IWearActuatorsWrapper::open(yarp::os::Searchable& config) +{ + 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"; + } + + // Parse configuration parameters + + const double period = config.check("period", yarp::os::Value(DefaultPeriod)).asFloat64(); + setPeriod(period); + + return true; +} + +void IWearActuatorsWrapper::onRead(msg::WearableActuatorCommand& wearableActuatorCommand) +{ + // Unpack the actuator in from incoming command + wearable::msg::ActuatorInfo info = wearableActuatorCommand.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(); + + switch (aType) { + case 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->setHapticCommand(wearableActuatorCommand.value); + } + + break; + } + case wearable::actuator::ActuatorType::Motor: { + + // Check if the actuator type in the wearable command is correct + if (info.type == wearable::msg::ActuatorType::MOTOR) + { + // Get motor actuator + wearable::ElementPtr castActuator = std::static_pointer_cast(pImpl->actuatorsMap[info.name]); + + // Send motor command + castActuator->setMotorPosition(wearableActuatorCommand.value); + } + + break; + } + default: { + return; + } + } + } +} + +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(); + return true; +} + +// ================== +// IWrapper interface +// ================== + +bool IWearActuatorsWrapper::attach(yarp::dev::PolyDriver* poly) +{ + if (!poly) { + yError() << LogPrefix << "Passed PolyDriver is nullptr."; + return false; + } + + if (pImpl->iWear || !poly->view(pImpl->iWear) || !pImpl->iWear) { + yError() << LogPrefix << "Failed to view the IWear interface from the PolyDriver."; + return false; + } + + // Check and add all the available actuators + + yInfo() << LogPrefix << "Finding available actuators from " << pImpl->attachedWearableDeviceKey << " wearable deive ..."; + + for (const auto& a : pImpl->iWear->getHapticActuators()) + { + pImpl->actuatorsMap[a->getActuatorName()] = a; + yInfo() << LogPrefix << "Adding actuator" << a->getActuatorName(); + } + + for (const auto& a : pImpl->iWear->getMotorActuators()) + { + pImpl->actuatorsMap[a->getActuatorName()] = a; + yInfo() << LogPrefix << "Adding actuator" << a->getActuatorName(); + } + + for (const auto& a : pImpl->iWear->getHeaterActuators()) + { + pImpl->actuatorsMap[a->getActuatorName()] = a; + yInfo() << LogPrefix << "Adding actuator" << a->getActuatorName(); + } + + // Start the PeriodicThread loop + if (!start()) { + yError() << LogPrefix << "Failed to start the periodic thread."; + return false; + } + + yDebug() << LogPrefix << "attach() successful"; + return true; +} + +void IWearActuatorsWrapper::threadRelease() +{ + +} + +bool IWearActuatorsWrapper::detach() +{ + while (isRunning()) { + stop(); + } + + pImpl->iWear = nullptr; + pImpl->wearableActuatorCommand = {}; + + return true; +} + +// ========================== +// IMultipleWrapper interface +// ========================== + +bool IWearActuatorsWrapper::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + if (driverList.size() > 1) { + yError() << LogPrefix << "This wrapper accepts only one attached PolyDriver."; + return false; + } + + const yarp::dev::PolyDriverDescriptor* driver = driverList[0]; + + if (!driver) { + yError() << LogPrefix << "Passed PolyDriverDescriptor is nullptr."; + return false; + } + + // Save key that identifies the driver + pImpl->attachedWearableDeviceKey = driver->key; + + return attach(driver->poly); +} + +bool IWearActuatorsWrapper::detachAll() +{ + return detach(); +} diff --git a/wrappers/IWearLogger/CMakeLists.txt b/wrappers/IWearLogger/CMakeLists.txt new file mode 100644 index 000000000..9cb155d59 --- /dev/null +++ b/wrappers/IWearLogger/CMakeLists.txt @@ -0,0 +1,29 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +find_package(YARP COMPONENTS dev REQUIRED) +find_package(robometry REQUIRED) + +yarp_prepare_plugin(iwear_logger + TYPE wearable::wrappers::IWearLogger + INCLUDE include/IWearLogger.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(IWearLogger + src/IWearLogger.cpp + include/IWearLogger.h) + +target_include_directories(IWearLogger PRIVATE + $) + +target_link_libraries(IWearLogger PUBLIC + IWear WearableData YARP::YARP_dev YARP::YARP_init robometry::robometry) + +yarp_install( + TARGETS IWearLogger + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) diff --git a/wrappers/IWearLogger/include/IWearLogger.h b/wrappers/IWearLogger/include/IWearLogger.h new file mode 100644 index 000000000..d0ae27bbb --- /dev/null +++ b/wrappers/IWearLogger/include/IWearLogger.h @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IWEARLOGGER_H +#define IWEARLOGGER_H + +#include +#include +#include +#include +#include + +namespace wearable +{ +namespace wrappers +{ +class IWearLogger; +} +} // namespace wearable + +class wearable::wrappers::IWearLogger + : public yarp::dev::DeviceDriver + , public yarp::dev::IWrapper + , public yarp::dev::IMultipleWrapper + , public yarp::os::PeriodicThread +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + IWearLogger(); + ~IWearLogger() override; + + // PeriodicThread + void run() override; + void threadRelease() override; + + // DeviceDriver interface + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // IWrapper interface + bool attach(yarp::dev::PolyDriver* poly) override; + bool detach() override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() override; +}; + +#endif // IWEARLOGGER_H diff --git a/wrappers/IWearLogger/src/IWearLogger.cpp b/wrappers/IWearLogger/src/IWearLogger.cpp new file mode 100644 index 000000000..29d6678d0 --- /dev/null +++ b/wrappers/IWearLogger/src/IWearLogger.cpp @@ -0,0 +1,1378 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "IWearLogger.h" +#include "Wearable/IWear/IWear.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +const std::string WrapperName = "IWearLogger"; +const std::string logPrefix = WrapperName + " :"; +constexpr double DefaultPeriod = 0.01; + +namespace wearable { + namespace wrappers { + struct IWearLoggerSettings; + + enum class LoggerType + { + MATLAB = 0, + YARP, + MATLAB_YARP, + NONE, + }; + } // namespace wrappers +} // namespace wearable + +struct wearable::wrappers::IWearLoggerSettings +{ + bool saveBufferManagerConfiguration{false}; + bool logAllQuantities{false}; + bool logAccelerometers{false}; + bool logEMGSensors{false}; + bool logForce3DSensors{false}; + bool logForceTorque6DSensors{false}; + bool logFreeBodyAccelerationSensors{false}; + bool logGyroscopes{false}; + bool logMagnetometers{false}; + bool logOrientationSensors{false}; + bool logPoseSensors{false}; + bool logPositionSensors{false}; + bool logTemperatureSensors{false}; + bool logTorque3DSensors{false}; + bool logVirtualLinkKinSensors{false}; + bool logVirtualJointKinSensors{false}; + bool logVirtualSphericalJointKinSensors{false}; + bool logSkinSensors{false}; +}; + +using namespace wearable; +using namespace wearable::wrappers; + +using WerableSensorName = std::string; +using MatlabChannelName = std::string; +using YarpBufferedPort = yarp::os::BufferedPort; + +class IWearLogger::impl +{ +public: + LoggerType loggerType; + void setLoggerType(std::string& str); + + bool loadSettingsFromConfig(yarp::os::Searchable& config); + void checkAndLoadBooleanOption(yarp::os::Property& prop, + const std::string& optionName, + bool& option); + + bool configureMatlabBufferManager(const std::string& sensorName, const size_t& channelSize); + bool configureYarpBufferManager(const std::string& sensorName); + bool configureBufferManager(); + + inline void prepareYarpBottle(const std::vector& sensorData, yarp::sig::Vector& b) + { + b.clear(); + + for (const auto& e : sensorData) { + b.push_back(e); + } + } + + inline std::vector split(const std::string& s, const std::string& delimiter) + { + std::size_t pos_start = 0, pos_end, delim_len = delimiter.length(); + std::string token; + std::vector res; + + while ((pos_end = s.find(delimiter, pos_start)) != std::string::npos) { + token = s.substr(pos_start, pos_end - pos_start); + pos_start = pos_end + delim_len; + res.push_back(token); + } + + res.push_back(s.substr(pos_start)); + return res; + } + + inline std::string getValidName(const std::string& sensorName, const char& c) + { + std::string name{sensorName}; + + // Replace special characters with desired char c + std::replace(name.begin(), name.end(), '#', c); + std::replace(name.begin(), name.end(), '@', c); + std::replace(name.begin(), name.end(), '/', c); + std::replace(name.begin(), name.end(), '(', c); + std::replace(name.begin(), name.end(), ')', c); + + auto vecStr = split(name, wearable::Separator); + + std::string validName; + for (auto& str : vecStr) { + if (validName.empty()) { + validName = str; + } + else { + validName = validName + c + str; + } + } + + return validName; + } + + inline std::string convertSensorNameToValidMatlabVarName(const std::string& sensorName) + { + return getValidName(sensorName, '_'); + } + + inline std::string convertSensorNameToValidYarpPortName(const std::string& sensorName) + { + return ('/' + getValidName(sensorName, '/')); + } + + inline void + prefixVecWithSensorStatus(const std::shared_ptr& sensor, + std::vector& saveVar) + { + // prefix sensor status + auto it = saveVar.begin(); + saveVar.insert(it, static_cast(sensor->getSensorStatus())); + } + + bool firstRun = true; + size_t waitingFirstReadCounter = 1; + + wearable::IWear* iWear = nullptr; + yarp::dev::IPreciselyTimed* iPreciselyTimed = nullptr; + std::mutex loggerMutex; + IWearLoggerSettings settings; + robometry::BufferConfig bufferConfig; + robometry::BufferManager bufferManager; + + wearable::VectorOfSensorPtr accelerometers; + wearable::VectorOfSensorPtr emgSensors; + wearable::VectorOfSensorPtr force3DSensors; + wearable::VectorOfSensorPtr forceTorque6DSensors; + wearable::VectorOfSensorPtr + freeBodyAccelerationSensors; + wearable::VectorOfSensorPtr gyroscopes; + wearable::VectorOfSensorPtr magnetometers; + wearable::VectorOfSensorPtr orientationSensors; + wearable::VectorOfSensorPtr poseSensors; + wearable::VectorOfSensorPtr positionSensors; + wearable::VectorOfSensorPtr temperatureSensors; + wearable::VectorOfSensorPtr torque3DSensors; + wearable::VectorOfSensorPtr + virtualLinkKinSensors; + wearable::VectorOfSensorPtr + virtualJointKinSensors; + wearable::VectorOfSensorPtr + virtualSphericalJointKinSensors; + wearable::VectorOfSensorPtr + skinSensors; + + std::unordered_map wearable2MatlabNameLookup; + std::unordered_map> + wearable2YarpPortLookup; +}; + +IWearLogger::IWearLogger() + : yarp::os::PeriodicThread(DefaultPeriod) + , pImpl{new impl()} +{} + +IWearLogger::~IWearLogger() +{ + detachAll(); + close(); +} + +// ======================== +// PeriodicThread interface +// ======================== + +void IWearLogger::run() +{ + if (!pImpl->iWear) { + yError() << logPrefix << "The IWear pointer is null in the driver loop."; + askToStop(); + return; + } + + if (!pImpl->iPreciselyTimed) { + yError() << logPrefix << "The IPreciselyTimed pointer is null in the driver loop."; + askToStop(); + return; + } + + while (pImpl->iWear->getStatus() == WearStatus::Calibrating + || pImpl->iWear->getStatus() == WearStatus::WaitingForFirstRead) { + if (pImpl->waitingFirstReadCounter++ % 1000 == 0) { + pImpl->waitingFirstReadCounter = 1; + yInfo() << logPrefix << "IWear interface waiting for first data. Waiting..."; + } + return; + } + + if (pImpl->iWear->getStatus() == WearStatus::Error + || pImpl->iWear->getStatus() == WearStatus::Unknown) { + yError() << logPrefix << "The status of the IWear interface is not Ok (" + << static_cast(pImpl->iWear->getStatus()) << ")"; + askToStop(); + return; + } + + // case status is TIMEOUT or DATA_OVERFLOW + if (pImpl->iWear->getStatus() != WearStatus::Ok) { + yWarning() << logPrefix << "The status of the IWear interface is not Ok (" + << static_cast(pImpl->iWear->getStatus()) << ")"; + } + + if (pImpl->firstRun) { + pImpl->firstRun = false; + pImpl->accelerometers = pImpl->iWear->getAccelerometers(); + pImpl->emgSensors = pImpl->iWear->getEmgSensors(); + pImpl->force3DSensors = pImpl->iWear->getForce3DSensors(); + pImpl->forceTorque6DSensors = pImpl->iWear->getForceTorque6DSensors(); + pImpl->freeBodyAccelerationSensors = pImpl->iWear->getFreeBodyAccelerationSensors(); + pImpl->gyroscopes = pImpl->iWear->getGyroscopes(); + pImpl->magnetometers = pImpl->iWear->getMagnetometers(); + pImpl->orientationSensors = pImpl->iWear->getOrientationSensors(); + pImpl->poseSensors = pImpl->iWear->getPoseSensors(); + pImpl->positionSensors = pImpl->iWear->getPositionSensors(); + pImpl->temperatureSensors = pImpl->iWear->getTemperatureSensors(); + pImpl->torque3DSensors = pImpl->iWear->getTorque3DSensors(); + pImpl->virtualLinkKinSensors = pImpl->iWear->getVirtualLinkKinSensors(); + pImpl->virtualJointKinSensors = pImpl->iWear->getVirtualJointKinSensors(); + pImpl->virtualSphericalJointKinSensors = pImpl->iWear->getVirtualSphericalJointKinSensors(); + pImpl->skinSensors = pImpl->iWear->getSkinSensors(); + } + + yarp::os::Stamp timestamp = pImpl->iPreciselyTimed->getLastInputStamp(); + + if (pImpl->settings.logAllQuantities || pImpl->settings.logAccelerometers) { + for (const auto& sensor : pImpl->accelerometers) { + wearable::Vector3 vector3; + if (!sensor->getLinearAcceleration(vector3)) { + yWarning() << logPrefix << "[Accelerometers] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + + std::vector saveVar{vector3.begin(), vector3.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logEMGSensors) { + for (const auto& sensor : pImpl->emgSensors) { + double value, normalization; + // double normalizationValue; + if (!sensor->getEmgSignal(value) || !sensor->getNormalizationValue(normalization)) { + yWarning() << logPrefix << "[EmgSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar; + saveVar.emplace_back(value); + saveVar.emplace_back(normalization); + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logForce3DSensors) { + for (const auto& sensor : pImpl->force3DSensors) { + wearable::Vector3 vector3; + if (!sensor->getForce3D(vector3)) { + yWarning() << logPrefix << "[Force3DSensors] " + << "Failed to read data, " + << "sensor status is "; + } + else { + std::vector saveVar{vector3.begin(), vector3.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logForceTorque6DSensors) { + for (const auto& sensor : pImpl->forceTorque6DSensors) { + wearable::Vector6 vector6; + if (!sensor->getForceTorque6D(vector6)) { + yWarning() << logPrefix << "[ForceTorque6DSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar{vector6.begin(), vector6.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logFreeBodyAccelerationSensors) { + for (const auto& sensor : pImpl->freeBodyAccelerationSensors) { + wearable::Vector3 vector3; + if (!sensor->getFreeBodyAcceleration(vector3)) { + yWarning() << logPrefix << "[FreeBodyAccelerationSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar{vector3.begin(), vector3.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logGyroscopes) { + for (const auto& sensor : pImpl->gyroscopes) { + wearable::Vector3 vector3; + if (!sensor->getAngularRate(vector3)) { + yWarning() << logPrefix << "[Gyroscopes] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar{vector3.begin(), vector3.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logMagnetometers) { + for (const auto& sensor : pImpl->magnetometers) { + wearable::Vector3 vector3; + if (!sensor->getMagneticField(vector3)) { + yWarning() << logPrefix << "[Magnetometers] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar{vector3.begin(), vector3.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logOrientationSensors) { + for (const auto& sensor : pImpl->orientationSensors) { + wearable::Quaternion quaternion; + if (!sensor->getOrientationAsQuaternion(quaternion)) { + yWarning() << logPrefix << "[OrientationSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar{quaternion.begin(), quaternion.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logPoseSensors) { + for (const auto& sensor : pImpl->poseSensors) { + wearable::Vector3 vector3; + wearable::Quaternion quaternion; + if (!sensor->getPose(quaternion, vector3)) { + yWarning() << logPrefix << "[PoseSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar; + std::copy(vector3.begin(), vector3.end(), std::back_inserter(saveVar)); + std::copy(quaternion.begin(), quaternion.end(), std::back_inserter(saveVar)); + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logPositionSensors) { + for (const auto& sensor : pImpl->positionSensors) { + wearable::Vector3 vector3; + if (!sensor->getPosition(vector3)) { + yWarning() << logPrefix << "[PositionSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar{vector3.begin(), vector3.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logTemperatureSensors) { + for (const auto& sensor : pImpl->temperatureSensors) { + double value; + if (!sensor->getTemperature(value)) { + yWarning() << logPrefix << "[TemperatureSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar; + saveVar.emplace_back(value); + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logTorque3DSensors) { + for (const auto& sensor : pImpl->torque3DSensors) { + wearable::Vector3 vector3; + if (!sensor->getTorque3D(vector3)) { + yWarning() << logPrefix << "[Torque3DSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar{vector3.begin(), vector3.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logVirtualLinkKinSensors) { + for (const auto& sensor : pImpl->virtualLinkKinSensors) { + wearable::Vector3 linearAcc; + wearable::Vector3 angularAcc; + wearable::Vector3 linearVel; + wearable::Vector3 angularVel; + wearable::Vector3 position; + wearable::Quaternion orientation; + if (!sensor->getLinkAcceleration(linearAcc, angularAcc) + || !sensor->getLinkPose(position, orientation) + || !sensor->getLinkVelocity(linearVel, angularVel)) { + yWarning() << logPrefix << "[VirtualLinkKinSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar; + std::copy(position.begin(), position.end(), std::back_inserter(saveVar)); + std::copy(orientation.begin(), orientation.end(), std::back_inserter(saveVar)); + std::copy(linearVel.begin(), linearVel.end(), std::back_inserter(saveVar)); + std::copy(angularVel.begin(), angularVel.end(), std::back_inserter(saveVar)); + std::copy(linearAcc.begin(), linearAcc.end(), std::back_inserter(saveVar)); + std::copy(angularAcc.begin(), angularAcc.end(), std::back_inserter(saveVar)); + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logVirtualJointKinSensors) { + for (const auto& sensor : pImpl->virtualJointKinSensors) { + double jointPos; + double jointVel; + double jointAcc; + if (!sensor->getJointPosition(jointPos) || !sensor->getJointVelocity(jointVel) + || !sensor->getJointAcceleration(jointAcc)) { + yError() << logPrefix << "[VirtualJointKinSensors] " + << "Failed to read data"; + askToStop(); + return; + } + else { + std::vector saveVar; + saveVar.emplace_back(jointPos); + saveVar.emplace_back(jointVel); + saveVar.emplace_back(jointAcc); + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logVirtualSphericalJointKinSensors) { + for (const auto& sensor : pImpl->virtualSphericalJointKinSensors) { + wearable::Vector3 jointAngles; + wearable::Vector3 jointVel; + wearable::Vector3 jointAcc; + if (!sensor->getJointAnglesAsRPY(jointAngles) || !sensor->getJointVelocities(jointVel) + || !sensor->getJointAccelerations(jointAcc)) { + yWarning() << logPrefix << "[VirtualSphericalJointKinSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else { + std::vector saveVar; + std::copy(jointAngles.begin(), jointAngles.end(), std::back_inserter(saveVar)); + std::copy(jointVel.begin(), jointVel.end(), std::back_inserter(saveVar)); + std::copy(jointAcc.begin(), jointAcc.end(), std::back_inserter(saveVar)); + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } + + if (pImpl->settings.logAllQuantities || pImpl->settings.logSkinSensors) + { + for (const auto& sensor : pImpl->skinSensors) { + std::vector pressureVector; + if (!sensor->getPressure(pressureVector)) { + yWarning() << logPrefix << "[SkinSensors] " + << "Failed to read data, " + << "sensor status is " << static_cast(sensor->getSensorStatus()); + } + else + { + std::vector saveVar{pressureVector.begin(), pressureVector.end()}; + pImpl->prefixVecWithSensorStatus(sensor, saveVar); + + if (pImpl->loggerType == LoggerType::MATLAB + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + const auto& channelName = + pImpl->wearable2MatlabNameLookup.at(sensor->getSensorName()); + pImpl->bufferManager.push_back(saveVar, timestamp.getTime(), channelName); + } + + if (pImpl->loggerType == LoggerType::YARP + || pImpl->loggerType == LoggerType::MATLAB_YARP) { + auto& port = pImpl->wearable2YarpPortLookup.at(sensor->getSensorName()); + yarp::sig::Vector& data = port->prepare(); + pImpl->prepareYarpBottle(saveVar, data); + port->setEnvelope(timestamp); + port->write(); + } + } + } + } +} + +// ====================== +// DeviceDriver interface +// ====================== + +bool IWearLogger::open(yarp::os::Searchable& config) +{ + std::lock_guard guard(pImpl->loggerMutex); + if (!config.check("period")) { + yInfo() << logPrefix << "Using default period: " << DefaultPeriod << "s"; + } + + const double period = config.check("period", yarp::os::Value(DefaultPeriod)).asFloat64(); + setPeriod(period); + + // Load settings in the class + bool ok = pImpl->loadSettingsFromConfig(config); + if (!ok) { + yError() << logPrefix << "Problem in loading settings from config."; + return false; + } + + return true; +} + +void IWearLogger::impl::setLoggerType(std::string& str) +{ + if (!std::strcmp(str.c_str(), "matlab")) { + this->loggerType = LoggerType::MATLAB; + } + + if (!std::strcmp(str.c_str(), "yarp")) { + if (this->loggerType == LoggerType::MATLAB) { + this->loggerType = LoggerType::MATLAB_YARP; + } + else { + this->loggerType = LoggerType::YARP; + } + } +} + +bool IWearLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config) +{ + // Check for logLevel parameter + this->loggerType = LoggerType::NONE; + if (!(config.check("LoggerType") + && (config.find("LoggerType").isString() || config.find("LoggerType").isList()))) { + yInfo() << logPrefix << "Using default LoggerType : MATLAB"; + this->loggerType = LoggerType::MATLAB; + } + else if (config.check("LoggerType") && config.find("LoggerType").isList()) { + yarp::os::Bottle* loggerTypeList = config.find("LoggerType").asList(); + + for (size_t i = 0; i < loggerTypeList->size(); i++) { + std::string option = loggerTypeList->get(i).asString(); + + setLoggerType(option); + } + } + else if (config.check("LoggerType") && config.find("LoggerType").isString()) { + std::string option = config.find("LoggerType").asString(); + setLoggerType(option); + } + + // Display the current logger level + switch (this->loggerType) { + case LoggerType::MATLAB: { + yInfo() << logPrefix << "LoggerType set to MATLAB"; + break; + } + case LoggerType::YARP: { + yInfo() << logPrefix << "LoggerType set to YARP"; + break; + } + case LoggerType::MATLAB_YARP: { + yInfo() << logPrefix << "LoggerType set to MATLAB & YARP"; + break; + } + default: + break; + } + + yarp::os::Property prop; + prop.fromString(config.toString().c_str()); + + // load logger flag settings + checkAndLoadBooleanOption(prop, "logAllQuantities", settings.logAllQuantities); + checkAndLoadBooleanOption(prop, "logAccelerometers", settings.logAccelerometers); + checkAndLoadBooleanOption(prop, "logEMGSensors", settings.logEMGSensors); + checkAndLoadBooleanOption(prop, "logForce3DSensors", settings.logForce3DSensors); + checkAndLoadBooleanOption(prop, "logForceTorque6DSensors", settings.logForceTorque6DSensors); + checkAndLoadBooleanOption( + prop, "logFreeBodyAccelerationSensors", settings.logFreeBodyAccelerationSensors); + checkAndLoadBooleanOption(prop, "logGyroscopes", settings.logGyroscopes); + checkAndLoadBooleanOption(prop, "logMagnetometers", settings.logMagnetometers); + checkAndLoadBooleanOption(prop, "logOrientationSensors", settings.logOrientationSensors); + checkAndLoadBooleanOption(prop, "logPoseSensors", settings.logPoseSensors); + checkAndLoadBooleanOption(prop, "logPositionSensors", settings.logPositionSensors); + checkAndLoadBooleanOption(prop, "logTemperatureSensors", settings.logTemperatureSensors); + checkAndLoadBooleanOption(prop, "logTorque3DSensors", settings.logTorque3DSensors); + checkAndLoadBooleanOption(prop, "logVirtualLinkKinSensors", settings.logVirtualLinkKinSensors); + checkAndLoadBooleanOption( + prop, "logVirtualJointKinSensors", settings.logVirtualJointKinSensors); + checkAndLoadBooleanOption( + prop, "logVirtualSphericalJointKinSensors", settings.logVirtualSphericalJointKinSensors); + checkAndLoadBooleanOption(prop, "logSkinSensors", settings.logSkinSensors); + + // load buffer manager configuration settings + checkAndLoadBooleanOption( + prop, "saveBufferManagerConfiguration", settings.saveBufferManagerConfiguration); + + std::string experimentName = "experimentName"; + if (prop.check(experimentName.c_str()) && prop.find(experimentName.c_str()).isString()) { + bufferConfig.filename = prop.find(experimentName.c_str()).asString(); + } + else { + yError() << logPrefix << " missing parameter: " << experimentName; + return false; + } + + std::string path = "path"; + if (prop.check(path.c_str()) && prop.find(path.c_str()).isString()) { + bufferConfig.path = prop.find(path.c_str()).asString(); + } + + std::string n_samples = "n_samples"; + if (prop.check(n_samples.c_str()) && prop.find(n_samples.c_str()).isInt32()) { + bufferConfig.n_samples = prop.find(n_samples.c_str()).asInt32(); + } + else { + yError() << logPrefix << " missing parameter: " << n_samples; + return false; + } + + std::string save_periodically = "save_periodically"; + if (prop.check(save_periodically.c_str()) && prop.find(save_periodically.c_str()).isBool()) { + bufferConfig.save_periodically = prop.find(save_periodically.c_str()).asBool(); + } + + if (bufferConfig.save_periodically) { + std::string save_period = "save_period"; + if (prop.check(save_period.c_str()) && prop.find(save_period.c_str()).isFloat64()) { + bufferConfig.save_period = prop.find(save_period.c_str()).asFloat64(); + } + else { + yError() << logPrefix << " missing parameter: " << save_period; + return false; + } + + std::string data_threshold = "data_threshold"; + if (prop.check(data_threshold.c_str()) && prop.find(data_threshold.c_str()).isInt32()) { + bufferConfig.data_threshold = prop.find(data_threshold.c_str()).asInt32(); + } + } + + std::string auto_save = "auto_save"; + if (prop.check(auto_save.c_str()) && prop.find(auto_save.c_str()).isBool()) { + bufferConfig.auto_save = prop.find(auto_save.c_str()).asBool(); + } + + if (!(bufferConfig.auto_save || bufferConfig.save_periodically)) { + yError() + << logPrefix + << " both auto_save and save_periodically are set to false, nothing will be saved."; + return false; + } + + return true; +} + +void IWearLogger::impl::checkAndLoadBooleanOption(yarp::os::Property& prop, + const std::string& optionName, + bool& option) +{ + if (prop.check(optionName.c_str())) { + option = prop.find(optionName.c_str()).asBool(); + } +} + +bool IWearLogger::close() +{ + if (!pImpl->bufferConfig.auto_save) { + pImpl->bufferManager.saveToFile(); + } + bool ok{true}; + if (pImpl->settings.saveBufferManagerConfiguration) { + auto buffConfToSave = pImpl->bufferManager.getBufferConfig(); + ok = bufferConfigToJson(buffConfToSave, + buffConfToSave.path + "bufferConfig" + buffConfToSave.filename + + ".json"); + } + return ok; +} + +// ================== +// IWrapper interface +// ================== + +bool IWearLogger::attach(yarp::dev::PolyDriver* poly) +{ + if (!poly) { + yError() << logPrefix << "Passed PolyDriver is nullptr."; + return false; + } + + if (pImpl->iWear || !poly->view(pImpl->iWear) || !pImpl->iWear) { + yError() << logPrefix << "Failed to view the IWear interface from the PolyDriver."; + return false; + } + + if (pImpl->iPreciselyTimed || !poly->view(pImpl->iPreciselyTimed) || !pImpl->iPreciselyTimed) { + yError() << logPrefix + << "Failed to view the IPreciselyTimed interface from the PolyDriver."; + return false; + } + + if (!pImpl->configureBufferManager()) { + yError() << logPrefix << "Failed to configure buffer manager for the logger."; + return false; + } + + // Start the PeriodicThread loop + if (!start()) { + yError() << logPrefix << "Failed to start the loop."; + return false; + } + + yDebug() << logPrefix << "attach() successful"; + return true; +} + +bool IWearLogger::impl::configureMatlabBufferManager(const std::string& sensorName, + const size_t& channelSize) +{ + MatlabChannelName channelName = convertSensorNameToValidMatlabVarName(sensorName); + wearable2MatlabNameLookup[sensorName] = channelName; + + bool ok = bufferManager.addChannel({channelName, {channelSize, 1}}); + + if (!ok) { + yError() << logPrefix << " matlab buffer manager configuration failed for " << sensorName; + return false; + } + + return true; +} + +bool IWearLogger::impl::configureYarpBufferManager(const std::string& sensorName) +{ + auto portName = convertSensorNameToValidYarpPortName(sensorName); + + auto port = std::make_unique(); + + // Check yarp network initialization + if (!yarp::os::Network::isNetworkInitialized()) { + yInfo() << logPrefix << "Initializing yarp network"; + yarp::os::Network::init(); + } + + // Open yarp port + if (!port->open(portName)) { + yError() << logPrefix << "Failed to open yarp port " << portName; + return false; + } + + wearable2YarpPortLookup[sensorName] = std::move(port); + + return true; +} + +bool IWearLogger::impl::configureBufferManager() +{ + bool ok{true}; + // Prepare the buffer manager for the logger + if (ok && (settings.logAllQuantities || settings.logAccelerometers)) { + for (const auto& s : iWear->getAccelerometers()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (4, 1) accelerometer channels for " << sensorName + << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 4); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logEMGSensors)) { + for (const auto& s : iWear->getEmgSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (3, 1) EMG sensor channels value+normalization for " + << sensorName << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 3); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logForce3DSensors)) { + for (const auto& s : iWear->getForce3DSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (4, 1) 3d force sensor channels for " << sensorName + << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 4); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logForceTorque6DSensors)) { + for (const auto& s : iWear->getForceTorque6DSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (7, 1) 6D force torque sensor channels for " + << sensorName << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 7); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logFreeBodyAccelerationSensors)) { + for (const auto& s : iWear->getFreeBodyAccelerationSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (4, 1) free body acceleration sensor channels for " + << sensorName << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 4); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logGyroscopes)) { + for (const auto& s : iWear->getGyroscopes()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (4, 1) gyroscope channels for " << sensorName + << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 4); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logMagnetometers)) { + for (const auto& s : iWear->getMagnetometers()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (4, 1) magnetometer channels for " << sensorName + << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 4); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logOrientationSensors)) { + for (const auto& s : iWear->getOrientationSensors()) { + std::string sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (5, 1) quaternion wxyz channels for " << sensorName + << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 5); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logPoseSensors)) { + for (const auto& s : iWear->getPoseSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (8, 1) pose sensor (pos+quat) channels for " + << sensorName << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 8); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logPositionSensors)) { + for (const auto& s : iWear->getPositionSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (4, 1) pose sensor channels for " << sensorName + << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 4); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logTemperatureSensors)) { + for (const auto& s : iWear->getTemperatureSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (2, 1) temperature sensor channels for " << sensorName + << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 2); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logTorque3DSensors)) { + for (const auto& s : iWear->getTorque3DSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (4, 1) 3D torque sensor channels for " << sensorName + << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 4); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logVirtualLinkKinSensors)) { + for (const auto& s : iWear->getVirtualLinkKinSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (20, 1) pos+quat+v+omega+a+alpha channels for " + << sensorName << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 20); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logVirtualJointKinSensors)) { + for (const auto& s : iWear->getVirtualJointKinSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix << "Adding (4, 1) virtual joint kinematics channels for " + << sensorName << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 4); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logVirtualSphericalJointKinSensors)) { + for (const auto& s : iWear->getVirtualSphericalJointKinSensors()) { + auto sensorName = s->getSensorName(); + yInfo() << logPrefix + << "Adding (10, 1) rpy+vel+acc virtual spherical joint kinematics channels for " + << sensorName << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, 10); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + if (ok && (settings.logAllQuantities || settings.logSkinSensors)) { + for (const auto& s : iWear->getSkinSensors()) { + auto sensorName = s->getSensorName(); + + // retrieve data size + std::vector pressureVector; + s->getPressure(pressureVector); + size_t dataSize = pressureVector.size()+1; + + yInfo() << logPrefix + << "Adding ("<< dataSize<<", 1) pressure vector channels for " + << sensorName << " prefixed with sensor status."; + + if (loggerType == LoggerType::MATLAB || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureMatlabBufferManager(sensorName, dataSize); + } + + if (loggerType == LoggerType::YARP || loggerType == LoggerType::MATLAB_YARP) { + ok = ok && configureYarpBufferManager(sensorName); + } + } + } + + ok = ok && bufferManager.configure(bufferConfig); + if (ok) { + yDebug() << logPrefix << " buffer manager configured successfully."; + } + + return ok; +} + +void IWearLogger::threadRelease() {} + +bool IWearLogger::detach() +{ + std::lock_guard guard(pImpl->loggerMutex); + while (isRunning()) { + stop(); + } + + pImpl->iWear = nullptr; + pImpl->iPreciselyTimed = nullptr; + + return true; +} + +// ========================== +// IMultipleWrapper interface +// ========================== + +bool IWearLogger::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + if (driverList.size() > 1) { + yError() << logPrefix << "This wrapper accepts only one attached PolyDriver."; + return false; + } + + const yarp::dev::PolyDriverDescriptor* driver = driverList[0]; + + if (!driver) { + yError() << logPrefix << "Passed PolyDriverDescriptor is nullptr."; + return false; + } + + return attach(driver->poly); +} + +bool IWearLogger::detachAll() +{ + return detach(); +} diff --git a/wrappers/IXsensMVNControl/CMakeLists.txt b/wrappers/IXsensMVNControl/CMakeLists.txt new file mode 100644 index 000000000..df7757ea5 --- /dev/null +++ b/wrappers/IXsensMVNControl/CMakeLists.txt @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(ixsensmvncontrol_wrapper + TYPE wearable::wrappers::IXsensMVNControlWrapper + INCLUDE include/IXsensMVNControlWrapper.h + CATEGORY device + ADVANCED + DEFAULT ON) + +yarp_add_plugin(IXsensMVNControlWrapper + src/IXsensMVNControlWrapper.cpp + include/IXsensMVNControlWrapper.h) + +target_include_directories(IXsensMVNControlWrapper PRIVATE + $) + +target_link_libraries(IXsensMVNControlWrapper PUBLIC + XsensSuitControl IXsensMVNControl YARP::YARP_dev) + +yarp_install( + TARGETS IXsensMVNControlWrapper + COMPONENT runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) diff --git a/wrappers/IXsensMVNControl/include/IXsensMVNControlWrapper.h b/wrappers/IXsensMVNControl/include/IXsensMVNControlWrapper.h new file mode 100644 index 000000000..254f54947 --- /dev/null +++ b/wrappers/IXsensMVNControl/include/IXsensMVNControlWrapper.h @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IXSENS_MVN_CONTROL_WRAPPER_H +#define IXSENS_MVN_CONTROL_WRAPPER_H + +#include +#include +#include +#include + +#include + +//#include "IXsensMVNControl.h" + +namespace wearable { + namespace wrappers { + class IXsensMVNControlWrapper; + } +} // namespace wearable + +// using namespace xsensmvn; + +class wearable::wrappers::IXsensMVNControlWrapper + : public yarp::dev::DeviceDriver + , public yarp::dev::IWrapper + , public yarp::dev::IMultipleWrapper + , public yarp::os::PeriodicThread +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + IXsensMVNControlWrapper(); + ~IXsensMVNControlWrapper() override; + + // DeviceDriver interface + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // IWrapper interface + bool attach(yarp::dev::PolyDriver* poly) override; + bool detach() override; + + // PeriodicThread + void run() override; + + // IMultipleWrapper interface + bool attachAll(const yarp::dev::PolyDriverList& driverList) override; + bool detachAll() override; +}; + +#endif // IXSENS_MVN_CONTROL_WRAPPER_H diff --git a/wrappers/IXsensMVNControl/src/IXsensMVNControlWrapper.cpp b/wrappers/IXsensMVNControl/src/IXsensMVNControlWrapper.cpp new file mode 100644 index 000000000..28aa8cdb0 --- /dev/null +++ b/wrappers/IXsensMVNControl/src/IXsensMVNControlWrapper.cpp @@ -0,0 +1,177 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "IXsensMVNControlWrapper.h" +#include "IXsensMVNControl.h" + +#include "thrift/XsensSuitControlService.h" + +#include +#include +#include + +const std::string WrapperName = "IXsensMVNControlWrapper"; +const std::string logPrefix = WrapperName + " : "; + +// using namespace xsensmvn; +using namespace wearable::wrappers; + +class IXsensMVNControlWrapper::impl : public wearable::msg::XsensSuitControlService +{ +public: + ~impl() override = default; + + yarp::os::Port rpcPort; + xsensmvn::IXsensMVNControl* xsControl = nullptr; + + bool calibrate() override; + bool calibrateWithType(const std::string& calibrationType) override; + bool abortCalibration() override; + bool startAcquisition() override; + bool stopAcquisition() override; + std::vector help(const std::string& functionName = "--all") override + { + return {}; // TODO} + } +}; + +wearable::wrappers::IXsensMVNControlWrapper::IXsensMVNControlWrapper() + : PeriodicThread(0.5) + , pImpl{new impl()} +{} + +wearable::wrappers::IXsensMVNControlWrapper::~IXsensMVNControlWrapper() = default; + +bool IXsensMVNControlWrapper::open(yarp::os::Searchable& config) +{ + std::string defaultRpcPortName = "/" + WrapperName + "/rpc:i"; + + if (!config.check("rpcPortName") || !config.find("rpcPortName").isString()) { + yInfo() << logPrefix << "rpcPortName parameter not found, using default defaultRpcPortName"; + } + else { + defaultRpcPortName = config.find("rpcPortName").asString(); + } + + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + yError() << logPrefix << "YARP server wasn't found active."; + return false; + } + + if (!pImpl->rpcPort.open(defaultRpcPortName)) { + yError() << "Failed to open local port " << defaultRpcPortName; + return false; + } + + if (!pImpl->yarp().attachAsServer(pImpl->rpcPort)) { + yError() << "Failed to attach " << defaultRpcPortName << " to the RPC service"; + return false; + } + + start(); + return true; +} + +bool IXsensMVNControlWrapper::close() +{ + pImpl->rpcPort.close(); + return true; +} + +bool IXsensMVNControlWrapper::attach(yarp::dev::PolyDriver* poly) +{ + if (!poly) { + yError() << logPrefix << "Passed PolyDriver is nullptr."; + return false; + } + + if (pImpl->xsControl || !poly->view(pImpl->xsControl) || !pImpl->xsControl) { + yError() << logPrefix + << "Failed to view the IXsensMVNControl interface from the PolyDriver."; + return false; + } + + return true; +} + +bool IXsensMVNControlWrapper::detach() +{ + pImpl->xsControl = nullptr; + return true; +} + +void IXsensMVNControlWrapper::run() +{ + return; +} + +bool IXsensMVNControlWrapper::impl::calibrate() +{ + if (!xsControl) { + return false; + } + + return xsControl->calibrate(); +} + +bool IXsensMVNControlWrapper::impl::calibrateWithType(const std::string& calibrationType) +{ + if (!xsControl) { + return false; + } + + return xsControl->calibrate(calibrationType); +} + +bool IXsensMVNControlWrapper::impl::abortCalibration() +{ + if (!xsControl) { + return false; + } + + return xsControl->abortCalibration(); +} + +bool IXsensMVNControlWrapper::impl::startAcquisition() +{ + if (!xsControl) { + return false; + } + + return xsControl->startAcquisition(); +} + +bool IXsensMVNControlWrapper::impl::stopAcquisition() +{ + if (!xsControl) { + return false; + } + + return xsControl->stopAcquisition(); +} + +// ========================== +// IMultipleWrapper interface +// ========================== + +bool IXsensMVNControlWrapper::attachAll(const yarp::dev::PolyDriverList& driverList) +{ + if (driverList.size() > 1) { + yError() << logPrefix << "This wrapper accepts only one attached PolyDriver."; + return false; + } + + const yarp::dev::PolyDriverDescriptor* driver = driverList[0]; + + if (!driver) { + yError() << logPrefix << "Passed PolyDriverDescriptor is nullptr."; + return false; + } + + return attach(driver->poly); +} + +bool IXsensMVNControlWrapper::detachAll() +{ + return detach(); +}