mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Add more fallback tests
This commit is contained in:
parent
4e0f295d1e
commit
35dc037365
@ -70,19 +70,53 @@ TEST_F(FallbacksFixturePropagate, ComputeFirstSuccessfulStageOnly) {
|
||||
}
|
||||
|
||||
TEST_F(FallbacksFixturePropagate, ComputeFirstSuccessfulStagePerSolutionOnly) {
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0 })));
|
||||
// duplicate generator solutions with resulting costs: 4, 2 | 3, 1
|
||||
t.add(std::make_unique<ForwardMockup>(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2));
|
||||
|
||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF, 10.0 })));
|
||||
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 20.0, INF })));
|
||||
auto fwd1 = fallbacks->findChild("FWD1");
|
||||
auto fwd2 = fallbacks->findChild("FWD2");
|
||||
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF, INF, 110.0, 120.0 })));
|
||||
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 210.0, 220.0, 0, 0 })));
|
||||
t.add(std::move(fallbacks));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(12, 21));
|
||||
EXPECT_EQ(fwd1->solutions().size(), 1u);
|
||||
EXPECT_EQ(fwd2->solutions().size(), 1u);
|
||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222));
|
||||
}
|
||||
|
||||
TEST_F(FallbacksFixturePropagate, UpdateSolutionOrder) {
|
||||
t.add(std::make_unique<BackwardMockup>(PredefinedCosts({ 10.0, 0.0 })));
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
|
||||
// available solutions (sorted) in individual runs of fallbacks: 1 | 11, 2 | 2, 11
|
||||
|
||||
// use a fallback container to delay computation twice: only the last child succeeds
|
||||
auto inner = std::make_unique<Fallbacks>("Inner");
|
||||
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false)));
|
||||
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false)));
|
||||
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
|
||||
|
||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||
fallbacks->add(std::move(inner));
|
||||
t.add(std::move(fallbacks));
|
||||
|
||||
EXPECT_TRUE(t.plan(1)); // only return 1st solution
|
||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result
|
||||
}
|
||||
|
||||
TEST_F(FallbacksFixturePropagate, MultipleActivePendingStates) {
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0, 3.0 })));
|
||||
// use a fallback container to delay computation: the 1st child never succeeds, but only the 2nd
|
||||
auto inner = std::make_unique<Fallbacks>("Inner");
|
||||
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false))); // always fail
|
||||
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 10.0, INF, 30.0 })));
|
||||
|
||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||
fallbacks->add(std::move(inner));
|
||||
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF })));
|
||||
t.add(std::move(fallbacks));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 33));
|
||||
// check that first solution is not marked as pruned
|
||||
}
|
||||
|
||||
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) {
|
||||
@ -128,14 +162,28 @@ TEST_F(FallbacksFixturePropagate, ActiveChildReset) {
|
||||
using FallbacksFixtureConnect = TaskTestBase;
|
||||
|
||||
TEST_F(FallbacksFixtureConnect, ConnectStageInsideFallbacks) {
|
||||
t.add(std::make_unique<GeneratorMockup>());
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
|
||||
|
||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||
fallbacks->add(std::make_unique<ConnectMockup>());
|
||||
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(0.0)));
|
||||
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(100.0)));
|
||||
t.add(std::move(fallbacks));
|
||||
|
||||
t.add(std::make_unique<GeneratorMockup>());
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 10.0, 20.0 })));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_EQ(t.numSolutions(), 1u);
|
||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
if (strcmp(argv[i], "--debug") == 0) {
|
||||
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
|
||||
ros::console::notifyLoggerLevelsChanged();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user