recursive interface auto-detection

trigger auto-detection from top (task) level:
only there we now for sure the accepted interface
This commit is contained in:
Robert Haschke 2018-02-18 20:57:14 +01:00
parent f45a4e3e2c
commit 35f1540369
6 changed files with 91 additions and 32 deletions

View File

@ -172,19 +172,20 @@ class SerialContainerPrivate : public ContainerBasePrivate {
public:
SerialContainerPrivate(SerialContainer* me, const std::string &name);
// called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override;
private:
// connect cur stage to its predecessor and successor
bool connect(container_type::const_iterator cur);
// called by parent asking for pruning of this' interface
void pruneInterface(PropagatingEitherWay::Direction dir) override;
// called by init() to prune interfaces for children in range [first, last)
void pruneInterfaces(container_type::const_iterator first,
container_type::const_iterator end);
// prune interface for children in range [first, last) to given direction
void pruneInterfaces(container_type::const_iterator first,
container_type::const_iterator end,
PropagatingEitherWay::Direction dir);
InterfaceFlags accepted);
// set of all solutions
ordered<SerialSolution> solutions_;

View File

@ -64,7 +64,7 @@ public:
// if this is unknown (because interface is auto-detected from context), return 0
virtual InterfaceFlags requiredInterface() const = 0;
// prune interface to the given propagation direction
virtual void pruneInterface(PropagatingEitherWay::Direction direction) {}
virtual void pruneInterface(InterfaceFlags accepted) {}
virtual bool canCompute() const = 0;
virtual bool compute() = 0;
@ -169,9 +169,7 @@ public:
// initialize pull interfaces for given propagation directions
void initInterface(PropagatingEitherWay::Direction dir);
// prune interface to the given propagation direction
void pruneInterface(PropagatingEitherWay::Direction direction) override {
initInterface(direction);
}
void pruneInterface(InterfaceFlags accepted) override;
bool canCompute() const override;
bool compute() override;

View File

@ -427,8 +427,26 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
}
// called by parent asking for pruning of this' interface
void SerialContainerPrivate::pruneInterface(PropagatingEitherWay::Direction dir) {
pruneInterfaces(children().begin(), children().end(), dir);
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (children().empty()) return;
constexpr InterfaceFlags BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS});
// I think, we only need to deal with the special case of the whole sequence to be pruned.
if (accepted != BOTHWAYS && // will interface be restricted at all?
children().front()->pimpl()->interfaceFlags() == BOTHWAYS) // still undecided?
{
pruneInterfaces(children().begin(), children().end(), accepted);
// reset my pull interfaces, if first/last child don't pull anymore
if (!children().front()->pimpl()->starts())
starts_.reset();
if (!children().back()->pimpl()->ends())
ends_.reset();
}
// If my assumption doesn't hold, we would need to prune partial sequences of stages
// with unknown-only interfaces at the *beginning* and *end* of the whole sequence.
// Reuse code from init() in that case.
}
// called by init() to prune interfaces for children in range [first, last)
@ -438,40 +456,51 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
{
if (first == end) return; // nothing to do in this case
// determine feasible propagation directions from available push interfaces
int dir = 0;
// determine accepted interface from available push interfaces
InterfaceFlags accepted;
// if previous stage pushes forward, we accept forward propagation
if (first != children().begin()) {
auto prev = first; --prev; // pointer to previous stage
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START)
dir |= PropagatingEitherWay::FORWARD;
accepted |= WRITES_NEXT_START;
} // else: for first child we cannot determine the interface yet
// if end stage pushes backward, we accept backward propagation
if (end != children().end()) {
if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_END)
dir |= PropagatingEitherWay::BACKWARD;
accepted |= WRITES_PREV_END;
} // else: for last child we cannot determine the interface yet
// nothing to do if:
// - dir == 0: [first, last) covers all children, cannot determine interface
// - dir == PropagatingEitherWay::BOTHWAY: nothing changed
if (dir != 0 && dir != PropagatingEitherWay::BOTHWAY)
pruneInterfaces(first, end, PropagatingEitherWay::Direction(dir));
// - accepted == 0: interface still unknown
// - accepted == PROPAGATE_FORWARDS | PROPAGATE_BACKWARDS: no change
if (accepted != 0 && accepted != InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}))
pruneInterfaces(first, end, accepted);
}
// prune interface for children in range [first, last) to given direction
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first,
container_type::const_iterator end,
PropagatingEitherWay::Direction dir)
InterfaceFlags accepted)
{
// 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() == PropagatingEitherWay::AUTO);
// let the child do the actual work (recursively)
impl->pruneInterface(dir);
// remove push interfaces
if (!(accepted & WRITES_PREV_END))
impl->setPrevEnds(InterfacePtr());
if (!(accepted & WRITES_NEXT_START))
impl->setNextStarts(InterfacePtr());
}
// 2nd sweep: recursively prune children
for (auto it = first; it != end; ++it) {
StagePrivate* impl = (*it)->pimpl();
impl->pruneInterface(accepted);
}
}

View File

@ -283,13 +283,22 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
}
}
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
int dir = 0;
if (accepted & PROPAGATE_FORWARDS)
dir |= PropagatingEitherWay::FORWARD;
if (accepted & PROPAGATE_BACKWARDS)
dir |= PropagatingEitherWay::BACKWARD;
initInterface(PropagatingEitherWay::Direction(dir));
}
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
{
InterfaceFlags f;
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD)
f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
f |= PROPAGATE_FORWARDS;
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
f |= InterfaceFlags({READS_END, WRITES_PREV_END});
f |= PROPAGATE_BACKWARDS;
// if required_interface_dirs_ == ANYWAY, we don't require an interface
// but the parent container auto-derives the propagation direction
return f;
@ -495,7 +504,7 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
}
InterfaceFlags ConnectingPrivate::requiredInterface() const {
return InterfaceFlags( { READS_START, READS_END } );
return InterfaceFlags(CONNECT);
}
void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)

View File

@ -195,7 +195,7 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
// and *afterwards* initialize all children recursively
stages()->init(scene);
// task expects its wrapped child to push to both ends, this triggers interface resolution
// TODO: stages()->pruneInterface(InterfaceFlags({WRITES_PREV_END, WRITES_NEXT_START}));
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
// and *finally* validate connectivity
stages()->validateConnectivity();

View File

@ -113,6 +113,11 @@ protected:
append(serial, types);
try {
serial.init(scene);
// prune pull interfaces based on provided external interface (start, end)
InterfaceFlags accepted;
if (start) accepted |= WRITES_PREV_END;
if (end) accepted |= WRITES_NEXT_START;
serial.pimpl()->pruneInterface(accepted);
serial.validateConnectivity();
if (!expect_failure) return; // as expected
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << serial;
@ -188,11 +193,11 @@ TEST_F(SerialTest, init_empty) {
TEST_F(SerialTest, init_connecting) {
EXPECT_INIT_SUCCESS(false, false, CONN);
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, READS_END}));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags());
EXPECT_INIT_FAILURE(true, true, CONN);
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END}));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({CONNECT, GENERATE}));
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
}
@ -253,11 +258,28 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
#if 0
// derive propagation direction from outer interface
EXPECT_INIT_SUCCESS(false, true, ANY); // -> ->
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START}));
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // <- <-
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START}));
#endif
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
it = serial.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY, ANY); // <- <-
it = serial.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <>
it = serial.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}));
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- --
it = serial.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags());
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags());
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags());
}