From f99d03386ad8751fb0ade71c09a9810019a950ad Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 30 Dec 2020 11:07:57 +0100 Subject: [PATCH] Use RobotModelBuilder to simplify tests (#225) * Simplify RobotModel definition using RobotModelBuilder * Silent RobotModel errors once in models.cpp --- core/test/CMakeLists.txt | 4 +- core/test/models.cpp | 189 ++--------------------------- core/test/test_container.cpp | 15 +-- core/test/test_cost_terms.cpp | 8 -- core/test/test_interface_state.cpp | 10 +- core/test/test_serial.cpp | 12 +- core/test/test_stage.cpp | 12 +- 7 files changed, 23 insertions(+), 227 deletions(-) diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index b4a45460..99836d13 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -16,7 +16,7 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) catkin_add_gtest(${PROJECT_NAME}-test-serial test_serial.cpp) - target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_main) + target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp) target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main) @@ -25,7 +25,7 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main) catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp) - target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main) + target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_utils gtest_main) catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp) target_link_libraries(${PROJECT_NAME}-test-cost_terms ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) diff --git a/core/test/models.cpp b/core/test/models.cpp index c0614c64..9d514b1c 100644 --- a/core/test/models.cpp +++ b/core/test/models.cpp @@ -1,187 +1,20 @@ #include "models.h" -#include +#include #include -#include using namespace moveit::core; -namespace { - -const std::string R_MODEL0 = "" - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - ""; - -const std::string S_MODEL0 = - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - "" - ""; -} // namespace 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)); + // suppress RobotModel errors and warnings + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME ".moveit_core.robot_model", ros::console::levels::Fatal); + + // create dummy robot model + moveit::core::RobotModelBuilder builder("robot", "base"); + builder.addChain("base->link1->link2->tip", "continuous"); + builder.addGroupChain("base", "link2", "group"); + builder.addGroupChain("link2", "tip", "eef_group"); + builder.addEndEffector("eef", "link2", "group", "eef_group"); + return builder.build(); } moveit::core::RobotModelPtr loadModel() { diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 0e9957d3..688e944a 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -3,8 +3,8 @@ #include #include #include -#include +#include "models.h" #include "gtest_value_printers.h" #include #include @@ -686,11 +686,7 @@ TEST(Task, move) { } TEST(Task, reuse) { - // create dummy robot model - moveit::core::RobotModelBuilder builder("robot", "base"); - builder.addChain("base->a->b->c", "continuous"); - builder.addGroupChain("base", "c", "group"); - moveit::core::RobotModelConstPtr robot_model = builder.build(); + moveit::core::RobotModelConstPtr robot_model = getModel(); Task t("first"); t.setRobotModel(robot_model); @@ -725,13 +721,8 @@ TEST(Task, reuse) { TEST(Task, timeout) { MOCK_ID = 0; - // create dummy robot model - moveit::core::RobotModelBuilder builder("robot", "base"); - builder.addChain("base->a->b->c", "continuous"); - builder.addGroupChain("base", "c", "group"); - Task t; - t.setRobotModel(builder.build()); + t.setRobotModel(getModel()); auto timeout = std::chrono::milliseconds(10); t.add(std::make_unique(100)); // allow up to 100 solutions spawned diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index ee62f0c8..5eb1a056 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -172,7 +172,6 @@ public: }; TEST(CostTerm, SetLambdaCostTerm) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); const moveit::core::RobotModelConstPtr robot{ getModel() }; Standalone container(robot); @@ -198,7 +197,6 @@ TEST(CostTerm, SetLambdaCostTerm) { } TEST(CostTerm, CostOverwrite) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); const moveit::core::RobotModelConstPtr robot{ getModel() }; Standalone container(robot); @@ -215,7 +213,6 @@ TEST(CostTerm, CostOverwrite) { } TEST(CostTerm, StageTypes) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; Standalone container(robot); @@ -237,7 +234,6 @@ TEST(CostTerm, StageTypes) { } TEST(CostTerm, PassThroughUsesCost) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; Standalone container(robot); @@ -253,7 +249,6 @@ TEST(CostTerm, PassThroughUsesCost) { } TEST(CostTerm, PassThroughOverwritesCost) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; Standalone container(robot); @@ -271,7 +266,6 @@ TEST(CostTerm, PassThroughOverwritesCost) { } TEST(CostTerm, PassThroughCanModifyCost) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; Standalone container(robot); @@ -287,7 +281,6 @@ TEST(CostTerm, PassThroughCanModifyCost) { } TEST(CostTerm, CompositeSolutions) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); Standalone container{ getModel() }; { @@ -330,7 +323,6 @@ TEST(CostTerm, CompositeSolutions) { } TEST(CostTerm, CompositeSolutionsContainerCost) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); Standalone container{ getModel() }; auto s1{ std::make_unique() }; diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 0ea85747..0d2135f7 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -1,7 +1,8 @@ #include -#include +#include #include +#include "models.h" #include #include #include @@ -28,13 +29,6 @@ TEST(InterfaceStatePriority, compare) { EXPECT_TRUE(Prio(0, inf) > Prio(0, 0)); } -moveit::core::RobotModelConstPtr getModel() { - ros::console::set_logger_level("ros.moveit_core.robot_model", ros::console::levels::Error); - moveit::core::RobotModelBuilder builder("robot", "base"); - builder.addChain("base->tip", "continuous"); - return builder.build(); -} - using Prio = InterfaceState::Priority; // Interface that also stores passed states diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index 901b7547..526367ff 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -5,6 +5,7 @@ #include #include +#include "models.h" #include #include #include @@ -107,7 +108,7 @@ struct Connect : stages::Connect static GroupPlannerVector getPlanners() { auto planner = std::make_shared(); - return { { "group1", planner }, { "group2", planner } }; + return { { "group", planner }, { "eef_group", planner } }; } Connect(std::initializer_list costs = {}, bool enforce_sequential = false) @@ -129,15 +130,6 @@ unsigned int ForwardMockup::id_ = 0; unsigned int BackwardMockup::id_ = 0; unsigned int Connect::id_ = 0; -moveit::core::RobotModelConstPtr getModel() { - ros::console::set_logger_level("ros.moveit_core.robot_model", ros::console::levels::Error); - moveit::core::RobotModelBuilder builder("robot", "base"); - builder.addChain("base->a->b->c", "continuous"); - builder.addGroupChain("base", "b", "group1"); - builder.addGroupChain("b", "c", "group2"); - return builder.build(); -} - // https://github.com/ros-planning/moveit_task_constructor/issues/182 TEST(ConnectConnect, SuccSucc) { GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index e39d450b..2dd7b2df 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -47,8 +47,6 @@ public: }; TEST(Stage, registerCallbacks) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); - GeneratorMockup g; g.init(getModel()); @@ -68,8 +66,6 @@ TEST(Stage, registerCallbacks) { } 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(); @@ -93,13 +89,11 @@ TEST(ComputeIK, init) { EXPECT_THROW(ik.init(robot_model), InitStageException); // valid group should not throw - props.set("group", std::string("base_from_base_to_tip")); + props.set("group", std::string("group")); EXPECT_NO_THROW(ik.init(robot_model)); } TEST(ModifyPlanningScene, allowCollisions) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); - auto s = std::make_unique(); std::string first = "foo", second = "boom"; s->allowCollisions(first, second, true); @@ -168,13 +162,13 @@ TEST(Connect, compatible) { // attached objects other = scene->diff(); - attachObject(*scene, "object", "base_link", true); + attachObject(*scene, "object", "tip", true); EXPECT_FALSE(connect.compatible(scene, other)) << "detached and attached object"; other = scene->diff(); EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes, attached object"; spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 }); - attachObject(*other, "object", "base_link", true); + attachObject(*other, "object", "tip", true); EXPECT_FALSE(connect.compatible(scene, other)) << "different pose"; }