move validateConnectivity() from ContainerBase to StagePrivate

... to allow specific stage types (PropagatingEitherWay) implementing their own validation
No need for a public interface.
This commit is contained in:
Robert Haschke 2019-03-11 06:26:37 +01:00
parent 10c7a9cfd7
commit 905b6a3b0c
7 changed files with 96 additions and 71 deletions

View File

@ -64,8 +64,6 @@ public:
void reset() override; void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done)
virtual void validateConnectivity() const;
virtual bool canCompute() const = 0; virtual bool canCompute() const = 0;
virtual void compute() = 0; virtual void compute() = 0;
@ -89,9 +87,6 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done)
void validateConnectivity() const override;
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
@ -132,9 +127,6 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done)
void validateConnectivity() const override;
protected: protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl); ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -97,6 +97,8 @@ public:
return const_cast<const ContainerBasePrivate*>(this)->traverseStages(const_processor, cur_depth, max_depth); return const_cast<const ContainerBasePrivate*>(this)->traverseStages(const_processor, cur_depth, max_depth);
} }
void validateConnectivity() const override;
// 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;
@ -162,6 +164,8 @@ public:
// 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
void validateConnectivity() const override;
private: private:
// connect cur stage to its predecessor and successor // connect cur stage to its predecessor and successor
@ -214,6 +218,8 @@ public:
// 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;
void validateConnectivity() const override;
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);

View File

@ -74,6 +74,9 @@ public:
* PropagatingEitherWay uses this in restrictDirection() */ * PropagatingEitherWay uses this in restrictDirection() */
virtual void pruneInterface(InterfaceFlags accepted) {} virtual void pruneInterface(InterfaceFlags accepted) {}
/// validate connectivity of children (after init() was done)
virtual void validateConnectivity() const;
virtual bool canCompute() const = 0; virtual bool canCompute() const = 0;
virtual void compute() = 0; virtual void compute() = 0;
@ -179,6 +182,8 @@ 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;

View File

@ -89,6 +89,17 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
return true; return true;
} }
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;
}
bool ContainerBasePrivate::canCompute() const bool ContainerBasePrivate::canCompute() const
{ {
// call the method of the public interface // call the method of the public interface
@ -239,25 +250,6 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
if (errors) throw errors; if (errors) throw errors;
} }
void ContainerBase::validateConnectivity() const
{
InitStageException errors;
for (const auto& child : pimpl()->children()) {
// check that child's required interface is provided
InterfaceFlags required = child->pimpl()->requiredInterface();
InterfaceFlags actual = child->pimpl()->interfaceFlags();
if ((required & actual) != required)
errors.push_back(*child, "required interface is not satisfied");
// recursively validate all children and accumulate errors
ContainerBase* child_container = dynamic_cast<ContainerBase*>(child.get());
if (!child_container) continue; // only containers provide validateConnectivity()
try { child_container->validateConnectivity(); } catch (InitStageException &e) { errors.append(e); }
}
if (errors) throw errors;
}
std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool { ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool {
os << std::string(2*depth, ' ') << *stage.pimpl() << std::endl; os << std::string(2*depth, ' ') << *stage.pimpl() << std::endl;
@ -546,42 +538,45 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
} }
} }
void SerialContainer::validateConnectivity() const void SerialContainerPrivate::validateConnectivity() const
{ {
auto impl = pimpl();
InitStageException errors; InitStageException errors;
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)"); boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
// recursively validate children
try { ContainerBasePrivate::validateConnectivity(); }
catch (InitStageException& e) { errors.append(e); }
// 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 (!impl->children().empty()) { if (!children().empty()) {
const StagePrivate* start = impl->children().front()->pimpl(); const StagePrivate* start = children().front()->pimpl();
const auto my_flags = this->pimpl()->interfaceFlags(); const auto my_flags = this->interfaceFlags();
auto child_flags = start->interfaceFlags() & INPUT_IF_MASK; auto child_flags = start->interfaceFlags() & INPUT_IF_MASK;
if (child_flags != (my_flags & INPUT_IF_MASK)) if (child_flags != (my_flags & INPUT_IF_MASK))
errors.push_back(*this, (desc % "input" % start->name() % flowSymbol(child_flags) errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags)
% flowSymbol(my_flags & INPUT_IF_MASK)).str()); % flowSymbol(my_flags & INPUT_IF_MASK)).str());
const StagePrivate* last = impl->children().back()->pimpl(); const StagePrivate* last = children().back()->pimpl();
child_flags = last->interfaceFlags() & OUTPUT_IF_MASK; child_flags = last->interfaceFlags() & OUTPUT_IF_MASK;
if (child_flags != (my_flags & OUTPUT_IF_MASK)) if (child_flags != (my_flags & OUTPUT_IF_MASK))
errors.push_back(*this, (desc % "output" % last->name() % flowSymbol(child_flags) errors.push_back(*me(), (desc % "output" % last->name() % flowSymbol(child_flags)
% flowSymbol(my_flags & OUTPUT_IF_MASK)).str()); % flowSymbol(my_flags & OUTPUT_IF_MASK)).str());
} }
// validate connectivity of children amongst each other // validate connectivity of children amongst each other
// ContainerBase::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 requires a pull interface - it's indeed feeded. // Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
for (auto cur = impl->children().begin(), end = impl->children().end(); cur != end; ++cur) { for (auto cur = children().begin(), end = children().end(); cur != end; ++cur) {
const StagePrivate* const cur_impl = **cur; const StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->requiredInterface(); InterfaceFlags required = cur_impl->interfaceFlags();
// get iterators to prev / next stage in sequence // get iterators to prev / next stage in sequence
auto prev = cur; --prev; auto prev = cur; --prev;
auto next = cur; ++next; auto next = cur; ++next;
// start pull interface fed? // start pull interface fed?
if (cur != impl->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"); errors.push_back(**cur, "start interface is not fed");
@ -591,9 +586,6 @@ void SerialContainer::validateConnectivity() const
errors.push_back(**cur, "end interface is not fed"); errors.push_back(**cur, "end interface is not fed");
} }
// recursively validate children
try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }
if (errors) throw errors; if (errors) throw errors;
} }
@ -693,6 +685,31 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted)
} }
} }
void ParallelContainerBasePrivate::validateConnectivity() const
{
InitStageException errors;
InterfaceFlags my_interface = interfaceFlags();
InterfaceFlags children_interfaces;
boost::format desc("interface of child '%1%' (%2%) doesn't match mine (%3%)");
// check that input / output interfaces of all children are handled by my interface
for (const auto& child : children()) {
InterfaceFlags current = child->pimpl()->interfaceFlags();
children_interfaces |= current; // compute union of all children interfaces
if ((current & my_interface) != current)
errors.push_back(*me(), (desc % child->name() % flowSymbol(current) % flowSymbol(my_interface)).str());
}
// check that there is a child matching the expected push interfaces
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
errors.push_back(*me(), "no child provides expected push interface");
// recursively validate children
try { ContainerBasePrivate::validateConnectivity(); }
catch (InitStageException& e) { errors.append(e); }
if (errors) throw errors;
}
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) { void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) {
for (const Stage::pointer& stage : children()) for (const Stage::pointer& stage : children())
copyState(external, stage->pimpl()->pullInterface(dir), updated); copyState(external, stage->pimpl()->pullInterface(dir), updated);
@ -736,31 +753,6 @@ void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_m
} }
} }
void ParallelContainerBase::validateConnectivity() const
{
InitStageException errors;
auto impl = pimpl();
InterfaceFlags my_interface = impl->interfaceFlags();
InterfaceFlags children_interfaces;
boost::format desc("interface of child '%1%' (%2%) doesn't match mine (%3%)");
// check that input / output interfaces of all children are handled by my interface
for (const auto& child : pimpl()->children()) {
InterfaceFlags current = child->pimpl()->interfaceFlags();
children_interfaces |= current; // compute union of all children interfaces
if ((current & my_interface) != current)
errors.push_back(*this, (desc % child->name() % flowSymbol(current) % flowSymbol(my_interface)).str());
}
// check that there is a child matching the expected push interfaces
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
errors.push_back(*this, "no child provides expected push interface");
// recursively validate children
try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }
if (errors) throw errors;
}
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();

