Improve debug output

- printChildrenInterfaces(): fix/add usage
- printPendingPairs(): full colorization according to status
This commit is contained in:
Robert Haschke 2021-11-15 19:43:52 +01:00
parent 0587129916
commit 4be448641f
4 changed files with 78 additions and 53 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.
@ -221,6 +223,16 @@ private:
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
std::ostream& operator<<(std::ostream& os, const Interface& interface);
/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
template <typename T>
size_t getIndex(const T& container, typename T::const_iterator search) {
size_t index = 1;
for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index)
if (it == search)
return index;
return 0;
}
class CostTerm;
class StagePrivate;
class ContainerBasePrivate;

View File

@ -53,6 +53,35 @@ using namespace std::placeholders;
namespace moveit {
namespace task_constructor {
// for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) {
auto printPendingPairs = [](const StagePrivate* impl, std::ostream& os) -> std::ostream& {
if (auto conn = dynamic_cast<const ConnectingPrivate*>(impl))
conn->printPendingPairs(os);
return os;
};
static unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
if (success)
os << ++id << ' ';
printPendingPairs(creator.pimpl(), os) << std::endl;
for (const auto& child : container.children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())
os << "" << std::endl;
if (cimpl->starts())
os << "" << *child->pimpl()->starts() << std::endl;
if (cimpl->starts() && cimpl->ends())
os << std::setw(width) << " ";
if (cimpl->ends())
os << "" << *child->pimpl()->ends() << std::endl;
}
}
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name)
, required_interface_(UNKNOWN)
@ -414,31 +443,6 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
return os;
}
// 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 unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
if (success)
os << ++id << ' ';
if (const Connecting* conn = dynamic_cast<const Connecting*>(&creator))
conn->pimpl()->printPendingPairs(os);
os << std::endl;
for (const auto& child : container.pimpl()->children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())
os << "" << std::endl;
if (cimpl->starts())
os << "" << *child->pimpl()->starts() << std::endl;
if (cimpl->starts() && cimpl->ends())
os << std::setw(width) << " ";
if (cimpl->ends())
os << "" << *child->pimpl()->ends() << std::endl;
}
}
/** Collect all partial solution sequences originating from start into given direction */
template <Interface::Direction dir>
struct SolutionCollector
@ -537,7 +541,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
}
}
}
// printChildrenInterfaces(*this, true, *current.creator());
// printChildrenInterfaces(*this->pimpl(), true, *current.creator());
// finally, store + announce new solutions to external interface
for (const auto& solution : sorted)
@ -874,6 +878,7 @@ void Fallbacks::compute() {
void Fallbacks::onNewSolution(const SolutionBase& s) {
pimpl()->job_has_solutions_ = true;
// printChildrenInterfaces(*this->pimpl(), true, *s.creator());
liftSolution(s);
}
@ -915,11 +920,13 @@ void FallbacksPrivate::initializeExternalInterfaces() {
static_cast<Fallbacks*>(me())->replaceImpl();
}
void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
// This override is deliberately empty.
// The method prunes solution paths when a child failed to find a valid solution for it,
// but in Fallbacks the next child might still yield a successful solution
// Thus pruning must only occur once the last child is exhausted (inside computePropagate)
// printChildrenInterfaces(*this, false, child);
(void)child;
}

View File

@ -759,9 +759,23 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
}
}
}
// std::cerr << name_ << ": ";
// printPendingPairs(std::cerr);
// std::cerr << std::endl;
#if 0
auto& os = std::cerr;
for (auto d : { Interface::FORWARD, Interface::BACKWARD }) {
bool fw = (d == Interface::FORWARD);
if (fw)
os << " " << std::setw(10) << std::left << this->name();
else
os << std::setw(12) << std::right << "";
if (dir != d)
os << (updated ? " !" : " +");
else
os << " ";
os << (fw ? "" : "") << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl;
}
os << std::setw(15) << " ";
printPendingPairs(os) << std::endl;
#endif
}
// Check whether there are pending feasible states that could connect to source.
@ -802,24 +816,15 @@ 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) {
++first;
return &*candidate.first == s;
});
std::find_if(ends()->begin(), ends()->end(), [&](const InterfaceState* s) {
++second;
return &*candidate.second == s;
});
os << first << ":" << second << " ";
size_t first = getIndex(*starts(), candidate.first);
size_t second = getIndex(*ends(), candidate.second);
os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":"
<< InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " ";
}
os << reset;
if (pending.empty())
os << "---";
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;
}