mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Cleanup unit tests
- Unify move_to.launch.py and test_task_model.launch.py - Rename them to test.launch.py as they are independent of the executable - Move Node creation to the fixture constructor - Replace Task::setRobotModel(loadModel()) with Task::loadRobotModel()
This commit is contained in:
parent
a9ddbe1998
commit
bb047c894c
@ -54,8 +54,8 @@ if (BUILD_TESTING)
|
||||
mtc_add_gmock(test_cost_queue.cpp)
|
||||
mtc_add_gmock(test_interface_state.cpp)
|
||||
|
||||
mtc_add_gtest(test_move_to.cpp move_to.launch.py)
|
||||
mtc_add_gtest(test_move_relative.cpp move_to.launch.py)
|
||||
mtc_add_gtest(test_move_to.cpp test.launch.py)
|
||||
mtc_add_gtest(test_move_relative.cpp test.launch.py)
|
||||
mtc_add_gtest(test_pipeline_planner.cpp)
|
||||
|
||||
# building these integration tests works without moveit config packages
|
||||
|
||||
@ -17,8 +17,3 @@ RobotModelPtr getModel() {
|
||||
builder.addEndEffector("eef", "link2", "group", "eef_group");
|
||||
return builder.build();
|
||||
}
|
||||
|
||||
moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node) {
|
||||
robot_model_loader::RobotModelLoader loader(node);
|
||||
return loader.getModel();
|
||||
}
|
||||
|
||||
@ -11,6 +11,3 @@ MOVEIT_CLASS_FORWARD(RobotModel);
|
||||
|
||||
// get a hard-coded model
|
||||
moveit::core::RobotModelPtr getModel();
|
||||
|
||||
// load a model from robot_description
|
||||
moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node);
|
||||
|
||||
@ -26,18 +26,18 @@ struct PandaMoveRelative : public testing::Test
|
||||
Task t;
|
||||
stages::MoveRelative* move;
|
||||
PlanningScenePtr scene;
|
||||
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_relative");
|
||||
rclcpp::Node::SharedPtr node;
|
||||
|
||||
const JointModelGroup* group;
|
||||
|
||||
PandaMoveRelative() {
|
||||
t.setRobotModel(loadModel(node));
|
||||
t.loadRobotModel(rclcpp::Node::make_shared("panda_move_relative"));
|
||||
|
||||
group = t.getRobotModel()->getJointModelGroup("panda_arm");
|
||||
|
||||
scene = std::make_shared<PlanningScene>(t.getRobotModel());
|
||||
scene->getCurrentStateNonConst().setToDefaultValues();
|
||||
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
|
||||
scene->getCurrentStateNonConst().setToDefaultValues(group, "ready");
|
||||
t.add(std::make_unique<stages::FixedState>("start", scene));
|
||||
|
||||
auto move_relative = std::make_unique<stages::MoveRelative>("move", std::make_shared<solvers::CartesianPath>());
|
||||
|
||||
@ -34,20 +34,22 @@ struct PandaMoveTo : public testing::Test
|
||||
Task t;
|
||||
stages::MoveTo* move_to;
|
||||
PlanningScenePtr scene;
|
||||
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_to");
|
||||
rclcpp::Node::SharedPtr node;
|
||||
|
||||
PandaMoveTo() {
|
||||
node = rclcpp::Node::make_shared("panda_move_to");
|
||||
t.loadRobotModel(node);
|
||||
|
||||
auto group = t.getRobotModel()->getJointModelGroup("panda_arm");
|
||||
|
||||
scene = std::make_shared<PlanningScene>(t.getRobotModel());
|
||||
scene->getCurrentStateNonConst().setToDefaultValues();
|
||||
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"),
|
||||
"extended");
|
||||
scene->getCurrentStateNonConst().setToDefaultValues(group, "extended");
|
||||
t.add(std::make_unique<stages::FixedState>("start", scene));
|
||||
|
||||
auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
|
||||
move_to = move.get();
|
||||
move_to->setGroup("panda_arm");
|
||||
move_to->setGroup(group->getName());
|
||||
t.add(std::move(move));
|
||||
}
|
||||
};
|
||||
@ -162,7 +164,7 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
|
||||
// will strongly deviate from the joint-space goal.
|
||||
TEST(Panda, connectCartesianBranchesFails) {
|
||||
Task t;
|
||||
t.setRobotModel(loadModel(rclcpp::Node::make_shared("panda_move_to")));
|
||||
t.loadRobotModel(rclcpp::Node::make_shared("panda_move_to"));
|
||||
auto scene = std::make_shared<PlanningScene>(t.getRobotModel());
|
||||
scene->getCurrentStateNonConst().setToDefaultValues();
|
||||
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
|
||||
|
||||
@ -19,6 +19,6 @@ if (BUILD_TESTING)
|
||||
ament_add_gtest_executable(${PROJECT_NAME}-test-task_model test_task_model.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}-test-task_model
|
||||
motion_planning_tasks_rviz_plugin)
|
||||
add_launch_test(test_task_model.launch.py
|
||||
ARGS "test_binary_dir:=$<TARGET_FILE_DIR:${PROJECT_NAME}-test-task_model>")
|
||||
add_launch_test(test.launch.py
|
||||
ARGS "test_binary:=$<TARGET_FILE:${PROJECT_NAME}-test-task_model>")
|
||||
endif()
|
||||
|
||||
@ -13,36 +13,29 @@ import pytest
|
||||
|
||||
@pytest.mark.launch_test
|
||||
def generate_test_description():
|
||||
test_task_model = GTest(
|
||||
path=[
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
LaunchConfiguration("test_binary_dir"),
|
||||
"moveit_task_constructor_visualization-test-task_model",
|
||||
]
|
||||
)
|
||||
],
|
||||
test_exec = GTest(
|
||||
path=[LaunchConfiguration("test_binary")],
|
||||
output="screen",
|
||||
)
|
||||
return (
|
||||
LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument(
|
||||
name="test_binary_dir",
|
||||
description="Binary directory of package containing test executables",
|
||||
name="test_binary",
|
||||
description="Test executable",
|
||||
),
|
||||
test_task_model,
|
||||
test_exec,
|
||||
KeepAliveProc(),
|
||||
ReadyToTest(),
|
||||
]
|
||||
),
|
||||
{"test_task_model": test_task_model},
|
||||
{"test_exec": test_exec},
|
||||
)
|
||||
|
||||
|
||||
class TestTerminatingProcessStops(unittest.TestCase):
|
||||
def test_gtest_run_complete(self, proc_info, test_task_model):
|
||||
proc_info.assertWaitForShutdown(process=test_task_model, timeout=4000.0)
|
||||
def test_gtest_run_complete(self, proc_info, test_exec):
|
||||
proc_info.assertWaitForShutdown(process=test_exec, timeout=4000.0)
|
||||
|
||||
|
||||
@launch_testing.post_shutdown_test()
|
||||
Loading…
Reference in New Issue
Block a user