View File

@ -83,6 +83,15 @@ InterfaceFlags StagePrivate::interfaceFlags() const
return f; return f;
} }
void StagePrivate::validateConnectivity() const
{
// check that the required interface is provided
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if ((required & actual) != required)
throw InitStageException(*me(), "required interface is not satisfied");
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) bool StagePrivate::storeSolution(const SolutionBasePtr& solution)
{ {
solution->setCreator(this); solution->setCreator(this);
@ -427,6 +436,27 @@ void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
initInterface(PropagatingEitherWay::Direction(dir)); initInterface(PropagatingEitherWay::Direction(dir));
} }
void PropagatingEitherWayPrivate::validateConnectivity() const
{
InterfaceFlags required = requiredInterface();
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 (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY &&
(required & actual) != required)
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate " <<
(actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
if (errors) throw errors;
}
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
{ {
InterfaceFlags f; InterfaceFlags f;
@ -434,7 +464,7 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
f |= PROPAGATE_FORWARDS; f |= PROPAGATE_FORWARDS;
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD) if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
f |= PROPAGATE_BACKWARDS; f |= PROPAGATE_BACKWARDS;
// if required_interface_dirs_ == ANYWAY, we don't require an interface // if required_interface_dirs_ == AUTO, we don't require an interface
// but the parent container auto-derives the propagation direction // but the parent container auto-derives the propagation direction
return f; return f;
} }

View File

@ -237,7 +237,7 @@ void Task::init()
// 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 // and *finally* validate connectivity
stages()->validateConnectivity(); stages()->pimpl()->validateConnectivity();
// provide introspection instance to all stages // provide introspection instance to all stages
impl->setIntrospection(introspection_.get()); impl->setIntrospection(introspection_.get());

View File

@ -134,7 +134,7 @@ protected:
if (start) accepted |= WRITES_PREV_END; if (start) accepted |= WRITES_PREV_END;
if (end) accepted |= WRITES_NEXT_START; if (end) accepted |= WRITES_NEXT_START;
container.pimpl()->pruneInterface(accepted); container.pimpl()->pruneInterface(accepted);
container.validateConnectivity(); container.pimpl()->validateConnectivity();
if (!expect_failure) return; // as expected if (!expect_failure) return; // as expected
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container; ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
} catch (const InitStageException &e) { } catch (const InitStageException &e) {