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