From 4be448641fce0a8ea27ec732a8f1bc73c21888f0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 15 Nov 2021 19:43:52 +0100 Subject: [PATCH] Improve debug output - printChildrenInterfaces(): fix/add usage - printPendingPairs(): full colorization according to status --- .../include/moveit/task_constructor/storage.h | 12 ++++ core/src/container.cpp | 61 +++++++++++-------- core/src/stage.cpp | 43 +++++++------ core/src/storage.cpp | 15 ++--- 4 files changed, 78 insertions(+), 53 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index b7a6d023..53313971 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -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 +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; diff --git a/core/src/container.cpp b/core/src/container.cpp index de14c469..1cc4a617 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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(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(&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 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(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; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index efa730dc..bdde7f80 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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; } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index a98ffd3c..08185225 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -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; }