Merge branch 'fix-interface-deduction' into master

This commit is contained in:
Robert Haschke 2019-05-03 01:53:57 +02:00
commit fee73bcd78
5 changed files with 85 additions and 50 deletions

View File

@ -125,6 +125,9 @@ protected:
bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN); bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); 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 /// 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); void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
@ -181,6 +184,12 @@ private:
void pruneInterfaces(container_type::const_iterator first, void pruneInterfaces(container_type::const_iterator first,
container_type::const_iterator end, container_type::const_iterator end,
InterfaceFlags accepted); 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) PIMPL_FUNCTIONS(SerialContainer)

View File

@ -83,8 +83,8 @@ typedef Flags<InterfaceFlag> InterfaceFlags;
// some useful constants // some useful constants
constexpr InterfaceFlags UNKNOWN; constexpr InterfaceFlags UNKNOWN;
constexpr InterfaceFlags INPUT_IF_MASK({READS_START, WRITES_PREV_END}); constexpr InterfaceFlags START_IF_MASK({READS_START, WRITES_PREV_END});
constexpr InterfaceFlags OUTPUT_IF_MASK({READS_END, WRITES_NEXT_START}); constexpr InterfaceFlags END_IF_MASK({READS_END, WRITES_NEXT_START});
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}); constexpr InterfaceFlags PROPAGATE_BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS});
MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Interface)
@ -235,6 +235,13 @@ protected:
class PropagatingEitherWayPrivate; 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 { class PropagatingEitherWay : public ComputeBase {
public: public:
PRIVATE_CLASS(PropagatingEitherWay) PRIVATE_CLASS(PropagatingEitherWay)

View File

@ -101,6 +101,16 @@ void ContainerBasePrivate::validateConnectivity() const
if (errors) throw errors; 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 bool ContainerBasePrivate::canCompute() const
{ {
// call the method of the public interface // call the method of the public interface
@ -389,14 +399,17 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
{} {}
// a serial container's required interface is derived from the actual input interface // a serial container's required interface is derived from the required input interfaces
// of the first child and the actual output interface of the last child // of the first and last children. After resolving, it is remembered in required_interface_.
InterfaceFlags SerialContainerPrivate::requiredInterface() const InterfaceFlags SerialContainerPrivate::requiredInterface() const
{ {
if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK))
return required_interface_;
if (children().empty()) if (children().empty())
return UNKNOWN; return UNKNOWN;
return (children().front()->pimpl()->interfaceFlags() & INPUT_IF_MASK) return (children().front()->pimpl()->requiredInterface() & START_IF_MASK)
| (children().back()->pimpl()->interfaceFlags() & OUTPUT_IF_MASK); | (children().back()->pimpl()->requiredInterface() & END_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
@ -445,6 +458,7 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
auto impl = pimpl(); auto impl = pimpl();
impl->starts_.reset(); impl->starts_.reset();
impl->ends_.reset(); impl->ends_.reset();
impl->required_interface_ = UNKNOWN;
// recursively init all children, throws if there are no children // recursively init all children, throws if there are no children
ContainerBase::init(robot_model); ContainerBase::init(robot_model);
@ -485,6 +499,16 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
std::placeholders::_2))); 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 // called by parent asking for pruning of this' interface
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (children().empty()) return; 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 } else { // otherwise only prune the first / last child's input / output interface
StagePrivate* child_impl; StagePrivate* child_impl;
child_impl = children().front()->pimpl(); 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 = 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 // 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()) if (!children().back()->pimpl()->ends())
ends_.reset(); ends_.reset();
if (interfaceFlags() == InterfaceFlags()) if (interfaceFlags() == UNKNOWN)
throw InitStageException(*me(), "failed to derive propagation direction"); 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, void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first,
container_type::const_iterator end) 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 // determine accepted interface from available push interfaces
InterfaceFlags accepted; InterfaceFlags accepted;
@ -549,7 +576,7 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
} // else: for last child we cannot determine the interface yet } // else: for last child we cannot determine the interface yet
// nothing to do if: // nothing to do if:
// - accepted == 0: interface still unknown // - accepted == UNKNOWN: interface still unknown
// - accepted == PROPAGATE_BOTHWAYS: no change // - accepted == PROPAGATE_BOTHWAYS: no change
if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS) if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
pruneInterfaces(first, end, accepted); pruneInterfaces(first, end, accepted);
@ -563,8 +590,9 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
// 1st sweep: remove push interfaces // 1st sweep: remove push interfaces
for (auto it = first; it != end; ++it) { for (auto it = first; it != end; ++it) {
StagePrivate* impl = (*it)->pimpl(); StagePrivate* impl = (*it)->pimpl();
// range should only contain stages with unknown required interface // the required interface should be a subset of the accepted one
assert(impl->requiredInterface() == UNKNOWN); if ((impl->requiredInterface() & accepted) != impl->requiredInterface())
throw InitStageException(*impl->me(), "Required interface not satisfied after pruning");
// remove push interfaces if not accepted // remove push interfaces if not accepted
if (!(accepted & WRITES_PREV_END)) if (!(accepted & WRITES_PREV_END))
@ -578,12 +606,13 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
StagePrivate* impl = (*it)->pimpl(); StagePrivate* impl = (*it)->pimpl();
impl->pruneInterface(accepted); impl->pruneInterface(accepted);
} }
storeRequiredInterface(first, end);
} }
void SerialContainerPrivate::validateConnectivity() const void SerialContainerPrivate::validateConnectivity() const
{ {
InitStageException errors; InitStageException errors;
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
// recursively validate children // recursively validate children
try { ContainerBasePrivate::validateConnectivity(); } try { ContainerBasePrivate::validateConnectivity(); }
@ -593,16 +622,14 @@ void SerialContainerPrivate::validateConnectivity() const
if (!children().empty()) { if (!children().empty()) {
const StagePrivate* start = children().front()->pimpl(); const StagePrivate* start = children().front()->pimpl();
const auto my_flags = this->interfaceFlags(); const auto my_flags = this->interfaceFlags();
auto child_flags = start->interfaceFlags() & INPUT_IF_MASK; auto child_flags = start->interfaceFlags() & START_IF_MASK;
if (child_flags != (my_flags & INPUT_IF_MASK)) if (child_flags != (my_flags & START_IF_MASK))
errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags) mismatchingInterface(errors, *start, START_IF_MASK);
% flowSymbol(my_flags & INPUT_IF_MASK)).str());
const StagePrivate* last = children().back()->pimpl(); const StagePrivate* last = children().back()->pimpl();
child_flags = last->interfaceFlags() & OUTPUT_IF_MASK; child_flags = last->interfaceFlags() & END_IF_MASK;
if (child_flags != (my_flags & OUTPUT_IF_MASK)) if (child_flags != (my_flags & END_IF_MASK))
errors.push_back(*me(), (desc % "output" % last->name() % flowSymbol(child_flags) mismatchingInterface(errors, *last, END_IF_MASK);
% flowSymbol(my_flags & OUTPUT_IF_MASK)).str());
} }
// validate connectivity of children amongst each other // validate connectivity of children amongst each other
@ -738,14 +765,16 @@ void ParallelContainerBasePrivate::validateConnectivity() const
InitStageException errors; InitStageException errors;
InterfaceFlags my_interface = interfaceFlags(); InterfaceFlags my_interface = interfaceFlags();
InterfaceFlags children_interfaces; 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 // check that input / output interfaces of all children are handled by my interface
for (const auto& child : children()) { for (const auto& child : children()) {
InterfaceFlags current = child->pimpl()->interfaceFlags(); InterfaceFlags current = child->pimpl()->interfaceFlags();
children_interfaces |= current; // compute union of all children interfaces 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 // check that there is a child matching the expected push interfaces
if ((my_interface & GENERATE) != (children_interfaces & GENERATE)) if ((my_interface & GENERATE) != (children_interfaces & GENERATE))

View File

@ -352,7 +352,7 @@ const char* direction(const StagePrivate& stage) {
bool own_if = f & own; bool own_if = f & own;
bool other_if = f & other; 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 (!own_if && !other_if) return "--"; if (!own_if && !other_if) return "--";
if (other_if ^ reverse) return "->"; if (other_if ^ reverse) return "->";
@ -360,20 +360,20 @@ const char* direction(const StagePrivate& stage) {
} }
const char* flowSymbol(InterfaceFlags f) { const char* flowSymbol(InterfaceFlags f) {
if (f == InterfaceFlags()) if (f == UNKNOWN)
return "?"; // unknown interface return "?"; // unknown interface
// f should have either INPUT or OUTPUT flags set (not both) // f should have either INPUT or OUTPUT flags set (not both)
assert(static_cast<bool>(f & INPUT_IF_MASK) ^ static_cast<bool>(f & OUTPUT_IF_MASK)); assert(static_cast<bool>(f & START_IF_MASK) ^ static_cast<bool>(f & END_IF_MASK));
if (f & INPUT_IF_MASK) { if (f & START_IF_MASK) {
if (f == READS_START) return "->"; if (f == READS_START) return "";
if (f == WRITES_PREV_END) 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 == READS_END) return "";
if (f == WRITES_NEXT_START) return "->"; if (f == WRITES_NEXT_START) return "";
} }
return "<->"; return "";
} }
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {

View File

@ -304,18 +304,11 @@ TEST_F(SerialTest, init_forward) {
EXPECT_INIT_SUCCESS(true, false, BW, ANY); EXPECT_INIT_SUCCESS(true, false, BW, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
// BOTH requires propagation in both directions, while FW/BW can only match one dir
EXPECT_INIT_SUCCESS(false, true, BOTH, FW); EXPECT_INIT_FAILURE(true, true, BOTH, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); EXPECT_INIT_FAILURE(true, true, FW, BOTH);
EXPECT_INIT_FAILURE(true, true, BOTH, BW);
EXPECT_INIT_SUCCESS(false, true, FW, BOTH); EXPECT_INIT_FAILURE(true, true, BW, 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));
} }
TEST_F(SerialTest, init_backward) { TEST_F(SerialTest, init_backward) {
@ -333,9 +326,6 @@ TEST_F(SerialTest, init_backward) {
EXPECT_INIT_SUCCESS(true, true, BW, GEN); EXPECT_INIT_SUCCESS(true, true, BW, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); 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) { TEST_F(SerialTest, interface_detection) {