mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
f45a4e3e2c
commit
35f1540369
@ -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_;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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());
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user