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());
|
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
||||||
}
|
}
|
||||||
// report error about mismatching interface (start or end as determined by mask)
|
// 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
|
/// 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);
|
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;
|
throw errors;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child,
|
template <unsigned int mask>
|
||||||
const InterfaceFlags mask) const {
|
void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child) const {
|
||||||
boost::format desc("%1% interface of '%2%' (%3%) does not match mine (%4%)");
|
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() %
|
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());
|
.str());
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -417,8 +417,8 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2)
|
|||||||
stage2.setPrevEnds(stage1.ends());
|
stage2.setPrevEnds(stage1.ends());
|
||||||
else {
|
else {
|
||||||
boost::format desc("end interface of '%1%' (%2%) does not match start interface of '%3%' (%4%)");
|
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 % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
|
||||||
desc % stage2.name() % flowSymbol(flags2 & START_IF_MASK);
|
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
|
||||||
throw InitStageException(*me(), desc.str());
|
throw InitStageException(*me(), desc.str());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -454,7 +454,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
(last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) {
|
(last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) {
|
||||||
boost::format desc(
|
boost::format desc(
|
||||||
"requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)");
|
"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());
|
throw InitStageException(*me(), desc.str());
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -490,12 +490,12 @@ void SerialContainerPrivate::validateConnectivity() const {
|
|||||||
const auto my_flags = this->interfaceFlags();
|
const auto my_flags = this->interfaceFlags();
|
||||||
auto child_flags = start->interfaceFlags() & START_IF_MASK;
|
auto child_flags = start->interfaceFlags() & START_IF_MASK;
|
||||||
if (child_flags != (my_flags & 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();
|
const StagePrivate* last = children().back()->pimpl();
|
||||||
child_flags = last->interfaceFlags() & END_IF_MASK;
|
child_flags = last->interfaceFlags() & END_IF_MASK;
|
||||||
if (child_flags != (my_flags & 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
|
// 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 "
|
boost::format desc("inferred interface of stage '%1%' (%2%/%3%) does not agree with the inferred interface of "
|
||||||
"its siblings (%4%/%5%).");
|
"its siblings (%4%/%5%).");
|
||||||
desc % child->name();
|
desc % child->name();
|
||||||
desc % flowSymbol(child_interface & START_IF_MASK) % flowSymbol(child_interface & END_IF_MASK);
|
desc % flowSymbol<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
|
||||||
desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK);
|
desc % flowSymbol<START_IF_MASK>(interface) % flowSymbol<END_IF_MASK>(interface);
|
||||||
exceptions.push_back(*me(), desc.str());
|
exceptions.push_back(*me(), desc.str());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((interface & accepted) != accepted) {
|
if ((interface & accepted) != accepted) {
|
||||||
boost::format desc("required interface (%1%/%2%) does not match children (%3%/%4%).");
|
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<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
|
||||||
desc % flowSymbol(interface & START_IF_MASK) % flowSymbol(interface & END_IF_MASK);
|
desc % flowSymbol<START_IF_MASK>(interface) % flowSymbol<END_IF_MASK>(interface);
|
||||||
exceptions.push_back(*me(), desc.str());
|
exceptions.push_back(*me(), desc.str());
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -649,9 +649,9 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
|
|||||||
for (const auto& child : children()) {
|
for (const auto& child : children()) {
|
||||||
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
||||||
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
|
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))
|
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
|
// recursively validate children
|
||||||
|
|||||||
@ -52,6 +52,30 @@
|
|||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
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) {
|
void InitStageException::push_back(const Stage& stage, const std::string& msg) {
|
||||||
errors_.emplace_back(std::make_pair(&stage, 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
|
// 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) ||
|
if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) ||
|
||||||
((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) {
|
((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) {
|
||||||
boost::format desc("this' interface %1%/%2% cannot match inferred interface %3%/%4%");
|
boost::format desc("interface %1% %2% does not match inferred interface %3% %4%");
|
||||||
desc % flowSymbol(required_start) % flowSymbol(required_end);
|
desc % flowSymbol<START_IF_MASK>(required_start) % flowSymbol<END_IF_MASK>(required_end);
|
||||||
desc % flowSymbol(accepted_start) % flowSymbol(accepted_end);
|
desc % flowSymbol<START_IF_MASK>(accepted_start) % flowSymbol<END_IF_MASK>(accepted_end);
|
||||||
throw InitStageException(*me(), desc.str());
|
throw InitStageException(*me(), desc.str());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -115,9 +139,9 @@ void StagePrivate::validateConnectivity() const {
|
|||||||
InterfaceFlags required = requiredInterface();
|
InterfaceFlags required = requiredInterface();
|
||||||
InterfaceFlags actual = interfaceFlags();
|
InterfaceFlags actual = interfaceFlags();
|
||||||
if ((required & actual) != required) {
|
if ((required & actual) != required) {
|
||||||
boost::format desc("interface %1%/%2% does not satisfy required interface %3%/%4%");
|
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<START_IF_MASK>(actual) % flowSymbol<END_IF_MASK>(actual);
|
||||||
desc % flowSymbol(required & START_IF_MASK) % flowSymbol(required & END_IF_MASK);
|
desc % flowSymbol<START_IF_MASK>(required) % flowSymbol<END_IF_MASK>(required);
|
||||||
throw InitStageException(*me(), desc.str());
|
throw InitStageException(*me(), desc.str());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -362,43 +386,6 @@ void Stage::reportPropertyError(const Property::error& e) {
|
|||||||
throw std::runtime_error(oss.str());
|
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) {
|
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
||||||
// starts
|
// starts
|
||||||
for (const InterfaceConstPtr& i : { impl.prevEnds(), impl.starts() }) {
|
for (const InterfaceConstPtr& i : { impl.prevEnds(), impl.starts() }) {
|
||||||
@ -409,8 +396,9 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
|||||||
os << "-";
|
os << "-";
|
||||||
}
|
}
|
||||||
// trajectories
|
// trajectories
|
||||||
os << std::setw(5) << direction<READS_START, WRITES_PREV_END>(impl) << std::setw(3) << impl.solutions_.size()
|
os << " " << flowSymbol<START_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
|
||||||
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(impl);
|
os << std::setw(3) << impl.solutions_.size();
|
||||||
|
os << " " << flowSymbol<END_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
|
||||||
// ends
|
// ends
|
||||||
for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) {
|
for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) {
|
||||||
os << std::setw(3);
|
os << std::setw(3);
|
||||||
@ -436,11 +424,11 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
|
|||||||
auto flow = [](PropagatingEitherWay::Direction dir) -> const char* {
|
auto flow = [](PropagatingEitherWay::Direction dir) -> const char* {
|
||||||
switch (dir) {
|
switch (dir) {
|
||||||
case PropagatingEitherWay::AUTO:
|
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:
|
case PropagatingEitherWay::FORWARD:
|
||||||
return flowSymbol(InterfaceFlags{ READS_START });
|
return flowSymbol<START_IF_MASK>(InterfaceFlags{ READS_START });
|
||||||
case PropagatingEitherWay::BACKWARD:
|
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) {
|
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||||
if (accepted == UNKNOWN)
|
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)
|
if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START)
|
||||||
initInterface(PropagatingEitherWay::FORWARD);
|
initInterface(PropagatingEitherWay::FORWARD);
|
||||||
else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
|
else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
|
||||||
initInterface(PropagatingEitherWay::BACKWARD);
|
initInterface(PropagatingEitherWay::BACKWARD);
|
||||||
else {
|
else {
|
||||||
boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)");
|
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());
|
throw InitStageException(*me(), desc.str());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user