diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index 2c2d95c..064d40b 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -98,9 +98,14 @@ bool Imu::setup(yarp::os::Property& property) if(inputSensorsList->size() == 0 || inputSensorsList->get(0).asString() == "all") { ROBOTTESTINGFRAMEWORK_TEST_REPORT("Testing all the IMUs available..."); - sensorsNamesList.addString("l_arm_ft"); - sensorsNamesList.addString("r_arm_ft"); - sensorsNamesList.addString("head_imu_0"); + + yarp::dev::IOrientationSensors* ior; + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.view(ior), "Unable to open the orientation interface"); + for(int sensorIndex = 0; sensorIndex < ior->getNrOfOrientationSensors(); sensorIndex++) + { + ior->getOrientationSensorName(sensorIndex, sensorName); + sensorsNamesList.addString(sensorName); + } } else diff --git a/suites/contexts/icub/test_imu.ini b/suites/contexts/icub/test_imu.ini index 0b0425d..0d9cbc3 100644 --- a/suites/contexts/icub/test_imu.ini +++ b/suites/contexts/icub/test_imu.ini @@ -3,5 +3,6 @@ model model.urdf port /${robotname}/alljoints/inertials remoteControlBoards ("torso", "head", "left_arm", "right_arm") axesNames ("torso_pitch", "torso_roll", "torso_yaw", "neck_pitch", "neck_roll", "neck_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw") -sensorsList ("all") -maxError 0.1 +; sensorsList ("head_imu_0", "l_arm_ft", "r_arm_ft") +sensorsList ("all") +maxError 0.1