remove logical flow BOTH / simplify pruning

The PROPAGATE concept BOTH declared the stages *will* propagate solutions in
either direction. ANY, on the other hand, only means the propagation
direction is *not resolved yet* (but will be at planning time).

BOTH was originally described to support a more general control flow
than was eventually decided to support. The four exclusive Stage interfaces
CONNECT, PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS, and GENERATOR
do not allow for BOTH as a valid setup anymore, unless you setup a very
convolved task like `Alternatives(GEN, PROP) - Alternatives(PROP, GEN)`
which would be very complex to inspect. The same functionality can still
be achieved more readable as `Alternatives(Seq(GEN, PROP), Seq(PROP, GEN))`.

The confusion between BOTH (propagator *will* send in both directions) and
ANY (propagator will send in *either* direction, decided during init) led
to a lot of confusion with users and was not fully accounted
throughout the pipeline.

Adjust tests.

Notice the difference between ANY (unresolved propagator) and UNKNOWN
(a container before introspecting its children). propagators still
report UNKNOWN as requiredInterface though to simplify control flow.

The simplification enables a much simpler linear inference of the connective
structure of a task, as the first interface direction is always given.

Additionally, unify the resource setup for static interfaces to run
in the constructor, and for dynamic initialization in `pruneInterface`,
getting rid of partial initializations in `init`.
This commit is contained in:
v4hn 2020-03-26 16:27:50 +01:00
parent a193fc2ec5
commit b6a5f89307
8 changed files with 373 additions and 463 deletions

View File

@ -91,8 +91,6 @@ public:
PRIVATE_CLASS(SerialContainer) PRIVATE_CLASS(SerialContainer)
SerialContainer(const std::string& name = "serial container"); SerialContainer(const std::string& name = "serial container");
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
@ -130,8 +128,6 @@ public:
PRIVATE_CLASS(ParallelContainerBase) PRIVATE_CLASS(ParallelContainerBase)
ParallelContainerBase(const std::string& name = "parallel container"); ParallelContainerBase(const std::string& name = "parallel container");
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
protected: protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl); ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -105,6 +105,10 @@ public:
void validateConnectivity() const override; void validateConnectivity() const override;
// containers derive their required interface from their children
// UNKNOWN until pruneInterface was called
InterfaceFlags requiredInterface() const override { return required_interface_; }
// forward these methods to the public interface for containers // forward these methods to the public interface for containers
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
@ -139,6 +143,9 @@ protected:
auto& internalToExternalMap() { return internal_to_external_; } auto& internalToExternalMap() { return internal_to_external_; }
const auto& internalToExternalMap() const { return internal_to_external_; } const auto& internalToExternalMap() const { return internal_to_external_; }
// set by containers in pruneInterface()
InterfaceFlags required_interface_;
private: private:
container_type children_; container_type children_;
@ -166,28 +173,16 @@ class SerialContainerPrivate : public ContainerBasePrivate
public: public:
SerialContainerPrivate(SerialContainer* me, const std::string& name); SerialContainerPrivate(SerialContainer* me, const std::string& name);
// containers derive their required interface from their children
InterfaceFlags requiredInterface() const override;
// called by parent asking for pruning of this' interface // called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override; void pruneInterface(InterfaceFlags accepted) override;
// validate connectivity of chain // validate connectivity of chain
void validateConnectivity() const override; void validateConnectivity() const override;
private: void reset();
// connect cur stage to its predecessor and successor
bool connect(container_type::const_iterator cur);
// called by init() to prune interfaces for children in range [first, last) protected:
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end); // connect two neighbors
// prune interface for children in range [first, last) to given direction void connect(StagePrivate& stage1, StagePrivate& stage2);
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) PIMPL_FUNCTIONS(SerialContainer)
@ -219,9 +214,6 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
public: public:
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name); ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
// containers derive their required interface from their children
InterfaceFlags requiredInterface() const override;
// called by parent asking for pruning of this' interface // called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override; void pruneInterface(InterfaceFlags accepted) override;

View File

@ -83,13 +83,30 @@ enum InterfaceFlag
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END, PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
GENERATE = WRITES_PREV_END | WRITES_NEXT_START, GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
}; };
typedef Flags<InterfaceFlag> InterfaceFlags; typedef Flags<InterfaceFlag> InterfaceFlags;
/** invert interface such that
* - new end can connect to old start
* - new start can connect to old end
*/
constexpr InterfaceFlags invert(InterfaceFlags f) {
InterfaceFlags inv;
if (f & READS_START)
inv |= WRITES_NEXT_START;
if (f & WRITES_PREV_END)
inv |= READS_END;
if (f & READS_END)
inv |= WRITES_PREV_END;
if (f & WRITES_NEXT_START)
inv |= READS_START;
return inv;
};
// some useful constants // some useful constants
constexpr InterfaceFlags UNKNOWN; constexpr InterfaceFlags UNKNOWN;
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END }); constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START }); constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({ PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS });
MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(Stage) MOVEIT_CLASS_FORWARD(Stage)
@ -253,12 +270,9 @@ public:
AUTO = 0x00, // auto-derive direction from context AUTO = 0x00, // auto-derive direction from context
FORWARD = 0x01, // propagate forward only FORWARD = 0x01, // propagate forward only
BACKWARD = 0x02, // propagate backward only BACKWARD = 0x02, // propagate backward only
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
}; };
void restrictDirection(Direction dir); void restrictDirection(Direction dir);
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
virtual void computeForward(const InterfaceState& from) = 0; virtual void computeForward(const InterfaceState& from) = 0;
virtual void computeBackward(const InterfaceState& to) = 0; virtual void computeBackward(const InterfaceState& to) = 0;

View File

