improve SerialContainerPrivate::pruneInterface

This commit is contained in:
Robert Haschke 2019-03-11 09:53:39 +01:00
parent 905b6a3b0c
commit 48d93e803d

View File

@ -366,7 +366,7 @@ InterfaceFlags SerialContainerPrivate::requiredInterface() const
if (children().empty()) if (children().empty())
return UNKNOWN; return UNKNOWN;
return (children().front()->pimpl()->interfaceFlags() & INPUT_IF_MASK) return (children().front()->pimpl()->interfaceFlags() & INPUT_IF_MASK)
| (children().back()->pimpl()->interfaceFlags() & OUTPUT_IF_MASK); | (children().back()->pimpl()->interfaceFlags() & OUTPUT_IF_MASK);
} }
// connect cur stage to its predecessor and successor by setting the push interface pointers // connect cur stage to its predecessor and successor by setting the push interface pointers
@ -459,10 +459,12 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (children().empty()) return; if (children().empty()) return;
// We only need to deal with the special case of the whole sequence to be pruned. if (accepted == PROPAGATE_BOTHWAYS)
if (accepted != PROPAGATE_BOTHWAYS && // will interface be restricted at all? return; // There is nothing to prune
children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) // still undecided?
{ // If whole chain is still undecided, prune all children
if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS &&
children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) {
pruneInterfaces(children().begin(), children().end(), accepted); pruneInterfaces(children().begin(), children().end(), accepted);
// reset my pull interfaces, if first/last child don't pull anymore // reset my pull interfaces, if first/last child don't pull anymore
@ -471,7 +473,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (!children().back()->pimpl()->ends()) if (!children().back()->pimpl()->ends())
ends_.reset(); ends_.reset();
} }
if (interfaceFlags() == UNKNOWN)
if (interfaceFlags() == InterfaceFlags())
throw InitStageException(*me(), "failed to derive propagation direction"); throw InitStageException(*me(), "failed to derive propagation direction");
} }
@ -508,8 +511,8 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
// nothing to do if: // nothing to do if:
// - accepted == 0: interface still unknown // - accepted == 0: interface still unknown
// - accepted == PROPAGATE_FORWARDS | PROPAGATE_BACKWARDS: no change // - accepted == PROPAGATE_BOTHWAYS: no change
if (accepted != UNKNOWN && accepted != InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})) if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
pruneInterfaces(first, end, accepted); pruneInterfaces(first, end, accepted);
} }