mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Combine pruneInterface() + validateConnectivity()
Validate interfaces during resolution. No need to separately validate interfaces. Thus, validateConnectivity() is removed from Task::init(). Functions are kept (but simplified) for unit testing.
This commit is contained in:
parent
10260b9dd7
commit
e56c772ecb
@ -132,9 +132,6 @@ protected:
|
||||
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)
|
||||
template <unsigned int mask>
|
||||
void mismatchingInterface(InitStageException& errors, const StagePrivate& child) const;
|
||||
|
||||
/// copy external_state to a child's interface and remember the link in internal_to map
|
||||
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||
@ -184,6 +181,10 @@ public:
|
||||
protected:
|
||||
// connect two neighbors
|
||||
void connect(StagePrivate& stage1, StagePrivate& stage2);
|
||||
|
||||
// validate that child's interface matches mine (considering start or end only as determined by mask)
|
||||
template <unsigned int mask>
|
||||
void validateInterface(const StagePrivate& child, InterfaceFlags required) const;
|
||||
};
|
||||
PIMPL_FUNCTIONS(SerialContainer)
|
||||
|
||||
@ -220,6 +221,9 @@ public:
|
||||
|
||||
void validateConnectivity() const override;
|
||||
|
||||
protected:
|
||||
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
|
||||
|
||||
private:
|
||||
/// callback for new externally received states
|
||||
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
|
||||
|
||||
@ -222,8 +222,6 @@ public:
|
||||
void initInterface(PropagatingEitherWay::Direction dir);
|
||||
// prune interface to the given propagation direction
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
// validate that we can propagate in one direction at least
|
||||
void validateConnectivity() const override;
|
||||
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
|
||||
@ -91,25 +91,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback& pr
|
||||
}
|
||||
|
||||
void ContainerBasePrivate::validateConnectivity() const {
|
||||
InitStageException errors;
|
||||
// recursively validate all children and accumulate errors
|
||||
for (const auto& child : children()) {
|
||||
try {
|
||||
child->pimpl()->validateConnectivity();
|
||||
} catch (InitStageException& e) {
|
||||
errors.append(e);
|
||||
}
|
||||
}
|
||||
if (errors)
|
||||
throw errors;
|
||||
}
|
||||
|
||||
template <unsigned int mask>
|
||||
void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child) const {
|
||||
boost::format desc("%1% interface of '%2%' (%3%) does not match mine (%4%)");
|
||||
errors.push_back(*me(), (desc % (mask == START_IF_MASK ? "start" : "end") % child.name() %
|
||||
flowSymbol<mask>(child.interfaceFlags()) % flowSymbol<mask>(interfaceFlags()))
|
||||
.str());
|
||||
for (const auto& child : children())
|
||||
child->pimpl()->validateConnectivity();
|
||||
}
|
||||
|
||||
bool ContainerBasePrivate::canCompute() const {
|
||||
@ -416,13 +400,27 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2)
|
||||
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%)");
|
||||
boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)");
|
||||
desc % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
|
||||
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
}
|
||||
|
||||
template <unsigned int mask>
|
||||
void SerialContainerPrivate::validateInterface(const StagePrivate& child, InterfaceFlags required) const {
|
||||
required = required & mask;
|
||||
if (required == UNKNOWN)
|
||||
return; // cannot yet validate
|
||||
InterfaceFlags child_interface = child.interfaceFlags() & mask;
|
||||
if (required != child_interface) {
|
||||
boost::format desc("%1% interface (%3%) of '%2%' does not match mine (%4%)");
|
||||
desc % (mask == START_IF_MASK ? "start" : "end") % child.name();
|
||||
desc % flowSymbol<mask>(child_interface) % flowSymbol<mask>(required);
|
||||
throw InitStageException(*me_, desc.str());
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
@ -439,6 +437,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
first.pimpl()->pruneInterface(accepted & START_IF_MASK);
|
||||
// connect first child's (start) push interface
|
||||
setChildsPushBackwardInterface(first);
|
||||
// validate that first child's and this container's start interfaces match
|
||||
validateInterface<START_IF_MASK>(*first.pimpl(), accepted);
|
||||
// connect first child's (start) pull interface
|
||||
if (const InterfacePtr& target = first.pimpl()->starts())
|
||||
starts_.reset(new Interface(
|
||||
@ -454,6 +454,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
|
||||
// connect last child's (end) push interface
|
||||
setChildsPushForwardInterface(last);
|
||||
// validate that last child's and this container's end interfaces match
|
||||
validateInterface<END_IF_MASK>(*last.pimpl(), accepted);
|
||||
// connect last child's (end) pull interface
|
||||
if (const InterfacePtr& target = last.pimpl()->ends())
|
||||
ends_.reset(new Interface(
|
||||
@ -463,30 +465,14 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
}
|
||||
|
||||
void SerialContainerPrivate::validateConnectivity() const {
|
||||
InitStageException errors;
|
||||
|
||||
// recursively validate children
|
||||
try {
|
||||
ContainerBasePrivate::validateConnectivity();
|
||||
} catch (InitStageException& e) {
|
||||
errors.append(e);
|
||||
}
|
||||
ContainerBasePrivate::validateConnectivity();
|
||||
|
||||
InterfaceFlags mine = interfaceFlags();
|
||||
// check that input / output interface of first / last child matches this' resp. interface
|
||||
if (!children().empty()) {
|
||||
const StagePrivate* start = children().front()->pimpl();
|
||||
const auto my_flags = this->interfaceFlags();
|
||||
auto child_flags = start->interfaceFlags() & START_IF_MASK;
|
||||
if (child_flags != (my_flags & START_IF_MASK))
|
||||
mismatchingInterface<START_IF_MASK>(errors, *start);
|
||||
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
|
||||
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
|
||||
|
||||
const StagePrivate* last = children().back()->pimpl();
|
||||
child_flags = last->interfaceFlags() & END_IF_MASK;
|
||||
if (child_flags != (my_flags & END_IF_MASK))
|
||||
mismatchingInterface<END_IF_MASK>(errors, *last);
|
||||
}
|
||||
|
||||
// validate connectivity of children amongst each other
|
||||
// validate connectivity of children between each other
|
||||
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
|
||||
// that is, neighbouring stages have a corresponding pull interface.
|
||||
// Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
|
||||
@ -503,16 +489,13 @@ void SerialContainerPrivate::validateConnectivity() const {
|
||||
// start pull interface fed?
|
||||
if (cur != children().begin() && // first child has not a previous one
|
||||
(required & READS_START) && !(*prev)->pimpl()->nextStarts())
|
||||
errors.push_back(**cur, "start interface is not fed");
|
||||
throw InitStageException(**cur, "start interface is not fed");
|
||||
|
||||
// end pull interface fed?
|
||||
if (next != end && // last child has not a next one
|
||||
(required & READS_END) && !(*next)->pimpl()->prevEnds())
|
||||
errors.push_back(**cur, "end interface is not fed");
|
||||
throw InitStageException(**cur, "end interface is not fed");
|
||||
}
|
||||
|
||||
if (errors)
|
||||
throw errors;
|
||||
}
|
||||
|
||||
bool SerialContainer::canCompute() const {
|
||||
@ -575,80 +558,68 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
throw InitStageException(*me(), "no children");
|
||||
|
||||
InitStageException exceptions;
|
||||
InterfaceFlags interface;
|
||||
|
||||
bool first = true;
|
||||
for (const Stage::pointer& child : children()) {
|
||||
try {
|
||||
child->pimpl()->pruneInterface(accepted);
|
||||
auto child_impl = child->pimpl();
|
||||
child_impl->pruneInterface(accepted);
|
||||
validateInterfaces(*child_impl, accepted, first);
|
||||
// initialize push connections of children according to their demands
|
||||
setChildsPushForwardInterface(*child);
|
||||
setChildsPushBackwardInterface(*child);
|
||||
first = false;
|
||||
} 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<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
|
||||
desc % flowSymbol<START_IF_MASK>(interface) % flowSymbol<END_IF_MASK>(interface);
|
||||
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<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
|
||||
desc % flowSymbol<START_IF_MASK>(interface) % flowSymbol<END_IF_MASK>(interface);
|
||||
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)
|
||||
if (accepted & READS_START)
|
||||
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||
this->onNewExternalState(Interface::FORWARD, external, updated);
|
||||
}));
|
||||
if (interface & READS_END)
|
||||
if (accepted & 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_ = accepted;
|
||||
}
|
||||
|
||||
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
|
||||
bool first) const {
|
||||
const InterfaceFlags child_interface = child.requiredInterface();
|
||||
bool valid = true;
|
||||
for (InterfaceFlags mask : { START_IF_MASK, END_IF_MASK }) {
|
||||
if ((external & mask) == UNKNOWN)
|
||||
external |= child_interface & mask;
|
||||
|
||||
valid = valid & ((external & mask) == (child_interface & mask));
|
||||
}
|
||||
|
||||
required_interface_ = interface;
|
||||
if (!valid) {
|
||||
boost::format desc("interface of '%1%' (%3% %4%) does not match %2% (%5% %6%).");
|
||||
desc % child.name();
|
||||
desc % (first ? "external one" : "other children's");
|
||||
desc % flowSymbol<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
|
||||
desc % flowSymbol<START_IF_MASK>(external) % flowSymbol<END_IF_MASK>(external);
|
||||
throw InitStageException(*me_, desc.str());
|
||||
}
|
||||
}
|
||||
|
||||
void ParallelContainerBasePrivate::validateConnectivity() const {
|
||||
InitStageException errors;
|
||||
InterfaceFlags my_interface = interfaceFlags();
|
||||
|
||||
// check that input / output interfaces of all children are handled by my interface
|
||||
for (const auto& child : children()) {
|
||||
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
||||
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
|
||||
mismatchingInterface<START_IF_MASK>(errors, *child->pimpl());
|
||||
if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK))
|
||||
mismatchingInterface<END_IF_MASK>(errors, *child->pimpl());
|
||||
}
|
||||
for (const auto& child : children())
|
||||
validateInterfaces(*child->pimpl(), my_interface);
|
||||
|
||||
// recursively validate children
|
||||
try {
|
||||
ContainerBasePrivate::validateConnectivity();
|
||||
} catch (InitStageException& e) {
|
||||
errors.append(e);
|
||||
}
|
||||
|
||||
if (errors)
|
||||
throw errors;
|
||||
ContainerBasePrivate::validateConnectivity();
|
||||
}
|
||||
|
||||
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
|
||||
|
||||
@ -443,21 +443,6 @@ void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
initInterface(dir);
|
||||
}
|
||||
|
||||
void PropagatingEitherWayPrivate::validateConnectivity() const {
|
||||
InterfaceFlags actual = interfaceFlags();
|
||||
if (actual == UNKNOWN)
|
||||
throw InitStageException(*me(), "not connected in any direction");
|
||||
|
||||
InitStageException errors;
|
||||
if ((actual & READS_START) && !(actual & WRITES_NEXT_START))
|
||||
errors.push_back(*me(), "Cannot push forwards");
|
||||
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
|
||||
errors.push_back(*me(), "Cannot push backwards");
|
||||
|
||||
if (errors)
|
||||
throw errors;
|
||||
}
|
||||
|
||||
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
|
||||
return required_interface_;
|
||||
}
|
||||
|
||||
@ -262,8 +262,6 @@ void Task::init() {
|
||||
stages()->init(impl->robot_model_);
|
||||
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
||||
stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
|
||||
// and *finally* validate connectivity
|
||||
stages()->pimpl()->validateConnectivity();
|
||||
|
||||
// provide introspection instance to all stages
|
||||
impl->setIntrospection(impl->introspection_.get());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user