diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 3d7777c4a..a05f62049 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -18,13 +18,16 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(${PROJECT_NAME}-test-serial test_serial.cpp) target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) + catkin_add_gmock(${PROJECT_NAME}-test-fallback test_fallback.cpp) + target_link_libraries(${PROJECT_NAME}-test-fallback ${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) catkin_add_gmock(${PROJECT_NAME}-test-cost_queue test_cost_queue.cpp) target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main) - catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp) + catkin_add_gmock(${PROJECT_NAME}-test-interface_state test_interface_state.cpp) 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) diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h index fdab8afaa..2e517a3b8 100644 --- a/core/test/stage_mockups.h +++ b/core/test/stage_mockups.h @@ -1,8 +1,14 @@ #pragma once -#include +#include + #include +#include "models.h" + +#include +#include + namespace moveit { namespace task_constructor { @@ -113,5 +119,23 @@ struct BackwardMockup : public PropagatorMockup // reset ids of all Mockup types (used to generate unique stage names) void resetMockupIds(); +// provide a basic test fixture that prepares a Task +struct TaskTestBase : public testing::Test +{ + Task t; + TaskTestBase() { + resetMockupIds(); + t.setRobotModel(getModel()); + } +}; + +#define EXPECT_COSTS(value, matcher) \ + { \ + std ::vector costs; \ + std::transform(value.begin(), value.end(), std::back_inserter(costs), \ + [](const SolutionBaseConstPtr& s) { return s->cost(); }); \ + EXPECT_THAT(costs, matcher); \ + } + } // namespace task_constructor } // namespace moveit diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 168f5ac03..4eaa143bd 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -669,19 +669,3 @@ TEST(Task, timeout) { EXPECT_TRUE(t.plan()); EXPECT_EQ(t.solutions().size(), 2u); } - -TEST(Fallback, failing) { - resetMockupIds(); - Task t; - t.setRobotModel(getModel()); - - t.add(std::make_unique(PredefinedCosts::single(0.0))); - - auto fallback = std::make_unique("Fallbacks"); - fallback->add(std::make_unique(PredefinedCosts::constant(0.0), 0)); - fallback->add(std::make_unique(PredefinedCosts::constant(0.0), 0)); - t.add(std::move(fallback)); - - EXPECT_FALSE(t.plan()); - EXPECT_EQ(t.solutions().size(), 0u); -} diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp new file mode 100644 index 000000000..e721604ce --- /dev/null +++ b/core/test/test_fallback.cpp @@ -0,0 +1,189 @@ +#include +#include +#include +#include +#include + +#include "stage_mockups.h" +#include "models.h" +#include "gtest_value_printers.h" + +#include +#include +#include +#include + +using namespace moveit::task_constructor; + +constexpr double INF = std::numeric_limits::infinity(); + +using FallbacksFixtureGenerator = TaskTestBase; + +TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) { + auto fallback = std::make_unique("Fallbacks"); + fallback->add(std::make_unique(PredefinedCosts::single(INF))); + fallback->add(std::make_unique(PredefinedCosts::single(1.0))); + fallback->add(std::make_unique(PredefinedCosts::single(2.0))); + t.add(std::move(fallback)); + + EXPECT_TRUE(t.plan()); + ASSERT_EQ(t.solutions().size(), 1u); + EXPECT_EQ(t.solutions().front()->cost(), 1.0); +} + +using FallbacksFixturePropagate = TaskTestBase; + +TEST_F(FallbacksFixturePropagate, failingNoSolutions) { + t.add(std::make_unique(PredefinedCosts::single(0.0))); + + auto fallback = std::make_unique("Fallbacks"); + fallback->add(std::make_unique(PredefinedCosts({}), 0)); + fallback->add(std::make_unique(PredefinedCosts({}), 0)); + t.add(std::move(fallback)); + + EXPECT_FALSE(t.plan()); + EXPECT_EQ(t.solutions().size(), 0u); +} + +TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) { + t.add(std::make_unique(PredefinedCosts::single(0.0))); + + auto fallback = std::make_unique("Fallbacks"); + fallback->add(std::make_unique(PredefinedCosts::constant(INF))); + fallback->add(std::make_unique(PredefinedCosts::constant(INF))); + t.add(std::move(fallback)); + + EXPECT_FALSE(t.plan()); + EXPECT_EQ(t.solutions().size(), 0u); +} + +TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) { + t.add(std::make_unique()); + + auto fallbacks = std::make_unique("Fallbacks"); + fallbacks->add(std::make_unique(PredefinedCosts::constant(0.0))); + fallbacks->add(std::make_unique(PredefinedCosts::constant(0.0))); + t.add(std::move(fallbacks)); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.numSolutions(), 1u); +} + +TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutionOnly) { + t.add(std::make_unique(PredefinedCosts({ 2.0, 1.0 }))); + // duplicate generator solutions with resulting costs: 4, 2 | 3, 1 + t.add(std::make_unique(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2)); + + auto fallbacks = std::make_unique("Fallbacks"); + fallbacks->add(std::make_unique(PredefinedCosts({ INF, INF, 110.0, 120.0 }))); + fallbacks->add(std::make_unique(PredefinedCosts({ 210.0, 220.0, 0, 0 }))); + t.add(std::move(fallbacks)); + + EXPECT_TRUE(t.plan()); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222)); +} + +TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { + t.add(std::make_unique(PredefinedCosts({ 10.0, 0.0 }))); + t.add(std::make_unique(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("Inner"); + inner->add(std::make_unique(PredefinedCosts({ INF }, false))); + inner->add(std::make_unique(PredefinedCosts({ INF }, false))); + inner->add(std::make_unique(PredefinedCosts::constant(0.0))); + + auto fallbacks = std::make_unique("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, DISABLED_MultipleActivePendingStates) { + t.add(std::make_unique(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("Inner"); + inner->add(std::make_unique(PredefinedCosts({ INF }, false))); // always fail + inner->add(std::make_unique(PredefinedCosts({ 10.0, INF, 30.0 }))); + + auto fallbacks = std::make_unique("Fallbacks"); + fallbacks->add(std::move(inner)); + fallbacks->add(std::make_unique(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, DISABLED_successfulWithMixedSolutions) { + t.add(std::make_unique()); + + auto fallback = std::make_unique("Fallbacks"); + fallback->add(std::make_unique(PredefinedCosts({ INF, 1.0 }), 2)); + fallback->add(std::make_unique(PredefinedCosts::single(2.0))); + t.add(std::move(fallback)); + + EXPECT_TRUE(t.plan()); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); +} + +TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) { + t.add(std::make_unique()); + + auto fallback = std::make_unique("Fallbacks"); + fallback->add(std::make_unique(PredefinedCosts({ 1.0, INF }), 2)); + fallback->add(std::make_unique(PredefinedCosts::single(2.0))); + t.add(std::move(fallback)); + + EXPECT_TRUE(t.plan()); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); +} + +TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) { + t.add(std::make_unique(PredefinedCosts({ 1.0, INF, 3.0 }))); + + auto fallbacks = std::make_unique("Fallbacks"); + fallbacks->add(std::make_unique(PredefinedCosts::constant(10.0))); + fallbacks->add(std::make_unique(PredefinedCosts::constant(20.0))); + auto fwd1 = fallbacks->findChild("FWD1"); + auto fwd2 = fallbacks->findChild("FWD2"); + t.add(std::move(fallbacks)); + + EXPECT_TRUE(t.plan()); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 13)); + EXPECT_COSTS(fwd1->solutions(), testing::ElementsAre(10, 10)); + EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty()); +} + +using FallbacksFixtureConnect = TaskTestBase; + +TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) { + t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); + + auto fallbacks = std::make_unique("Fallbacks"); + fallbacks->add(std::make_unique(PredefinedCosts::constant(0.0))); + fallbacks->add(std::make_unique(PredefinedCosts::constant(100.0))); + t.add(std::move(fallbacks)); + + t.add(std::make_unique(PredefinedCosts({ 10.0, 20.0 }))); + + EXPECT_TRUE(t.plan()); + 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(); +}