mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Simplify tests
This commit is contained in:
parent
4e09d78239
commit
dc8cd34b86
@ -138,234 +138,184 @@ unsigned int ForwardMockup::id_ = 0;
|
||||
unsigned int BackwardMockup::id_ = 0;
|
||||
unsigned int Connect::id_ = 0;
|
||||
|
||||
void resetIds() {
|
||||
struct TestBase : public testing::Test
|
||||
{
|
||||
Task task;
|
||||
TestBase() {
|
||||
resetIds();
|
||||
task.setRobotModel(getModel());
|
||||
}
|
||||
|
||||
void resetIds() {
|
||||
GeneratorMockup::id_ = 0;
|
||||
ForwardMockup::id_ = 0;
|
||||
BackwardMockup::id_ = 0;
|
||||
Connect::id_ = 0;
|
||||
}
|
||||
|
||||
template <typename C, typename S>
|
||||
auto add(C& container, S* stage) -> S* {
|
||||
Stage::pointer ptr{ stage };
|
||||
container.add(std::move(ptr));
|
||||
}
|
||||
template <typename C, typename S>
|
||||
auto add(C& container, S* stage) -> S* {
|
||||
container.add(Stage::pointer(stage));
|
||||
return stage;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
using ConnectConnect = TestBase;
|
||||
// https://github.com/ros-planning/moveit_task_constructor/issues/182
|
||||
TEST(ConnectConnect, SuccSucc) {
|
||||
resetIds();
|
||||
TEST_F(ConnectConnect, SuccSucc) {
|
||||
add(task, new GeneratorMockup({ 1, 2, 3 }));
|
||||
add(task, new Connect());
|
||||
add(task, new GeneratorMockup({ 10, 20 }));
|
||||
add(task, new Connect());
|
||||
add(task, new GeneratorMockup());
|
||||
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
t.add(Stage::pointer(new GeneratorMockup({ 1, 2, 3 })));
|
||||
t.add(Stage::pointer(new Connect()));
|
||||
t.add(Stage::pointer(new GeneratorMockup({ 10, 20 })));
|
||||
t.add(Stage::pointer(new Connect()));
|
||||
t.add(Stage::pointer(new GeneratorMockup()));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
ASSERT_EQ(t.solutions().size(), 3u * 2u);
|
||||
EXPECT_TRUE(task.plan());
|
||||
ASSERT_EQ(task.solutions().size(), 3u * 2u);
|
||||
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
|
||||
auto expected_cost = expected_costs.begin();
|
||||
for (const auto& s : t.solutions()) {
|
||||
for (const auto& s : task.solutions()) {
|
||||
EXPECT_EQ(s->cost(), *expected_cost);
|
||||
++expected_cost;
|
||||
}
|
||||
}
|
||||
|
||||
// https://github.com/ros-planning/moveit_task_constructor/issues/218
|
||||
TEST(ConnectConnect, FailSucc) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
t.add(Stage::pointer(new GeneratorMockup()));
|
||||
t.add(Stage::pointer(new Connect({ inf }, true)));
|
||||
t.add(Stage::pointer(new GeneratorMockup()));
|
||||
t.add(Stage::pointer(new Connect()));
|
||||
t.add(Stage::pointer(new GeneratorMockup()));
|
||||
t.add(Stage::pointer(new ForwardDummy()));
|
||||
TEST_F(ConnectConnect, FailSucc) {
|
||||
add(task, new GeneratorMockup());
|
||||
add(task, new Connect({ inf }, true));
|
||||
add(task, new GeneratorMockup());
|
||||
add(task, new Connect());
|
||||
add(task, new GeneratorMockup());
|
||||
add(task, new ForwardDummy());
|
||||
|
||||
EXPECT_FALSE(t.plan());
|
||||
EXPECT_FALSE(task.plan());
|
||||
}
|
||||
|
||||
TEST(Pruning, PropagatorFailure) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
BackwardMockup* b;
|
||||
t.add(Stage::pointer(b = new BackwardMockup()));
|
||||
t.add(Stage::pointer(new GeneratorMockup({ 0 })));
|
||||
t.add(Stage::pointer(new ForwardMockup({ inf })));
|
||||
using Pruning = TestBase;
|
||||
TEST_F(Pruning, PropagatorFailure) {
|
||||
auto back = add(task, new BackwardMockup());
|
||||
add(task, new GeneratorMockup({ 0 }));
|
||||
add(task, new ForwardMockup({ inf }));
|
||||
|
||||
EXPECT_FALSE(t.plan());
|
||||
|
||||
ASSERT_EQ(t.solutions().size(), 0);
|
||||
EXPECT_FALSE(task.plan());
|
||||
ASSERT_EQ(task.solutions().size(), 0);
|
||||
// ForwardMockup fails, so the backward stage should never compute
|
||||
EXPECT_EQ(b->calls_, 0);
|
||||
EXPECT_EQ(back->calls_, 0);
|
||||
}
|
||||
|
||||
TEST(Pruning, PruningMultiForward) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
|
||||
t.add(Stage::pointer(new BackwardMockup()));
|
||||
t.add(Stage::pointer(new BackwardMockup()));
|
||||
t.add(Stage::pointer(new GeneratorMockup()));
|
||||
TEST_F(Pruning, PruningMultiForward) {
|
||||
add(task, new BackwardMockup());
|
||||
add(task, new BackwardMockup());
|
||||
add(task, new GeneratorMockup());
|
||||
// spawn two solutions for the only incoming state
|
||||
t.add(Stage::pointer(new ForwardMockup({ 0, 0 }, 2)));
|
||||
add(task, new ForwardMockup({ 0, 0 }, 2));
|
||||
// fail to extend the second solution
|
||||
t.add(Stage::pointer(new ForwardMockup({ 0, inf })));
|
||||
add(task, new ForwardMockup({ 0, inf }));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_TRUE(task.plan());
|
||||
|
||||
// the second (infeasible) solution in the last stage must not disable
|
||||
// the earlier partial solution just because they share stage solutions
|
||||
ASSERT_EQ(t.solutions().size(), 1);
|
||||
EXPECT_EQ((*t.solutions().begin())->cost(), 0u);
|
||||
ASSERT_EQ(task.solutions().size(), 1);
|
||||
EXPECT_EQ((*task.solutions().begin())->cost(), 0u);
|
||||
}
|
||||
|
||||
TEST(Pruning, ConnectConnectForward) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
Connect *c1, *c2;
|
||||
t.add(Stage::pointer(new GeneratorMockup()));
|
||||
t.add(Stage::pointer(c1 = new Connect({ inf, 0 }))); // 1st attempt is a failure
|
||||
t.add(Stage::pointer(new GeneratorMockup({ 0, 10, 20 })));
|
||||
t.add(Stage::pointer(new ForwardMockup()));
|
||||
t.add(Stage::pointer(c2 = new Connect()));
|
||||
t.add(Stage::pointer(new GeneratorMockup({ 1, 2, 3 })));
|
||||
TEST_F(Pruning, ConnectConnectForward) {
|
||||
add(task, new GeneratorMockup());
|
||||
auto c1 = add(task, new Connect({ inf, 0 })); // 1st attempt is a failue
|
||||
add(task, new GeneratorMockup({ 0, 10, 20 }));
|
||||
add(task, new ForwardMockup());
|
||||
auto c2 = add(task, new Connect());
|
||||
add(task, new GeneratorMockup({ 1, 2, 3 }));
|
||||
|
||||
t.plan();
|
||||
task.plan();
|
||||
|
||||
ASSERT_EQ(t.solutions().size(), 3u * 2u);
|
||||
ASSERT_EQ(task.solutions().size(), 3u * 2u);
|
||||
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
|
||||
auto expected_cost = expected_costs.begin();
|
||||
for (const auto& s : t.solutions()) {
|
||||
for (const auto& s : task.solutions()) {
|
||||
EXPECT_EQ(s->cost(), *expected_cost);
|
||||
++expected_cost;
|
||||
}
|
||||
EXPECT_EQ(c1->calls_, 3u);
|
||||
EXPECT_EQ(c2->calls_, 6u);
|
||||
EXPECT_EQ(c2->calls_, 6u); // expect 6 instead of 9 calls
|
||||
}
|
||||
|
||||
TEST(Pruning, ConnectConnectBackward) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
Connect *c1, *c2;
|
||||
t.add(Stage::pointer(new GeneratorMockup({ 1, 2, 3 })));
|
||||
t.add(Stage::pointer(c1 = new Connect()));
|
||||
t.add(Stage::pointer(new BackwardMockup()));
|
||||
t.add(Stage::pointer(new GeneratorMockup({ 0, inf, 10, 20 }))); // 2nd is a dummy to postpone creation of 3rd
|
||||
t.add(Stage::pointer(c2 = new Connect({ inf, 0 }))); // 1st attempt is a failure
|
||||
t.add(Stage::pointer(new GeneratorMockup()));
|
||||
TEST_F(Pruning, ConnectConnectBackward) {
|
||||
add(task, new GeneratorMockup({ 1, 2, 3 }));
|
||||
auto c1 = add(task, new Connect());
|
||||
add(task, new BackwardMockup());
|
||||
add(task, new GeneratorMockup({ 0, inf, 10, 20 })); // 2nd is a dummy to postpone creation of 3rd
|
||||
auto c2 = add(task, new Connect({ inf, 0 })); // 1st attempt is a failure
|
||||
add(task, new GeneratorMockup());
|
||||
|
||||
t.plan();
|
||||
task.plan();
|
||||
|
||||
ASSERT_EQ(t.solutions().size(), 3u * 2u);
|
||||
ASSERT_EQ(task.solutions().size(), 3u * 2u);
|
||||
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
|
||||
auto expected_cost = expected_costs.begin();
|
||||
for (const auto& s : t.solutions()) {
|
||||
for (const auto& s : task.solutions()) {
|
||||
EXPECT_EQ(s->cost(), *expected_cost);
|
||||
++expected_cost;
|
||||
}
|
||||
EXPECT_EQ(c1->calls_, 6u); // expect 6 instead of 9 calls
|
||||
EXPECT_EQ(c2->calls_, 3u);
|
||||
EXPECT_EQ(c1->calls_, 6u);
|
||||
}
|
||||
|
||||
TEST(Pruning, PropagateInsideContainerBoundaries) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
add(t, new BackwardMockup({ inf }));
|
||||
add(t, new GeneratorMockup({ 0 }));
|
||||
auto c{ std::make_unique<SerialContainer>() };
|
||||
auto con = add(*c, new Connect());
|
||||
add(*c, new GeneratorMockup({ 0 }));
|
||||
t.add(std::move(c));
|
||||
TEST_F(Pruning, PropagateIntoContainer) {
|
||||
add(task, new BackwardMockup({ inf }));
|
||||
add(task, new GeneratorMockup({ 0 }));
|
||||
|
||||
EXPECT_FALSE(t.plan());
|
||||
auto inner = add(task, new SerialContainer());
|
||||
auto con = add(*inner, new Connect());
|
||||
add(*inner, new GeneratorMockup({ 0 }));
|
||||
|
||||
EXPECT_FALSE(task.plan());
|
||||
|
||||
// the failure in the backward stage (outside the container)
|
||||
// should prune the expected computation of con
|
||||
// should prune the expected computation of con inside the container
|
||||
EXPECT_EQ(con->calls_, 0);
|
||||
}
|
||||
|
||||
TEST(Pruning, PropagateOutsideContainerBoundaries) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
auto back = add(t, new BackwardMockup());
|
||||
add(t, new BackwardMockup());
|
||||
add(t, new GeneratorMockup({ 0 }));
|
||||
auto c{ std::make_unique<SerialContainer>() };
|
||||
add(*c, new ForwardMockup({ inf }));
|
||||
add(*c, new ForwardMockup());
|
||||
t.add(std::move(c));
|
||||
TEST_F(Pruning, PropagateFromContainerPull) {
|
||||
auto back = add(task, new BackwardMockup());
|
||||
add(task, new BackwardMockup());
|
||||
add(task, new GeneratorMockup({ 0 }));
|
||||
|
||||
EXPECT_FALSE(t.plan());
|
||||
auto inner = add(task, new SerialContainer());
|
||||
add(*inner, new ForwardMockup());
|
||||
add(*inner, new ForwardMockup({ inf }));
|
||||
|
||||
EXPECT_FALSE(task.plan());
|
||||
|
||||
// the failure inside the container should prune computing of back
|
||||
EXPECT_EQ(back->calls_, 0);
|
||||
}
|
||||
|
||||
TEST(Pruning, PropagateOutsideParallelContainerBoundariesSinglePathPull) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
TEST_F(Pruning, PropagateFromContainerPush) {
|
||||
auto inner = add(task, new SerialContainer());
|
||||
add(*inner, new BackwardMockup({ inf }));
|
||||
|
||||
auto back = add(t, new BackwardMockup());
|
||||
add(t, new GeneratorMockup({ 0 }));
|
||||
auto c{ new Alternatives };
|
||||
add(*c, new ForwardMockup({ inf }));
|
||||
add(t, c);
|
||||
add(task, new GeneratorMockup({ 0 }));
|
||||
auto con = add(task, new Connect());
|
||||
add(task, new GeneratorMockup({ 0 }));
|
||||
|
||||
EXPECT_FALSE(t.plan());
|
||||
EXPECT_FALSE(task.plan());
|
||||
|
||||
// the failure in Alternatives must prune computing back
|
||||
EXPECT_EQ(back->calls_, 0);
|
||||
}
|
||||
|
||||
TEST(Pruning, PropagateOutsideParallelContainerBoundariesSinglePathPush) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
|
||||
auto c{ new SerialContainer };
|
||||
add(*c, new BackwardMockup({ inf }));
|
||||
add(*c, new GeneratorMockup({ 0 }));
|
||||
add(t, c);
|
||||
|
||||
auto con = add(t, new Connect());
|
||||
add(t, new GeneratorMockup({ 0 }));
|
||||
|
||||
EXPECT_FALSE(t.plan());
|
||||
|
||||
// currently this is trivially true because containers only push full solutions
|
||||
// (so c never contributes a solution con could operate on), but it might change
|
||||
// in the future and the failure inside the container must still prune computing con
|
||||
// the failure inside container should prune computing of con
|
||||
EXPECT_EQ(con->calls_, 0);
|
||||
}
|
||||
|
||||
TEST(Pruning, PropagateOutsideParallelContainerBoundariesMultiplePaths) {
|
||||
resetIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
|
||||
auto back = add(task, new BackwardMockup());
|
||||
add(task, new GeneratorMockup({ 0 }));
|
||||
auto inner = add(task, new Alternatives());
|
||||
|
||||
auto back = add(t, new BackwardMockup());
|
||||
add(t, new GeneratorMockup({ 0 }));
|
||||
auto c{ new Alternatives };
|
||||
auto s1{ new SerialContainer };
|
||||
add(*s1, new Connect());
|
||||
add(*s1, new GeneratorMockup({ 0 }));
|
||||
add(*c, s1);
|
||||
add(*c, new ForwardMockup({ inf }));
|
||||
add(t, c);
|
||||
add(*inner, new ForwardMockup({ inf }));
|
||||
auto serial = add(*inner, new SerialContainer());
|
||||
add(*serial, new Connect());
|
||||
add(*serial, new GeneratorMockup({ 0 }));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_TRUE(task.plan());
|
||||
|
||||
// the failure in one branch of Alternatives must not prune computing back
|
||||
EXPECT_EQ(back->calls_, 1);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user