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);
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)

View File

@ -83,8 +83,8 @@ typedef Flags<InterfaceFlag> 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)

View File

@ -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))

View File

@ -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<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 == 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) {

View File

@ -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) {