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 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 void compute() = 0;
@ -89,9 +87,6 @@ public:
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;
void compute() override;
@ -132,9 +127,6 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done)
void validateConnectivity() const override;
protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -97,6 +97,8 @@ public:
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
bool canCompute() const override;
void compute() override;
@ -162,6 +164,8 @@ public:
// called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override;
// validate connectivity of chain
void validateConnectivity() const override;
private:
// connect cur stage to its predecessor and successor
@ -214,6 +218,8 @@ public:
// called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override;
void validateConnectivity() const override;
private:
/// callback for new externally received states
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);

View File

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

View File

@ -89,6 +89,17 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
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
{
// call the method of the public interface
@ -239,25 +250,6 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
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) {
ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool {
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;
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
if (!impl->children().empty()) {
const StagePrivate* start = impl->children().front()->pimpl();
const auto my_flags = this->pimpl()->interfaceFlags();
if (!children().empty()) {
const StagePrivate* start = children().front()->pimpl();
const auto my_flags = this->interfaceFlags();
auto child_flags = start->interfaceFlags() & 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());
const StagePrivate* last = impl->children().back()->pimpl();
const StagePrivate* last = children().back()->pimpl();
child_flags = last->interfaceFlags() & 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());
}
// 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.
// Here, it remains to check that - if a child requires a pull interface - it's indeed feeded.
for (auto cur = impl->children().begin(), end = impl->children().end(); cur != end; ++cur) {
// Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
for (auto cur = children().begin(), end = children().end(); cur != end; ++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
auto prev = cur; --prev;
auto next = cur; ++next;
// 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())
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");
}
// recursively validate children
try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }
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) {
for (const Stage::pointer& stage : children())
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)
{
auto impl = pimpl();

View File

@ -83,6 +83,15 @@ InterfaceFlags StagePrivate::interfaceFlags() const
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)
{
solution->setCreator(this);
@ -427,6 +436,27 @@ void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
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 f;
@ -434,7 +464,7 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
f |= PROPAGATE_FORWARDS;
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
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
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
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
// and *finally* validate connectivity
stages()->validateConnectivity();
stages()->pimpl()->validateConnectivity();
// provide introspection instance to all stages
impl->setIntrospection(introspection_.get());

View File

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