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 PRUNED, // state is disabled because a required connected state failed
FAILED, // state that failed, causing the whole partial solution to be disabled FAILED, // state that failed, causing the whole partial solution to be disabled
}; };
static const char* STATUS_COLOR[];
/** InterfaceStates are ordered according to two values: /** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * 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. * 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 moveit {
namespace task_constructor { 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) ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name) : StagePrivate(me, name)
, required_interface_(UNKNOWN) , 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 // for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator, static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) { std::ostream& os) {
static unsigned int id = 0; static unsigned int id = 0;
const unsigned int width = 10; // indentation of name const unsigned int width = 10; // indentation of name
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
@ -386,7 +389,7 @@ static void printChildrenInterfaces(const ContainerBase& container, bool success
conn->pimpl()->printPendingPairs(os); conn->pimpl()->printPendingPairs(os);
os << std::endl; os << std::endl;
for (const auto& child : container.pimpl()->children()) { for (const auto& child : container.children()) {
auto cimpl = child->pimpl(); auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name(); os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends()) if (!cimpl->starts() && !cimpl->ends())

View File

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

View File

@ -144,15 +144,16 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) {
os << istate->priority() << " "; os << istate->priority() << " ";
return os; 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) { std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) {
// maps InterfaceState::Status values to output (color-changing) prefix // maps InterfaceState::Status values to output (color-changing) prefix
static const char* prefix[] = { os << InterfaceState::STATUS_COLOR[prio.status()] << prio.depth() << ":" << prio.cost()
"\033[32me:", // ENABLED - green << InterfaceState::STATUS_COLOR[3];
"\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;
return os; return os;
} }