@ -76,10 +76,11 @@ public:
* If the interface is unknown (because it is auto-detected from context), return UNKNOWN */ * If the interface is unknown (because it is auto-detected from context), return UNKNOWN */
virtual InterfaceFlags requiredInterface() const = 0; virtual InterfaceFlags requiredInterface() const = 0;
/** Prune interface to comply with the given propagation direction /** validate correct interface usage and resolve ambiguous interfaces */
* virtual void pruneInterface(InterfaceFlags accepted);
* PropagatingEitherWay uses this in restrictDirection() */
virtual void pruneInterface(InterfaceFlags accepted) {} void pruneStartInterface(InterfaceFlags start_flags) { pruneInterface(start_flags & START_IF_MASK); }
void pruneEndInterface(InterfaceFlags end_flags) { pruneInterface(end_flags & END_IF_MASK); }
/// validate connectivity of children (after init() was done) /// validate connectivity of children (after init() was done)
virtual void validateConnectivity() const; virtual void validateConnectivity() const;

View File

@ -53,10 +53,11 @@ using namespace std::placeholders;
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) { ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
pending_backward_.reset(new Interface); : StagePrivate(me, name)
pending_forward_.reset(new Interface); , required_interface_(UNKNOWN)
} , pending_backward_(new Interface)
, pending_forward_(new Interface) {}
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const { ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
if (!for_insert && index < 0) if (!for_insert && index < 0)
@ -252,6 +253,11 @@ void ContainerBase::reset() {
// ... and state mapping // ... and state mapping
impl->internal_to_external_.clear(); impl->internal_to_external_.clear();
// interfaces depend on children which might change
impl->required_interface_ = UNKNOWN;
impl->starts_.reset();
impl->ends_.reset();
Stage::reset(); Stage::reset();
} }
@ -401,112 +407,20 @@ SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name) SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
: ContainerBasePrivate(me, name) {} : ContainerBasePrivate(me, name) {}
// a serial container's required interface is derived from the required input interfaces void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
// of the first and last children. After resolving, it is remembered in required_interface_. InterfaceFlags flags1 = stage1.requiredInterface();
InterfaceFlags SerialContainerPrivate::requiredInterface() const { InterfaceFlags flags2 = stage2.requiredInterface();
if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK))
return required_interface_;
if (children().empty()) if ((flags1 & WRITES_NEXT_START) && (flags2 & READS_START))
return UNKNOWN; stage1.setNextStarts(stage2.starts());
return (children().front()->pimpl()->requiredInterface() & START_IF_MASK) | else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
(children().back()->pimpl()->requiredInterface() & END_IF_MASK); stage2.setPrevEnds(stage1.ends());
} else {
boost::format desc("end interface of '%1%' (%2%) does not match start interface of '%3%' (%4%)");
// connect cur stage to its predecessor and successor by setting the push interface pointers desc % stage1.name() % flowSymbol(flags1 & END_IF_MASK);
// return true if cur stage should be scheduled for a second sweep desc % stage2.name() % flowSymbol(flags2 & START_IF_MASK);
bool SerialContainerPrivate::connect(container_type::const_iterator cur) { throw InitStageException(*me(), desc.str());
StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->requiredInterface();
// get iterators to prev / next stage in sequence
auto prev = cur;
--prev;
auto next = cur;
++next;
// set push forward connection using next's starts
if ((required == UNKNOWN || required & WRITES_NEXT_START) &&
next != children().end()) // last child has not a next one
cur_impl->setNextStarts((*next)->pimpl()->starts());
// set push backward connection using prev's ends
if ((required == UNKNOWN || required & WRITES_PREV_END) &&
cur != children().begin()) // first child has not a previous one
cur_impl->setPrevEnds((*prev)->pimpl()->ends());
// schedule stage with unknown interface for 2nd sweep
return required == UNKNOWN || required == PROPAGATE_BOTHWAYS;
}
/* Establishing the interface connections, we face a chicken-egg-problem:
* To establish a connection, a predecessors/successors pull interface is
* assigned to the current's stage push interface.
* However, propagating stages (in auto-detection mode) can only create
* their pull interfaces if the corresponding, opposite-side push interface
* is present already (because that's the mechanism to determine the supported
* propagation directions).
*
* Hence, we need to resolve this by performing two sweeps:
* - initialization, assuming both propagation directions should be supported,
* thus generating both pull interfaces, i.e. providing the egg
* - stripping down the interfaces to the actual context
* This context is provided by two stages pushing from both ends
* into a (potentially long) sequence of propagating stages (tbd).
*/
void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) {
// reset pull interfaces
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);
auto start = impl->children().begin();
auto last = --impl->children().end();
// connect first / last child's push interfaces to our pending_* buffers
// if they require pushing
impl->setChildsPushBackwardInterface(**start);
impl->setChildsPushForwardInterface(**last);
// initialize and connect remaining children in two sweeps
// to allow interface auto-detection for propagating stages
auto first_unknown = start; // pointer to first stage with unknown interface
for (auto cur = start, end = impl->children().end(); cur != end; ++cur) {
// 1st sweep: connect everything potentially possible,
// remembering start of unknown sub sequence
if (impl->connect(cur))
;
else { // reached a stage with known interface
// 2nd sweep: prune interfaces from [first_unknown, cur)
impl->pruneInterfaces(first_unknown, cur);
// restart with first_unknown = ++cur
first_unknown = cur;
++first_unknown;
} }
}
// prune stages [first_unknown, end())
impl->pruneInterfaces(first_unknown, impl->children().end());
// initialize this' pull interfaces if first/last child pulls
if (const InterfacePtr& target = (*start)->pimpl()->starts())
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, std::placeholders::_1,
std::cref(target), std::placeholders::_2)));
if (const InterfacePtr& target = (*last)->pimpl()->ends())
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, std::placeholders::_1,
std::cref(target), 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 // called by parent asking for pruning of this' interface
@ -514,98 +428,50 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (children().empty()) if (children().empty())
throw InitStageException(*me(), "container is empty"); throw InitStageException(*me(), "container is empty");
// reading is always allowed if current interface flags do so // TODO(v4hn): if ever there is a use case to start pruning
accepted |= (interfaceFlags() & InterfaceFlags({ READS_START, READS_END })); // with a specified end interface, this would need to be extended
if (!(accepted & (READS_START | WRITES_PREV_END)))
return; // The start interface direction is not decided
if (accepted == PROPAGATE_BOTHWAYS) Stage& first = *children().front();
return; // There is nothing to prune Stage& last = *children().back();
// If whole chain is still undecided, prune all children // sweep through children once: infer and connect interfaces
if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS && first.pimpl()->pruneStartInterface(accepted);
children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) { setChildsPushBackwardInterface(first);
pruneInterfaces(children().begin(), children().end(), accepted);
} else { // otherwise only prune the first / last child's input / output interface for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
StagePrivate* child_impl; StagePrivate* child_impl = (**it).pimpl();
child_impl = children().front()->pimpl(); StagePrivate* previous_impl = (**previous_it).pimpl();
child_impl->pruneInterface((accepted & START_IF_MASK) | (child_impl->interfaceFlags() & END_IF_MASK)); child_impl->pruneStartInterface(invert(previous_impl->requiredInterface()));
child_impl = children().back()->pimpl(); connect(*previous_impl, *child_impl);
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 // potentially connect outmost push interface to pending_ buffer
if (!children().front()->pimpl()->starts()) setChildsPushForwardInterface(last);
if ((accepted & END_IF_MASK) != UNKNOWN &&
(last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) {
boost::format desc(
"requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)");
desc % flowSymbol(accepted & END_IF_MASK) % flowSymbol(last.pimpl()->requiredInterface() & END_IF_MASK);
throw InitStageException(*me(), desc.str());
}
// if first/last pull, this needs to pull to and forward to the children
if (const InterfacePtr& target = first.pimpl()->starts())
starts_.reset(new Interface(
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
else
starts_.reset(); starts_.reset();
if (!children().back()->pimpl()->ends())
if (const InterfacePtr& target = last.pimpl()->ends())
ends_.reset(new Interface(
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
else
ends_.reset(); ends_.reset();
if (interfaceFlags() == UNKNOWN) required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK;
throw InitStageException(*me(), "failed to derive propagation direction");
}
// called by init() to prune interfaces for children in range [first, last)
// this function determines the feasible propagation directions
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end) {
if (first == end) {
storeRequiredInterface(first, end);
return; // nothing to do in this case
}
// determine accepted interface from available push interfaces
InterfaceFlags accepted;
// if first stage ...
if (first != children().begin()) {
auto prev = first;
--prev; // pointer to previous stage
// ... pushes forward, we accept forward propagation
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START)
accepted |= PROPAGATE_FORWARDS;
// ... pulls backward, we accept backward propagation
if ((*prev)->pimpl()->requiredInterface() & READS_END)
accepted |= PROPAGATE_BACKWARDS;
} // else: for first child we cannot determine the interface yet
// if end stage ...
if (end != children().end()) {
// ... pushes backward, we accept backward propagation
if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_END)
accepted |= PROPAGATE_BACKWARDS;
// ... pulls forward, we accept forward propagation
if ((*end)->pimpl()->requiredInterface() & READS_START)
accepted |= PROPAGATE_FORWARDS;
} // else: for last child we cannot determine the interface yet
// nothing to do if:
// - accepted == UNKNOWN: interface still unknown
// - accepted == PROPAGATE_BOTHWAYS: no change
if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
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,
InterfaceFlags accepted) {
// 1st sweep: remove push interfaces
for (auto it = first; it != end; ++it) {
StagePrivate* impl = (*it)->pimpl();
// 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))
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);
}
storeRequiredInterface(first, end);
} }
void SerialContainerPrivate::validateConnectivity() const { void SerialContainerPrivate::validateConnectivity() const {
@ -715,64 +581,78 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name) ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
: ContainerBasePrivate(me, name) {} : ContainerBasePrivate(me, name) {}
// A parallel container's required interface is derived from the required interfaces of all of its children.
// They must not conflict to each other. Otherwise an InitStageException is thrown.
InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const {
if (children().empty())
return UNKNOWN;
/* The interfaces of all children need to be consistent with each other. Allowed combinations are:
* = (connecting stages)
* = (backward propagating)
* = (forward propagating)
* = = = (propagating in both directions)
* = (generating)
*/
InterfaceFlags accumulated = children().front()->pimpl()->requiredInterface();
for (const Stage::pointer& stage : children()) {
InterfaceFlags current = stage->pimpl()->requiredInterface();
if (accumulated != PROPAGATE_BOTHWAYS &&
(accumulated & current) == current) // all flags of current are already available in accumulated
continue;
bool current_is_propagating =
(current == PROPAGATE_BOTHWAYS || current == PROPAGATE_FORWARDS || current == PROPAGATE_BACKWARDS);
if (current_is_propagating && accumulated != CONNECT && accumulated != GENERATE)
accumulated |= current; // propagating is compatible to all except CONNECT and GENERATE
else
throw InitStageException(*me(),
"child '" + stage->name() + "' has conflicting interface to previous children");
}
return accumulated;
}
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
// forward pruning to all children with UNKNOWN required interface if (children().empty())
for (const Stage::pointer& stage : children()) { throw InitStageException(*me(), "trying to prune empty container");
if (stage->pimpl()->requiredInterface() == UNKNOWN)
stage->pimpl()->pruneInterface(accepted); if (accepted == UNKNOWN)
return; // nothing to prune
InitStageException exceptions;
InterfaceFlags interface;
for (const Stage::pointer& child : children()) {
try {
child->pimpl()->pruneInterface(accepted);
} catch (InitStageException& e) {
exceptions.append(e);
continue;
} }
InterfaceFlags child_interface = child->pimpl()->requiredInterface();
if (interface == UNKNOWN)
interface = child_interface;
else if ((interface & child_interface) != child_interface) {
boost::format desc("inferred interface of stage '%1%' (%2%/%3%) does not agree with the inferred interface of "
"its siblings (%4%/%5%).");
desc % child->name();
desc % flowSymbol(child_interface & START_IF_MASK) % flowSymbol(child_interface & END_IF_MASK);
desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK);
exceptions.push_back(*me(), desc.str());
}
}
if ((interface & accepted) != accepted) {
boost::format desc("required interface (%1%/%2%) does not match children (%3%/%4%).");
desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK);
desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK);
exceptions.push_back(*me(), desc.str());
}
if (exceptions)
throw exceptions;
// States received by the container need to be copied to all children's pull interfaces.
if (interface & READS_START)
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState(Interface::FORWARD, external, updated);
}));
if (interface & READS_END)
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState(Interface::BACKWARD, external, updated);
}));
// initialize push connections of children according to their demands
for (const Stage::pointer& stage : children()) {
setChildsPushForwardInterface(*stage);
setChildsPushBackwardInterface(*stage);
}
required_interface_ = interface;
} }
void ParallelContainerBasePrivate::validateConnectivity() const { void ParallelContainerBasePrivate::validateConnectivity() const {
InitStageException errors; InitStageException errors;
InterfaceFlags my_interface = interfaceFlags(); InterfaceFlags my_interface = interfaceFlags();
InterfaceFlags children_interfaces;
// check that input / output interfaces of all children are handled by my interface // check that input / output interfaces of all children are handled by my interface
for (const auto& child : children()) { for (const auto& child : children()) {
InterfaceFlags current = child->pimpl()->interfaceFlags(); InterfaceFlags current = child->pimpl()->interfaceFlags();
children_interfaces |= current; // compute union of all children interfaces
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK)) if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
mismatchingInterface(errors, *child->pimpl(), START_IF_MASK); mismatchingInterface(errors, *child->pimpl(), START_IF_MASK);
if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK)) if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK))
mismatchingInterface(errors, *child->pimpl(), 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))
errors.push_back(*me(), "no child provides expected push interface");
// recursively validate children // recursively validate children
try { try {
@ -795,35 +675,6 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
ParallelContainerBase::ParallelContainerBase(const std::string& name) ParallelContainerBase::ParallelContainerBase(const std::string& name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {} : ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
/* States received by the container need to be copied to all children's pull interfaces.
* States generated by children can be directly forwarded into the container's push interfaces.
*/
void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
// recursively init children
ContainerBase::init(robot_model);
auto impl = pimpl();
// determine the union of interfaces required by children
// TODO: should we better use the least common interface?
InterfaceFlags required = impl->requiredInterface();
// initialize this' pull connections
impl->starts().reset(required & READS_START ?
new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl,
Interface::FORWARD, std::placeholders::_1, std::placeholders::_2)) :
nullptr);
impl->ends().reset(required & READS_END ?
new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl,
Interface::BACKWARD, std::placeholders::_1, std::placeholders::_2)) :
nullptr);
// initialize push connections of children according to their demands
for (const Stage::pointer& stage : impl->children()) {
impl->setChildsPushForwardInterface(*stage);
impl->setChildsPushBackwardInterface(*stage);
}
}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) { void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
auto impl = pimpl(); auto impl = pimpl();
impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost, std::move(comment)), solution.start(), impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost, std::move(comment)), solution.start(),

