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

View File

@ -64,7 +64,7 @@ public:
// if this is unknown (because interface is auto-detected from context), return 0 // if this is unknown (because interface is auto-detected from context), return 0
virtual InterfaceFlags requiredInterface() const = 0; virtual InterfaceFlags requiredInterface() const = 0;
// prune interface to the given propagation direction // 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 canCompute() const = 0;
virtual bool compute() = 0; virtual bool compute() = 0;
@ -169,9 +169,7 @@ public:
// initialize pull interfaces for given propagation directions // initialize pull interfaces for given propagation directions
void initInterface(PropagatingEitherWay::Direction dir); void initInterface(PropagatingEitherWay::Direction dir);
// prune interface to the given propagation direction // prune interface to the given propagation direction
void pruneInterface(PropagatingEitherWay::Direction direction) override { void pruneInterface(InterfaceFlags accepted) override;
initInterface(direction);
}
bool canCompute() const override; bool canCompute() const override;
bool compute() 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 // called by parent asking for pruning of this' interface
void SerialContainerPrivate::pruneInterface(PropagatingEitherWay::Direction dir) { void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
pruneInterfaces(children().begin(), children().end(), dir); 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) // 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 if (first == end) return; // nothing to do in this case
// determine feasible propagation directions from available push interfaces // determine accepted interface from available push interfaces
int dir = 0; InterfaceFlags accepted;
// if previous stage pushes forward, we accept forward propagation // if previous stage pushes forward, we accept forward propagation
if (first != children().begin()) { if (first != children().begin()) {
auto prev = first; --prev; // pointer to previous stage auto prev = first; --prev; // pointer to previous stage
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START) if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START)
dir |= PropagatingEitherWay::FORWARD; accepted |= WRITES_NEXT_START;
} // else: for first child we cannot determine the interface yet } // else: for first child we cannot determine the interface yet
// if end stage pushes backward, we accept backward propagation // if end stage pushes backward, we accept backward propagation
if (end != children().end()) { if (end != children().end()) {
if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_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 } // else: for last child we cannot determine the interface yet
// nothing to do if: // nothing to do if:
// - dir == 0: [first, last) covers all children, cannot determine interface // - accepted == 0: interface still unknown
// - dir == PropagatingEitherWay::BOTHWAY: nothing changed // - accepted == PROPAGATE_FORWARDS | PROPAGATE_BACKWARDS: no change
if (dir != 0 && dir != PropagatingEitherWay::BOTHWAY) if (accepted != 0 && accepted != InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}))
pruneInterfaces(first, end, PropagatingEitherWay::Direction(dir)); pruneInterfaces(first, end, accepted);
} }
// prune interface for children in range [first, last) to given direction // prune interface for children in range [first, last) to given direction
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,
PropagatingEitherWay::Direction dir) InterfaceFlags accepted)
{ {
// 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 // range should only contain stages with unknown required interface
assert(impl->requiredInterface() == PropagatingEitherWay::AUTO); 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 PropagatingEitherWayPrivate::requiredInterface() const
{ {
InterfaceFlags f; InterfaceFlags f;
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD) if (required_interface_dirs_ & PropagatingEitherWay::FORWARD)
f |= InterfaceFlags({READS_START, WRITES_NEXT_START}); f |= PROPAGATE_FORWARDS;
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD) 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 // if required_interface_dirs_ == ANYWAY, we don't require an interface
// but the parent container auto-derives the propagation direction // but the parent container auto-derives the propagation direction
return f; return f;
@ -495,7 +504,7 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
} }
InterfaceFlags ConnectingPrivate::requiredInterface() const { InterfaceFlags ConnectingPrivate::requiredInterface() const {
return InterfaceFlags( { READS_START, READS_END } ); return InterfaceFlags(CONNECT);
} }
void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) 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 // and *afterwards* initialize all children recursively
stages()->init(scene); stages()->init(scene);
// task expects its wrapped child to push to both ends, this triggers interface resolution // 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 // and *finally* validate connectivity
stages()->validateConnectivity(); stages()->validateConnectivity();

View File

@ -113,6 +113,11 @@ protected:
append(serial, types); append(serial, types);
try { try {
serial.init(scene); 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(); serial.validateConnectivity();
if (!expect_failure) return; // as expected if (!expect_failure) return; // as expected
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << serial; 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) { TEST_F(SerialTest, init_connecting) {
EXPECT_INIT_SUCCESS(false, false, CONN); 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_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags());
EXPECT_INIT_FAILURE(true, true, CONN); 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 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((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
#if 0
// derive propagation direction from outer interface // derive propagation direction from outer interface
EXPECT_INIT_SUCCESS(false, true, ANY); // -> -> EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START})); it = serial.pimpl()->children().begin();
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // <- <- EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START})); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
#endif 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());
} }