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

View File

@ -105,6 +105,10 @@ public:
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
bool canCompute() const override;
void compute() override;
@ -139,6 +143,9 @@ protected:
auto& internalToExternalMap() { return internal_to_external_; }
const auto& internalToExternalMap() const { return internal_to_external_; }
// set by containers in pruneInterface()
InterfaceFlags required_interface_;
private:
container_type children_;
@ -166,28 +173,16 @@ class SerialContainerPrivate : public ContainerBasePrivate
public:
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
void pruneInterface(InterfaceFlags accepted) override;
// validate connectivity of chain
void validateConnectivity() const override;
private:
// connect cur stage to its predecessor and successor
bool connect(container_type::const_iterator cur);
void reset();
// 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,
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_;
protected:
// connect two neighbors
void connect(StagePrivate& stage1, StagePrivate& stage2);
};
PIMPL_FUNCTIONS(SerialContainer)
@ -219,9 +214,6 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
public:
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
void pruneInterface(InterfaceFlags accepted) override;

View File

@ -83,13 +83,30 @@ enum InterfaceFlag
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
};
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
constexpr InterfaceFlags UNKNOWN;
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
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(Stage)
@ -253,12 +270,9 @@ public:
AUTO = 0x00, // auto-derive direction from context
FORWARD = 0x01, // propagate forward only
BACKWARD = 0x02, // propagate backward only
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
};
void restrictDirection(Direction dir);
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
virtual void computeForward(const InterfaceState& from) = 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 */
virtual InterfaceFlags requiredInterface() const = 0;
/** Prune interface to comply with the given propagation direction
*
* PropagatingEitherWay uses this in restrictDirection() */
virtual void pruneInterface(InterfaceFlags accepted) {}
/** validate correct interface usage and resolve ambiguous interfaces */
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)
virtual void validateConnectivity() const;

View File

