From 28703af384fb1a0b60fb889582ef15c3a8e8b28f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 2 May 2019 17:33:13 +0200 Subject: [PATCH 1/6] improve comments --- core/include/moveit/task_constructor/stage.h | 7 +++++++ core/src/container.cpp | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index d606ab81..eeda36ec 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -235,6 +235,13 @@ protected: class PropagatingEitherWayPrivate; +/** Base class for stages that can propagate InterfaceStates + * + * They read an InterfaceState on one side (start or end) and + * push a new InterfaceState to the opposite site. + * By default, the class auto-derives its actual propagation direction from the context. + * In order to enforce forward, backward, or bothway propagation, one can use restrictDirection(). + */ class PropagatingEitherWay : public ComputeBase { public: PRIVATE_CLASS(PropagatingEitherWay) diff --git a/core/src/container.cpp b/core/src/container.cpp index ae1bc21c..27ca9ba7 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -519,7 +519,7 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs } // else: for last child we cannot determine the interface yet // nothing to do if: - // - accepted == 0: interface still unknown + // - accepted == UNKNOWN: interface still unknown // - accepted == PROPAGATE_BOTHWAYS: no change if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS) pruneInterfaces(first, end, accepted); From f402cdec5e8db421f45817f1eb6fe7efae87cb87 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 2 May 2019 17:49:18 +0200 Subject: [PATCH 2/6] rename [INPUT|OUTPUT]_IF_MASK -> [START|END]_IF_MASK --- core/include/moveit/task_constructor/stage.h | 4 ++-- core/src/container.cpp | 20 ++++++++++---------- core/src/stage.cpp | 8 ++++---- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index eeda36ec..60bca27e 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -83,8 +83,8 @@ typedef Flags InterfaceFlags; // some useful constants 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 START_IF_MASK({READS_START, WRITES_PREV_END}); +constexpr InterfaceFlags END_IF_MASK({READS_END, WRITES_NEXT_START}); constexpr InterfaceFlags PROPAGATE_BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}); MOVEIT_CLASS_FORWARD(Interface) diff --git a/core/src/container.cpp b/core/src/container.cpp index 27ca9ba7..0f7d6d9a 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -365,8 +365,8 @@ InterfaceFlags SerialContainerPrivate::requiredInterface() const { if (children().empty()) return UNKNOWN; - return (children().front()->pimpl()->interfaceFlags() & INPUT_IF_MASK) - | (children().back()->pimpl()->interfaceFlags() & OUTPUT_IF_MASK); + return (children().front()->pimpl()->interfaceFlags() & START_IF_MASK) + | (children().back()->pimpl()->interfaceFlags() & END_IF_MASK); } // connect cur stage to its predecessor and successor by setting the push interface pointers @@ -472,9 +472,9 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { } else { // otherwise only prune the first / last child's input / output interface StagePrivate* child_impl; child_impl = children().front()->pimpl(); - child_impl->pruneInterface((accepted & INPUT_IF_MASK) | (child_impl->interfaceFlags() & OUTPUT_IF_MASK)); + child_impl->pruneInterface((accepted & START_IF_MASK) | (child_impl->interfaceFlags() & END_IF_MASK)); child_impl = children().back()->pimpl(); - child_impl->pruneInterface((accepted & OUTPUT_IF_MASK) | (child_impl->interfaceFlags() & INPUT_IF_MASK)); + child_impl->pruneInterface((accepted & END_IF_MASK) | (child_impl->interfaceFlags() & START_IF_MASK)); } // reset my pull interfaces, if first/last child don't pull anymore @@ -563,16 +563,16 @@ void SerialContainerPrivate::validateConnectivity() const if (!children().empty()) { const StagePrivate* start = children().front()->pimpl(); const auto my_flags = this->interfaceFlags(); - auto child_flags = start->interfaceFlags() & INPUT_IF_MASK; - if (child_flags != (my_flags & INPUT_IF_MASK)) + auto child_flags = start->interfaceFlags() & START_IF_MASK; + if (child_flags != (my_flags & START_IF_MASK)) errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags) - % flowSymbol(my_flags & INPUT_IF_MASK)).str()); + % flowSymbol(my_flags & START_IF_MASK)).str()); const StagePrivate* last = children().back()->pimpl(); - child_flags = last->interfaceFlags() & OUTPUT_IF_MASK; - if (child_flags != (my_flags & OUTPUT_IF_MASK)) + child_flags = last->interfaceFlags() & END_IF_MASK; + if (child_flags != (my_flags & END_IF_MASK)) errors.push_back(*me(), (desc % "output" % last->name() % flowSymbol(child_flags) - % flowSymbol(my_flags & OUTPUT_IF_MASK)).str()); + % flowSymbol(my_flags & END_IF_MASK)).str()); } // validate connectivity of children amongst each other diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b868a232..a2dfe914 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -351,7 +351,7 @@ const char* direction(const StagePrivate& stage) { bool own_if = f & own; bool other_if = f & other; - bool reverse = own & INPUT_IF_MASK; + bool reverse = own & START_IF_MASK; if (own_if && other_if) return "<>"; if (!own_if && !other_if) return "--"; if (other_if ^ reverse) return "->"; @@ -363,12 +363,12 @@ const char* flowSymbol(InterfaceFlags f) { return "?"; // unknown interface // f should have either INPUT or OUTPUT flags set (not both) - assert(static_cast(f & INPUT_IF_MASK) ^ static_cast(f & OUTPUT_IF_MASK)); + assert(static_cast(f & START_IF_MASK) ^ static_cast(f & END_IF_MASK)); - if (f & INPUT_IF_MASK) { + if (f & START_IF_MASK) { if (f == READS_START) return "->"; if (f == WRITES_PREV_END) return "<-"; - } else if (f & OUTPUT_IF_MASK) { + } else if (f & END_IF_MASK) { if (f == READS_END) return "<-"; if (f == WRITES_NEXT_START) return "->"; } From 135951f69083fc475d525b5bf3e98e4620b47f46 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 2 May 2019 18:07:44 +0200 Subject: [PATCH 3/6] replace assertion by exception --- 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 0f7d6d9a..0e3924b0 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -533,8 +533,9 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs // 1st sweep: remove push interfaces for (auto it = first; it != end; ++it) { StagePrivate* impl = (*it)->pimpl(); - // range should only contain stages with unknown required interface - assert(impl->requiredInterface() == UNKNOWN); + // the required interface should be a subset of the accepted one + if ((impl->requiredInterface() & accepted) != impl->requiredInterface()) + throw InitStageException(*impl->me(), "Required interface not satisfied after pruning"); // remove push interfaces if not accepted if (!(accepted & WRITES_PREV_END)) From fd1aa38a39550382fe8879316d1d2cb79c302b4f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 2 May 2019 18:09:39 +0200 Subject: [PATCH 4/6] fix unit test definitions --- core/test/test_container.cpp | 20 +++++--------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 17403b25..65af6002 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -287,18 +287,11 @@ TEST_F(SerialTest, init_forward) { EXPECT_INIT_SUCCESS(true, false, BW, ANY); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); - - EXPECT_INIT_SUCCESS(false, true, BOTH, FW); - EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); - - EXPECT_INIT_SUCCESS(false, true, FW, BOTH); - EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); - - EXPECT_INIT_SUCCESS(true, false, BOTH, BW); - EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); - - EXPECT_INIT_SUCCESS(true, false, BW, BOTH); - EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); + // BOTH requires propagation in both directions, while FW/BW can only match one dir + EXPECT_INIT_FAILURE(true, true, BOTH, FW); + EXPECT_INIT_FAILURE(true, true, FW, BOTH); + EXPECT_INIT_FAILURE(true, true, BOTH, BW); + EXPECT_INIT_FAILURE(true, true, BW, BOTH); } TEST_F(SerialTest, init_backward) { @@ -316,9 +309,6 @@ TEST_F(SerialTest, init_backward) { EXPECT_INIT_SUCCESS(true, true, BW, GEN); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); - - EXPECT_INIT_SUCCESS(true, true, BW, GEN, FW); - EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); } TEST_F(SerialTest, interface_detection) { From d9a38f46ab1c82d859d3c2e0f465e72eccac5956 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 2 May 2019 23:26:32 +0200 Subject: [PATCH 5/6] SerialContainer: fix nested interface resolution We need to distinguish two cases for how the interface of a nested serial container is determined: 1. from its children 2. from its (outer) context As long as the interface is not fully resolved, requiredInterface() returns UNKNOWN. After pruning, the first/last child's interface is remembered and reported instead. --- .../moveit/task_constructor/container_p.h | 6 ++++ core/src/container.cpp | 31 +++++++++++++++---- 2 files changed, 31 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index fe05a772..436ec1cb 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -178,6 +178,12 @@ private: void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end, InterfaceFlags accepted); + // store first/last child's interface as required for this container + void storeRequiredInterface(container_type::const_iterator first, + container_type::const_iterator end); + +private: + InterfaceFlags required_interface_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/src/container.cpp b/core/src/container.cpp index 0e3924b0..fd21b66c 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -359,14 +359,17 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s : ContainerBasePrivate(me, name) {} -// a serial container's required interface is derived from the actual input interface -// of the first child and the actual output interface of the last child +// a serial container's required interface is derived from the required input interfaces +// of the first and last children. After resolving, it is remembered in required_interface_. InterfaceFlags SerialContainerPrivate::requiredInterface() const { + if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK)) + return required_interface_; + if (children().empty()) return UNKNOWN; - return (children().front()->pimpl()->interfaceFlags() & START_IF_MASK) - | (children().back()->pimpl()->interfaceFlags() & END_IF_MASK); + return (children().front()->pimpl()->requiredInterface() & START_IF_MASK) + | (children().back()->pimpl()->requiredInterface() & END_IF_MASK); } // connect cur stage to its predecessor and successor by setting the push interface pointers @@ -415,6 +418,7 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) auto impl = pimpl(); impl->starts_.reset(); impl->ends_.reset(); + impl->required_interface_ = UNKNOWN; // recursively init all children, throws if there are no children ContainerBase::init(robot_model); @@ -455,6 +459,16 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) std::placeholders::_2))); } +// prune interface for children in range [first, last) to given direction +void SerialContainerPrivate::storeRequiredInterface(container_type::const_iterator first, + container_type::const_iterator end) +{ + if (first == children().begin()) + required_interface_ |= children().front()->pimpl()->interfaceFlags() & START_IF_MASK; + if (end == children().end() && !children().empty()) + required_interface_ |= children().back()->pimpl()->interfaceFlags() & END_IF_MASK; +} + // called by parent asking for pruning of this' interface void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (children().empty()) return; @@ -483,7 +497,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (!children().back()->pimpl()->ends()) ends_.reset(); - if (interfaceFlags() == InterfaceFlags()) + if (interfaceFlags() == UNKNOWN) throw InitStageException(*me(), "failed to derive propagation direction"); } @@ -492,7 +506,10 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end) { - if (first == end) return; // nothing to do in this case + if (first == end) { + storeRequiredInterface(first, end); + return; // nothing to do in this case + } // determine accepted interface from available push interfaces InterfaceFlags accepted; @@ -549,6 +566,8 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs StagePrivate* impl = (*it)->pimpl(); impl->pruneInterface(accepted); } + + storeRequiredInterface(first, end); } void SerialContainerPrivate::validateConnectivity() const From 102c80dda1f99c686d4b7109c94edcb0c40eeb9a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 3 May 2019 01:04:06 +0200 Subject: [PATCH 6/6] ParallelContainer: fix interface error reporting need to separately check for start/end interface --- .../moveit/task_constructor/container_p.h | 3 +++ core/src/container.cpp | 25 +++++++++++++------ core/src/stage.cpp | 12 ++++----- 3 files changed, 26 insertions(+), 14 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 436ec1cb..013741d6 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -122,6 +122,9 @@ protected: bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN); child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } + // report error about mismatching interface (start or end as determined by mask) + void mismatchingInterface(InitStageException& errors, const StagePrivate& child, + const InterfaceFlags mask) const; /// copy external_state to a child's interface and remember the link in internal_to map void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); diff --git a/core/src/container.cpp b/core/src/container.cpp index fd21b66c..ede9e86c 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -100,6 +100,16 @@ void ContainerBasePrivate::validateConnectivity() const if (errors) throw errors; } +void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child, + const InterfaceFlags mask) const { + boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)"); + errors.push_back(*me(), (desc + % (mask == START_IF_MASK ? "start" : "end") + % child.name() + % flowSymbol(child.interfaceFlags() & mask) + % flowSymbol(interfaceFlags() & mask)).str()); +} + bool ContainerBasePrivate::canCompute() const { // call the method of the public interface @@ -573,7 +583,6 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs void SerialContainerPrivate::validateConnectivity() const { InitStageException errors; - boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)"); // recursively validate children try { ContainerBasePrivate::validateConnectivity(); } @@ -585,14 +594,12 @@ void SerialContainerPrivate::validateConnectivity() const const auto my_flags = this->interfaceFlags(); auto child_flags = start->interfaceFlags() & START_IF_MASK; if (child_flags != (my_flags & START_IF_MASK)) - errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags) - % flowSymbol(my_flags & START_IF_MASK)).str()); + mismatchingInterface(errors, *start, START_IF_MASK); const StagePrivate* last = children().back()->pimpl(); child_flags = last->interfaceFlags() & END_IF_MASK; if (child_flags != (my_flags & END_IF_MASK)) - errors.push_back(*me(), (desc % "output" % last->name() % flowSymbol(child_flags) - % flowSymbol(my_flags & END_IF_MASK)).str()); + mismatchingInterface(errors, *last, END_IF_MASK); } // validate connectivity of children amongst each other @@ -722,14 +729,16 @@ void ParallelContainerBasePrivate::validateConnectivity() const InitStageException errors; InterfaceFlags my_interface = interfaceFlags(); InterfaceFlags children_interfaces; - boost::format desc("interface of child '%1%' (%2%) doesn't match mine (%3%)"); // check that input / output interfaces of all children are handled by my interface for (const auto& child : children()) { InterfaceFlags current = child->pimpl()->interfaceFlags(); children_interfaces |= current; // compute union of all children interfaces - if ((current & my_interface) != current) - errors.push_back(*me(), (desc % child->name() % flowSymbol(current) % flowSymbol(my_interface)).str()); + + if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK)) + mismatchingInterface(errors, *child->pimpl(), START_IF_MASK); + if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK)) + mismatchingInterface(errors, *child->pimpl(), END_IF_MASK); } // check that there is a child matching the expected push interfaces if ((my_interface & GENERATE) != (children_interfaces & GENERATE)) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index a2dfe914..9ba86fea 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -359,20 +359,20 @@ const char* direction(const StagePrivate& stage) { } const char* flowSymbol(InterfaceFlags f) { - if (f == InterfaceFlags()) + if (f == UNKNOWN) return "?"; // unknown interface // f should have either INPUT or OUTPUT flags set (not both) assert(static_cast(f & START_IF_MASK) ^ static_cast(f & END_IF_MASK)); if (f & START_IF_MASK) { - if (f == READS_START) return "->"; - if (f == WRITES_PREV_END) return "<-"; + if (f == READS_START) return "↓"; + if (f == WRITES_PREV_END) return "↑"; } else if (f & END_IF_MASK) { - if (f == READS_END) return "<-"; - if (f == WRITES_NEXT_START) return "->"; + if (f == READS_END) return "↑"; + if (f == WRITES_NEXT_START) return "↓"; } - return "<->"; + return "⇅"; } std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {