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:
|
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_;
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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();
|
||||||
|
|
||||||
|
|||||||
@ -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());
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user