Skip to content

Commit e89c68c

Browse files
v4hnv4hn
authored
Merge pull request #294 from v4hn/pr-fallbacks-split-tests
new set of fallback tests
2 parents 9da3a80 + dbfa7e2 commit e89c68c

File tree

4 files changed

+218
-18
lines changed

4 files changed

+218
-18
lines changed

‎core/test/CMakeLists.txt‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,13 +18,16 @@ if (CATKIN_ENABLE_TESTING)
1818
catkin_add_gtest(${PROJECT_NAME}-test-serial test_serial.cpp)
1919
target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
2020

21+
catkin_add_gmock(${PROJECT_NAME}-test-fallback test_fallback.cpp)
22+
target_link_libraries(${PROJECT_NAME}-test-fallback ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
23+
2124
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
2225
target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main)
2326

2427
catkin_add_gmock(${PROJECT_NAME}-test-cost_queue test_cost_queue.cpp)
2528
target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main)
2629

27-
catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
30+
catkin_add_gmock(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
2831
target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_utils gtest_main)
2932

3033
catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp)

‎core/test/stage_mockups.h‎

Lines changed: 25 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,14 @@
11
#pragma once
22

3-
#include <moveit/task_constructor/stage_p.h>
3+
#include <moveit/task_constructor/task.h>
4+
45
#include <moveit/planning_scene/planning_scene.h>
56

7+
#include "models.h"
8+
9+
#include <gtest/gtest.h>
10+
#include <gmock/gmock.h>
11+
612
namespace moveit {
713
namespace task_constructor {
814

@@ -113,5 +119,23 @@ struct BackwardMockup : public PropagatorMockup
113119
// reset ids of all Mockup types (used to generate unique stage names)
114120
void resetMockupIds();
115121

122+
// provide a basic test fixture that prepares a Task
123+
struct TaskTestBase : public testing::Test
124+
{
125+
Task t;
126+
TaskTestBase() {
127+
resetMockupIds();
128+
t.setRobotModel(getModel());
129+
}
130+
};
131+
132+
#define EXPECT_COSTS(value, matcher) \
133+
{ \
134+
std ::vector<double> costs; \
135+
std::transform(value.begin(), value.end(), std::back_inserter(costs), \
136+
[](const SolutionBaseConstPtr& s) { return s->cost(); }); \
137+
EXPECT_THAT(costs, matcher); \
138+
}
139+
116140
} // namespace task_constructor
117141
} // namespace moveit

‎core/test/test_container.cpp‎

Lines changed: 0 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -669,19 +669,3 @@ TEST(Task, timeout) {
669669
EXPECT_TRUE(t.plan());
670670
EXPECT_EQ(t.solutions().size(), 2u);
671671
}
672-
673-
TEST(Fallback, failing) {
674-
resetMockupIds();
675-
Task t;
676-
t.setRobotModel(getModel());
677-
678-
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
679-
680-
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
681-
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
682-
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
683-
t.add(std::move(fallback));
684-
685-
EXPECT_FALSE(t.plan());
686-
EXPECT_EQ(t.solutions().size(), 0u);
687-
}

‎core/test/test_fallback.cpp‎