View File

@ -39,10 +39,14 @@
#include <moveit/task_constructor/container_p.h> #include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h> #include <moveit/task_constructor/introspection.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
#include <boost/format.hpp>
#include <iostream> #include <iostream>
#include <iomanip> #include <iomanip>
#include <algorithm> #include <algorithm>
#include <ros/console.h>
#include <utility> #include <utility>
namespace moveit { namespace moveit {
@ -84,12 +88,38 @@ InterfaceFlags StagePrivate::interfaceFlags() const {
return f; return f;
} }
void StagePrivate::pruneInterface(InterfaceFlags accepted) {
const InterfaceFlags accepted_start{ accepted & START_IF_MASK };
const InterfaceFlags accepted_end{ accepted & END_IF_MASK };
const InterfaceFlags required{ requiredInterface() };
const InterfaceFlags required_start{ required & START_IF_MASK };
const InterfaceFlags required_end{ required & END_IF_MASK };
if (required == UNKNOWN)
throw std::logic_error("stage type does not specify interface, but also does not override pruneInterface. "
"Who sets up its interfaces?");
// only one side needs to be specified but the value has to be compatible with this stage
if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) ||
((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) {
boost::format desc("this' interface %1%/%2% cannot match inferred interface %3%/%4%");
desc % flowSymbol(required_start) % flowSymbol(required_end);
desc % flowSymbol(accepted_start) % flowSymbol(accepted_end);
throw InitStageException(*me(), desc.str());
}
}
void StagePrivate::validateConnectivity() const { void StagePrivate::validateConnectivity() const {
// check that the required interface is provided // check that the required interface is provided
InterfaceFlags required = requiredInterface(); InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags(); InterfaceFlags actual = interfaceFlags();
if ((required & actual) != required) if ((required & actual) != required) {
throw InitStageException(*me(), "required interface is not satisfied"); boost::format desc("interface %1%/%2% does not satisfy required interface %3%/%4%");
desc % flowSymbol(actual & START_IF_MASK) % flowSymbol(actual & END_IF_MASK);
desc % flowSymbol(required & START_IF_MASK) % flowSymbol(required & END_IF_MASK);
throw InitStageException(*me(), desc.str());
}
} }
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
@ -398,40 +428,50 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir, PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
const std::string& name) const std::string& name)
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) { : ComputeBasePrivate(me, name), required_interface_dirs_(dir) {}
initInterface(required_interface_dirs_);
}
// initialize pull interfaces to match requested propagation directions // initialize pull interfaces to match requested propagation directions
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) { void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
if (dir & PropagatingEitherWay::FORWARD) { if (required_interface_dirs_ != PropagatingEitherWay::AUTO && dir != required_interface_dirs_) {
if (!starts_) // keep existing interface if possible auto flow = [](PropagatingEitherWay::Direction dir) -> const char* {
starts_.reset( switch (dir) {
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1))); case PropagatingEitherWay::AUTO:
} else { return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END });
starts_.reset(); case PropagatingEitherWay::FORWARD:
return flowSymbol(InterfaceFlags{ READS_START });
case PropagatingEitherWay::BACKWARD:
return flowSymbol(InterfaceFlags{ WRITES_PREV_END });
}
};
boost::format desc("propagation direction %1% incompatible with inferred direction %2%");
desc % flow(required_interface_dirs_) % flow(dir);
throw InitStageException(*me(), desc.str());
} }
if (dir & PropagatingEitherWay::BACKWARD) { if (dir == PropagatingEitherWay::FORWARD)
if (!ends_) // keep existing interface if possible starts_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedStarts(it); }));
ends_.reset( else if (dir == PropagatingEitherWay::BACKWARD)
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1))); ends_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedEnds(it); }));
} else {
ends_.reset(); required_interface_dirs_ = dir;
}
} }
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
int dir = 0; if (accepted == UNKNOWN)
if ((accepted & PROPAGATE_FORWARDS) == PROPAGATE_FORWARDS) throw InitStageException(*me(), std::string("cannot prune to ") + flowSymbol(UNKNOWN));
dir |= PropagatingEitherWay::FORWARD; if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START)
if ((accepted & PROPAGATE_BACKWARDS) == PROPAGATE_BACKWARDS) initInterface(PropagatingEitherWay::FORWARD);
dir |= PropagatingEitherWay::BACKWARD; else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
initInterface(PropagatingEitherWay::Direction(dir)); initInterface(PropagatingEitherWay::BACKWARD);
else {
boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)");
desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK);
throw InitStageException(*me(), desc.str());
}
} }
void PropagatingEitherWayPrivate::validateConnectivity() const { void PropagatingEitherWayPrivate::validateConnectivity() const {
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags(); InterfaceFlags actual = interfaceFlags();
if (actual == UNKNOWN) if (actual == UNKNOWN)
throw InitStageException(*me(), "not connected in any direction"); throw InitStageException(*me(), "not connected in any direction");
@ -442,23 +482,19 @@ void PropagatingEitherWayPrivate::validateConnectivity() const {
if ((actual & READS_END) && !(actual & WRITES_PREV_END)) if ((actual & READS_END) && !(actual & WRITES_PREV_END))
errors.push_back(*me(), "Cannot push backwards"); errors.push_back(*me(), "Cannot push backwards");
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY && (required & actual) != required)
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate "
<< (actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
if (errors) if (errors)
throw errors; throw errors;
} }
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const { InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
InterfaceFlags f; switch (required_interface_dirs_) {
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD) case PropagatingEitherWay::FORWARD:
f |= PROPAGATE_FORWARDS; return PROPAGATE_FORWARDS;
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD) case PropagatingEitherWay::BACKWARD:
f |= PROPAGATE_BACKWARDS; return PROPAGATE_BACKWARDS;
// if required_interface_dirs_ == AUTO, we don't require an interface case PropagatingEitherWay::AUTO:
// but the parent container auto-derives the propagation direction return UNKNOWN;
return f; }
} }
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
@ -516,26 +552,10 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) :
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) { void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
auto impl = pimpl(); auto impl = pimpl();
if (impl->required_interface_dirs_ == dir) if (impl->required_interface_dirs_ != AUTO)
return;
if (impl->prevEnds() || impl->nextStarts())
throw std::runtime_error("Cannot change direction after being connected"); throw std::runtime_error("Cannot change direction after being connected");
impl->required_interface_dirs_ = dir;
impl->initInterface(impl->required_interface_dirs_);
}
void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model) { impl->initInterface(dir);
Stage::init(robot_model);
auto impl = pimpl();
// In AUTO-mode, i.e. when auto-detecting direction of propagation from context,
// pretend that we offer both interface directions during init().
// This is needed due to a chicken-egg-problem: interface auto-detection requires
// the context (external pushing interfaces prevEnds, nextStarts) to be set,
// while they are ony set if we detected the correct interface...
if (impl->required_interface_dirs_ == AUTO)
impl->initInterface(BOTHWAY);
// otherwise the interface is already fixed and well-defined
} }
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) { void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {

View File

@ -88,32 +88,25 @@ enum StageType
FW, FW,
BW, BW,
ANY, ANY,
BOTH,
CONN CONN
}; };
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) { void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
for (StageType t : types) { for (StageType t : types) {
switch (t) { switch (t) {
case GEN: case GEN:
c.insert(std::make_unique<GeneratorMockup>(runs)); c.add(std::make_unique<GeneratorMockup>(runs));
break; break;
case FW: case FW:
c.insert(std::make_unique<ForwardMockup>(runs)); c.add(std::make_unique<ForwardMockup>(runs));
break; break;
case BW: case BW:
c.insert(std::make_unique<BackwardMockup>(runs)); c.add(std::make_unique<BackwardMockup>(runs));
break; break;
case ANY: case ANY:
c.insert(std::make_unique<PropagatorMockup>(runs, runs)); c.add(std::make_unique<PropagatorMockup>(runs, runs));
break; break;
case BOTH: {
auto s = std::make_unique<PropagatorMockup>(runs, runs);
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
c.insert(std::move(s));
break;
}
case CONN: case CONN:
c.insert(std::make_unique<ConnectMockup>(runs)); c.add(std::make_unique<ConnectMockup>(runs));
break; break;
} }
} }
@ -134,13 +127,13 @@ TEST(ContainerBase, positionForInsert) {
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end()); EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
EXPECT_EQ(impl->childByIndex(-2, true), impl->children().end()); EXPECT_EQ(impl->childByIndex(-2, true), impl->children().end());
s.insert(std::make_unique<NamedStage>("0")); s.add(std::make_unique<NamedStage>("0"));
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0"); EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end()); EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0"); EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0");
EXPECT_EQ(impl->childByIndex(-3, true), impl->children().end()); EXPECT_EQ(impl->childByIndex(-3, true), impl->children().end());
s.insert(std::make_unique<NamedStage>("1")); s.add(std::make_unique<NamedStage>("1"));
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0"); EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
EXPECT_STREQ((*impl->childByIndex(1, true))->name().c_str(), "1"); EXPECT_STREQ((*impl->childByIndex(1, true))->name().c_str(), "1");
EXPECT_EQ(impl->childByIndex(2, true), impl->children().end()); EXPECT_EQ(impl->childByIndex(2, true), impl->children().end());
@ -151,15 +144,16 @@ TEST(ContainerBase, positionForInsert) {
EXPECT_EQ(impl->childByIndex(-4, true), impl->children().end()); EXPECT_EQ(impl->childByIndex(-4, true), impl->children().end());
} }
/* TODO: remove interface as it returns raw pointers */
TEST(ContainerBase, findChild) { TEST(ContainerBase, findChild) {
SerialContainer s, *c2; SerialContainer s, *c2;
Stage *a, *b, *c1, *d; Stage *a, *b, *c1, *d;
s.insert(Stage::pointer(a = new NamedStage("a"))); s.add(Stage::pointer(a = new NamedStage("a")));
s.insert(Stage::pointer(b = new NamedStage("b"))); s.add(Stage::pointer(b = new NamedStage("b")));
s.insert(Stage::pointer(c1 = new NamedStage("c"))); s.add(Stage::pointer(c1 = new NamedStage("c")));
auto sub = Stage::pointer(c2 = new SerialContainer("c")); auto sub = ContainerBase::pointer(c2 = new SerialContainer("c"));
c2->insert(Stage::pointer(d = new NamedStage("d"))); sub->add(Stage::pointer(d = new NamedStage("d")));
s.insert(std::move(sub)); s.add(std::move(sub));
EXPECT_EQ(s.findChild("a"), a); EXPECT_EQ(s.findChild("a"), a);
EXPECT_EQ(s.findChild("b"), b); EXPECT_EQ(s.findChild("b"), b);
@ -194,17 +188,25 @@ protected:
pushInterface(start, end); pushInterface(start, end);
} }
void validateInit(bool start, bool end, bool expect_failure) { validateInit(start, end, {}, expect_failure); }
void validateInit(bool start, bool end, const std::initializer_list<StageType>& types, bool expect_failure) { void validateInit(bool start, bool end, const std::initializer_list<StageType>& types, bool expect_failure) {
reset(start, end); reset(start, end);
append(container, types); append(container, types);
try { try {
container.init(robot_model); container.init(robot_model);
// prune pull interfaces based on provided external interface (start, end) // prune interfaces based on provided external interface (start, end)
InterfaceFlags accepted; InterfaceFlags accepted;
if (start) if (start)
accepted |= WRITES_PREV_END; accepted |= WRITES_PREV_END;
else
accepted |= READS_START;
if (end) if (end)
accepted |= WRITES_NEXT_START; accepted |= WRITES_NEXT_START;
else
accepted |= READS_END;
container.pimpl()->pruneInterface(accepted); container.pimpl()->pruneInterface(accepted);
container.pimpl()->validateConnectivity(); container.pimpl()->validateConnectivity();
if (!expect_failure) if (!expect_failure)
@ -314,8 +316,7 @@ TEST_F(SerialTest, init_connecting) {
EXPECT_INIT_SUCCESS(false, false, CONN); EXPECT_INIT_SUCCESS(false, false, CONN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
EXPECT_INIT_FAILURE(true, true, CONN); EXPECT_INIT_FAILURE(true, true, CONN); // external interface says GENERATOR, stage is CONNECTOR
EXPECT_EQ(container.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
} }
@ -363,12 +364,6 @@ TEST_F(SerialTest, init_forward) {
EXPECT_INIT_SUCCESS(true, false, BW, ANY); EXPECT_INIT_SUCCESS(true, false, BW, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); 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) { TEST_F(SerialTest, init_backward) {
@ -389,6 +384,18 @@ TEST_F(SerialTest, init_backward) {
} }
TEST_F(SerialTest, interface_detection) { TEST_F(SerialTest, interface_detection) {
// ANY resolves in either direction
EXPECT_INIT_SUCCESS(false, true, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
// derive propagation direction from inner generator // derive propagation direction from inner generator
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> -> EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
auto it = container.pimpl()->children().begin(); auto it = container.pimpl()->children().begin();
@ -397,12 +404,6 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
// derive propagation direction from inner connector // derive propagation direction from inner connector
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <- EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
it = container.pimpl()->children().begin(); it = container.pimpl()->children().begin();
@ -435,109 +436,94 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <> EXPECT_INIT_FAILURE(true, true, ANY, ANY); // no generator
it = container.pimpl()->children().begin();
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- -- EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connect
it = container.pimpl()->children().begin();
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), UNKNOWN);
} }
TEST_F(SerialTest, nested_interface_detection) { TEST_F(SerialTest, nested_interface_detection) {
SerialContainer* inner; SerialContainer* inner;
// direction imposed from outer generator // direction imposed from outer generator
container.clear(); container.clear();
inner = new SerialContainer("inner serial"); inner = new SerialContainer("inner serial");
append(*inner, { ANY, ANY }); append(*inner, { ANY, ANY });
append(container, { GEN, ANY, ANY });
container.insert(Stage::pointer(inner), -2); append(container, { GEN, ANY });
container.add(Stage::pointer(inner));
append(container, { ANY });
{ {
SCOPED_TRACE("GEN - ANY - inner - ANY"); SCOPED_TRACE("GEN - ANY - Serial( ANY - ANY ) - ANY");
validateInit(true, true, {}, false); validateInit(true, true, false);
} }
// direction imposed from inner generator // direction imposed from inner generator
container.clear(); container.clear();
inner = new SerialContainer("inner serial"); inner = new SerialContainer("inner serial");
append(*inner, { ANY, GEN, ANY }); append(*inner, { ANY, GEN, ANY });
append(container, { ANY, ANY });
container.insert(Stage::pointer(inner), -2); append(container, { ANY });
container.add(Stage::pointer(inner));
append(container, { ANY });
{ {
SCOPED_TRACE("ANY - inner - ANY"); SCOPED_TRACE("ANY - Serial( ANY - GEN - ANY ) - ANY");
validateInit(true, true, {}, false); validateInit(true, true, false);
} }
container.clear(); container.clear();
inner = new SerialContainer("inner serial"); inner = new SerialContainer("inner serial");
append(*inner, { GEN, ANY }); append(*inner, { GEN, ANY });
container.insert(Stage::pointer(inner)); container.insert(Stage::pointer(inner));
append(container, { ANY });
{ {
SCOPED_TRACE("inner - ANY"); SCOPED_TRACE("Serial( GEN - ANY ) - ANY");
validateInit(true, true, { ANY }, false); validateInit(true, true, false);
} }
// outer and inner generators conflict with each other // outer and inner generators conflict with each other
container.clear(); container.clear();
inner = new SerialContainer("inner serial"); inner = new SerialContainer("inner serial");
append(*inner, { ANY, GEN, ANY }); append(*inner, { ANY, GEN, ANY });
append(container, { GEN, ANY, ANY });
container.insert(Stage::pointer(inner), -2); append(container, { GEN, ANY });
container.add(Stage::pointer(inner));
append(container, { ANY });
{ {
SCOPED_TRACE("GEN - ANY - inner - ANY"); SCOPED_TRACE("GEN - ANY - Serial( ANY - GEN - ANY ) - ANY");
validateInit(true, true, {}, true); validateInit(true, true, {}, true); // expected to fail
}
}
TEST_F(SerialTest, nested_parallel) {
container.clear();
append(container, { GEN });
auto inner_alt = new Alternatives("inner alternatives");
append(*inner_alt, { ANY, ANY });
container.add(Stage::pointer(inner_alt));
{
SCOPED_TRACE("GEN - Alternatives( ANY - ANY )");
validateInit(true, true, false);
}
container.clear();
append(container, { GEN });
auto inner_fb = new Fallbacks("inner alternatives");
append(*inner_fb, { ANY, FW });
container.add(Stage::pointer(inner_fb));
{
SCOPED_TRACE("GEN - Fallback( ANY - FW )");
validateInit(true, true, false);
} }
} }
class ParallelTest : public InitTest<Alternatives> class ParallelTest : public InitTest<Alternatives>
{}; {};
TEST_F(ParallelTest, init_propagating) {
EXPECT_INIT_SUCCESS(true, true, BW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, BOTH, BOTH);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, BW, BOTH);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, FW, BOTH);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, FW, BOTH, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, FW, BW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, BW, FW, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(false, true, FW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
EXPECT_INIT_SUCCESS(true, false, BW, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
// external interface doesn't match derived
EXPECT_INIT_FAILURE(false, false, BOTH);
EXPECT_INIT_FAILURE(false, true, BOTH);
EXPECT_INIT_FAILURE(true, false, BOTH);
EXPECT_INIT_FAILURE(false, false, BW, BW);
EXPECT_INIT_FAILURE(false, true, BW, BW);
EXPECT_INIT_FAILURE(true, true, BW, BW);
EXPECT_INIT_FAILURE(false, false, FW, FW);
EXPECT_INIT_FAILURE(true, false, FW, FW);
EXPECT_INIT_FAILURE(true, true, FW, FW);
}
TEST_F(ParallelTest, init_matching) { TEST_F(ParallelTest, init_matching) {
EXPECT_INIT_SUCCESS(true, true, GEN, GEN); EXPECT_INIT_SUCCESS(true, true, GEN, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), GENERATE); EXPECT_EQ(container.pimpl()->interfaceFlags(), GENERATE);
@ -566,6 +552,11 @@ TEST_F(ParallelTest, init_mismatching) {
EXPECT_INIT_FAILURE(false, true, FW, CONN); EXPECT_INIT_FAILURE(false, true, FW, CONN);
EXPECT_INIT_FAILURE(true, true, FW, CONN); EXPECT_INIT_FAILURE(true, true, FW, CONN);
EXPECT_INIT_FAILURE(false, false, ANY, CONN);
EXPECT_INIT_FAILURE(true, false, ANY, CONN);
EXPECT_INIT_FAILURE(false, true, ANY, CONN);
EXPECT_INIT_FAILURE(true, true, ANY, CONN);
EXPECT_INIT_FAILURE(false, false, BW, GEN); EXPECT_INIT_FAILURE(false, false, BW, GEN);
EXPECT_INIT_FAILURE(true, false, BW, GEN); EXPECT_INIT_FAILURE(true, false, BW, GEN);
EXPECT_INIT_FAILURE(false, true, BW, GEN); EXPECT_INIT_FAILURE(false, true, BW, GEN);
@ -576,20 +567,66 @@ TEST_F(ParallelTest, init_mismatching) {
EXPECT_INIT_FAILURE(false, true, FW, GEN); EXPECT_INIT_FAILURE(false, true, FW, GEN);
EXPECT_INIT_FAILURE(true, true, FW, GEN); EXPECT_INIT_FAILURE(true, true, FW, GEN);
EXPECT_INIT_FAILURE(false, false, ANY, GEN);
EXPECT_INIT_FAILURE(true, false, ANY, GEN);
EXPECT_INIT_FAILURE(false, true, ANY, GEN);
EXPECT_INIT_FAILURE(true, true, ANY, GEN);
EXPECT_INIT_FAILURE(false, false, CONN, GEN); EXPECT_INIT_FAILURE(false, false, CONN, GEN);
EXPECT_INIT_FAILURE(true, false, CONN, GEN); EXPECT_INIT_FAILURE(true, false, CONN, GEN);
EXPECT_INIT_FAILURE(false, true, CONN, GEN); EXPECT_INIT_FAILURE(false, true, CONN, GEN);
EXPECT_INIT_FAILURE(true, true, CONN, GEN); EXPECT_INIT_FAILURE(true, true, CONN, GEN);
}
EXPECT_INIT_FAILURE(false, false, BOTH, CONN); TEST_F(ParallelTest, init_propagating) {
EXPECT_INIT_FAILURE(true, false, BOTH, CONN); EXPECT_INIT_SUCCESS(false, true, FW, FW);
EXPECT_INIT_FAILURE(false, true, BOTH, CONN); EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
EXPECT_INIT_FAILURE(true, true, BOTH, CONN);
EXPECT_INIT_FAILURE(false, false, BOTH, GEN); EXPECT_INIT_SUCCESS(true, false, BW, BW);
EXPECT_INIT_FAILURE(true, false, BOTH, GEN); EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
EXPECT_INIT_FAILURE(true, true, BOTH, GEN); EXPECT_INIT_FAILURE(false, false, BW, BW);
EXPECT_INIT_FAILURE(false, true, BW, BW);
EXPECT_INIT_FAILURE(true, true, BW, BW);
EXPECT_INIT_FAILURE(false, false, FW, FW);
EXPECT_INIT_FAILURE(true, false, FW, FW);
EXPECT_INIT_FAILURE(true, true, FW, FW);
// conflicting definitions
EXPECT_INIT_FAILURE(true, true, BW, FW); // no generator
EXPECT_INIT_FAILURE(false, false, BW, FW); // no connect
// The following two cases could in principle be allowed,
// assuming the non-matching children are plain disabled,
// but that behavior leads to very frustrating troubleshooting
EXPECT_INIT_FAILURE(false, true, BW, FW);
EXPECT_INIT_FAILURE(true, false, BW, FW);
}
TEST_F(ParallelTest, init_any) {
EXPECT_INIT_FAILURE(true, true, ANY, ANY); // no generator
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connector
// infer container (and children) direction from outside
EXPECT_INIT_SUCCESS(false, true, ANY, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
auto it = container.pimpl()->children().cbegin();
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
it = container.pimpl()->children().cbegin();
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, false, BW, ANY); // infer ANY as BACKWARDS
EXPECT_EQ((*container.pimpl()->childByIndex(1))->pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
EXPECT_INIT_SUCCESS(false, true, ANY, FW); // infer ANY as FORWARDS
EXPECT_EQ((*container.pimpl()->childByIndex(0))->pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
EXPECT_INIT_SUCCESS(false, true, FW, ANY, FW); // infer ANY as FORWARDS
EXPECT_EQ((*container.pimpl()->childByIndex(1))->pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
} }
TEST(Task, move) { TEST(Task, move) {

View File

@ -124,8 +124,9 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
static const QIcon CONNECT_ICON = icons::CONNECT.icon(); static const QIcon CONNECT_ICON = icons::CONNECT.icon();
static const QIcon FORWARD_ICON = icons::FORWARD.icon(); static const QIcon FORWARD_ICON = icons::FORWARD.icon();
static const QIcon BACKWARD_ICON = icons::BACKWARD.icon(); static const QIcon BACKWARD_ICON = icons::BACKWARD.icon();
static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon();
static const QIcon GENERATE_ICON = icons::GENERATE.icon(); static const QIcon GENERATE_ICON = icons::GENERATE.icon();
// might be needed for graphical setup of tasks
// static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon();
if (f == InterfaceFlags(CONNECT)) if (f == InterfaceFlags(CONNECT))
return CONNECT_ICON; return CONNECT_ICON;
@ -133,8 +134,6 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
return FORWARD_ICON; return FORWARD_ICON;
if (f == InterfaceFlags(PROPAGATE_BACKWARDS)) if (f == InterfaceFlags(PROPAGATE_BACKWARDS))
return BACKWARD_ICON; return BACKWARD_ICON;
if (f == PROPAGATE_BOTHWAYS)
return BOTHWAY_ICON;
if (f == InterfaceFlags(GENERATE)) if (f == InterfaceFlags(GENERATE))
return GENERATE_ICON; return GENERATE_ICON;