diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index a9798562b1fbc6922ca945e5073e6b9248e361f8..1143aa8759b44968ed74006a9328f43438b7ead2 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -80,8 +80,12 @@ IF(URDFDOM_FOUND) IF(BUILD_TESTS_WITH_HPP) ADD_OPTIONAL_DEPENDENCY("hpp-model-urdf") IF(HPP_MODEL_URDF_FOUND) + ADD_OPTIONAL_DEPENDENCY("romeo_description >= 0.2") PKG_CONFIG_USE_DEPENDENCY(geom hpp-model-urdf) ADD_TEST_CFLAGS(geom "-DWITH_HPP_MODEL_URDF") + IF(ROMEO_DESCRIPTION_FOUND) + ADD_TEST_CFLAGS(geom '-DROMEO_DESCRIPTION_MODEL_DIR=\\\"${ROMEO_DESCRIPTION_DATAROOTDIR}\\\"') + ENDIF(ROMEO_DESCRIPTION_FOUND) ENDIF(HPP_MODEL_URDF_FOUND) ENDIF(BUILD_TESTS_WITH_HPP) ENDIF(HPP_FCL_FOUND) diff --git a/unittest/geom.cpp b/unittest/geom.cpp index b997f0fd43c2b21d52f972a242a7a25b2cf19c4e..434cd996f2c8b947e156926e92f5a5cc92bd4b59 100644 --- a/unittest/geom.cpp +++ b/unittest/geom.cpp @@ -227,11 +227,16 @@ BOOST_AUTO_TEST_CASE ( loading_model ) #if defined(WITH_URDFDOM) && defined(WITH_HPP_FCL) BOOST_AUTO_TEST_CASE (radius) { - // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino +#ifdef ROMEO_DESCRIPTION_MODEL_DIR + std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf"; + std::vector < std::string > package_dirs; + package_dirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR); +#else std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf"; std::vector < std::string > package_dirs; std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/"; package_dirs.push_back(meshDir); +#endif // ROMEO_DESCRIPTION_MODEL_DIR se3::Model model; se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model); @@ -243,7 +248,7 @@ BOOST_AUTO_TEST_CASE (radius) se3::computeBodyRadius(model, geom, geomData); BOOST_FOREACH( double radius, geomData.radius) BOOST_CHECK(radius>=0.); -#ifdef WITH_HPP_MODEL_URDF +#if defined(WITH_HPP_MODEL_URDF) && defined(ROMEO_DESCRIPTION_MODEL_DIR) /// ************* HPP ************* /// /// ********************************* /// using hpp::model::JointVector_t; @@ -254,7 +259,7 @@ BOOST_AUTO_TEST_CASE (radius) hpp::model::HumanoidRobotPtr_t humanoidRobot = hpp::model::HumanoidRobot::create ("romeo"); loadHumanoidPathPlanerModel(humanoidRobot, "freeflyer", - "romeo_pinocchio", "romeo", + "romeo_description", "romeo_small", ""); BOOST_CHECK_MESSAGE(model.nq == humanoidRobot->configSize () , "Pinocchio model & HPP model config sizes are not the same "); @@ -281,7 +286,7 @@ BOOST_AUTO_TEST_CASE (radius) } #endif // if defined(WITH_URDFDOM) && defined(WITH_HPP_FCL) -#ifdef WITH_HPP_MODEL_URDF +#if defined(WITH_HPP_MODEL_URDF) && defined(ROMEO_DESCRIPTION_MODEL_DIR) BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions ) { typedef se3::Model Model; @@ -301,10 +306,9 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions ) /// ********************************* /// // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino - std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf"; + std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf"; std::vector < std::string > package_dirs; - std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/"; - package_dirs.push_back(meshDir); + package_dirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR); se3::Model model; se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model); @@ -338,7 +342,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions ) hpp::model::HumanoidRobotPtr_t humanoidRobot = hpp::model::HumanoidRobot::create ("romeo"); loadHumanoidPathPlanerModel(humanoidRobot, "freeflyer", - "romeo_pinocchio", "romeo", + "romeo_description", "romeo_small", ""); BOOST_CHECK_MESSAGE(model.nq == humanoidRobot->configSize () , "Pinocchio model & HPP model config sizes are not the same "); @@ -409,10 +413,9 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance) /// ********************************* /// // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino - std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf"; + std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf"; std::vector < std::string > package_dirs; - std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/"; - package_dirs.push_back(meshDir); + package_dirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR); se3::Model model; se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model); @@ -446,7 +449,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance) hpp::model::HumanoidRobotPtr_t humanoidRobot = hpp::model::HumanoidRobot::create ("romeo"); loadHumanoidPathPlanerModel(humanoidRobot, "freeflyer", - "romeo_pinocchio", "romeo", + "romeo_description", "romeo_small", ""); BOOST_CHECK_MESSAGE(model.nq == humanoidRobot->configSize () , "Pinocchio model & HPP model config sizes are not the same ");