Lines changed: 189 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,189 @@
1+
#include <moveit/task_constructor/container_p.h>
2+
#include <moveit/task_constructor/stage_p.h>
3+
#include <moveit/task_constructor/task_p.h>
4+
#include <moveit/task_constructor/stages/fixed_state.h>
5+
#include <moveit/planning_scene/planning_scene.h>
6+
7+
#include "stage_mockups.h"
8+
#include "models.h"
9+
#include "gtest_value_printers.h"
10+
11+
#include <gtest/gtest.h>
12+
#include <initializer_list>
13+
#include <chrono>
14+
#include <thread>
15+
16+
using namespace moveit::task_constructor;
17+
18+
constexpr double INF = std::numeric_limits<double>::infinity();
19+
20+
using FallbacksFixtureGenerator = TaskTestBase;
21+
22+
TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) {
23+
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
24+
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF)));
25+
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0)));
26+
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(2.0)));
27+
t.add(std::move(fallback));
28+
29+
EXPECT_TRUE(t.plan());
30+
ASSERT_EQ(t.solutions().size(), 1u);
31+
EXPECT_EQ(t.solutions().front()->cost(), 1.0);
32+
}
33+
34+
using FallbacksFixturePropagate = TaskTestBase;
35+
36+
TEST_F(FallbacksFixturePropagate, failingNoSolutions) {
37+
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
38+
39+
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
40+
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts({}), 0));
41+
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts({}), 0));
42+
t.add(std::move(fallback));
43+
44+
EXPECT_FALSE(t.plan());
45+
EXPECT_EQ(t.solutions().size(), 0u);
46+
}
47+
48+
TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) {
49+
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
50+
51+
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
52+
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(INF)));
53+
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(INF)));
54+
t.add(std::move(fallback));
55+
56+
EXPECT_FALSE(t.plan());
57+
EXPECT_EQ(t.solutions().size(), 0u);
58+
}
59+
60+
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) {
61+
t.add(std::make_unique<GeneratorMockup>());
62+
63+
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
64+
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
65+
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
66+
t.add(std::move(fallbacks));
67+
68+
EXPECT_TRUE(t.plan());
69+
EXPECT_EQ(t.numSolutions(), 1u);
70+
}
71+
72+
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutionOnly) {
73+
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0 })));
74+
// duplicate generator solutions with resulting costs: 4, 2 | 3, 1
75+
t.add(std::make_unique<ForwardMockup>(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2));
76+
77+
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
78+
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF, INF, 110.0, 120.0 })));
79+
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 210.0, 220.0, 0, 0 })));
80+
t.add(std::move(fallbacks));
81+
82+
EXPECT_TRUE(t.plan());
83+
EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222));
84+
}
85+
86+
TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) {
87+
t.add(std::make_unique<BackwardMockup>(PredefinedCosts({ 10.0, 0.0 })));
88+
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
89+
// available solutions (sorted) in individual runs of fallbacks: 1 | 11, 2 | 2, 11
90+
91+
// use a fallback container to delay computation twice: only the last child succeeds
92+
auto inner = std::make_unique<Fallbacks>("Inner");
93+
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false)));
94+
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false)));
95+
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
96+
97+
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
98+
fallbacks->add(std::move(inner));
99+
t.add(std::move(fallbacks));
100+
101+
EXPECT_TRUE(t.plan(1)); // only return 1st solution
102+
EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result
103+
}
104+
105+
TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) {
106+
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0, 3.0 })));
107+
// use a fallback container to delay computation: the 1st child never succeeds, but only the 2nd
108+
auto inner = std::make_unique<Fallbacks>("Inner");
109+
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false))); // always fail
110+
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 10.0, INF, 30.0 })));
111+
112+
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
113+
fallbacks->add(std::move(inner));
114+
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF })));
115+
t.add(std::move(fallbacks));
116+
117+
EXPECT_TRUE(t.plan());
118+
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 33));
119+
// check that first solution is not marked as pruned
120+
}
121+
122+
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
123+
t.add(std::make_unique<GeneratorMockup>());
124+
125+
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
126+
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF, 1.0 }), 2));
127+
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::single(2.0)));
128+
t.add(std::move(fallback));
129+
130+
EXPECT_TRUE(t.plan());
131+
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
132+
}
133+
134+
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
135+
t.add(std::make_unique<GeneratorMockup>());
136+
137+
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
138+
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 1.0, INF }), 2));
139+
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::single(2.0)));
140+
t.add(std::move(fallback));
141+
142+
EXPECT_TRUE(t.plan());
143+
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
144+
}
145+
146+
TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) {
147+
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 })));
148+
149+
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
150+
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(10.0)));
151+
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(20.0)));
152+
auto fwd1 = fallbacks->findChild("FWD1");
153+
auto fwd2 = fallbacks->findChild("FWD2");
154+
t.add(std::move(fallbacks));
155+
156+
EXPECT_TRUE(t.plan());
157+
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 13));
158+
EXPECT_COSTS(fwd1->solutions(), testing::ElementsAre(10, 10));
159+
EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty());
160+
}
161+
162+
using FallbacksFixtureConnect = TaskTestBase;
163+
164+
TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) {
165+
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
166+
167+
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
168+
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(0.0)));
169+
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(100.0)));
170+
t.add(std::move(fallbacks));
171+
172+
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 10.0, 20.0 })));
173+
174+
EXPECT_TRUE(t.plan());
175+
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22));
176+
}
177+
178+
int main(int argc, char** argv) {
179+
for (int i = 1; i < argc; ++i) {
180+
if (strcmp(argv[i], "--debug") == 0) {
181+
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
182+
ros::console::notifyLoggerLevelsChanged();
183+
break;
184+
}
185+
}
186+
187+
testing::InitGoogleTest(&argc, argv);
188+
return RUN_ALL_TESTS();
189+
}

0 commit comments

Comments
 (0)