From 9e5e098339e0bd4cd1bd3bca5c3f034d3d5d9039 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 24 Mar 2018 16:18:57 +0100 Subject: [PATCH] unittests - provide simple hard-coded robot model - test ComputeIK::init() --- core/test/CMakeLists.txt | 6 +- core/test/models.cpp | 194 +++++++++++++++++++++++++++++++++++++++ core/test/models.h | 13 +++ core/test/test_stage.cpp | 60 +++++++++--- 4 files changed, 258 insertions(+), 15 deletions(-) create mode 100644 core/test/models.cpp create mode 100644 core/test/models.h diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 9f011577..a5920600 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -6,11 +6,11 @@ if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_library(gtest_utils gtest_value_printers.cpp) + add_library(gtest_utils gtest_value_printers.cpp models.cpp) target_link_libraries(gtest_utils ${PROJECT_NAME}) - add_rostest_gtest(${PROJECT_NAME}-test-stage test_stage.launch test_stage.cpp) - target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} gtest_main) + catkin_add_gtest(${PROJECT_NAME}-test-stage test_stage.cpp) + target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp) target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_utils gtest_main) diff --git a/core/test/models.cpp b/core/test/models.cpp new file mode 100644 index 00000000..683c651a --- /dev/null +++ b/core/test/models.cpp @@ -0,0 +1,194 @@ +#include "models.h" +#include +#include +#include + +using namespace moveit::core; +namespace { + +const std::string R_MODEL0 = + "" + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + "" + " " + " " + " " + " " + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + "" + " " + " " + " " + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + " " + " " + " " + " " + " " + " " + " " + " " + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + ""; + +const std::string S_MODEL0 = + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + ""; + +} + +RobotModelPtr getModel() +{ + urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(R_MODEL0); + srdf::ModelSharedPtr srdf_model(new srdf::Model()); + srdf_model->initString(*urdf_model, S_MODEL0); + return RobotModelPtr(new RobotModel(urdf_model, srdf_model)); +} + +moveit::core::RobotModelPtr loadModel() +{ + robot_model_loader::RobotModelLoader loader; + return loader.getModel(); +} diff --git a/core/test/models.h b/core/test/models.h new file mode 100644 index 00000000..91ed5e34 --- /dev/null +++ b/core/test/models.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(RobotModel) +} } + +// get a hard-coded model +moveit::core::RobotModelPtr getModel(); + +// load a model from robot_description +moveit::core::RobotModelPtr loadModel(); diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 0da92d5a..fbcb69b3 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -1,10 +1,12 @@ -#include -#include -#include -#include +#include "models.h" +#include +#include +#include +#include + +#include #include -#include using namespace moveit::task_constructor; @@ -15,27 +17,31 @@ class GeneratorMockup : public Generator { public: GeneratorMockup() : Generator("generator") { - robot_model_loader::RobotModelLoader loader; - ps.reset(new planning_scene::PlanningScene(loader.getModel())); - prev.reset(new Interface); next.reset(new Interface); pimpl()->setPrevEnds(prev); pimpl()->setNextStarts(next); } + + void init(const moveit::core::RobotModelConstPtr &robot_model) { + ps.reset((new planning_scene::PlanningScene(robot_model))); + } + bool canCompute() const override { return true; } bool compute() override { - spawn(InterfaceState(ps), 0.0); + InterfaceState state(ps); + state.properties().set("target_pose", geometry_msgs::PoseStamped()); + spawn(std::move(state), 0.0); return true; } }; TEST(Stage, registerCallbacks) { - int argc = 0; - char *argv = nullptr; - ros::init(argc, &argv, "testLocalTaskModel"); + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); GeneratorMockup g; + g.init(getModel()); + uint called = 0; auto cb = [&called](const SolutionBase &s) { ++called; @@ -50,3 +56,33 @@ TEST(Stage, registerCallbacks) { g.compute(); EXPECT_EQ(called, 1); } + +TEST(ComputeIK, init) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + + auto g = std::make_unique(); + stages::ComputeIK ik("ik", std::move(g)); + moveit::core::RobotModelPtr robot_model = getModel(); + auto& props = ik.properties(); + + // neither eef nor group defined should not throw + EXPECT_NO_THROW(ik.init(robot_model)); + + // invalid eef should throw + props.set("eef", std::string("undefined")); + EXPECT_THROW(ik.init(robot_model), InitStageException); + + // valid eef should not throw + props.set("eef", std::string("eef")); + EXPECT_NO_THROW(ik.init(robot_model)); + + props.set("eef", boost::any()); + + // invalid group should throw + props.set("group", std::string("undefined")); + EXPECT_THROW(ik.init(robot_model), InitStageException); + + // valid group should not throw + props.set("group", std::string("base_from_base_to_tip")); + EXPECT_NO_THROW(ik.init(robot_model)); +}