mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
templated flowSymbol<input/output mask>(flags)
* Enforce at compile time that either input or output flags are considered. * Use horizontal flow symbols (as in console output). * Replace redundant direction(flags).
This commit is contained in:
parent
7f33633e82
commit
a98d5ed8ae
@ -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 <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
|
||||
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||
|
||||
@ -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 <unsigned int mask>
|
||||
const char* flowSymbol(InterfaceFlags f);
|
||||
}
|
||||
}
|
||||
|
||||
@ -104,11 +104,11 @@ void ContainerBasePrivate::validateConnectivity() const {
|
||||
throw errors;
|
||||
}
|
||||
|
||||
void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child,
|
||||
const InterfaceFlags mask) const {
|
||||
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(child.interfaceFlags() & mask) % flowSymbol(interfaceFlags() & mask))
|
||||
flowSymbol<mask>(child.interfaceFlags()) % flowSymbol<mask>(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<END_IF_MASK>(flags1);
|
||||
desc % stage2.name() % flowSymbol<START_IF_MASK>(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<END_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(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<START_IF_MASK>(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<END_IF_MASK>(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<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(accepted & START_IF_MASK) % flowSymbol(accepted & END_IF_MASK);
|
||||
desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK);
|
||||
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());
|
||||
}
|
||||
|
||||
@ -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<START_IF_MASK>(errors, *child->pimpl());
|
||||
if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK))
|
||||
mismatchingInterface(errors, *child->pimpl(), END_IF_MASK);
|
||||
mismatchingInterface<END_IF_MASK>(errors, *child->pimpl());
|
||||
}
|
||||
|
||||
// recursively validate children
|
||||
|
||||
@ -52,6 +52,30 @@
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
template <>
|
||||
const char* flowSymbol<START_IF_MASK>(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<END_IF_MASK>(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<START_IF_MASK>(required_start) % flowSymbol<END_IF_MASK>(required_end);
|
||||
desc % flowSymbol<START_IF_MASK>(accepted_start) % flowSymbol<END_IF_MASK>(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<START_IF_MASK>(actual) % flowSymbol<END_IF_MASK>(actual);
|
||||
desc % flowSymbol<START_IF_MASK>(required) % flowSymbol<END_IF_MASK>(required);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
}
|
||||
@ -362,43 +386,6 @@ void Stage::reportPropertyError(const Property::error& e) {
|
||||
throw std::runtime_error(oss.str());
|
||||
}
|
||||
|
||||
template <InterfaceFlag own, InterfaceFlag other>
|
||||
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<bool>(f & START_IF_MASK) ^ static_cast<bool>(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<READS_START, WRITES_PREV_END>(impl) << std::setw(3) << impl.solutions_.size()
|
||||
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(impl);
|
||||
os << " " << flowSymbol<START_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
|
||||
os << std::setw(3) << impl.solutions_.size();
|
||||
os << " " << flowSymbol<END_IF_MASK>(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<START_IF_MASK>(InterfaceFlags{ READS_START, WRITES_PREV_END });
|
||||
case PropagatingEitherWay::FORWARD:
|
||||
return flowSymbol(InterfaceFlags{ READS_START });
|
||||
return flowSymbol<START_IF_MASK>(InterfaceFlags{ READS_START });
|
||||
case PropagatingEitherWay::BACKWARD:
|
||||
return flowSymbol(InterfaceFlags{ WRITES_PREV_END });
|
||||
return flowSymbol<START_IF_MASK>(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<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
|
||||
throw InitStageException(*me(), desc.str());
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user