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)
|
PRIVATE_CLASS(SerialContainer)
|
||||||
SerialContainer(const std::string& name = "serial container");
|
SerialContainer(const std::string& name = "serial container");
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
|
||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
|
|
||||||
@ -130,8 +128,6 @@ public:
|
|||||||
PRIVATE_CLASS(ParallelContainerBase)
|
PRIVATE_CLASS(ParallelContainerBase)
|
||||||
ParallelContainerBase(const std::string& name = "parallel container");
|
ParallelContainerBase(const std::string& name = "parallel container");
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||||
|
|
||||||
|
|||||||
@ -105,6 +105,10 @@ public:
|
|||||||
|
|
||||||
void validateConnectivity() const override;
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
|
// containers derive their required interface from their children
|
||||||
|
// UNKNOWN until pruneInterface was called
|
||||||
|
InterfaceFlags requiredInterface() const override { return required_interface_; }
|
||||||
|
|
||||||
// forward these methods to the public interface for containers
|
// forward these methods to the public interface for containers
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
@ -139,6 +143,9 @@ protected:
|
|||||||
auto& internalToExternalMap() { return internal_to_external_; }
|
auto& internalToExternalMap() { return internal_to_external_; }
|
||||||
const auto& internalToExternalMap() const { return internal_to_external_; }
|
const auto& internalToExternalMap() const { return internal_to_external_; }
|
||||||
|
|
||||||
|
// set by containers in pruneInterface()
|
||||||
|
InterfaceFlags required_interface_;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
container_type children_;
|
container_type children_;
|
||||||
|
|
||||||
@ -166,28 +173,16 @@ class SerialContainerPrivate : public ContainerBasePrivate
|
|||||||
public:
|
public:
|
||||||
SerialContainerPrivate(SerialContainer* me, const std::string& name);
|
SerialContainerPrivate(SerialContainer* me, const std::string& name);
|
||||||
|
|
||||||
// containers derive their required interface from their children
|
|
||||||
InterfaceFlags requiredInterface() const override;
|
|
||||||
|
|
||||||
// called by parent asking for pruning of this' interface
|
// called by parent asking for pruning of this' interface
|
||||||
void pruneInterface(InterfaceFlags accepted) override;
|
void pruneInterface(InterfaceFlags accepted) override;
|
||||||
// validate connectivity of chain
|
// validate connectivity of chain
|
||||||
void validateConnectivity() const override;
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
private:
|
void reset();
|
||||||
// connect cur stage to its predecessor and successor
|
|
||||||
bool connect(container_type::const_iterator cur);
|
|
||||||
|
|
||||||
// called by init() to prune interfaces for children in range [first, last)
|
protected:
|
||||||
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end);
|
// connect two neighbors
|
||||||
// prune interface for children in range [first, last) to given direction
|
void connect(StagePrivate& stage1, StagePrivate& stage2);
|
||||||
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end,
|
|
||||||
InterfaceFlags accepted);
|
|
||||||
// store first/last child's interface as required for this container
|
|
||||||
void storeRequiredInterface(container_type::const_iterator first, container_type::const_iterator end);
|
|
||||||
|
|
||||||
private:
|
|
||||||
InterfaceFlags required_interface_;
|
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(SerialContainer)
|
PIMPL_FUNCTIONS(SerialContainer)
|
||||||
|
|
||||||
@ -219,9 +214,6 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
|
|||||||
public:
|
public:
|
||||||
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
|
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
|
||||||
|
|
||||||
// containers derive their required interface from their children
|
|
||||||
InterfaceFlags requiredInterface() const override;
|
|
||||||
|
|
||||||
// called by parent asking for pruning of this' interface
|
// called by parent asking for pruning of this' interface
|
||||||
void pruneInterface(InterfaceFlags accepted) override;
|
void pruneInterface(InterfaceFlags accepted) override;
|
||||||
|
|
||||||
|
|||||||
@ -83,13 +83,30 @@ enum InterfaceFlag
|
|||||||
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
|
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
|
||||||
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
|
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef Flags<InterfaceFlag> InterfaceFlags;
|
typedef Flags<InterfaceFlag> InterfaceFlags;
|
||||||
|
|
||||||
|
/** invert interface such that
|
||||||
|
* - new end can connect to old start
|
||||||
|
* - new start can connect to old end
|
||||||
|
*/
|
||||||
|
constexpr InterfaceFlags invert(InterfaceFlags f) {
|
||||||
|
InterfaceFlags inv;
|
||||||
|
if (f & READS_START)
|
||||||
|
inv |= WRITES_NEXT_START;
|
||||||
|
if (f & WRITES_PREV_END)
|
||||||
|
inv |= READS_END;
|
||||||
|
if (f & READS_END)
|
||||||
|
inv |= WRITES_PREV_END;
|
||||||
|
if (f & WRITES_NEXT_START)
|
||||||
|
inv |= READS_START;
|
||||||
|
return inv;
|
||||||
|
};
|
||||||
|
|
||||||
// some useful constants
|
// some useful constants
|
||||||
constexpr InterfaceFlags UNKNOWN;
|
constexpr InterfaceFlags UNKNOWN;
|
||||||
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
|
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
|
||||||
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
|
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
|
||||||
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({ PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS });
|
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Interface)
|
MOVEIT_CLASS_FORWARD(Interface)
|
||||||
MOVEIT_CLASS_FORWARD(Stage)
|
MOVEIT_CLASS_FORWARD(Stage)
|
||||||
@ -253,12 +270,9 @@ public:
|
|||||||
AUTO = 0x00, // auto-derive direction from context
|
AUTO = 0x00, // auto-derive direction from context
|
||||||
FORWARD = 0x01, // propagate forward only
|
FORWARD = 0x01, // propagate forward only
|
||||||
BACKWARD = 0x02, // propagate backward only
|
BACKWARD = 0x02, // propagate backward only
|
||||||
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
|
|
||||||
};
|
};
|
||||||
void restrictDirection(Direction dir);
|
void restrictDirection(Direction dir);
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
|
||||||
|
|
||||||
virtual void computeForward(const InterfaceState& from) = 0;
|
virtual void computeForward(const InterfaceState& from) = 0;
|
||||||
virtual void computeBackward(const InterfaceState& to) = 0;
|
virtual void computeBackward(const InterfaceState& to) = 0;
|
||||||
|
|
||||||
|
|||||||
@ -76,10 +76,11 @@ public:
|
|||||||
* If the interface is unknown (because it is auto-detected from context), return UNKNOWN */
|
* If the interface is unknown (because it is auto-detected from context), return UNKNOWN */
|
||||||
virtual InterfaceFlags requiredInterface() const = 0;
|
virtual InterfaceFlags requiredInterface() const = 0;
|
||||||
|
|
||||||
/** Prune interface to comply with the given propagation direction
|
/** validate correct interface usage and resolve ambiguous interfaces */
|
||||||
*
|
virtual void pruneInterface(InterfaceFlags accepted);
|
||||||
* PropagatingEitherWay uses this in restrictDirection() */
|
|
||||||
virtual void pruneInterface(InterfaceFlags accepted) {}
|
void pruneStartInterface(InterfaceFlags start_flags) { pruneInterface(start_flags & START_IF_MASK); }
|
||||||
|
void pruneEndInterface(InterfaceFlags end_flags) { pruneInterface(end_flags & END_IF_MASK); }
|
||||||
|
|
||||||
/// validate connectivity of children (after init() was done)
|
/// validate connectivity of children (after init() was done)
|
||||||
virtual void validateConnectivity() const;
|
virtual void validateConnectivity() const;
|
||||||
|
|||||||
@ -53,10 +53,11 @@ using namespace std::placeholders;
|
|||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) {
|
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
|
||||||
pending_backward_.reset(new Interface);
|
: StagePrivate(me, name)
|
||||||
pending_forward_.reset(new Interface);
|
, required_interface_(UNKNOWN)
|
||||||
}
|
, pending_backward_(new Interface)
|
||||||
|
, pending_forward_(new Interface) {}
|
||||||
|
|
||||||
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
|
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
|
||||||
if (!for_insert && index < 0)
|
if (!for_insert && index < 0)
|
||||||
@ -252,6 +253,11 @@ void ContainerBase::reset() {
|
|||||||
// ... and state mapping
|
// ... and state mapping
|
||||||
impl->internal_to_external_.clear();
|
impl->internal_to_external_.clear();
|
||||||
|
|
||||||
|
// interfaces depend on children which might change
|
||||||
|
impl->required_interface_ = UNKNOWN;
|
||||||
|
impl->starts_.reset();
|
||||||
|
impl->ends_.reset();
|
||||||
|
|
||||||
Stage::reset();
|
Stage::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -401,112 +407,20 @@ SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new
|
|||||||
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
|
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
|
||||||
: ContainerBasePrivate(me, name) {}
|
: ContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
// a serial container's required interface is derived from the required input interfaces
|
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
|
||||||
// of the first and last children. After resolving, it is remembered in required_interface_.
|
InterfaceFlags flags1 = stage1.requiredInterface();
|
||||||
InterfaceFlags SerialContainerPrivate::requiredInterface() const {
|
InterfaceFlags flags2 = stage2.requiredInterface();
|
||||||
if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK))
|
|
||||||
return required_interface_;
|
|
||||||
|
|
||||||
if (children().empty())
|
if ((flags1 & WRITES_NEXT_START) && (flags2 & READS_START))
|
||||||
return UNKNOWN;
|
stage1.setNextStarts(stage2.starts());
|
||||||
return (children().front()->pimpl()->requiredInterface() & START_IF_MASK) |
|
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
|
||||||
(children().back()->pimpl()->requiredInterface() & END_IF_MASK);
|
stage2.setPrevEnds(stage1.ends());
|
||||||
}
|
else {
|
||||||
|
boost::format desc("end interface of '%1%' (%2%) does not match start interface of '%3%' (%4%)");
|
||||||
// connect cur stage to its predecessor and successor by setting the push interface pointers
|
desc % stage1.name() % flowSymbol(flags1 & END_IF_MASK);
|
||||||
// return true if cur stage should be scheduled for a second sweep
|
desc % stage2.name() % flowSymbol(flags2 & START_IF_MASK);
|
||||||
bool SerialContainerPrivate::connect(container_type::const_iterator cur) {
|
throw InitStageException(*me(), desc.str());
|
||||||
StagePrivate* const cur_impl = **cur;
|
|
||||||
InterfaceFlags required = cur_impl->requiredInterface();
|
|
||||||
|
|
||||||
// get iterators to prev / next stage in sequence
|
|
||||||
auto prev = cur;
|
|
||||||
--prev;
|
|
||||||
auto next = cur;
|
|
||||||
++next;
|
|
||||||
|
|
||||||
// set push forward connection using next's starts
|
|
||||||
if ((required == UNKNOWN || required & WRITES_NEXT_START) &&
|
|
||||||
next != children().end()) // last child has not a next one
|
|
||||||
cur_impl->setNextStarts((*next)->pimpl()->starts());
|
|
||||||
|
|
||||||
// set push backward connection using prev's ends
|
|
||||||
if ((required == UNKNOWN || required & WRITES_PREV_END) &&
|
|
||||||
cur != children().begin()) // first child has not a previous one
|
|
||||||
cur_impl->setPrevEnds((*prev)->pimpl()->ends());
|
|
||||||
|
|
||||||
// schedule stage with unknown interface for 2nd sweep
|
|
||||||
return required == UNKNOWN || required == PROPAGATE_BOTHWAYS;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Establishing the interface connections, we face a chicken-egg-problem:
|
|
||||||
* To establish a connection, a predecessors/successors pull interface is
|
|
||||||
* assigned to the current's stage push interface.
|
|
||||||
* However, propagating stages (in auto-detection mode) can only create
|
|
||||||
* their pull interfaces if the corresponding, opposite-side push interface
|
|
||||||
* is present already (because that's the mechanism to determine the supported
|
|
||||||
* propagation directions).
|
|
||||||
*
|
|
||||||
* Hence, we need to resolve this by performing two sweeps:
|
|
||||||
* - initialization, assuming both propagation directions should be supported,
|
|
||||||
* thus generating both pull interfaces, i.e. providing the egg
|
|
||||||
* - stripping down the interfaces to the actual context
|
|
||||||
* This context is provided by two stages pushing from both ends
|
|
||||||
* into a (potentially long) sequence of propagating stages (tbd).
|
|
||||||
*/
|
|
||||||
void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
|
||||||
// reset pull interfaces
|
|
||||||
auto impl = pimpl();
|
|
||||||
impl->starts_.reset();
|
|
||||||
impl->ends_.reset();
|
|
||||||
impl->required_interface_ = UNKNOWN;
|
|
||||||
|
|
||||||
// recursively init all children, throws if there are no children
|
|
||||||
ContainerBase::init(robot_model);
|
|
||||||
|
|
||||||
auto start = impl->children().begin();
|
|
||||||
auto last = --impl->children().end();
|
|
||||||
|
|
||||||
// connect first / last child's push interfaces to our pending_* buffers
|
|
||||||
// if they require pushing
|
|
||||||
impl->setChildsPushBackwardInterface(**start);
|
|
||||||
impl->setChildsPushForwardInterface(**last);
|
|
||||||
|
|
||||||
// initialize and connect remaining children in two sweeps
|
|
||||||
// to allow interface auto-detection for propagating stages
|
|
||||||
auto first_unknown = start; // pointer to first stage with unknown interface
|
|
||||||
for (auto cur = start, end = impl->children().end(); cur != end; ++cur) {
|
|
||||||
// 1st sweep: connect everything potentially possible,
|
|
||||||
// remembering start of unknown sub sequence
|
|
||||||
if (impl->connect(cur))
|
|
||||||
;
|
|
||||||
else { // reached a stage with known interface
|
|
||||||
// 2nd sweep: prune interfaces from [first_unknown, cur)
|
|
||||||
impl->pruneInterfaces(first_unknown, cur);
|
|
||||||
// restart with first_unknown = ++cur
|
|
||||||
first_unknown = cur;
|
|
||||||
++first_unknown;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
// prune stages [first_unknown, end())
|
|
||||||
impl->pruneInterfaces(first_unknown, impl->children().end());
|
|
||||||
|
|
||||||
// initialize this' pull interfaces if first/last child pulls
|
|
||||||
if (const InterfacePtr& target = (*start)->pimpl()->starts())
|
|
||||||
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, std::placeholders::_1,
|
|
||||||
std::cref(target), std::placeholders::_2)));
|
|
||||||
if (const InterfacePtr& target = (*last)->pimpl()->ends())
|
|
||||||
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, std::placeholders::_1,
|
|
||||||
std::cref(target), std::placeholders::_2)));
|
|
||||||
}
|
|
||||||
|
|
||||||
// prune interface for children in range [first, last) to given direction
|
|
||||||
void SerialContainerPrivate::storeRequiredInterface(container_type::const_iterator first,
|
|
||||||
container_type::const_iterator end) {
|
|
||||||
if (first == children().begin())
|
|
||||||
required_interface_ |= children().front()->pimpl()->interfaceFlags() & START_IF_MASK;
|
|
||||||
if (end == children().end() && !children().empty())
|
|
||||||
required_interface_ |= children().back()->pimpl()->interfaceFlags() & END_IF_MASK;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// called by parent asking for pruning of this' interface
|
// called by parent asking for pruning of this' interface
|
||||||
@ -514,98 +428,50 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
if (children().empty())
|
if (children().empty())
|
||||||
throw InitStageException(*me(), "container is empty");
|
throw InitStageException(*me(), "container is empty");
|
||||||
|
|
||||||
// reading is always allowed if current interface flags do so
|
// TODO(v4hn): if ever there is a use case to start pruning
|
||||||
accepted |= (interfaceFlags() & InterfaceFlags({ READS_START, READS_END }));
|
// with a specified end interface, this would need to be extended
|
||||||
|
if (!(accepted & (READS_START | WRITES_PREV_END)))
|
||||||
|
return; // The start interface direction is not decided
|
||||||
|
|
||||||
if (accepted == PROPAGATE_BOTHWAYS)
|
Stage& first = *children().front();
|
||||||
return; // There is nothing to prune
|
Stage& last = *children().back();
|
||||||
|
|
||||||
// If whole chain is still undecided, prune all children
|
// sweep through children once: infer and connect interfaces
|
||||||
if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS &&
|
first.pimpl()->pruneStartInterface(accepted);
|
||||||
children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) {
|
setChildsPushBackwardInterface(first);
|
||||||
pruneInterfaces(children().begin(), children().end(), accepted);
|
|
||||||
} else { // otherwise only prune the first / last child's input / output interface
|
for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
|
||||||
StagePrivate* child_impl;
|
StagePrivate* child_impl = (**it).pimpl();
|
||||||
child_impl = children().front()->pimpl();
|
StagePrivate* previous_impl = (**previous_it).pimpl();
|
||||||
child_impl->pruneInterface((accepted & START_IF_MASK) | (child_impl->interfaceFlags() & END_IF_MASK));
|
child_impl->pruneStartInterface(invert(previous_impl->requiredInterface()));
|
||||||
child_impl = children().back()->pimpl();
|
connect(*previous_impl, *child_impl);
|
||||||
child_impl->pruneInterface((accepted & END_IF_MASK) | (child_impl->interfaceFlags() & START_IF_MASK));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset my pull interfaces, if first/last child don't pull anymore
|
// potentially connect outmost push interface to pending_ buffer
|
||||||
if (!children().front()->pimpl()->starts())
|
setChildsPushForwardInterface(last);
|
||||||
|
|
||||||
|
if ((accepted & END_IF_MASK) != UNKNOWN &&
|
||||||
|
(last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) {
|
||||||
|
boost::format desc(
|
||||||
|
"requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)");
|
||||||
|
desc % flowSymbol(accepted & END_IF_MASK) % flowSymbol(last.pimpl()->requiredInterface() & END_IF_MASK);
|
||||||
|
throw InitStageException(*me(), desc.str());
|
||||||
|
}
|
||||||
|
|
||||||
|
// if first/last pull, this needs to pull to and forward to the children
|
||||||
|
if (const InterfacePtr& target = first.pimpl()->starts())
|
||||||
|
starts_.reset(new Interface(
|
||||||
|
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
|
||||||
|
else
|
||||||
starts_.reset();
|
starts_.reset();
|
||||||
if (!children().back()->pimpl()->ends())
|
|
||||||
|
if (const InterfacePtr& target = last.pimpl()->ends())
|
||||||
|
ends_.reset(new Interface(
|
||||||
|
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
|
||||||
|
else
|
||||||
ends_.reset();
|
ends_.reset();
|
||||||
|
|
||||||
if (interfaceFlags() == UNKNOWN)
|
required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK;
|
||||||
throw InitStageException(*me(), "failed to derive propagation direction");
|
|
||||||
}
|
|
||||||
|
|
||||||
// called by init() to prune interfaces for children in range [first, last)
|
|
||||||
// this function determines the feasible propagation directions
|
|
||||||
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end) {
|
|
||||||
if (first == end) {
|
|
||||||
storeRequiredInterface(first, end);
|
|
||||||
return; // nothing to do in this case
|
|
||||||
}
|
|
||||||
|
|
||||||
// determine accepted interface from available push interfaces
|
|
||||||
InterfaceFlags accepted;
|
|
||||||
|
|
||||||
// if first stage ...
|
|
||||||
if (first != children().begin()) {
|
|
||||||
auto prev = first;
|
|
||||||
--prev; // pointer to previous stage
|
|
||||||
// ... pushes forward, we accept forward propagation
|
|
||||||
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START)
|
|
||||||
accepted |= PROPAGATE_FORWARDS;
|
|
||||||
// ... pulls backward, we accept backward propagation
|
|
||||||
if ((*prev)->pimpl()->requiredInterface() & READS_END)
|
|
||||||
accepted |= PROPAGATE_BACKWARDS;
|
|
||||||
} // else: for first child we cannot determine the interface yet
|
|
||||||
|
|
||||||
// if end stage ...
|
|
||||||
if (end != children().end()) {
|
|
||||||
// ... pushes backward, we accept backward propagation
|
|
||||||
if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_END)
|
|
||||||
accepted |= PROPAGATE_BACKWARDS;
|
|
||||||
// ... pulls forward, we accept forward propagation
|
|
||||||
if ((*end)->pimpl()->requiredInterface() & READS_START)
|
|
||||||
accepted |= PROPAGATE_FORWARDS;
|
|
||||||
} // else: for last child we cannot determine the interface yet
|
|
||||||
|
|
||||||
// nothing to do if:
|
|
||||||
// - accepted == UNKNOWN: interface still unknown
|
|
||||||
// - accepted == PROPAGATE_BOTHWAYS: no change
|
|
||||||
if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
|
|
||||||
pruneInterfaces(first, end, accepted);
|
|
||||||
}
|
|
||||||
|
|
||||||
// prune interface for children in range [first, last) to given direction
|
|
||||||
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end,
|
|
||||||
InterfaceFlags accepted) {
|
|
||||||
// 1st sweep: remove push interfaces
|
|
||||||
for (auto it = first; it != end; ++it) {
|
|
||||||
StagePrivate* impl = (*it)->pimpl();
|
|
||||||
// the required interface should be a subset of the accepted one
|
|
||||||
if ((impl->requiredInterface() & accepted) != impl->requiredInterface())
|
|
||||||
throw InitStageException(*impl->me(), "Required interface not satisfied after pruning");
|
|
||||||
|
|
||||||
// remove push interfaces if not accepted
|
|
||||||
if (!(accepted & WRITES_PREV_END))
|
|
||||||
impl->setPrevEnds(InterfacePtr());
|
|
||||||
|
|
||||||
if (!(accepted & WRITES_NEXT_START))
|
|
||||||
impl->setNextStarts(InterfacePtr());
|
|
||||||
}
|
|
||||||
// 2nd sweep: recursively prune children
|
|
||||||
for (auto it = first; it != end; ++it) {
|
|
||||||
StagePrivate* impl = (*it)->pimpl();
|
|
||||||
impl->pruneInterface(accepted);
|
|
||||||
}
|
|
||||||
|
|
||||||
storeRequiredInterface(first, end);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void SerialContainerPrivate::validateConnectivity() const {
|
void SerialContainerPrivate::validateConnectivity() const {
|
||||||
@ -715,64 +581,78 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
|
|||||||
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
|
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
|
||||||
: ContainerBasePrivate(me, name) {}
|
: ContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
// A parallel container's required interface is derived from the required interfaces of all of its children.
|
|
||||||
// They must not conflict to each other. Otherwise an InitStageException is thrown.
|
|
||||||
InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const {
|
|
||||||
if (children().empty())
|
|
||||||
return UNKNOWN;
|
|
||||||
/* The interfaces of all children need to be consistent with each other. Allowed combinations are:
|
|
||||||
* ❘ ❘ = ❘ (connecting stages)
|
|
||||||
* ↑ ↑ = ↑ (backward propagating)
|
|
||||||
* ↓ ↓ = ↓ (forward propagating)
|
|
||||||
* ↑ ↓ = ⇅ = ⇅ ↑ = ⇅ ↓ (propagating in both directions)
|
|
||||||
* ↕ ↕ = ↕ (generating)
|
|
||||||
*/
|
|
||||||
|
|
||||||
InterfaceFlags accumulated = children().front()->pimpl()->requiredInterface();
|
|
||||||
for (const Stage::pointer& stage : children()) {
|
|
||||||
InterfaceFlags current = stage->pimpl()->requiredInterface();
|
|
||||||
if (accumulated != PROPAGATE_BOTHWAYS &&
|
|
||||||
(accumulated & current) == current) // all flags of current are already available in accumulated
|
|
||||||
continue;
|
|
||||||
|
|
||||||
bool current_is_propagating =
|
|
||||||
(current == PROPAGATE_BOTHWAYS || current == PROPAGATE_FORWARDS || current == PROPAGATE_BACKWARDS);
|
|
||||||
|
|
||||||
if (current_is_propagating && accumulated != CONNECT && accumulated != GENERATE)
|
|
||||||
accumulated |= current; // propagating is compatible to all except CONNECT and GENERATE
|
|
||||||
else
|
|
||||||
throw InitStageException(*me(),
|
|
||||||
"child '" + stage->name() + "' has conflicting interface to previous children");
|
|
||||||
}
|
|
||||||
return accumulated;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
|
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
|
||||||
// forward pruning to all children with UNKNOWN required interface
|
if (children().empty())
|
||||||
for (const Stage::pointer& stage : children()) {
|
throw InitStageException(*me(), "trying to prune empty container");
|
||||||
if (stage->pimpl()->requiredInterface() == UNKNOWN)
|
|
||||||
stage->pimpl()->pruneInterface(accepted);
|
if (accepted == UNKNOWN)
|
||||||
|
return; // nothing to prune
|
||||||
|
|
||||||
|
InitStageException exceptions;
|
||||||
|
InterfaceFlags interface;
|
||||||
|
|
||||||
|
for (const Stage::pointer& child : children()) {
|
||||||
|
try {
|
||||||
|
child->pimpl()->pruneInterface(accepted);
|
||||||
|
} catch (InitStageException& e) {
|
||||||
|
exceptions.append(e);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
InterfaceFlags child_interface = child->pimpl()->requiredInterface();
|
||||||
|
if (interface == UNKNOWN)
|
||||||
|
interface = child_interface;
|
||||||
|
else if ((interface & child_interface) != child_interface) {
|
||||||
|
boost::format desc("inferred interface of stage '%1%' (%2%/%3%) does not agree with the inferred interface of "
|
||||||
|
"its siblings (%4%/%5%).");
|
||||||
|
desc % child->name();
|
||||||
|
desc % flowSymbol(child_interface & START_IF_MASK) % flowSymbol(child_interface & END_IF_MASK);
|
||||||
|
desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK);
|
||||||
|
exceptions.push_back(*me(), desc.str());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if ((interface & accepted) != accepted) {
|
||||||
|
boost::format desc("required interface (%1%/%2%) does not match children (%3%/%4%).");
|
||||||
|
desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK);
|
||||||
|
desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK);
|
||||||
|
exceptions.push_back(*me(), desc.str());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (exceptions)
|
||||||
|
throw exceptions;
|
||||||
|
|
||||||
|
// States received by the container need to be copied to all children's pull interfaces.
|
||||||
|
if (interface & READS_START)
|
||||||
|
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||||
|
this->onNewExternalState(Interface::FORWARD, external, updated);
|
||||||
|
}));
|
||||||
|
if (interface & READS_END)
|
||||||
|
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||||
|
this->onNewExternalState(Interface::BACKWARD, external, updated);
|
||||||
|
}));
|
||||||
|
|
||||||
|
// initialize push connections of children according to their demands
|
||||||
|
for (const Stage::pointer& stage : children()) {
|
||||||
|
setChildsPushForwardInterface(*stage);
|
||||||
|
setChildsPushBackwardInterface(*stage);
|
||||||
|
}
|
||||||
|
|
||||||
|
required_interface_ = interface;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBasePrivate::validateConnectivity() const {
|
void ParallelContainerBasePrivate::validateConnectivity() const {
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
InterfaceFlags my_interface = interfaceFlags();
|
InterfaceFlags my_interface = interfaceFlags();
|
||||||
InterfaceFlags children_interfaces;
|
|
||||||
|
|
||||||
// check that input / output interfaces of all children are handled by my interface
|
// check that input / output interfaces of all children are handled by my interface
|
||||||
for (const auto& child : children()) {
|
for (const auto& child : children()) {
|
||||||
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
||||||
children_interfaces |= current; // compute union of all children interfaces
|
|
||||||
|
|
||||||
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
|
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
|
||||||
mismatchingInterface(errors, *child->pimpl(), START_IF_MASK);
|
mismatchingInterface(errors, *child->pimpl(), START_IF_MASK);
|
||||||
if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK))
|
if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK))
|
||||||
mismatchingInterface(errors, *child->pimpl(), END_IF_MASK);
|
mismatchingInterface(errors, *child->pimpl(), END_IF_MASK);
|
||||||
}
|
}
|
||||||
// check that there is a child matching the expected push interfaces
|
|
||||||
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
|
|
||||||
errors.push_back(*me(), "no child provides expected push interface");
|
|
||||||
|
|
||||||
// recursively validate children
|
// recursively validate children
|
||||||
try {
|
try {
|
||||||
@ -795,35 +675,6 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
|
|||||||
ParallelContainerBase::ParallelContainerBase(const std::string& name)
|
ParallelContainerBase::ParallelContainerBase(const std::string& name)
|
||||||
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
|
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
|
||||||
|
|
||||||
/* States received by the container need to be copied to all children's pull interfaces.
|
|
||||||
* States generated by children can be directly forwarded into the container's push interfaces.
|
|
||||||
*/
|
|
||||||
void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
|
||||||
// recursively init children
|
|
||||||
ContainerBase::init(robot_model);
|
|
||||||
auto impl = pimpl();
|
|
||||||
|
|
||||||
// determine the union of interfaces required by children
|
|
||||||
// TODO: should we better use the least common interface?
|
|
||||||
InterfaceFlags required = impl->requiredInterface();
|
|
||||||
|
|
||||||
// initialize this' pull connections
|
|
||||||
impl->starts().reset(required & READS_START ?
|
|
||||||
new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl,
|
|
||||||
Interface::FORWARD, std::placeholders::_1, std::placeholders::_2)) :
|
|
||||||
nullptr);
|
|
||||||
impl->ends().reset(required & READS_END ?
|
|
||||||
new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl,
|
|
||||||
Interface::BACKWARD, std::placeholders::_1, std::placeholders::_2)) :
|
|
||||||
nullptr);
|
|
||||||
|
|
||||||
// initialize push connections of children according to their demands
|
|
||||||
for (const Stage::pointer& stage : impl->children()) {
|
|
||||||
impl->setChildsPushForwardInterface(*stage);
|
|
||||||
impl->setChildsPushBackwardInterface(*stage);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost, std::move(comment)), solution.start(),
|
impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost, std::move(comment)), solution.start(),
|
||||||
|
|||||||
@ -39,10 +39,14 @@
|
|||||||
#include <moveit/task_constructor/container_p.h>
|
#include <moveit/task_constructor/container_p.h>
|
||||||
#include <moveit/task_constructor/introspection.h>
|
#include <moveit/task_constructor/introspection.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
|
#include <ros/console.h>
|
||||||
|
|
||||||
|
#include <boost/format.hpp>
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <ros/console.h>
|
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
@ -84,12 +88,38 @@ InterfaceFlags StagePrivate::interfaceFlags() const {
|
|||||||
return f;
|
return f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void StagePrivate::pruneInterface(InterfaceFlags accepted) {
|
||||||
|
const InterfaceFlags accepted_start{ accepted & START_IF_MASK };
|
||||||
|
const InterfaceFlags accepted_end{ accepted & END_IF_MASK };
|
||||||
|
|
||||||
|
const InterfaceFlags required{ requiredInterface() };
|
||||||
|
const InterfaceFlags required_start{ required & START_IF_MASK };
|
||||||
|
const InterfaceFlags required_end{ required & END_IF_MASK };
|
||||||
|
|
||||||
|
if (required == UNKNOWN)
|
||||||
|
throw std::logic_error("stage type does not specify interface, but also does not override pruneInterface. "
|
||||||
|
"Who sets up its interfaces?");
|
||||||
|
|
||||||
|
// only one side needs to be specified but the value has to be compatible with this stage
|
||||||
|
if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) ||
|
||||||
|
((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) {
|
||||||
|
boost::format desc("this' interface %1%/%2% cannot match inferred interface %3%/%4%");
|
||||||
|
desc % flowSymbol(required_start) % flowSymbol(required_end);
|
||||||
|
desc % flowSymbol(accepted_start) % flowSymbol(accepted_end);
|
||||||
|
throw InitStageException(*me(), desc.str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void StagePrivate::validateConnectivity() const {
|
void StagePrivate::validateConnectivity() const {
|
||||||
// check that the required interface is provided
|
// check that the required interface is provided
|
||||||
InterfaceFlags required = requiredInterface();
|
InterfaceFlags required = requiredInterface();
|
||||||
InterfaceFlags actual = interfaceFlags();
|
InterfaceFlags actual = interfaceFlags();
|
||||||
if ((required & actual) != required)
|
if ((required & actual) != required) {
|
||||||
throw InitStageException(*me(), "required interface is not satisfied");
|
boost::format desc("interface %1%/%2% does not satisfy required interface %3%/%4%");
|
||||||
|
desc % flowSymbol(actual & START_IF_MASK) % flowSymbol(actual & END_IF_MASK);
|
||||||
|
desc % flowSymbol(required & START_IF_MASK) % flowSymbol(required & END_IF_MASK);
|
||||||
|
throw InitStageException(*me(), desc.str());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
||||||
@ -398,40 +428,50 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
|
|||||||
|
|
||||||
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
|
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
|
||||||
const std::string& name)
|
const std::string& name)
|
||||||
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {
|
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {}
|
||||||
initInterface(required_interface_dirs_);
|
|
||||||
}
|
|
||||||
|
|
||||||
// initialize pull interfaces to match requested propagation directions
|
// initialize pull interfaces to match requested propagation directions
|
||||||
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
|
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
|
||||||
if (dir & PropagatingEitherWay::FORWARD) {
|
if (required_interface_dirs_ != PropagatingEitherWay::AUTO && dir != required_interface_dirs_) {
|
||||||
if (!starts_) // keep existing interface if possible
|
auto flow = [](PropagatingEitherWay::Direction dir) -> const char* {
|
||||||
starts_.reset(
|
switch (dir) {
|
||||||
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1)));
|
case PropagatingEitherWay::AUTO:
|
||||||
} else {
|
return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END });
|
||||||
starts_.reset();
|
case PropagatingEitherWay::FORWARD:
|
||||||
|
return flowSymbol(InterfaceFlags{ READS_START });
|
||||||
|
case PropagatingEitherWay::BACKWARD:
|
||||||
|
return flowSymbol(InterfaceFlags{ WRITES_PREV_END });
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
boost::format desc("propagation direction %1% incompatible with inferred direction %2%");
|
||||||
|
desc % flow(required_interface_dirs_) % flow(dir);
|
||||||
|
throw InitStageException(*me(), desc.str());
|
||||||
}
|
}
|
||||||
|
|
||||||
if (dir & PropagatingEitherWay::BACKWARD) {
|
if (dir == PropagatingEitherWay::FORWARD)
|
||||||
if (!ends_) // keep existing interface if possible
|
starts_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedStarts(it); }));
|
||||||
ends_.reset(
|
else if (dir == PropagatingEitherWay::BACKWARD)
|
||||||
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1)));
|
ends_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedEnds(it); }));
|
||||||
} else {
|
|
||||||
ends_.reset();
|
required_interface_dirs_ = dir;
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||||
int dir = 0;
|
if (accepted == UNKNOWN)
|
||||||
if ((accepted & PROPAGATE_FORWARDS) == PROPAGATE_FORWARDS)
|
throw InitStageException(*me(), std::string("cannot prune to ") + flowSymbol(UNKNOWN));
|
||||||
dir |= PropagatingEitherWay::FORWARD;
|
if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START)
|
||||||
if ((accepted & PROPAGATE_BACKWARDS) == PROPAGATE_BACKWARDS)
|
initInterface(PropagatingEitherWay::FORWARD);
|
||||||
dir |= PropagatingEitherWay::BACKWARD;
|
else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
|
||||||
initInterface(PropagatingEitherWay::Direction(dir));
|
initInterface(PropagatingEitherWay::BACKWARD);
|
||||||
|
else {
|
||||||
|
boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)");
|
||||||
|
desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK);
|
||||||
|
throw InitStageException(*me(), desc.str());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::validateConnectivity() const {
|
void PropagatingEitherWayPrivate::validateConnectivity() const {
|
||||||
InterfaceFlags required = requiredInterface();
|
|
||||||
InterfaceFlags actual = interfaceFlags();
|
InterfaceFlags actual = interfaceFlags();
|
||||||
if (actual == UNKNOWN)
|
if (actual == UNKNOWN)
|
||||||
throw InitStageException(*me(), "not connected in any direction");
|
throw InitStageException(*me(), "not connected in any direction");
|
||||||
@ -442,23 +482,19 @@ void PropagatingEitherWayPrivate::validateConnectivity() const {
|
|||||||
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
|
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
|
||||||
errors.push_back(*me(), "Cannot push backwards");
|
errors.push_back(*me(), "Cannot push backwards");
|
||||||
|
|
||||||
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY && (required & actual) != required)
|
|
||||||
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate "
|
|
||||||
<< (actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
|
|
||||||
|
|
||||||
if (errors)
|
if (errors)
|
||||||
throw errors;
|
throw errors;
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
|
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
|
||||||
InterfaceFlags f;
|
switch (required_interface_dirs_) {
|
||||||
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD)
|
case PropagatingEitherWay::FORWARD:
|
||||||
f |= PROPAGATE_FORWARDS;
|
return PROPAGATE_FORWARDS;
|
||||||
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
|
case PropagatingEitherWay::BACKWARD:
|
||||||
f |= PROPAGATE_BACKWARDS;
|
return PROPAGATE_BACKWARDS;
|
||||||
// if required_interface_dirs_ == AUTO, we don't require an interface
|
case PropagatingEitherWay::AUTO:
|
||||||
// but the parent container auto-derives the propagation direction
|
return UNKNOWN;
|
||||||
return f;
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
||||||
@ -516,26 +552,10 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) :
|
|||||||
|
|
||||||
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
|
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
if (impl->required_interface_dirs_ == dir)
|
if (impl->required_interface_dirs_ != AUTO)
|
||||||
return;
|
|
||||||
if (impl->prevEnds() || impl->nextStarts())
|
|
||||||
throw std::runtime_error("Cannot change direction after being connected");
|
throw std::runtime_error("Cannot change direction after being connected");
|
||||||
impl->required_interface_dirs_ = dir;
|
|
||||||
impl->initInterface(impl->required_interface_dirs_);
|
|
||||||
}
|
|
||||||
|
|
||||||
void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
impl->initInterface(dir);
|
||||||
Stage::init(robot_model);
|
|
||||||
auto impl = pimpl();
|
|
||||||
|
|
||||||
// In AUTO-mode, i.e. when auto-detecting direction of propagation from context,
|
|
||||||
// pretend that we offer both interface directions during init().
|
|
||||||
// This is needed due to a chicken-egg-problem: interface auto-detection requires
|
|
||||||
// the context (external pushing interfaces prevEnds, nextStarts) to be set,
|
|
||||||
// while they are ony set if we detected the correct interface...
|
|
||||||
if (impl->required_interface_dirs_ == AUTO)
|
|
||||||
impl->initInterface(BOTHWAY);
|
|
||||||
// otherwise the interface is already fixed and well-defined
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
|
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
|
||||||
|
|||||||
@ -88,32 +88,25 @@ enum StageType
|
|||||||
FW,
|
FW,
|
||||||
BW,
|
BW,
|
||||||
ANY,
|
ANY,
|
||||||
BOTH,
|
|
||||||
CONN
|
CONN
|
||||||
};
|
};
|
||||||
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
|
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
|
||||||
for (StageType t : types) {
|
for (StageType t : types) {
|
||||||
switch (t) {
|
switch (t) {
|
||||||
case GEN:
|
case GEN:
|
||||||
c.insert(std::make_unique<GeneratorMockup>(runs));
|
c.add(std::make_unique<GeneratorMockup>(runs));
|
||||||
break;
|
break;
|
||||||
case FW:
|
case FW:
|
||||||
c.insert(std::make_unique<ForwardMockup>(runs));
|
c.add(std::make_unique<ForwardMockup>(runs));
|
||||||
break;
|
break;
|
||||||
case BW:
|
case BW:
|
||||||
c.insert(std::make_unique<BackwardMockup>(runs));
|
c.add(std::make_unique<BackwardMockup>(runs));
|
||||||
break;
|
break;
|
||||||
case ANY:
|
case ANY:
|
||||||
c.insert(std::make_unique<PropagatorMockup>(runs, runs));
|
c.add(std::make_unique<PropagatorMockup>(runs, runs));
|
||||||
break;
|
break;
|
||||||
case BOTH: {
|
|
||||||
auto s = std::make_unique<PropagatorMockup>(runs, runs);
|
|
||||||
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
|
|
||||||
c.insert(std::move(s));
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case CONN:
|
case CONN:
|
||||||
c.insert(std::make_unique<ConnectMockup>(runs));
|
c.add(std::make_unique<ConnectMockup>(runs));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -134,13 +127,13 @@ TEST(ContainerBase, positionForInsert) {
|
|||||||
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
|
||||||
EXPECT_EQ(impl->childByIndex(-2, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-2, true), impl->children().end());
|
||||||
|
|
||||||
s.insert(std::make_unique<NamedStage>("0"));
|
s.add(std::make_unique<NamedStage>("0"));
|
||||||
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
|
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
|
||||||
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
|
||||||
EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0");
|
EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0");
|
||||||
EXPECT_EQ(impl->childByIndex(-3, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-3, true), impl->children().end());
|
||||||
|
|
||||||
s.insert(std::make_unique<NamedStage>("1"));
|
s.add(std::make_unique<NamedStage>("1"));
|
||||||
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
|
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
|
||||||
EXPECT_STREQ((*impl->childByIndex(1, true))->name().c_str(), "1");
|
EXPECT_STREQ((*impl->childByIndex(1, true))->name().c_str(), "1");
|
||||||
EXPECT_EQ(impl->childByIndex(2, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(2, true), impl->children().end());
|
||||||
@ -151,15 +144,16 @@ TEST(ContainerBase, positionForInsert) {
|
|||||||
EXPECT_EQ(impl->childByIndex(-4, true), impl->children().end());
|
EXPECT_EQ(impl->childByIndex(-4, true), impl->children().end());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* TODO: remove interface as it returns raw pointers */
|
||||||
TEST(ContainerBase, findChild) {
|
TEST(ContainerBase, findChild) {
|
||||||
SerialContainer s, *c2;
|
SerialContainer s, *c2;
|
||||||
Stage *a, *b, *c1, *d;
|
Stage *a, *b, *c1, *d;
|
||||||
s.insert(Stage::pointer(a = new NamedStage("a")));
|
s.add(Stage::pointer(a = new NamedStage("a")));
|
||||||
s.insert(Stage::pointer(b = new NamedStage("b")));
|
s.add(Stage::pointer(b = new NamedStage("b")));
|
||||||
s.insert(Stage::pointer(c1 = new NamedStage("c")));
|
s.add(Stage::pointer(c1 = new NamedStage("c")));
|
||||||
auto sub = Stage::pointer(c2 = new SerialContainer("c"));
|
auto sub = ContainerBase::pointer(c2 = new SerialContainer("c"));
|
||||||
c2->insert(Stage::pointer(d = new NamedStage("d")));
|
sub->add(Stage::pointer(d = new NamedStage("d")));
|
||||||
s.insert(std::move(sub));
|
s.add(std::move(sub));
|
||||||
|
|
||||||
EXPECT_EQ(s.findChild("a"), a);
|
EXPECT_EQ(s.findChild("a"), a);
|
||||||
EXPECT_EQ(s.findChild("b"), b);
|
EXPECT_EQ(s.findChild("b"), b);
|
||||||
@ -194,17 +188,25 @@ protected:
|
|||||||
pushInterface(start, end);
|
pushInterface(start, end);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void validateInit(bool start, bool end, bool expect_failure) { validateInit(start, end, {}, expect_failure); }
|
||||||
|
|
||||||
void validateInit(bool start, bool end, const std::initializer_list<StageType>& types, bool expect_failure) {
|
void validateInit(bool start, bool end, const std::initializer_list<StageType>& types, bool expect_failure) {
|
||||||
reset(start, end);
|
reset(start, end);
|
||||||
append(container, types);
|
append(container, types);
|
||||||
try {
|
try {
|
||||||
container.init(robot_model);
|
container.init(robot_model);
|
||||||
// prune pull interfaces based on provided external interface (start, end)
|
// prune interfaces based on provided external interface (start, end)
|
||||||
InterfaceFlags accepted;
|
InterfaceFlags accepted;
|
||||||
if (start)
|
if (start)
|
||||||
accepted |= WRITES_PREV_END;
|
accepted |= WRITES_PREV_END;
|
||||||
|
else
|
||||||
|
accepted |= READS_START;
|
||||||
|
|
||||||
if (end)
|
if (end)
|
||||||
accepted |= WRITES_NEXT_START;
|
accepted |= WRITES_NEXT_START;
|
||||||
|
else
|
||||||
|
accepted |= READS_END;
|
||||||
|
|
||||||
container.pimpl()->pruneInterface(accepted);
|
container.pimpl()->pruneInterface(accepted);
|
||||||
container.pimpl()->validateConnectivity();
|
container.pimpl()->validateConnectivity();
|
||||||
if (!expect_failure)
|
if (!expect_failure)
|
||||||
@ -314,8 +316,7 @@ TEST_F(SerialTest, init_connecting) {
|
|||||||
EXPECT_INIT_SUCCESS(false, false, CONN);
|
EXPECT_INIT_SUCCESS(false, false, CONN);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(true, true, CONN);
|
EXPECT_INIT_FAILURE(true, true, CONN); // external interface says GENERATOR, stage is CONNECTOR
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({ CONNECT, GENERATE }));
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
|
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
|
||||||
}
|
}
|
||||||
@ -363,12 +364,6 @@ TEST_F(SerialTest, init_forward) {
|
|||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
|
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
// BOTH requires propagation in both directions, while FW/BW can only match one dir
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, FW);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, FW, BOTH);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, BW);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BW, BOTH);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, init_backward) {
|
TEST_F(SerialTest, init_backward) {
|
||||||
@ -389,6 +384,18 @@ TEST_F(SerialTest, init_backward) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, interface_detection) {
|
TEST_F(SerialTest, interface_detection) {
|
||||||
|
// ANY resolves in either direction
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
EXPECT_INIT_SUCCESS(true, false, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||||
|
|
||||||
// derive propagation direction from inner generator
|
// derive propagation direction from inner generator
|
||||||
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
|
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
|
||||||
auto it = container.pimpl()->children().begin();
|
auto it = container.pimpl()->children().begin();
|
||||||
@ -397,12 +404,6 @@ TEST_F(SerialTest, interface_detection) {
|
|||||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
|
||||||
|
|
||||||
// derive propagation direction from inner connector
|
// derive propagation direction from inner connector
|
||||||
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
|
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
|
||||||
it = container.pimpl()->children().begin();
|
it = container.pimpl()->children().begin();
|
||||||
@ -435,109 +436,94 @@ TEST_F(SerialTest, interface_detection) {
|
|||||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <>
|
EXPECT_INIT_FAILURE(true, true, ANY, ANY); // no generator
|
||||||
it = container.pimpl()->children().begin();
|
|
||||||
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- --
|
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connect
|
||||||
it = container.pimpl()->children().begin();
|
|
||||||
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), UNKNOWN);
|
|
||||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), UNKNOWN);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, nested_interface_detection) {
|
TEST_F(SerialTest, nested_interface_detection) {
|
||||||
SerialContainer* inner;
|
SerialContainer* inner;
|
||||||
|
|
||||||
// direction imposed from outer generator
|
// direction imposed from outer generator
|
||||||
container.clear();
|
container.clear();
|
||||||
|
|
||||||
inner = new SerialContainer("inner serial");
|
inner = new SerialContainer("inner serial");
|
||||||
append(*inner, { ANY, ANY });
|
append(*inner, { ANY, ANY });
|
||||||
append(container, { GEN, ANY, ANY });
|
|
||||||
container.insert(Stage::pointer(inner), -2);
|
append(container, { GEN, ANY });
|
||||||
|
container.add(Stage::pointer(inner));
|
||||||
|
append(container, { ANY });
|
||||||
{
|
{
|
||||||
SCOPED_TRACE("GEN - ANY - inner - ANY");
|
SCOPED_TRACE("GEN - ANY - Serial( ANY - ANY ) - ANY");
|
||||||
validateInit(true, true, {}, false);
|
validateInit(true, true, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// direction imposed from inner generator
|
// direction imposed from inner generator
|
||||||
container.clear();
|
container.clear();
|
||||||
|
|
||||||
inner = new SerialContainer("inner serial");
|
inner = new SerialContainer("inner serial");
|
||||||
append(*inner, { ANY, GEN, ANY });
|
append(*inner, { ANY, GEN, ANY });
|
||||||
append(container, { ANY, ANY });
|
|
||||||
container.insert(Stage::pointer(inner), -2);
|
append(container, { ANY });
|
||||||
|
container.add(Stage::pointer(inner));
|
||||||
|
append(container, { ANY });
|
||||||
{
|
{
|
||||||
SCOPED_TRACE("ANY - inner - ANY");
|
SCOPED_TRACE("ANY - Serial( ANY - GEN - ANY ) - ANY");
|
||||||
validateInit(true, true, {}, false);
|
validateInit(true, true, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
container.clear();
|
container.clear();
|
||||||
inner = new SerialContainer("inner serial");
|
inner = new SerialContainer("inner serial");
|
||||||
append(*inner, { GEN, ANY });
|
append(*inner, { GEN, ANY });
|
||||||
|
|
||||||
container.insert(Stage::pointer(inner));
|
container.insert(Stage::pointer(inner));
|
||||||
|
append(container, { ANY });
|
||||||
{
|
{
|
||||||
SCOPED_TRACE("inner - ANY");
|
SCOPED_TRACE("Serial( GEN - ANY ) - ANY");
|
||||||
validateInit(true, true, { ANY }, false);
|
validateInit(true, true, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// outer and inner generators conflict with each other
|
// outer and inner generators conflict with each other
|
||||||
container.clear();
|
container.clear();
|
||||||
|
|
||||||
inner = new SerialContainer("inner serial");
|
inner = new SerialContainer("inner serial");
|
||||||
append(*inner, { ANY, GEN, ANY });
|
append(*inner, { ANY, GEN, ANY });
|
||||||
append(container, { GEN, ANY, ANY });
|
|
||||||
container.insert(Stage::pointer(inner), -2);
|
append(container, { GEN, ANY });
|
||||||
|
container.add(Stage::pointer(inner));
|
||||||
|
append(container, { ANY });
|
||||||
{
|
{
|
||||||
SCOPED_TRACE("GEN - ANY - inner - ANY");
|
SCOPED_TRACE("GEN - ANY - Serial( ANY - GEN - ANY ) - ANY");
|
||||||
validateInit(true, true, {}, true);
|
validateInit(true, true, {}, true); // expected to fail
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(SerialTest, nested_parallel) {
|
||||||
|
container.clear();
|
||||||
|
|
||||||
|
append(container, { GEN });
|
||||||
|
auto inner_alt = new Alternatives("inner alternatives");
|
||||||
|
append(*inner_alt, { ANY, ANY });
|
||||||
|
container.add(Stage::pointer(inner_alt));
|
||||||
|
{
|
||||||
|
SCOPED_TRACE("GEN - Alternatives( ANY - ANY )");
|
||||||
|
validateInit(true, true, false);
|
||||||
|
}
|
||||||
|
|
||||||
|
container.clear();
|
||||||
|
append(container, { GEN });
|
||||||
|
auto inner_fb = new Fallbacks("inner alternatives");
|
||||||
|
append(*inner_fb, { ANY, FW });
|
||||||
|
container.add(Stage::pointer(inner_fb));
|
||||||
|
{
|
||||||
|
SCOPED_TRACE("GEN - Fallback( ANY - FW )");
|
||||||
|
validateInit(true, true, false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
class ParallelTest : public InitTest<Alternatives>
|
class ParallelTest : public InitTest<Alternatives>
|
||||||
{};
|
{};
|
||||||
|
|
||||||
TEST_F(ParallelTest, init_propagating) {
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BW, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BOTH, BOTH);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BW, BOTH);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, FW, BOTH);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, FW, BOTH, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, FW, BW, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BW, FW, BW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(false, true, FW, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, false, BW, BW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
|
|
||||||
|
|
||||||
// external interface doesn't match derived
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BOTH);
|
|
||||||
EXPECT_INIT_FAILURE(false, true, BOTH);
|
|
||||||
EXPECT_INIT_FAILURE(true, false, BOTH);
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BW, BW);
|
|
||||||
EXPECT_INIT_FAILURE(false, true, BW, BW);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BW, BW);
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, FW, FW);
|
|
||||||
EXPECT_INIT_FAILURE(true, false, FW, FW);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, FW, FW);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_F(ParallelTest, init_matching) {
|
TEST_F(ParallelTest, init_matching) {
|
||||||
EXPECT_INIT_SUCCESS(true, true, GEN, GEN);
|
EXPECT_INIT_SUCCESS(true, true, GEN, GEN);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), GENERATE);
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), GENERATE);
|
||||||
@ -566,6 +552,11 @@ TEST_F(ParallelTest, init_mismatching) {
|
|||||||
EXPECT_INIT_FAILURE(false, true, FW, CONN);
|
EXPECT_INIT_FAILURE(false, true, FW, CONN);
|
||||||
EXPECT_INIT_FAILURE(true, true, FW, CONN);
|
EXPECT_INIT_FAILURE(true, true, FW, CONN);
|
||||||
|
|
||||||
|
EXPECT_INIT_FAILURE(false, false, ANY, CONN);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, ANY, CONN);
|
||||||
|
EXPECT_INIT_FAILURE(false, true, ANY, CONN);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, ANY, CONN);
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BW, GEN);
|
EXPECT_INIT_FAILURE(false, false, BW, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, false, BW, GEN);
|
EXPECT_INIT_FAILURE(true, false, BW, GEN);
|
||||||
EXPECT_INIT_FAILURE(false, true, BW, GEN);
|
EXPECT_INIT_FAILURE(false, true, BW, GEN);
|
||||||
@ -576,20 +567,66 @@ TEST_F(ParallelTest, init_mismatching) {
|
|||||||
EXPECT_INIT_FAILURE(false, true, FW, GEN);
|
EXPECT_INIT_FAILURE(false, true, FW, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, true, FW, GEN);
|
EXPECT_INIT_FAILURE(true, true, FW, GEN);
|
||||||
|
|
||||||
|
EXPECT_INIT_FAILURE(false, false, ANY, GEN);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, ANY, GEN);
|
||||||
|
EXPECT_INIT_FAILURE(false, true, ANY, GEN);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, ANY, GEN);
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, CONN, GEN);
|
EXPECT_INIT_FAILURE(false, false, CONN, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, false, CONN, GEN);
|
EXPECT_INIT_FAILURE(true, false, CONN, GEN);
|
||||||
EXPECT_INIT_FAILURE(false, true, CONN, GEN);
|
EXPECT_INIT_FAILURE(false, true, CONN, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, true, CONN, GEN);
|
EXPECT_INIT_FAILURE(true, true, CONN, GEN);
|
||||||
|
}
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BOTH, CONN);
|
TEST_F(ParallelTest, init_propagating) {
|
||||||
EXPECT_INIT_FAILURE(true, false, BOTH, CONN);
|
EXPECT_INIT_SUCCESS(false, true, FW, FW);
|
||||||
EXPECT_INIT_FAILURE(false, true, BOTH, CONN);
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, CONN);
|
|
||||||
|
|
||||||
EXPECT_INIT_FAILURE(false, false, BOTH, GEN);
|
EXPECT_INIT_SUCCESS(true, false, BW, BW);
|
||||||
EXPECT_INIT_FAILURE(true, false, BOTH, GEN);
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
|
||||||
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
|
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
|
EXPECT_INIT_FAILURE(false, false, BW, BW);
|
||||||
|
EXPECT_INIT_FAILURE(false, true, BW, BW);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, BW, BW);
|
||||||
|
|
||||||
|
EXPECT_INIT_FAILURE(false, false, FW, FW);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, FW, FW);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, FW, FW);
|
||||||
|
|
||||||
|
// conflicting definitions
|
||||||
|
EXPECT_INIT_FAILURE(true, true, BW, FW); // no generator
|
||||||
|
EXPECT_INIT_FAILURE(false, false, BW, FW); // no connect
|
||||||
|
|
||||||
|
// The following two cases could in principle be allowed,
|
||||||
|
// assuming the non-matching children are plain disabled,
|
||||||
|
// but that behavior leads to very frustrating troubleshooting
|
||||||
|
EXPECT_INIT_FAILURE(false, true, BW, FW);
|
||||||
|
EXPECT_INIT_FAILURE(true, false, BW, FW);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(ParallelTest, init_any) {
|
||||||
|
EXPECT_INIT_FAILURE(true, true, ANY, ANY); // no generator
|
||||||
|
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connector
|
||||||
|
|
||||||
|
// infer container (and children) direction from outside
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, ANY, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
auto it = container.pimpl()->children().cbegin();
|
||||||
|
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, false, ANY, ANY);
|
||||||
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
it = container.pimpl()->children().cbegin();
|
||||||
|
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
|
EXPECT_INIT_SUCCESS(true, false, BW, ANY); // infer ANY as BACKWARDS
|
||||||
|
EXPECT_EQ((*container.pimpl()->childByIndex(1))->pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, ANY, FW); // infer ANY as FORWARDS
|
||||||
|
EXPECT_EQ((*container.pimpl()->childByIndex(0))->pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
|
||||||
|
EXPECT_INIT_SUCCESS(false, true, FW, ANY, FW); // infer ANY as FORWARDS
|
||||||
|
EXPECT_EQ((*container.pimpl()->childByIndex(1))->pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(Task, move) {
|
TEST(Task, move) {
|
||||||
|
|||||||
@ -124,8 +124,9 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
|
|||||||
static const QIcon CONNECT_ICON = icons::CONNECT.icon();
|
static const QIcon CONNECT_ICON = icons::CONNECT.icon();
|
||||||
static const QIcon FORWARD_ICON = icons::FORWARD.icon();
|
static const QIcon FORWARD_ICON = icons::FORWARD.icon();
|
||||||
static const QIcon BACKWARD_ICON = icons::BACKWARD.icon();
|
static const QIcon BACKWARD_ICON = icons::BACKWARD.icon();
|
||||||
static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon();
|
|
||||||
static const QIcon GENERATE_ICON = icons::GENERATE.icon();
|
static const QIcon GENERATE_ICON = icons::GENERATE.icon();
|
||||||
|
// might be needed for graphical setup of tasks
|
||||||
|
// static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon();
|
||||||
|
|
||||||
if (f == InterfaceFlags(CONNECT))
|
if (f == InterfaceFlags(CONNECT))
|
||||||
return CONNECT_ICON;
|
return CONNECT_ICON;
|
||||||
@ -133,8 +134,6 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
|
|||||||
return FORWARD_ICON;
|
return FORWARD_ICON;
|
||||||
if (f == InterfaceFlags(PROPAGATE_BACKWARDS))
|
if (f == InterfaceFlags(PROPAGATE_BACKWARDS))
|
||||||
return BACKWARD_ICON;
|
return BACKWARD_ICON;
|
||||||
if (f == PROPAGATE_BOTHWAYS)
|
|
||||||
return BOTHWAY_ICON;
|
|
||||||
if (f == InterfaceFlags(GENERATE))
|
if (f == InterfaceFlags(GENERATE))
|
||||||
return GENERATE_ICON;
|
return GENERATE_ICON;
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user