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);
|
bool allowed = (required & WRITES_NEXT_START);
|
||||||
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
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
|
/// 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);
|
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||||
@ -184,6 +181,10 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
// connect two neighbors
|
// connect two neighbors
|
||||||
void connect(StagePrivate& stage1, StagePrivate& stage2);
|
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)
|
PIMPL_FUNCTIONS(SerialContainer)
|
||||||
|
|
||||||
@ -220,6 +221,9 @@ public:
|
|||||||
|
|
||||||
void validateConnectivity() const override;
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/// callback for new externally received states
|
/// callback for new externally received states
|
||||||
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
|
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
|
||||||
|
|||||||
@ -222,8 +222,6 @@ public:
|
|||||||
void initInterface(PropagatingEitherWay::Direction dir);
|
void initInterface(PropagatingEitherWay::Direction dir);
|
||||||
// prune interface to the given propagation direction
|
// prune interface to the given propagation direction
|
||||||
void pruneInterface(InterfaceFlags accepted) override;
|
void pruneInterface(InterfaceFlags accepted) override;
|
||||||
// validate that we can propagate in one direction at least
|
|
||||||
void validateConnectivity() const override;
|
|
||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
|
|||||||
@ -91,25 +91,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback& pr
|
|||||||
}
|
}
|
||||||
|
|
||||||
void ContainerBasePrivate::validateConnectivity() const {
|
void ContainerBasePrivate::validateConnectivity() const {
|
||||||
InitStageException errors;
|
|
||||||
// recursively validate all children and accumulate errors
|
// recursively validate all children and accumulate errors
|
||||||
for (const auto& child : children()) {
|
for (const auto& child : children())
|
||||||
try {
|
child->pimpl()->validateConnectivity();
|
||||||
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());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ContainerBasePrivate::canCompute() const {
|
bool ContainerBasePrivate::canCompute() const {
|
||||||
@ -416,13 +400,27 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2)
|
|||||||
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
|
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
|
||||||
stage2.setPrevEnds(stage1.ends());
|
stage2.setPrevEnds(stage1.ends());
|
||||||
else {
|
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 % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
|
||||||
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
|
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
|
||||||
throw InitStageException(*me(), desc.str());
|
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
|
// called by parent asking for pruning of this' interface
|
||||||
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||||
// we need to have some children to do the actual work
|
// 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);
|
first.pimpl()->pruneInterface(accepted & START_IF_MASK);
|
||||||
// connect first child's (start) push interface
|
// connect first child's (start) push interface
|
||||||
setChildsPushBackwardInterface(first);
|
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
|
// connect first child's (start) pull interface
|
||||||
if (const InterfacePtr& target = first.pimpl()->starts())
|
if (const InterfacePtr& target = first.pimpl()->starts())
|
||||||
starts_.reset(new Interface(
|
starts_.reset(new Interface(
|
||||||
@ -454,6 +454,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
|
|
||||||
// connect last child's (end) push interface
|
// connect last child's (end) push interface
|
||||||
setChildsPushForwardInterface(last);
|
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
|
// connect last child's (end) pull interface
|
||||||
if (const InterfacePtr& target = last.pimpl()->ends())
|
if (const InterfacePtr& target = last.pimpl()->ends())
|
||||||
ends_.reset(new Interface(
|
ends_.reset(new Interface(
|
||||||
@ -463,30 +465,14 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void SerialContainerPrivate::validateConnectivity() const {
|
void SerialContainerPrivate::validateConnectivity() const {
|
||||||
InitStageException errors;
|
ContainerBasePrivate::validateConnectivity();
|
||||||
|
|
||||||
// recursively validate children
|
|
||||||
try {
|
|
||||||
ContainerBasePrivate::validateConnectivity();
|
|
||||||
} catch (InitStageException& e) {
|
|
||||||
errors.append(e);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
InterfaceFlags mine = interfaceFlags();
|
||||||
// check that input / output interface of first / last child matches this' resp. interface
|
// check that input / output interface of first / last child matches this' resp. interface
|
||||||
if (!children().empty()) {
|
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
|
||||||
const StagePrivate* start = children().front()->pimpl();
|
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
|
||||||
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);
|
|
||||||
|
|
||||||
const StagePrivate* last = children().back()->pimpl();
|
// validate connectivity of children between each other
|
||||||
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
|
|
||||||
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
|
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
|
||||||
// that is, neighbouring stages have a corresponding pull interface.
|
// 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.
|
// 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?
|
// start pull interface fed?
|
||||||
if (cur != children().begin() && // first child has not a previous one
|
if (cur != children().begin() && // first child has not a previous one
|
||||||
(required & READS_START) && !(*prev)->pimpl()->nextStarts())
|
(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?
|
// end pull interface fed?
|
||||||
if (next != end && // last child has not a next one
|
if (next != end && // last child has not a next one
|
||||||
(required & READS_END) && !(*next)->pimpl()->prevEnds())
|
(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 {
|
bool SerialContainer::canCompute() const {
|
||||||
@ -575,80 +558,68 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
throw InitStageException(*me(), "no children");
|
throw InitStageException(*me(), "no children");
|
||||||
|
|
||||||
InitStageException exceptions;
|
InitStageException exceptions;
|
||||||
InterfaceFlags interface;
|
|
||||||
|
|
||||||
|
bool first = true;
|
||||||
for (const Stage::pointer& child : children()) {
|
for (const Stage::pointer& child : children()) {
|
||||||
try {
|
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) {
|
} catch (InitStageException& e) {
|
||||||
exceptions.append(e);
|
exceptions.append(e);
|
||||||
continue;
|
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)
|
if (exceptions)
|
||||||
throw exceptions;
|
throw exceptions;
|
||||||
|
|
||||||
// States received by the container need to be copied to all children's pull interfaces.
|
// 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) {
|
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||||
this->onNewExternalState(Interface::FORWARD, external, 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) {
|
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||||
this->onNewExternalState(Interface::BACKWARD, external, updated);
|
this->onNewExternalState(Interface::BACKWARD, external, updated);
|
||||||
}));
|
}));
|
||||||
|
|
||||||
// initialize push connections of children according to their demands
|
required_interface_ = accepted;
|
||||||
for (const Stage::pointer& stage : children()) {
|
}
|
||||||
setChildsPushForwardInterface(*stage);
|
|
||||||
setChildsPushBackwardInterface(*stage);
|
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 {
|
void ParallelContainerBasePrivate::validateConnectivity() const {
|
||||||
InitStageException errors;
|
|
||||||
InterfaceFlags my_interface = interfaceFlags();
|
InterfaceFlags my_interface = interfaceFlags();
|
||||||
|
|
||||||
// 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();
|
validateInterfaces(*child->pimpl(), my_interface);
|
||||||
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());
|
|
||||||
}
|
|
||||||
|
|
||||||
// recursively validate children
|
ContainerBasePrivate::validateConnectivity();
|
||||||
try {
|
|
||||||
ContainerBasePrivate::validateConnectivity();
|
|
||||||
} catch (InitStageException& e) {
|
|
||||||
errors.append(e);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (errors)
|
|
||||||
throw errors;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
|
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
|
||||||
|
|||||||
@ -443,21 +443,6 @@ void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
initInterface(dir);
|
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 {
|
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
|
||||||
return required_interface_;
|
return required_interface_;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -262,8 +262,6 @@ void Task::init() {
|
|||||||
stages()->init(impl->robot_model_);
|
stages()->init(impl->robot_model_);
|
||||||
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
||||||
stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
|
stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
|
||||||
// and *finally* validate connectivity
|
|
||||||
stages()->pimpl()->validateConnectivity();
|
|
||||||
|
|
||||||
// provide introspection instance to all stages
|
// provide introspection instance to all stages
|
||||||
impl->setIntrospection(impl->introspection_.get());
|
impl->setIntrospection(impl->introspection_.get());
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user