move fallback to separate test

This commit is contained in:
v4hn 2021-08-18 11:42:49 +02:00
parent 14380c5552
commit 001eadaf3d
3 changed files with 104 additions and 84 deletions

View File

@ -18,6 +18,9 @@ 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_gtest(${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)

View File

@ -44,7 +44,6 @@ void append(ContainerBase& c, const std::initializer_list<StageType>& types) {
}
}
}
constexpr double INF = std::numeric_limits<double>::infinity();
class NamedStage : public GeneratorMockup
{
@ -670,86 +669,3 @@ TEST(Task, timeout) {
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.solutions().size(), 2u);
}
TEST(Fallback, failingNoSolutions) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
t.add(std::move(fallback));
EXPECT_FALSE(t.plan());
EXPECT_EQ(t.solutions().size(), 0u);
}
TEST(Fallback, failingWithFailedSolutions) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(INF)));
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(INF)));
t.add(std::move(fallback));
EXPECT_FALSE(t.plan());
EXPECT_EQ(t.solutions().size(), 0u);
}
TEST(Fallback, DISABLED_ConnectStageInsideFallbacks) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>());
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ConnectMockup>());
t.add(std::move(fallbacks));
t.add(std::make_unique<GeneratorMockup>());
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.numSolutions(), 1u);
}
TEST(Fallback, ComputeFirstSuccessfulStageOnly) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>());
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.numSolutions(), 1u);
}
TEST(Fallback, ActiveChildReset) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts{ { 0.0, INF, 0.0 } }));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
auto first = fallbacks->findChild("FWD1");
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.numSolutions(), 2u);
EXPECT_EQ(first->solutions().size(), 2u);
}

101
core/test/test_fallback.cpp Normal file
View File

@ -0,0 +1,101 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/task_p.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/planning_scene/planning_scene.h>
#include "stage_mockups.h"
#include "models.h"
#include "gtest_value_printers.h"
#include <gtest/gtest.h>
#include <initializer_list>
#include <chrono>
#include <thread>
using namespace moveit::task_constructor;
constexpr double INF = std::numeric_limits<double>::infinity();
TEST(Fallback, failingNoSolutions) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
t.add(std::move(fallback));
EXPECT_FALSE(t.plan());
EXPECT_EQ(t.solutions().size(), 0u);
}
TEST(Fallback, failingWithFailedSolutions) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(INF)));
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(INF)));
t.add(std::move(fallback));
EXPECT_FALSE(t.plan());
EXPECT_EQ(t.solutions().size(), 0u);
}
TEST(Fallback, DISABLED_ConnectStageInsideFallbacks) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>());
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ConnectMockup>());
t.add(std::move(fallbacks));
t.add(std::make_unique<GeneratorMockup>());
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.numSolutions(), 1u);
}
TEST(Fallback, ComputeFirstSuccessfulStageOnly) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>());
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.numSolutions(), 1u);
}
TEST(Fallback, ActiveChildReset) {
resetMockupIds();
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts{ { 0.0, INF, 0.0 } }));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
auto first = fallbacks->findChild("FWD1");
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.numSolutions(), 2u);
EXPECT_EQ(first->solutions().size(), 2u);
}