diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 1c7f0188..45858028 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -133,7 +133,8 @@ protected: child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } // report error about mismatching interface (start or end as determined by mask) - void mismatchingInterface(InitStageException& errors, const StagePrivate& child, const InterfaceFlags mask) const; + template + 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); diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index bdffc523..6961e1a9 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -382,6 +382,8 @@ protected: } }; -const char* flowSymbol(moveit::task_constructor::InterfaceFlags f); +/** Return (horizontal) flow symbol for start or end interface (specified by mask) */ +template +const char* flowSymbol(InterfaceFlags f); } } diff --git a/core/src/container.cpp b/core/src/container.cpp index ab6cc0d5..a21e83b5 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -104,11 +104,11 @@ void ContainerBasePrivate::validateConnectivity() const { throw errors; } -void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child, - const InterfaceFlags mask) const { +template +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(child.interfaceFlags() & mask) % flowSymbol(interfaceFlags() & mask)) + flowSymbol(child.interfaceFlags()) % flowSymbol(interfaceFlags())) .str()); } @@ -417,8 +417,8 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) stage2.setPrevEnds(stage1.ends()); else { boost::format desc("end interface of '%1%' (%2%) does not match start interface of '%3%' (%4%)"); - desc % stage1.name() % flowSymbol(flags1 & END_IF_MASK); - desc % stage2.name() % flowSymbol(flags2 & START_IF_MASK); + desc % stage1.name() % flowSymbol(flags1); + desc % stage2.name() % flowSymbol(flags2); throw InitStageException(*me(), desc.str()); } } @@ -454,7 +454,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { (last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) { boost::format desc( "requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)"); - desc % flowSymbol(accepted & END_IF_MASK) % flowSymbol(last.pimpl()->requiredInterface() & END_IF_MASK); + desc % flowSymbol(accepted) % flowSymbol(last.pimpl()->requiredInterface()); throw InitStageException(*me(), desc.str()); } @@ -490,12 +490,12 @@ void SerialContainerPrivate::validateConnectivity() const { const auto my_flags = this->interfaceFlags(); auto child_flags = start->interfaceFlags() & START_IF_MASK; if (child_flags != (my_flags & START_IF_MASK)) - mismatchingInterface(errors, *start, START_IF_MASK); + mismatchingInterface(errors, *start); const StagePrivate* last = children().back()->pimpl(); child_flags = last->interfaceFlags() & END_IF_MASK; if (child_flags != (my_flags & END_IF_MASK)) - mismatchingInterface(errors, *last, END_IF_MASK); + mismatchingInterface(errors, *last); } // validate connectivity of children amongst each other @@ -606,16 +606,16 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { 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(child_interface & START_IF_MASK) % flowSymbol(child_interface & END_IF_MASK); - desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK); + desc % flowSymbol(child_interface) % flowSymbol(child_interface); + desc % flowSymbol(interface) % flowSymbol(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(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK); - desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK); + desc % flowSymbol(accepted) % flowSymbol(accepted); + desc % flowSymbol(interface) % flowSymbol(interface); exceptions.push_back(*me(), desc.str()); } @@ -649,9 +649,9 @@ void ParallelContainerBasePrivate::validateConnectivity() const { for (const auto& child : children()) { InterfaceFlags current = child->pimpl()->interfaceFlags(); if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK)) - mismatchingInterface(errors, *child->pimpl(), START_IF_MASK); + mismatchingInterface(errors, *child->pimpl()); if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK)) - mismatchingInterface(errors, *child->pimpl(), END_IF_MASK); + mismatchingInterface(errors, *child->pimpl()); } // recursively validate children diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 7e315ff3..c48186db 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -52,6 +52,30 @@ namespace moveit { namespace task_constructor { +template <> +const char* flowSymbol(InterfaceFlags f) { + f = f & START_IF_MASK; + if (f == READS_START) + return "→"; + if (f == WRITES_PREV_END) + return "←"; + if (f == UNKNOWN) + return "?"; + return "↔"; +} + +template <> +const char* flowSymbol(InterfaceFlags f) { + f = f & END_IF_MASK; + if (f == READS_END) + return "←"; + if (f == WRITES_NEXT_START) + return "→"; + if (f == UNKNOWN) + return "?"; + return "↔"; +} + void InitStageException::push_back(const Stage& stage, const std::string& msg) { errors_.emplace_back(std::make_pair(&stage, msg)); } @@ -103,9 +127,9 @@ void StagePrivate::pruneInterface(InterfaceFlags accepted) { // only one side needs to be specified but the value has to be compatible with this stage if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) || ((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) { - boost::format desc("this' interface %1%/%2% cannot match inferred interface %3%/%4%"); - desc % flowSymbol(required_start) % flowSymbol(required_end); - desc % flowSymbol(accepted_start) % flowSymbol(accepted_end); + boost::format desc("interface %1% %2% does not match inferred interface %3% %4%"); + desc % flowSymbol(required_start) % flowSymbol(required_end); + desc % flowSymbol(accepted_start) % flowSymbol(accepted_end); throw InitStageException(*me(), desc.str()); } } @@ -115,9 +139,9 @@ void StagePrivate::validateConnectivity() const { InterfaceFlags required = requiredInterface(); InterfaceFlags actual = interfaceFlags(); if ((required & actual) != required) { - boost::format desc("interface %1%/%2% does not satisfy required interface %3%/%4%"); - desc % flowSymbol(actual & START_IF_MASK) % flowSymbol(actual & END_IF_MASK); - desc % flowSymbol(required & START_IF_MASK) % flowSymbol(required & END_IF_MASK); + boost::format desc("interface %1% %2% does not satisfy required interface %3% %4%"); + desc % flowSymbol(actual) % flowSymbol(actual); + desc % flowSymbol(required) % flowSymbol(required); throw InitStageException(*me(), desc.str()); } } @@ -362,43 +386,6 @@ void Stage::reportPropertyError(const Property::error& e) { throw std::runtime_error(oss.str()); } -template -const char* direction(const StagePrivate& stage) { - InterfaceFlags f = stage.interfaceFlags(); - - bool own_if = f & own; - bool other_if = f & other; - bool reverse = own & START_IF_MASK; - if (own_if && other_if) - return "<>"; - if (!own_if && !other_if) - return "--"; - if (other_if ^ reverse) - return "->"; - return "<-"; -} - -const char* flowSymbol(InterfaceFlags f) { - if (f == UNKNOWN) - return "?"; // unknown interface - - // f should have either INPUT or OUTPUT flags set (not both) - assert(static_cast(f & START_IF_MASK) ^ static_cast(f & END_IF_MASK)); - - if (f & START_IF_MASK) { - if (f == READS_START) - return "↓"; - if (f == WRITES_PREV_END) - return "↑"; - } else if (f & END_IF_MASK) { - if (f == READS_END) - return "↑"; - if (f == WRITES_NEXT_START) - return "↓"; - } - return "⇅"; -} - std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { // starts for (const InterfaceConstPtr& i : { impl.prevEnds(), impl.starts() }) { @@ -409,8 +396,9 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { os << "-"; } // trajectories - os << std::setw(5) << direction(impl) << std::setw(3) << impl.solutions_.size() - << std::setw(5) << direction(impl); + os << " " << flowSymbol(impl.interfaceFlags() | impl.requiredInterface()) << " "; + os << std::setw(3) << impl.solutions_.size(); + os << " " << flowSymbol(impl.interfaceFlags() | impl.requiredInterface()) << " "; // ends for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) { os << std::setw(3); @@ -436,11 +424,11 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction auto flow = [](PropagatingEitherWay::Direction dir) -> const char* { switch (dir) { case PropagatingEitherWay::AUTO: - return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END }); + return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END }); case PropagatingEitherWay::FORWARD: - return flowSymbol(InterfaceFlags{ READS_START }); + return flowSymbol(InterfaceFlags{ READS_START }); case PropagatingEitherWay::BACKWARD: - return flowSymbol(InterfaceFlags{ WRITES_PREV_END }); + return flowSymbol(InterfaceFlags{ WRITES_PREV_END }); } }; @@ -459,14 +447,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { if (accepted == UNKNOWN) - throw InitStageException(*me(), std::string("cannot prune to ") + flowSymbol(UNKNOWN)); + throw InitStageException(*me(), "cannot prune to unknown interface"); if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START) initInterface(PropagatingEitherWay::FORWARD); else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END) initInterface(PropagatingEitherWay::BACKWARD); else { boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)"); - desc % flowSymbol(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK); + desc % flowSymbol(accepted) % flowSymbol(accepted); throw InitStageException(*me(), desc.str()); } }