@ -53,10 +53,11 @@ using namespace std::placeholders;
namespace moveit {
namespace task_constructor {
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) {
pending_backward_.reset(new Interface);
pending_forward_.reset(new Interface);
}
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name)
, required_interface_(UNKNOWN)
, pending_backward_(new Interface)
, pending_forward_(new Interface) {}
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
if (!for_insert && index < 0)
@ -252,6 +253,11 @@ void ContainerBase::reset() {
// ... and state mapping
impl->internal_to_external_.clear();
// interfaces depend on children which might change
impl->required_interface_ = UNKNOWN;
impl->starts_.reset();
impl->ends_.reset();
Stage::reset();
}
@ -401,112 +407,20 @@ SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
: ContainerBasePrivate(me, name) {}
// a serial container's required interface is derived from the required input interfaces
// of the first and last children. After resolving, it is remembered in required_interface_.
InterfaceFlags SerialContainerPrivate::requiredInterface() const {
if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK))
return required_interface_;
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
InterfaceFlags flags1 = stage1.requiredInterface();
InterfaceFlags flags2 = stage2.requiredInterface();
if (children().empty())
return UNKNOWN;
return (children().front()->pimpl()->requiredInterface() & START_IF_MASK) |
(children().back()->pimpl()->requiredInterface() & END_IF_MASK);
}
// connect cur stage to its predecessor and successor by setting the push interface pointers
// return true if cur stage should be scheduled for a second sweep
bool SerialContainerPrivate::connect(container_type::const_iterator cur) {
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;
if ((flags1 & WRITES_NEXT_START) && (flags2 & READS_START))
stage1.setNextStarts(stage2.starts());
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
stage2.setPrevEnds(stage1.ends());
else {
boost::format desc("end interface of '%1%' (%2%) does not match start interface of '%3%' (%4%)");
desc % stage1.name() % flowSymbol(flags1 & END_IF_MASK);
desc % stage2.name() % flowSymbol(flags2 & START_IF_MASK);
throw InitStageException(*me(), desc.str());
}
}
// 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
@ -514,98 +428,50 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (children().empty())
throw InitStageException(*me(), "container is empty");
// reading is always allowed if current interface flags do so
accepted |= (interfaceFlags() & InterfaceFlags({ READS_START, READS_END }));
// TODO(v4hn): if ever there is a use case to start pruning
// 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)
return; // There is nothing to prune
Stage& first = *children().front();
Stage& last = *children().back();
// If whole chain is still undecided, prune all children
if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS &&
children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) {
pruneInterfaces(children().begin(), children().end(), accepted);
} else { // otherwise only prune the first / last child's input / output interface
StagePrivate* child_impl;
child_impl = children().front()->pimpl();
child_impl->pruneInterface((accepted & START_IF_MASK) | (child_impl->interfaceFlags() & END_IF_MASK));
child_impl = children().back()->pimpl();
child_impl->pruneInterface((accepted & END_IF_MASK) | (child_impl->interfaceFlags() & START_IF_MASK));
// sweep through children once: infer and connect interfaces
first.pimpl()->pruneStartInterface(accepted);
setChildsPushBackwardInterface(first);
for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
StagePrivate* child_impl = (**it).pimpl();
StagePrivate* previous_impl = (**previous_it).pimpl();
child_impl->pruneStartInterface(invert(previous_impl->requiredInterface()));
connect(*previous_impl, *child_impl);
}
// reset my pull interfaces, if first/last child don't pull anymore
if (!children().front()->pimpl()->starts())
// potentially connect outmost push interface to pending_ buffer
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();
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();
if (interfaceFlags() == UNKNOWN)
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);
required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK;
}
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)
: 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) {
// forward pruning to all children with UNKNOWN required interface
for (const Stage::pointer& stage : children()) {
if (stage->pimpl()->requiredInterface() == UNKNOWN)
stage->pimpl()->pruneInterface(accepted);
if (children().empty())
throw InitStageException(*me(), "trying to prune empty container");
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 {
InitStageException errors;
InterfaceFlags my_interface = interfaceFlags();
InterfaceFlags children_interfaces;
// check that input / output interfaces of all children are handled by my interface
for (const auto& child : children()) {
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))
mismatchingInterface(errors, *child->pimpl(), START_IF_MASK);
if ((current & my_interface & END_IF_MASK) != (current & 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
try {
@ -795,35 +675,6 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
ParallelContainerBase::ParallelContainerBase(const std::string& 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) {
auto impl = pimpl();
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/introspection.h>
#include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
#include <boost/format.hpp>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <ros/console.h>
#include <utility>
namespace moveit {
@ -84,12 +88,38 @@ InterfaceFlags StagePrivate::interfaceFlags() const {
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 {
// check that the required interface is provided
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if ((required & actual) != required)
throw InitStageException(*me(), "required interface is not satisfied");
if ((required & actual) != required) {
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) {
@ -398,40 +428,50 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
const std::string& name)
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {
initInterface(required_interface_dirs_);
}
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {}
// initialize pull interfaces to match requested propagation directions
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
if (dir & PropagatingEitherWay::FORWARD) {
if (!starts_) // keep existing interface if possible
starts_.reset(
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1)));
} else {
starts_.reset();
if (required_interface_dirs_ != PropagatingEitherWay::AUTO && dir != required_interface_dirs_) {
auto flow = [](PropagatingEitherWay::Direction dir) -> const char* {
switch (dir) {
case PropagatingEitherWay::AUTO:
return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END });
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 (!ends_) // keep existing interface if possible
ends_.reset(
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1)));
} else {
ends_.reset();
}
if (dir == PropagatingEitherWay::FORWARD)
starts_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedStarts(it); }));
else if (dir == PropagatingEitherWay::BACKWARD)
ends_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedEnds(it); }));
required_interface_dirs_ = dir;
}
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
int dir = 0;
if ((accepted & PROPAGATE_FORWARDS) == PROPAGATE_FORWARDS)
dir |= PropagatingEitherWay::FORWARD;
if ((accepted & PROPAGATE_BACKWARDS) == PROPAGATE_BACKWARDS)
dir |= PropagatingEitherWay::BACKWARD;
initInterface(PropagatingEitherWay::Direction(dir));
if (accepted == UNKNOWN)
throw InitStageException(*me(), std::string("cannot prune to ") + flowSymbol(UNKNOWN));
if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START)
initInterface(PropagatingEitherWay::FORWARD);
else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
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 {
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if (actual == UNKNOWN)
throw InitStageException(*me(), "not connected in any direction");
@ -442,23 +482,19 @@ void PropagatingEitherWayPrivate::validateConnectivity() const {
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
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)
throw errors;
}
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
InterfaceFlags f;
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD)
f |= PROPAGATE_FORWARDS;
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
f |= PROPAGATE_BACKWARDS;
// if required_interface_dirs_ == AUTO, we don't require an interface
// but the parent container auto-derives the propagation direction
return f;
switch (required_interface_dirs_) {
case PropagatingEitherWay::FORWARD:
return PROPAGATE_FORWARDS;
case PropagatingEitherWay::BACKWARD:
return PROPAGATE_BACKWARDS;
case PropagatingEitherWay::AUTO:
return UNKNOWN;
}
}
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
@ -516,26 +552,10 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) :
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
auto impl = pimpl();
if (impl->required_interface_dirs_ == dir)
return;
if (impl->prevEnds() || impl->nextStarts())
if (impl->required_interface_dirs_ != AUTO)
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) {
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
impl->initInterface(dir);
}
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {

View File

@ -88,32 +88,25 @@ enum StageType
FW,
BW,
ANY,
BOTH,
CONN
};
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
for (StageType t : types) {
switch (t) {
case GEN:
c.insert(std::make_unique<GeneratorMockup>(runs));
c.add(std::make_unique<GeneratorMockup>(runs));
break;
case FW:
c.insert(std::make_unique<ForwardMockup>(runs));
c.add(std::make_unique<ForwardMockup>(runs));
break;
case BW:
c.insert(std::make_unique<BackwardMockup>(runs));
c.add(std::make_unique<BackwardMockup>(runs));
break;
case ANY:
c.insert(std::make_unique<PropagatorMockup>(runs, runs));
c.add(std::make_unique<PropagatorMockup>(runs, runs));
break;
case BOTH: {
auto s = std::make_unique<PropagatorMockup>(runs, runs);
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
c.insert(std::move(s));
break;
}
case CONN:
c.insert(std::make_unique<ConnectMockup>(runs));
c.add(std::make_unique<ConnectMockup>(runs));
break;
}
}
@ -134,13 +127,13 @@ TEST(ContainerBase, positionForInsert) {
EXPECT_EQ(impl->childByIndex(-1, 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_EQ(impl->childByIndex(-1, true), impl->children().end());
EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0");
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(1, true))->name().c_str(), "1");
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());
}
/* TODO: remove interface as it returns raw pointers */
TEST(ContainerBase, findChild) {
SerialContainer s, *c2;
Stage *a, *b, *c1, *d;
s.insert(Stage::pointer(a = new NamedStage("a")));
s.insert(Stage::pointer(b = new NamedStage("b")));
s.insert(Stage::pointer(c1 = new NamedStage("c")));
auto sub = Stage::pointer(c2 = new SerialContainer("c"));
c2->insert(Stage::pointer(d = new NamedStage("d")));
s.insert(std::move(sub));
s.add(Stage::pointer(a = new NamedStage("a")));
s.add(Stage::pointer(b = new NamedStage("b")));
s.add(Stage::pointer(c1 = new NamedStage("c")));
auto sub = ContainerBase::pointer(c2 = new SerialContainer("c"));
sub->add(Stage::pointer(d = new NamedStage("d")));
s.add(std::move(sub));
EXPECT_EQ(s.findChild("a"), a);
EXPECT_EQ(s.findChild("b"), b);
@ -194,17 +188,25 @@ protected:
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) {
reset(start, end);
append(container, types);
try {
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;
if (start)
accepted |= WRITES_PREV_END;
else
accepted |= READS_START;
if (end)
accepted |= WRITES_NEXT_START;
else
accepted |= READS_END;
container.pimpl()->pruneInterface(accepted);
container.pimpl()->validateConnectivity();
if (!expect_failure)
@ -314,8 +316,7 @@ TEST_F(SerialTest, init_connecting) {
EXPECT_INIT_SUCCESS(false, false, CONN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
EXPECT_INIT_FAILURE(true, true, CONN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({ CONNECT, GENERATE }));
EXPECT_INIT_FAILURE(true, true, CONN); // external interface says GENERATOR, stage is CONNECTOR
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_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) {
@ -389,6 +384,18 @@ TEST_F(SerialTest, init_backward) {
}
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
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
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(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
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
it = container.pimpl()->children().begin();
@ -435,109 +436,94 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <>
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(true, true, ANY, ANY); // no generator
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- --
it = container.pimpl()->children().begin();
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connect
}
TEST_F(SerialTest, nested_interface_detection) {
SerialContainer* inner;
// direction imposed from outer generator
container.clear();
inner = new SerialContainer("inner serial");
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");
validateInit(true, true, {}, false);
SCOPED_TRACE("GEN - ANY - Serial( ANY - ANY ) - ANY");
validateInit(true, true, false);
}
// direction imposed from inner generator
container.clear();
inner = new SerialContainer("inner serial");
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");
validateInit(true, true, {}, false);
SCOPED_TRACE("ANY - Serial( ANY - GEN - ANY ) - ANY");
validateInit(true, true, false);
}
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, { GEN, ANY });
container.insert(Stage::pointer(inner));
append(container, { ANY });
{
SCOPED_TRACE("inner - ANY");
validateInit(true, true, { ANY }, false);
SCOPED_TRACE("Serial( GEN - ANY ) - ANY");
validateInit(true, true, false);
}
// outer and inner generators conflict with each other
container.clear();
inner = new SerialContainer("inner serial");
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");
validateInit(true, true, {}, true);
SCOPED_TRACE("GEN - ANY - Serial( ANY - GEN - ANY ) - ANY");
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>
{};
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) {
EXPECT_INIT_SUCCESS(true, true, GEN, GEN);
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(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(true, false, 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(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(true, false, CONN, GEN);
EXPECT_INIT_FAILURE(false, true, CONN, GEN);
EXPECT_INIT_FAILURE(true, true, CONN, GEN);
}
EXPECT_INIT_FAILURE(false, false, BOTH, CONN);
EXPECT_INIT_FAILURE(true, false, BOTH, CONN);
EXPECT_INIT_FAILURE(false, true, BOTH, CONN);
EXPECT_INIT_FAILURE(true, true, BOTH, CONN);
TEST_F(ParallelTest, init_propagating) {
EXPECT_INIT_SUCCESS(false, true, FW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
EXPECT_INIT_FAILURE(false, false, BOTH, GEN);
EXPECT_INIT_FAILURE(true, false, BOTH, GEN);
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
EXPECT_INIT_SUCCESS(true, false, BW, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
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) {

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