mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
move fallback to separate test
This commit is contained in:
parent
14380c5552
commit
001eadaf3d
@ -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)
|
||||
|
||||
|
||||
@ -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
101
core/test/test_fallback.cpp
Normal 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);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user