diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 58c358a7..178bed28 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -125,6 +125,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); @@ -181,6 +184,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/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 59fc8c2b..3d131aa5 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) @@ -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 e12a5475..5114a35a 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -101,6 +101,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 @@ -389,14 +399,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() & INPUT_IF_MASK) - | (children().back()->pimpl()->interfaceFlags() & OUTPUT_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 @@ -445,6 +458,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); @@ -485,6 +499,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; @@ -502,9 +526,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 @@ -513,7 +537,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"); } @@ -522,7 +546,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,7 +576,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); @@ -563,8 +590,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)) @@ -578,12 +606,13 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs StagePrivate* impl = (*it)->pimpl(); impl->pruneInterface(accepted); } + + storeRequiredInterface(first, end); } 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(); } @@ -593,16 +622,14 @@ 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)) - errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags) - % flowSymbol(my_flags & INPUT_IF_MASK)).str()); + auto child_flags = start->interfaceFlags() & START_IF_MASK; + if (child_flags != (my_flags & START_IF_MASK)) + mismatchingInterface(errors, *start, START_IF_MASK); const StagePrivate* last = children().back()->pimpl(); - child_flags = last->interfaceFlags() & OUTPUT_IF_MASK; - if (child_flags != (my_flags & OUTPUT_IF_MASK)) - errors.push_back(*me(), (desc % "output" % last->name() % flowSymbol(child_flags) - % flowSymbol(my_flags & OUTPUT_IF_MASK)).str()); + child_flags = last->interfaceFlags() & END_IF_MASK; + if (child_flags != (my_flags & END_IF_MASK)) + mismatchingInterface(errors, *last, END_IF_MASK); } // validate connectivity of children amongst each other @@ -738,14 +765,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 579ad9c6..2c34e20f 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -352,7 +352,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 "->"; @@ -360,20 +360,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 & 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 == READS_START) return "->"; - if (f == WRITES_PREV_END) return "<-"; - } else if (f & OUTPUT_IF_MASK) { - if (f == READS_END) return "<-"; - if (f == WRITES_NEXT_START) return "->"; + if (f & START_IF_MASK) { + 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 "↓"; } - return "<->"; + return "⇅"; } std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index b3bf51a7..8a215e0f 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -304,18 +304,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) { @@ -333,9 +326,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) {