diff --git a/.gitignore b/.gitignore index 96261bf..64730cf 100644 --- a/.gitignore +++ b/.gitignore @@ -31,6 +31,9 @@ *.out *.app - # Qtcreator project *.user + +# clangd +**/compile_commands.json +**/.cache/ diff --git a/CHANGELOG.md b/CHANGELOG.md index 72b55e1..7bf8b3c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed ### Fixed +- fix attaching both IMU and FTs as MAS network wrappers (https://github.com/robotology/whole-body-estimators/pull/167). - Implement `VirtualAnalogClient::getAxes` and `VirtualAnalogRemapper::getAxes` to fix compilation against YARP 3.8 (https://github.com/robotology/whole-body-estimators/pull/159, https://github.com/robotology/whole-body-estimators/pull/160). - Fix compilation against YARP 3.8 (https://github.com/robotology/whole-body-estimators/pull/166). diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index edf7fd9..e3dfa3f 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1,3 +1,4 @@ +#include #define SKIN_EVENTS_TIMEOUT 0.2 #include "WholeBodyDynamicsDevice.h" @@ -1687,9 +1688,12 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) ITemperatureSensors *tempS =nullptr; if( p[devIdx]->poly->view(fts) ) { - ftSensorList.push(const_cast(*p[devIdx])); - ftDeviceNames.push_back(p[devIdx]->key); auto nrFTsinThisDevice = fts->getNrOfSixAxisForceTorqueSensors(); + if(nrFTsinThisDevice > 0) + { + ftSensorList.push(const_cast(*p[devIdx])); + ftDeviceNames.push_back(p[devIdx]->key); + } nrMASFTSensors += nrFTsinThisDevice; for (auto ftDx = 0; ftDx < nrFTsinThisDevice; ftDx++) { @@ -1717,7 +1721,7 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) // tempDeviceNames.push_back(p[devIdx]->key); } } - yDebug()<<"wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas"<poly->view(pAcc) ) { - if (pAcc->getNrOfThreeAxisLinearAccelerometers() != 1) - { - yError() << "WholeBodyDynamicsDevice MAS IMU ERROR- Nr acc should be 1"; - return false; - } - - std::string accName; - pAcc->getThreeAxisLinearAccelerometerName(0, accName); - if (accName != masAccName) + // Check if the MAS device is actually an IMU = contains one accelerometer + // and the accelerometer's name matches the one in the configuration file + if(pAcc->getNrOfThreeAxisLinearAccelerometers() == 1) { - yError() << "WholeBodyDynamicsDevice MAS IMU ERROR- acc name mismatch"; - return false; + std::string accName; + pAcc->getThreeAxisLinearAccelerometerName(0, accName); + if (accName == masAccName) + { + masAccInterface = pAcc; + noOfMASDevicesWithOneAcc++; + } } - - masAccInterface = pAcc; } IThreeAxisGyroscopes * pGyro{nullptr}; + // All MAS devices should satisfy this condition if( p[devIdx]->poly->view(pGyro) ) { - if (pGyro->getNrOfThreeAxisGyroscopes() != 1) - { - yError() << "WholeBodyDynamicsDevice MAS IMU ERROR- Nr gyro should be 1"; - return false; - } - - std::string gyroName; - pGyro->getThreeAxisGyroscopeName(0, gyroName); - if (gyroName != masGyroName) + // Check if the MAS device is actually an IMU = contains one gyroscope + // and the gyroscope's name matches the one in the configuration file + if(pGyro->getNrOfThreeAxisGyroscopes() == 1) { - yError() << "WholeBodyDynamicsDevice MAS IMU ERROR - gyro name mismatch"; - return false; + std::string gyroName; + pGyro->getThreeAxisGyroscopeName(0, gyroName); + if (gyroName == masGyroName) + { + masGyroInterface = pGyro; + noOfMASDevicesWithOneGyro++; + } } - - masGyroInterface = pGyro; } } + + if((noOfMASDevicesWithOneAcc == 1) && (noOfMASDevicesWithOneGyro == 1)) + { + yInfo() << "wholeBodyDynamics : Found one IMU multipleAnalogSensor device with accelerometer " << masAccName << " and gyroscope " << masGyroName; + } + else if((noOfMASDevicesWithOneAcc > 1) || (noOfMASDevicesWithOneGyro > 1)) + { + yError() << "wholeBodyDynamics : Found more than one IMU multipleAnalogSensor devices attached, you need to attach one and only one IMU."; + } + else if((noOfMASDevicesWithOneAcc < 1) || (noOfMASDevicesWithOneGyro < 1)) + { + yError() << "wholeBodyDynamics : Did not find one IMU multipleAnalogSensor devices attached, you need to attach one and only one IMU."; + yError() << "wholeBodyDynamics : In case you are trying to attach an IMU device of the type IGenericSensor, remove the group \"HW_USE_MAS_IMU\" from your config file."; + } } if (!settings.disableSensorReadCheckAtStartup) { // We try to read for a brief moment the sensors for two reasons: