mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Further simplify interface resolution
* Do not modify explicitly configured direction of Propagator * Avoid code duplication in pruneInterface() and validateConnectivity() Only implement pruneInterface() for stages that actually need to adapt their interface.
This commit is contained in:
parent
4d8d32dda5
commit
10260b9dd7
@ -105,7 +105,7 @@ public:
|
||||
|
||||
void validateConnectivity() const override;
|
||||
|
||||
// containers derive their required interface from their children
|
||||
// Containers derive their required interface from their children
|
||||
// UNKNOWN until pruneInterface was called
|
||||
InterfaceFlags requiredInterface() const override { return required_interface_; }
|
||||
|
||||
@ -124,12 +124,12 @@ protected:
|
||||
// If, during pruneInterface() later, we notice that it's not needed, prune there.
|
||||
inline void setChildsPushBackwardInterface(Stage& child) {
|
||||
InterfaceFlags required = child.pimpl()->requiredInterface();
|
||||
bool allowed = (required & WRITES_PREV_END) || (required == UNKNOWN);
|
||||
bool allowed = (required & WRITES_PREV_END);
|
||||
child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
|
||||
}
|
||||
inline void setChildsPushForwardInterface(Stage& child) {
|
||||
InterfaceFlags required = child.pimpl()->requiredInterface();
|
||||
bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
|
||||
bool allowed = (required & WRITES_NEXT_START);
|
||||
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
||||
}
|
||||
// report error about mismatching interface (start or end as determined by mask)
|
||||
@ -144,7 +144,7 @@ protected:
|
||||
auto& internalToExternalMap() { return internal_to_external_; }
|
||||
const auto& internalToExternalMap() const { return internal_to_external_; }
|
||||
|
||||
// set by containers in pruneInterface()
|
||||
// set in pruneInterface()
|
||||
InterfaceFlags required_interface_;
|
||||
|
||||
private:
|
||||
@ -249,7 +249,7 @@ public:
|
||||
typedef std::function<void(SubTrajectory&&)> Spawner;
|
||||
MergerPrivate(Merger* me, const std::string& name);
|
||||
|
||||
InterfaceFlags requiredInterface() const override;
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
|
||||
void onNewPropagateSolution(const SolutionBase& s);
|
||||
void onNewGeneratorSolution(const SolutionBase& s);
|
||||
|
||||
@ -76,11 +76,10 @@ public:
|
||||
* If the interface is unknown (because it is auto-detected from context), return UNKNOWN */
|
||||
virtual InterfaceFlags requiredInterface() const = 0;
|
||||
|
||||
/** 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); }
|
||||
/** Prune interface to comply with the given propagation direction
|
||||
*
|
||||
* PropagatingEitherWay uses this in restrictDirection() */
|
||||
virtual void pruneInterface(InterfaceFlags /* accepted */) {}
|
||||
|
||||
/// validate connectivity of children (after init() was done)
|
||||
virtual void validateConnectivity() const;
|
||||
@ -212,10 +211,10 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate
|
||||
friend class PropagatingEitherWay;
|
||||
|
||||
public:
|
||||
PropagatingEitherWay::Direction required_interface_dirs_;
|
||||
PropagatingEitherWay::Direction configured_dir_;
|
||||
InterfaceFlags required_interface_;
|
||||
|
||||
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me,
|
||||
PropagatingEitherWay::Direction required_interface_dirs_,
|
||||
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction configured_dir_,
|
||||
const std::string& name);
|
||||
|
||||
InterfaceFlags requiredInterface() const override;
|
||||
|
||||
@ -425,51 +425,39 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2)
|
||||
|
||||
// called by parent asking for pruning of this' interface
|
||||
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
// we need to have some children to do the actual work
|
||||
if (children().empty())
|
||||
throw InitStageException(*me(), "container is empty");
|
||||
throw InitStageException(*me(), "no children");
|
||||
|
||||
// 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 & START_IF_MASK))
|
||||
throw InitStageException(*me(), "unknown start interface");
|
||||
|
||||
Stage& first = *children().front();
|
||||
Stage& last = *children().back();
|
||||
|
||||
// sweep through children once: infer and connect interfaces
|
||||
first.pimpl()->pruneStartInterface(accepted);
|
||||
// FIRST child
|
||||
first.pimpl()->pruneInterface(accepted & START_IF_MASK);
|
||||
// connect first child's (start) push interface
|
||||
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);
|
||||
}
|
||||
|
||||
// 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<END_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(last.pimpl()->requiredInterface());
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
|
||||
// if first/last pull, this needs to pull to and forward to the children
|
||||
// connect first child's (start) pull interface
|
||||
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();
|
||||
|
||||
// process all children and connect them
|
||||
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->pruneInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
|
||||
connect(*previous_impl, *child_impl);
|
||||
}
|
||||
|
||||
// connect last child's (end) push interface
|
||||
setChildsPushForwardInterface(last);
|
||||
// connect last child's (end) pull interface
|
||||
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();
|
||||
|
||||
required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK;
|
||||
}
|
||||
@ -582,11 +570,9 @@ ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase
|
||||
: ContainerBasePrivate(me, name) {}
|
||||
|
||||
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
// we need to have some children to do the actual work
|
||||
if (children().empty())
|
||||
throw InitStageException(*me(), "trying to prune empty container");
|
||||
|
||||
if (accepted == UNKNOWN)
|
||||
return; // nothing to prune
|
||||
throw InitStageException(*me(), "no children");
|
||||
|
||||
InitStageException exceptions;
|
||||
InterfaceFlags interface;
|
||||
@ -789,24 +775,14 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
|
||||
|
||||
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
|
||||
|
||||
InterfaceFlags MergerPrivate::requiredInterface() const {
|
||||
if (children().size() < 2)
|
||||
throw InitStageException(*me_, "Need 2 children at least.");
|
||||
|
||||
InterfaceFlags required = ParallelContainerBasePrivate::requiredInterface();
|
||||
|
||||
// all children need to share a common interface
|
||||
for (const Stage::pointer& stage : children()) {
|
||||
InterfaceFlags current = stage->pimpl()->requiredInterface();
|
||||
if (current != required)
|
||||
throw InitStageException(*stage, "Interface does not match the common one.");
|
||||
}
|
||||
|
||||
switch (required) {
|
||||
void MergerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
ContainerBasePrivate::pruneInterface(accepted);
|
||||
switch (interfaceFlags()) {
|
||||
case PROPAGATE_FORWARDS:
|
||||
case PROPAGATE_BACKWARDS:
|
||||
case UNKNOWN:
|
||||
break; // these are supported
|
||||
|
||||
case GENERATE:
|
||||
throw InitStageException(*me_, "Generator stages not yet supported.");
|
||||
case CONNECT:
|
||||
@ -814,7 +790,6 @@ InterfaceFlags MergerPrivate::requiredInterface() const {
|
||||
default:
|
||||
throw InitStageException(*me_, "Children's interface not supported.");
|
||||
}
|
||||
return required;
|
||||
}
|
||||
|
||||
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
|
||||
|
||||
@ -112,28 +112,6 @@ 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("interface %1% %2% does not match inferred interface %3% %4%");
|
||||
desc % flowSymbol<START_IF_MASK>(required_start) % flowSymbol<END_IF_MASK>(required_end);
|
||||
desc % flowSymbol<START_IF_MASK>(accepted_start) % flowSymbol<END_IF_MASK>(accepted_end);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
}
|
||||
|
||||
void StagePrivate::validateConnectivity() const {
|
||||
// check that the required interface is provided
|
||||
InterfaceFlags required = requiredInterface();
|
||||
@ -416,47 +394,53 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
|
||||
|
||||
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
|
||||
const std::string& name)
|
||||
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {}
|
||||
: ComputeBasePrivate(me, name), configured_dir_(dir) {
|
||||
initInterface(dir);
|
||||
}
|
||||
|
||||
// initialize pull interfaces to match requested propagation directions
|
||||
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
|
||||
if (required_interface_dirs_ != PropagatingEitherWay::AUTO && dir != required_interface_dirs_) {
|
||||
auto flow = [](PropagatingEitherWay::Direction dir) -> const char* {
|
||||
switch (dir) {
|
||||
case PropagatingEitherWay::AUTO:
|
||||
return flowSymbol<START_IF_MASK>(InterfaceFlags{ READS_START, WRITES_PREV_END });
|
||||
case PropagatingEitherWay::FORWARD:
|
||||
return flowSymbol<START_IF_MASK>(InterfaceFlags{ READS_START });
|
||||
case PropagatingEitherWay::BACKWARD:
|
||||
return flowSymbol<START_IF_MASK>(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());
|
||||
switch (dir) {
|
||||
case PropagatingEitherWay::FORWARD:
|
||||
required_interface_ = PROPAGATE_FORWARDS;
|
||||
if (!starts_) // keep existing interface if possible
|
||||
starts_.reset(
|
||||
new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedStarts(it); }));
|
||||
ends_.reset();
|
||||
return;
|
||||
case PropagatingEitherWay::BACKWARD:
|
||||
required_interface_ = PROPAGATE_BACKWARDS;
|
||||
starts_.reset();
|
||||
if (!ends_) // keep existing interface if possible
|
||||
ends_.reset(new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedEnds(it); }));
|
||||
return;
|
||||
case PropagatingEitherWay::AUTO:
|
||||
required_interface_ = UNKNOWN;
|
||||
return;
|
||||
}
|
||||
|
||||
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) {
|
||||
if (accepted == UNKNOWN)
|
||||
throw InitStageException(*me(), "cannot prune to unknown interface");
|
||||
throw InitStageException(*me(), "cannot initialize to unknown interface");
|
||||
|
||||
auto dir = PropagatingEitherWay::AUTO;
|
||||
if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START)
|
||||
initInterface(PropagatingEitherWay::FORWARD);
|
||||
dir = PropagatingEitherWay::FORWARD;
|
||||
else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
|
||||
initInterface(PropagatingEitherWay::BACKWARD);
|
||||
dir = PropagatingEitherWay::BACKWARD;
|
||||
else {
|
||||
boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)");
|
||||
boost::format desc("propagator cannot handle external interface %1% %2%");
|
||||
desc % flowSymbol<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
|
||||
boost::format desc("configured interface (%1% %2%) does not match external one (%3% %4%)");
|
||||
desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
|
||||
desc % flowSymbol<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
initInterface(dir);
|
||||
}
|
||||
|
||||
void PropagatingEitherWayPrivate::validateConnectivity() const {
|
||||
@ -475,14 +459,7 @@ void PropagatingEitherWayPrivate::validateConnectivity() const {
|
||||
}
|
||||
|
||||
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
|
||||
switch (required_interface_dirs_) {
|
||||
case PropagatingEitherWay::FORWARD:
|
||||
return PROPAGATE_FORWARDS;
|
||||
case PropagatingEitherWay::BACKWARD:
|
||||
return PROPAGATE_BACKWARDS;
|
||||
case PropagatingEitherWay::AUTO:
|
||||
return UNKNOWN;
|
||||
}
|
||||
return required_interface_;
|
||||
}
|
||||
|
||||
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
|
||||
@ -540,9 +517,11 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) :
|
||||
|
||||
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
|
||||
auto impl = pimpl();
|
||||
if (impl->required_interface_dirs_ != AUTO)
|
||||
if (impl->configured_dir_ == dir)
|
||||
return;
|
||||
if (impl->configured_dir_ != AUTO)
|
||||
throw std::runtime_error("Cannot change direction after being connected");
|
||||
|
||||
impl->configured_dir_ = dir;
|
||||
impl->initInterface(dir);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user