From 03c32b2286cb05d1231a55f9cbccf46a1aee2331 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Thu, 18 Jan 2024 11:09:00 +0100 Subject: [PATCH] Fix after review --- src/imu/CMakeLists.txt | 18 ++++++------- src/imu/imu.cpp | 42 +++++++++++++------------------ src/imu/imu.h | 12 ++++----- suites/contexts/icub/test_imu.ini | 2 +- 4 files changed, 33 insertions(+), 41 deletions(-) diff --git a/src/imu/CMakeLists.txt b/src/imu/CMakeLists.txt index 915f347..6f6f394 100644 --- a/src/imu/CMakeLists.txt +++ b/src/imu/CMakeLists.txt @@ -1,22 +1,20 @@ -cmake_minimum_required(VERSION 3.5) -project(imu) +if(NOT DEFINED CMAKE_MINIMUM_REQUIRED_VERSION) + cmake_minimum_required(VERSION 3.5) +endif() add_definitions(-D_USE_MATH_DEFINES) -include_directories(${CMAKE_SOURCE_DIR} - ${RobotTestingFramework_INCLUDE_DIRS}) - -robottestingframework_add_plugin(${PROJECT_NAME} HEADERS ${PROJECT_NAME}.h - SOURCES ${PROJECT_NAME}.cpp) +robottestingframework_add_plugin(imu HEADERS imu.h + SOURCES imu.cpp) # add required libraries -target_link_libraries(${PROJECT_NAME} RobotTestingFramework::RTF +target_link_libraries(imu RobotTestingFramework::RTF RobotTestingFramework::RTF_dll YARP::YARP_robottestingframework iDynTree::idyntree-high-level iDynTree::idyntree-estimation) # set the installation options -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} +install(TARGETS imu + EXPORT imu COMPONENT runtime LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index a9307a0..3cc3140 100644 --- a/src/imu/imu.cpp +++ b/src/imu/imu.cpp @@ -7,6 +7,7 @@ #include #include +#include #include "imu.h" @@ -27,9 +28,11 @@ bool Imu::setup(yarp::os::Property& property) { robotName = property.find("robot").asString(); // robot name portName = property.find("port").asString(); // name of the port from which the data are streamed - modelPath = property.find("model").asString(); // urdf model path frameName = property.find("frame").asString(); // frame on which the sensor is attached sensorName = property.find("sensor").asString(); // sensor name within urdf + modelName = property.find("model").asString(); // urdf model path + yarp::os::ResourceFinder &rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + std::string modelAbsolutePath = rf.findFileByName(modelName); ROBOTTESTINGFRAMEWORK_TEST_REPORT("Running IMU test on "+robotName+"..."); @@ -37,9 +40,9 @@ bool Imu::setup(yarp::os::Property& property) { MASclientOptions.put("device", "multipleanalogsensorsclient"); MASclientOptions.put("remote", portName); MASclientOptions.put("local", "/imuTest/"+portName); - - MASclientDriver = new yarp::dev::PolyDriver(MASclientOptions); - + + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.open(MASclientOptions), "Unable to open the MAS client driver"); + yarp::os::Property controlBoardOptions; controlBoardOptions.put("device", "remotecontrolboardremapper"); yarp::os::Bottle axesNames; @@ -70,15 +73,15 @@ bool Imu::setup(yarp::os::Property& property) { controlBoardOptions.put("remoteControlBoards", remoteControlBoards.get(0)); controlBoardOptions.put("localPortPrefix", "/test"); - controlBoardDriver = new yarp::dev::PolyDriver(controlBoardOptions); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver->isValid(), "Device driver cannot be opened"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver->view(iorientation), "Unable to open orientation interface"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver->isValid(), "Device driver cannot be opened"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver->view(ipos), "Unable to open position control interface"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver->view(ienc), "Unable to open encoder interface"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver->view(iaxes), "Unable to open axes interface"); - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver->view(ilim), "Unable to open limits interface"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.open(controlBoardOptions), "Unable to open the controlBoard driver"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.isValid(), "Device driver cannot be opened"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.view(iorientation), "Unable to open orientation interface"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.isValid(), "Device driver cannot be opened"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.view(ipos), "Unable to open position control interface"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.view(ienc), "Unable to open encoder interface"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.view(iaxes), "Unable to open axes interface"); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.view(ilim), "Unable to open limits interface"); outputPort.open("/test-imu/out"); ROBOTTESTINGFRAMEWORK_TEST_REPORT("Opening port "+outputPort.getName()+"..."); @@ -93,7 +96,7 @@ bool Imu::setup(yarp::os::Property& property) { axis.push_back(axisName); } - ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(model.loadReducedModelFromFile(modelPath.c_str(), axis), Asserter::format("Cannot load model %s", modelPath.c_str())); + ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(model.loadReducedModelFromFile(modelAbsolutePath.c_str(), axis), Asserter::format("Cannot load model from %s", modelAbsolutePath.c_str())); kinDynComp.loadRobotModel(model.model()); iDynTree::Vector3 baseLinkOrientationRad; @@ -136,17 +139,8 @@ void Imu::tearDown() { outputPort.interrupt(); outputPort.close(); - if(MASclientDriver) - { - delete MASclientDriver; - MASclientDriver = 0; - } - - if(controlBoardDriver) - { - delete controlBoardDriver; - controlBoardDriver = 0; - } + controlBoardDriver.close(); + MASclientDriver.close(); } void Imu::run() { diff --git a/src/imu/imu.h b/src/imu/imu.h index f874f81..e1d98c7 100644 --- a/src/imu/imu.h +++ b/src/imu/imu.h @@ -1,5 +1,5 @@ -#ifndef _IMU_H_ -#define _IMU_H_ +#ifndef IMU_H +#define IMU_H #include #include @@ -24,12 +24,12 @@ class Imu : public yarp::robottestingframework::TestCase { private: std::string robotName; std::string portName; - std::string modelPath; + std::string modelName; std::string frameName; std::string sensorName; - yarp::dev::PolyDriver *MASclientDriver; - yarp::dev::PolyDriver *controlBoardDriver; + yarp::dev::PolyDriver MASclientDriver; + yarp::dev::PolyDriver controlBoardDriver; yarp::dev::IOrientationSensors* iorientation; yarp::dev::IPositionControl* ipos; yarp::dev::IEncoders* ienc; @@ -59,4 +59,4 @@ class Imu : public yarp::robottestingframework::TestCase { bool moveJoint(int ax, double pos); }; -#endif //_IMU_H_ \ No newline at end of file +#endif //IMU_H \ No newline at end of file diff --git a/suites/contexts/icub/test_imu.ini b/suites/contexts/icub/test_imu.ini index 33bfe77..f21d60a 100644 --- a/suites/contexts/icub/test_imu.ini +++ b/suites/contexts/icub/test_imu.ini @@ -1,5 +1,5 @@ robot ${robotname} -model /path/to/your/robot/model.urdf +model model.urdf port /${robotname}/head/inertials sensor head_imu_0 frame head_imu_0