diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index da7666e2..7dd1ac4e 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -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); diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 1bd61ea8..fe05a772 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -97,6 +97,8 @@ public: return const_cast(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); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 78e77980..27d3a19d 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -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; diff --git a/core/src/container.cpp b/core/src/container.cpp index ff8df82a..63ebd1ce 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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(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(); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 1ed760c0..46a15bb9 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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; } diff --git a/core/src/task.cpp b/core/src/task.cpp index 26c6f693..b4e517b9 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -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()); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index e2eff17d..17403b25 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -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) {