From ec2b06292e9760a7bd019912a9fc1e1562fa6a5a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 17 Feb 2018 21:10:21 +0100 Subject: [PATCH] extended unittest for serial connection validation --- core/test/CMakeLists.txt | 5 +- core/test/gtest_value_printers.cpp | 25 ++++ core/test/gtest_value_printers.h | 5 + core/test/test_container.cpp | 227 ++++++++++++++++++++++++----- core/test/test_stage.cpp | 6 +- 5 files changed, 230 insertions(+), 38 deletions(-) create mode 100644 core/test/gtest_value_printers.cpp create mode 100644 core/test/gtest_value_printers.h diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 0b095eda..9f011577 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -6,11 +6,14 @@ if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) + add_library(gtest_utils gtest_value_printers.cpp) + target_link_libraries(gtest_utils ${PROJECT_NAME}) + add_rostest_gtest(${PROJECT_NAME}-test-stage test_stage.launch test_stage.cpp) target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} gtest_main) catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp) - target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_main) + target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} 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) diff --git a/core/test/gtest_value_printers.cpp b/core/test/gtest_value_printers.cpp new file mode 100644 index 00000000..2cbbbfaa --- /dev/null +++ b/core/test/gtest_value_printers.cpp @@ -0,0 +1,25 @@ +#include "gtest_value_printers.h" + +using namespace moveit::task_constructor; + +::std::ostream& operator<<(::std::ostream& os, const InterfaceFlag& flag) { + switch (flag) { + case READS_START: return os << "READS_START"; + case READS_END: return os << "READS_END"; + case WRITES_NEXT_START: return os << "WRITES_NEXT_START"; + case WRITES_PREV_END: return os << "WRITES_PREV_END"; + default: return os << "unknown"; + } +} + +::std::ostream& operator<<(::std::ostream& os, const InterfaceFlags& flags) { + os << "InterfaceFlags("; + bool have_previous = false; + for (InterfaceFlag f : {READS_START,READS_END,WRITES_NEXT_START,WRITES_PREV_END}) { + if (flags & f) { + os << (have_previous ? ", " : "{") << f; + have_previous = true; + } + } + return os << (have_previous ? "})" : ")"); +} diff --git a/core/test/gtest_value_printers.h b/core/test/gtest_value_printers.h new file mode 100644 index 00000000..6403e063 --- /dev/null +++ b/core/test/gtest_value_printers.h @@ -0,0 +1,5 @@ +#include +#include + +::std::ostream& operator<<(::std::ostream& os, const moveit::task_constructor::InterfaceFlag& flag); +::std::ostream& operator<<(::std::ostream& os, const moveit::task_constructor::InterfaceFlags& flags); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 29b5d2f5..7a4381d4 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -1,29 +1,93 @@ #include #include +#include "gtest_value_printers.h" #include #include using namespace moveit::task_constructor; -class TestGenerator : public Generator { +class GeneratorMockup : public Generator { + int runs = 0; public: - TestGenerator() : Generator("generator") {} - bool canCompute() const override { return true; } - bool compute() override { return false; } + GeneratorMockup(int runs = 0) : Generator("generator"), runs(runs) {} + bool canCompute() const override { return runs > 0; } + bool compute() override { return --runs >= 0 ? true : false; } }; -class TestPropagatingForward : public PropagatingForward { +class PropagatorMockup : public PropagatingEitherWay { + int fw_runs = 0; + int bw_runs = 0; public: - TestPropagatingForward() : PropagatingForward("forwarder") {} - bool computeForward(const InterfaceState &from) override { return false; } + PropagatorMockup(int fw = 0, int bw = 0) : PropagatingEitherWay("either way"), fw_runs(fw), bw_runs(bw) {} + bool computeForward(const InterfaceState &from) override { return --fw_runs >= 0 ? true : false; } + bool computeBackward(const InterfaceState &from) override { return --bw_runs >= 0 ? true : false; } +}; +class ForwardMockup : public PropagatorMockup { +public: + ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) { + restrictDirection(FORWARD); + setName("forward"); + } +}; +class BackwardMockup : public PropagatorMockup { +public: + BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) { + restrictDirection(BACKWARD); + setName("backward"); + } +}; + +class ConnectMockup : public Connecting { + int runs = 0; +public: + ConnectMockup(int runs = 0) : Connecting("connect"), runs(runs) {} + bool compute(const InterfaceState& from, const InterfaceState& to) override { return --runs >= 0 ? true : false; } }; -class BaseTest : public ::testing::Test { +enum StageType { GEN, FW, BW, ANY, BOTH, CONN }; +void append(ContainerBase& c, const std::initializer_list& types, int runs = 0) { + for (StageType t : types) { + switch (t) { + case GEN: c.insert(std::make_unique(runs)); break; + case FW: c.insert(std::make_unique(runs)); break; + case BW: c.insert(std::make_unique(runs)); break; + case ANY: c.insert(std::make_unique(runs, runs)); break; + case BOTH: { + auto s = std::make_unique(runs, runs); + s->restrictDirection(PropagatingEitherWay::BOTHWAY); + c.insert(std::move(s)); + break; + } + case CONN: c.insert(std::make_unique(runs)); break; + } + } +} + + +class SerialTest : public ::testing::Test { protected: - void SetUp() {} - void TearDown() {} + planning_scene::PlanningScenePtr scene; + SerialContainer serial; + InterfacePtr dummy; + + SerialTest() : ::testing::Test(), dummy(new Interface) {} + + void pushInterface(bool start=true, bool end=true) { + // pretend, that the container is connected + SerialContainerPrivate *impl = serial.pimpl(); + if (start) impl->setPrevEnds(dummy); + if (end) impl->setNextStarts(dummy); + } + void reset(bool start=true, bool end=true) { + serial.reset(); + serial.clear(); + SerialContainerPrivate *impl = serial.pimpl(); + impl->setNextStarts(InterfacePtr()); + impl->setPrevEnds(InterfacePtr()); + pushInterface(start, end); + } void validateOrder(const SerialContainerPrivate* container, const std::initializer_list &expected) { size_t num = container->children().size(); @@ -44,50 +108,145 @@ protected: EXPECT_EQ(it, container->position(pos-num-1)) << "bad backward position resolution"; } } + void validateInit(bool start, bool end, const std::initializer_list& types, bool expect_failure) { + reset(start, end); + append(serial, types); + try { + serial.init(scene); + if (!expect_failure) return; // as expected + ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << serial; + } catch (const InitStageException &e) { + if (expect_failure) return; // as expected + ADD_FAILURE() << "unexpected init failure: \n" << e << "\n" << serial; + } catch (const std::exception &e) { + ADD_FAILURE() << "unexpected exception thrown:\n" << e.what(); + } catch (...) { + ADD_FAILURE() << "unexpected unknown exception thrown"; + } + } }; #define VALIDATE(...) {\ SCOPED_TRACE("validateOrder({" #__VA_ARGS__ "})"); \ - validateOrder(cp, {__VA_ARGS__}); \ + validateOrder(impl, {__VA_ARGS__}); \ } -TEST_F(BaseTest, serialContainer) { - SerialContainer c("serial"); - SerialContainerPrivate *cp = c.pimpl(); - // pretend, that the container is connected - InterfacePtr dummy(new Interface); - cp->setNextStarts(dummy); - cp->setPrevEnds(dummy); - planning_scene::PlanningScenePtr scene; +TEST_F(SerialTest, insertion_order) { + SerialContainerPrivate *impl = serial.pimpl(); - EXPECT_EQ(cp->parent(), nullptr); - EXPECT_THROW(c.init(scene), InitStageException); + EXPECT_EQ(impl->parent(), nullptr); + EXPECT_THROW(serial.init(scene), InitStageException); VALIDATE(); /***** inserting first stage *****/ - auto g = std::make_unique(); - GeneratorPrivate *gp = g->pimpl(); - ASSERT_TRUE(c.insert(std::move(g))); + auto g = std::make_unique(); + StagePrivate *gp = g->pimpl(); + ASSERT_TRUE(serial.insert(std::move(g))); EXPECT_FALSE(g); // ownership transferred to container VALIDATE(gp); /***** inserting second stage *****/ - auto f = std::make_unique(); - PropagatingForwardPrivate *fp = f->pimpl(); - ASSERT_TRUE(c.insert(std::move(f))); + auto f = std::make_unique(); + StagePrivate *fp = f->pimpl(); + ASSERT_TRUE(serial.insert(std::move(f))); EXPECT_FALSE(f); // ownership transferred to container VALIDATE(gp, fp); /***** inserting third stage *****/ - auto f2 = std::make_unique(); - PropagatingForwardPrivate *fp2 = f2->pimpl(); - ASSERT_TRUE(c.insert(std::move(f2), 1)); + auto f2 = std::make_unique(); + StagePrivate *fp2 = f2->pimpl(); + ASSERT_TRUE(serial.insert(std::move(f2), 1)); EXPECT_FALSE(f2); // ownership transferred to container VALIDATE(gp, fp2, fp); - EXPECT_NO_THROW(c.init(scene)); - /***** inserting another generator stage *****/ - ASSERT_TRUE(c.insert(std::make_unique())); - EXPECT_THROW(c.init(scene), InitStageException); + auto g2 = std::make_unique(); + StagePrivate *gp2 = g2->pimpl(); + ASSERT_TRUE(serial.insert(std::move(g2))); + VALIDATE(gp, fp2, fp, gp2); +} + + +#define EXPECT_INIT_FAILURE(start, end, ...) {\ + SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \ + validateInit(start, end, {__VA_ARGS__}, true); \ +} + +#define EXPECT_INIT_SUCCESS(start, end, ...) {\ + SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \ + validateInit(start, end, {__VA_ARGS__}, false); \ +} + +TEST_F(SerialTest, init_empty) { + EXPECT_INIT_FAILURE(true, true); // no children + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END})); + + EXPECT_INIT_FAILURE(false, false); // no children + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags()); +} + +TEST_F(SerialTest, init_connecting) { + EXPECT_INIT_SUCCESS(false, false, CONN); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, READS_END})); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + + EXPECT_INIT_FAILURE(true, true, CONN); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END})); + + EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected +} + +TEST_F(SerialTest, init_generator) { + EXPECT_INIT_SUCCESS(true, true, GEN); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END})); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + + EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes + + EXPECT_INIT_FAILURE(true, true, GEN, GEN); // two generator stages cannot be connected +} + +TEST_F(SerialTest, init_forward) { + // container interface doesn't match children + EXPECT_INIT_FAILURE(false, false, FW); + EXPECT_INIT_FAILURE(true, true, FW); + EXPECT_INIT_FAILURE(true, false, FW); + + // these should be fine + EXPECT_INIT_SUCCESS(false, true, FW); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + + EXPECT_INIT_SUCCESS(false, true, FW, FW); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + + EXPECT_INIT_SUCCESS(true, true, GEN, FW); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END})); +} + +TEST_F(SerialTest, init_backward) { + // container interface doesn't match children + EXPECT_INIT_FAILURE(false, false, BW); + EXPECT_INIT_FAILURE(true, true, BW); + EXPECT_INIT_FAILURE(false, true, BW); + + // these should be fine + EXPECT_INIT_SUCCESS(true, false, BW); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + + EXPECT_INIT_SUCCESS(true, false, BW, BW); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + + EXPECT_INIT_SUCCESS(true, true, BW, GEN); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END})); + + EXPECT_INIT_SUCCESS(true, true, BW, GEN, FW); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END})); +} + +TEST_F(SerialTest, init_auto) { + // derive propagation direction from outer interface + EXPECT_INIT_SUCCESS(false, true, ANY); // -> -> + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START})); + EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // <- <- + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START})); } diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index bac4a247..0da92d5a 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -8,13 +8,13 @@ using namespace moveit::task_constructor; -class TestGenerator : public Generator { +class GeneratorMockup : public Generator { planning_scene::PlanningScenePtr ps; InterfacePtr prev; InterfacePtr next; public: - TestGenerator() : Generator("generator") { + GeneratorMockup() : Generator("generator") { robot_model_loader::RobotModelLoader loader; ps.reset(new planning_scene::PlanningScene(loader.getModel())); @@ -35,7 +35,7 @@ TEST(Stage, registerCallbacks) { char *argv = nullptr; ros::init(argc, &argv, "testLocalTaskModel"); - TestGenerator g; + GeneratorMockup g; uint called = 0; auto cb = [&called](const SolutionBase &s) { ++called;