Fix printChildrenInterfaces()

This commit is contained in:
Robert Haschke 2021-11-15 19:43:52 +01:00
parent ed459702bd
commit 66e141db7b
4 changed files with 19 additions and 16 deletions

View File

@ -85,6 +85,8 @@ public:
PRUNED, // state is disabled because a required connected state failed
FAILED, // state that failed, causing the whole partial solution to be disabled
};
static const char* STATUS_COLOR[];
/** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
* Preference ordering considers high-depth first and within same depth, minimal cost paths.

View File

@ -53,6 +53,9 @@ using namespace std::placeholders;
namespace moveit {
namespace task_constructor {
static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os = std::cerr);
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name)
, required_interface_(UNKNOWN)
@ -375,8 +378,8 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
}
// for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) {
static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os) {
static unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
@ -386,7 +389,7 @@ static void printChildrenInterfaces(const ContainerBase& container, bool success
conn->pimpl()->printPendingPairs(os);
os << std::endl;
for (const auto& child : container.pimpl()->children()) {
for (const auto& child : container.children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())

View File

@ -775,11 +775,8 @@ void ConnectingPrivate::compute() {
}
std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
static const char* red = "\033[31m";
static const char* reset = "\033[m";
const char* reset = InterfaceState::STATUS_COLOR[3];
for (const auto& candidate : pending) {
if (!candidate.first->priority().enabled() || !candidate.second->priority().enabled())
os << " " << red;
// find indeces of InterfaceState pointers in start/end Interfaces
unsigned int first = 0, second = 0;
std::find_if(starts()->begin(), starts()->end(), [&](const InterfaceState* s) {
@ -790,9 +787,9 @@ std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
++second;
return &*candidate.second == s;
});
os << first << ":" << second << " ";
os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":"
<< InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " ";
}
os << reset;
return os;
}

View File

@ -144,15 +144,16 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) {
os << istate->priority() << " ";
return os;
}
const char* InterfaceState::STATUS_COLOR[] = {
"\033[32m", // ENABLED - green
"\033[33m", // PRUNED - yellow
"\033[31m", // FAILED - red
"\033[m" // reset
};
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) {
// maps InterfaceState::Status values to output (color-changing) prefix
static const char* prefix[] = {
"\033[32me:", // ENABLED - green
"\033[33md:", // PRUNED - yellow
"\033[31mf:", // FAILED - red
};
static const char* color_reset = "\033[m";
os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset;
os << InterfaceState::STATUS_COLOR[prio.status()] << prio.depth() << ":" << prio.cost()
<< InterfaceState::STATUS_COLOR[3];
return os;
}