From a2fd5fb7b833b9bde350f025d720c021d2e08e8a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2018 11:50:23 +0100 Subject: [PATCH 1/6] more constexpr InterfaceFlags --- .../moveit/task_constructor/container_p.h | 4 ++-- core/include/moveit/task_constructor/stage.h | 1 + core/src/container.cpp | 5 ++--- core/test/test_container.cpp | 22 +++++++++---------- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 5cb8af15..fceb106f 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -105,12 +105,12 @@ protected: // If, during pruneInterface() later, we notice that it's not needed, prune there. inline void setChildsPushBackwardInterface(Stage& child) { bool allowed = (child.pimpl()->requiredInterface() & WRITES_PREV_END) || - (child.pimpl()->requiredInterface() == 0); // unknown interface + (child.pimpl()->requiredInterface() == UNKNOWN); child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr()); } inline void setChildsPushForwardInterface(Stage& child) { bool allowed = (child.pimpl()->requiredInterface() & WRITES_NEXT_START) || - (child.pimpl()->requiredInterface() == 0); // unknown interface + (child.pimpl()->requiredInterface() == UNKNOWN); child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 7c01cd96..a627d8fb 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -85,6 +85,7 @@ typedef Flags InterfaceFlags; constexpr InterfaceFlags UNKNOWN; constexpr InterfaceFlags INPUT_IF_MASK({READS_START, WRITES_PREV_END}); constexpr InterfaceFlags OUTPUT_IF_MASK({READS_END, WRITES_NEXT_START}); +constexpr InterfaceFlags PROPAGATE_BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}); MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Stage) diff --git a/core/src/container.cpp b/core/src/container.cpp index e66a8ee9..ce5dd8b6 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -483,11 +483,10 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) // called by parent asking for pruning of this' interface void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (children().empty()) return; - constexpr InterfaceFlags BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}); // We only need to deal with the special case of the whole sequence to be pruned. - if (accepted != BOTHWAYS && // will interface be restricted at all? - children().front()->pimpl()->interfaceFlags() == BOTHWAYS) // still undecided? + if (accepted != PROPAGATE_BOTHWAYS && // will interface be restricted at all? + children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) // still undecided? { pruneInterfaces(children().begin(), children().end(), accepted); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index fc7e77f3..62ef22c1 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -219,7 +219,7 @@ TEST_F(SerialTest, insertion_order) { 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_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_INIT_FAILURE(false, false); // no children EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags()); @@ -238,7 +238,7 @@ TEST_F(SerialTest, init_connecting) { 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(), InterfaceFlags(GENERATE)); 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 @@ -260,7 +260,7 @@ TEST_F(SerialTest, init_forward) { 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})); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); } TEST_F(SerialTest, init_backward) { @@ -277,10 +277,10 @@ TEST_F(SerialTest, init_backward) { 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_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_INIT_SUCCESS(true, true, BW, GEN, FW); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END})); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); } TEST_F(SerialTest, interface_detection) { @@ -327,13 +327,13 @@ TEST_F(SerialTest, interface_detection) { EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <> it = serial.pimpl()->children().begin(); - EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})); - EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})); + EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- -- it = serial.pimpl()->children().begin(); - EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags()); - EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags()); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags()); + EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), UNKNOWN); + EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), UNKNOWN); } From 3e4aa1924631bacabce6eca841420dd408c16ef5 Mon Sep 17 00:00:00 2001 From: eirtech Date: Fri, 23 Feb 2018 14:51:41 +0100 Subject: [PATCH 2/6] ParallelContainer: unittests --- .../moveit/task_constructor/container.h | 4 +- core/test/test_container.cpp | 235 +++++++++++++----- 2 files changed, 172 insertions(+), 67 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index e728e981..a9098c0d 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -187,7 +187,7 @@ protected: class Alternatives : public ParallelContainerBase { public: - Alternatives(const std::string &name) : ParallelContainerBase(name) {} + Alternatives(const std::string &name = "alternatives") : ParallelContainerBase(name) {} bool canCompute() const override; bool compute() override; @@ -205,7 +205,7 @@ class Fallbacks : public ParallelContainerBase mutable Stage* active_child_ = nullptr; public: - Fallbacks(const std::string &name) : ParallelContainerBase(name) {} + Fallbacks(const std::string &name = "fallbacks") : ParallelContainerBase(name) {} void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 62ef22c1..2ea6e6a3 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -100,29 +100,56 @@ TEST(ContainerBase, position) { } -class SerialTest : public ::testing::Test { +template +class InitTest : public ::testing::Test { protected: moveit::core::RobotModelConstPtr robot_model; - SerialContainer serial; + Container container; InterfacePtr dummy; - SerialTest() : ::testing::Test(), dummy(new Interface) {} + InitTest() : ::testing::Test(), dummy(new Interface) {} void pushInterface(bool start=true, bool end=true) { // pretend, that the container is connected - SerialContainerPrivate *impl = serial.pimpl(); + ContainerBasePrivate *impl = container.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(); + container.reset(); + container.clear(); + ContainerBasePrivate *impl = container.pimpl(); impl->setNextStarts(InterfacePtr()); impl->setPrevEnds(InterfacePtr()); pushInterface(start, end); } + void validateInit(bool start, bool end, const std::initializer_list& types, bool expect_failure) { + reset(start, end); + append(container, types); + try { + container.init(robot_model); + // prune pull interfaces based on provided external interface (start, end) + InterfaceFlags accepted; + if (start) accepted |= WRITES_PREV_END; + if (end) accepted |= WRITES_NEXT_START; + container.pimpl()->pruneInterface(accepted); + container.validateConnectivity(); + if (!expect_failure) return; // as expected + ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container; + } catch (const InitStageException &e) { + if (expect_failure) return; // as expected + ADD_FAILURE() << "unexpected init failure: \n" << e << "\n" << container; + } catch (const std::exception &e) { + ADD_FAILURE() << "unexpected exception thrown:\n" << e.what(); + } catch (...) { + ADD_FAILURE() << "unexpected unknown exception thrown"; + } + } +}; + +class SerialTest : public InitTest { +protected: void validateOrder(const SerialContainerPrivate* container, const std::initializer_list &expected) { size_t num = container->children().size(); ASSERT_TRUE(num == expected.size()) << "different list lengths"; @@ -142,28 +169,6 @@ 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(robot_model); - // prune pull interfaces based on provided external interface (start, end) - InterfaceFlags accepted; - if (start) accepted |= WRITES_PREV_END; - if (end) accepted |= WRITES_NEXT_START; - serial.pimpl()->pruneInterface(accepted); - serial.validateConnectivity(); - 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(...) {\ @@ -172,37 +177,37 @@ protected: } TEST_F(SerialTest, insertion_order) { - SerialContainerPrivate *impl = serial.pimpl(); + SerialContainerPrivate *impl = container.pimpl(); EXPECT_EQ(impl->parent(), nullptr); - EXPECT_THROW(serial.init(robot_model), InitStageException); + EXPECT_THROW(container.init(robot_model), InitStageException); VALIDATE(); /***** inserting first stage *****/ auto g = std::make_unique(); StagePrivate *gp = g->pimpl(); - ASSERT_TRUE(serial.insert(std::move(g))); + ASSERT_TRUE(container.insert(std::move(g))); EXPECT_FALSE(g); // ownership transferred to container VALIDATE(gp); /***** inserting second stage *****/ auto f = std::make_unique(); StagePrivate *fp = f->pimpl(); - ASSERT_TRUE(serial.insert(std::move(f))); + ASSERT_TRUE(container.insert(std::move(f))); EXPECT_FALSE(f); // ownership transferred to container VALIDATE(gp, fp); /***** inserting third stage *****/ auto f2 = std::make_unique(); StagePrivate *fp2 = f2->pimpl(); - ASSERT_TRUE(serial.insert(std::move(f2), 1)); + ASSERT_TRUE(container.insert(std::move(f2), 1)); EXPECT_FALSE(f2); // ownership transferred to container VALIDATE(gp, fp2, fp); /***** inserting another generator stage *****/ auto g2 = std::make_unique(); StagePrivate *gp2 = g2->pimpl(); - ASSERT_TRUE(serial.insert(std::move(g2))); + ASSERT_TRUE(container.insert(std::move(g2))); VALIDATE(gp, fp2, fp, gp2); } @@ -219,27 +224,27 @@ TEST_F(SerialTest, insertion_order) { TEST_F(SerialTest, init_empty) { EXPECT_INIT_FAILURE(true, true); // no children - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_INIT_FAILURE(false, false); // no children - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags()); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags()); } TEST_F(SerialTest, init_connecting) { EXPECT_INIT_SUCCESS(false, false, CONN); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_INIT_FAILURE(true, true, CONN); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({CONNECT, GENERATE})); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({CONNECT, GENERATE})); 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(GENERATE)); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes @@ -254,13 +259,13 @@ TEST_F(SerialTest, init_forward) { // these should be fine EXPECT_INIT_SUCCESS(false, true, FW); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_INIT_SUCCESS(false, true, FW, FW); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_INIT_SUCCESS(true, true, GEN, FW); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); } TEST_F(SerialTest, init_backward) { @@ -271,69 +276,169 @@ TEST_F(SerialTest, init_backward) { // these should be fine EXPECT_INIT_SUCCESS(true, false, BW); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_INIT_SUCCESS(true, false, BW, BW); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); + EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_INIT_SUCCESS(true, true, BW, GEN); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_INIT_SUCCESS(true, true, BW, GEN, FW); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); } TEST_F(SerialTest, interface_detection) { // derive propagation direction from inner generator EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> -> - auto it = serial.pimpl()->children().begin(); + auto it = container.pimpl()->children().begin(); EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_INIT_SUCCESS(true, true, GEN, ANY); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_INIT_SUCCESS(true, true, ANY, GEN); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); // derive propagation direction from inner connector EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <- - it = serial.pimpl()->children().begin(); + it = container.pimpl()->children().begin(); EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); EXPECT_INIT_SUCCESS(false, false, CONN, ANY); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); EXPECT_INIT_SUCCESS(false, false, ANY, CONN); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); // derive propagation direction from outer interface EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> -> - it = serial.pimpl()->children().begin(); + it = container.pimpl()->children().begin(); EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); EXPECT_INIT_SUCCESS(true, false, ANY, ANY); // <- <- - it = serial.pimpl()->children().begin(); + it = container.pimpl()->children().begin(); EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); + EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <> - it = serial.pimpl()->children().begin(); + it = container.pimpl()->children().begin(); EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- -- - it = serial.pimpl()->children().begin(); + it = container.pimpl()->children().begin(); EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), UNKNOWN); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), UNKNOWN); + EXPECT_EQ(container.pimpl()->interfaceFlags(), UNKNOWN); +} + + +class ParallelTest : public InitTest { +}; + +TEST_F(ParallelTest, init_propagating) { + EXPECT_INIT_SUCCESS(true, true, BW, FW); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + + EXPECT_INIT_SUCCESS(true, true, BOTH, BOTH); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + + EXPECT_INIT_SUCCESS(true, true, BW, BOTH); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + + EXPECT_INIT_SUCCESS(true, true, FW, BOTH); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + + EXPECT_INIT_SUCCESS(true, true, FW, BOTH, FW); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + + EXPECT_INIT_SUCCESS(true, true, FW, BW, FW); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + + EXPECT_INIT_SUCCESS(true, true, BW, FW, BW); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS); + + EXPECT_INIT_SUCCESS(false, true, FW, FW); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS); + + EXPECT_INIT_SUCCESS(true, false, BW, BW); + EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS); + + // external interface doesn't match derived + EXPECT_INIT_FAILURE(false, false, BOTH); + EXPECT_INIT_FAILURE(false, true, BOTH); + EXPECT_INIT_FAILURE(true, false, BOTH); + + EXPECT_INIT_FAILURE(false, false, BW, BW); + EXPECT_INIT_FAILURE(false, true, BW, BW); + EXPECT_INIT_FAILURE(true, true, BW, BW); + + EXPECT_INIT_FAILURE(false, false, FW, FW); + EXPECT_INIT_FAILURE(true, false, FW, FW); + EXPECT_INIT_FAILURE(true, true, FW, FW); +} + +TEST_F(ParallelTest, init_matching) { + EXPECT_INIT_SUCCESS(true, true, GEN, GEN); + EXPECT_EQ(container.pimpl()->interfaceFlags(), GENERATE); + + EXPECT_INIT_SUCCESS(false, false, CONN, CONN); + EXPECT_EQ(container.pimpl()->interfaceFlags(), CONNECT); + + // external interface doesn't match derived + EXPECT_INIT_FAILURE(false, false, GEN, GEN); + EXPECT_INIT_FAILURE(true, false, GEN, GEN); + EXPECT_INIT_FAILURE(false, true, GEN, GEN); + + EXPECT_INIT_FAILURE(true, false, CONN, CONN); + EXPECT_INIT_FAILURE(false, true, CONN, CONN); + EXPECT_INIT_FAILURE(true, true, CONN, CONN); +} + +TEST_F(ParallelTest, init_mismatching) { + EXPECT_INIT_FAILURE(false, false, BW, CONN); + EXPECT_INIT_FAILURE(true, false, BW, CONN); + EXPECT_INIT_FAILURE(false, true, BW, CONN); + EXPECT_INIT_FAILURE(true, true, BW, CONN); + + EXPECT_INIT_FAILURE(false, false, FW, CONN); + EXPECT_INIT_FAILURE(true, false, FW, CONN); + EXPECT_INIT_FAILURE(false, true, FW, CONN); + EXPECT_INIT_FAILURE(true, true, FW, CONN); + + EXPECT_INIT_FAILURE(false, false, BW, GEN); + EXPECT_INIT_FAILURE(true, false, BW, GEN); + EXPECT_INIT_FAILURE(false, true, BW, GEN); + EXPECT_INIT_FAILURE(true, true, BW, GEN); + + EXPECT_INIT_FAILURE(false, false, FW, GEN); + EXPECT_INIT_FAILURE(true, false, FW, GEN); + EXPECT_INIT_FAILURE(false, true, FW, GEN); + EXPECT_INIT_FAILURE(true, true, FW, GEN); + + EXPECT_INIT_FAILURE(false, false, CONN, GEN); + EXPECT_INIT_FAILURE(true, false, CONN, GEN); + EXPECT_INIT_FAILURE(false, true, CONN, GEN); + EXPECT_INIT_FAILURE(true, true, CONN, GEN); + + EXPECT_INIT_FAILURE(false, false, BOTH, CONN); + EXPECT_INIT_FAILURE(true, false, BOTH, CONN); + EXPECT_INIT_FAILURE(false, true, BOTH, CONN); + EXPECT_INIT_FAILURE(true, true, BOTH, CONN); + + EXPECT_INIT_FAILURE(false, false, BOTH, GEN); + EXPECT_INIT_FAILURE(true, false, BOTH, GEN); + EXPECT_INIT_FAILURE(false, true, BOTH, GEN); + EXPECT_INIT_FAILURE(true, true, BOTH, GEN); } From 0dc4f5c53ec651f7e56f9a572ffbf0c819d53a34 Mon Sep 17 00:00:00 2001 From: eirtech Date: Fri, 23 Feb 2018 14:55:06 +0100 Subject: [PATCH 3/6] ParallelContainer's requiredInterface() --- core/src/container.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index ce5dd8b6..09e2db0a 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -702,14 +702,31 @@ InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const { if (children().empty()) return UNKNOWN; - /* TODO: replace this with a proper check for consistency of interfaces. Allowed combinations are: + /* The interfaces of all children need to be consistent with each other. Allowed combinations are: * ❘ ❘ = ❘ (connecting stages) * ↑ ↑ = ↑ (backward propagating) * ↓ ↓ = ↓ (forward propagating) * ↑ ↓ = ⇅ = ⇅ ↑ = ⇅ ↓ (propagating in both directions) * ↕ ↕ = ↕ (generating) */ - return children().front()->pimpl()->requiredInterface(); + + InterfaceFlags accumulated = children().front()->pimpl()->requiredInterface(); + for (const Stage::pointer& stage : children()) { + InterfaceFlags current = stage->pimpl()->requiredInterface(); + if (accumulated != PROPAGATE_BOTHWAYS && + (accumulated & current) == current) // all flags of current are already available in accumulated + continue; + + bool current_is_propagating = (current == PROPAGATE_BOTHWAYS || + current == PROPAGATE_FORWARDS || + current == PROPAGATE_BACKWARDS); + + if (current_is_propagating && accumulated != CONNECT && accumulated != GENERATE) + accumulated |= current; // propagating is compatible to all except CONNECT and GENERATE + else + throw InitStageException(*me(), "child '" + stage->name() + "' has conflicting interface to previous children"); + } + return accumulated; } void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) From bcacda85070f7d1dd42bebf6cf04427bb9f659ae Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2018 12:04:30 +0100 Subject: [PATCH 4/6] implement ParallelContainer's pruneInterface() --- core/src/container.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 09e2db0a..288b5ca4 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -731,7 +731,11 @@ InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { - // TODO: forward pruning to all children with UNKNOWN required interface + // forward pruning to all children with UNKNOWN required interface + for (const Stage::pointer& stage : children()) { + if (stage->pimpl()->requiredInterface() == UNKNOWN) + stage->pimpl()->pruneInterface(accepted); + } } void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) { From dcf23f6147624c7eee02f0f9daec48693e319949 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2018 12:05:17 +0100 Subject: [PATCH 5/6] relax ParallelContainer's validateConnectivity() --- core/src/container.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 288b5ca4..c00b8c41 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -802,9 +802,10 @@ void ParallelContainerBase::validateConnectivity() const auto impl = pimpl(); InterfaceFlags my_interface = impl->interfaceFlags(); - // check that input / output interfaces of all children match my_interface + // check that input / output interfaces of all children are handled by my interface for (const auto& child : pimpl()->children()) { - if (child->pimpl()->interfaceFlags() != my_interface) + InterfaceFlags current = child->pimpl()->interfaceFlags(); + if ((current & my_interface) != current) errors.push_back(*this, "interface of child '" + child->name() + "' doesn't match mine"); } From 99adfaf45407b6f1875938835c07d55556d3cf91 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2018 13:21:15 +0100 Subject: [PATCH 6/6] fix ParallelContainers' init() --- core/src/container.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index c00b8c41..1ba3ea72 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -775,9 +775,7 @@ void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_m // determine the union of interfaces required by children // TODO: should we better use the least common interface? - InterfaceFlags required; - for (const Stage::pointer& stage : impl->children()) - required |= stage->pimpl()->requiredInterface(); + InterfaceFlags required = impl->requiredInterface(); // initialize this' pull connections impl->starts().reset(required & READS_START @@ -801,13 +799,18 @@ void ParallelContainerBase::validateConnectivity() const InitStageException errors; auto impl = pimpl(); InterfaceFlags my_interface = impl->interfaceFlags(); + InterfaceFlags children_interfaces; // check that input / output interfaces of all children are handled by my interface for (const auto& child : pimpl()->children()) { InterfaceFlags current = child->pimpl()->interfaceFlags(); + children_interfaces |= current; // compute union of all children interfaces if ((current & my_interface) != current) errors.push_back(*this, "interface of child '" + child->name() + "' doesn't match mine"); } + // check that there is a child matching the expected push interfaces + if ((my_interface & GENERATE) != (children_interfaces & GENERATE)) + errors.push_back(*this, "no child provides expected push interface"); // recursively validate children try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }