From f22974302464497054a4bf5a95932cfa32cc44c0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 7 Dec 2020 08:20:21 +0100 Subject: [PATCH] Debug evolution of interfaces --- .../include/moveit/task_constructor/stage_p.h | 2 ++ core/src/container.cpp | 27 +++++++++++++++++++ core/src/stage.cpp | 24 +++++++++++++++++ 3 files changed, 53 insertions(+) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 7fe118e3..3d14731a 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -328,6 +328,8 @@ public: template bool hasPendingOpposites(const InterfaceState* source) const; + std::ostream& printPendingPairs(std::ostream& os = std::cerr) const; + private: // Create a pair of Interface states for pending list, such that the order (start, end) is maintained template diff --git a/core/src/container.cpp b/core/src/container.cpp index c3e05c3f..aebd2b92 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -281,6 +281,31 @@ 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(); + if (!cimpl->starts() && !cimpl->ends()) + continue; // skip generator + os << std::setw(width) << std::left << child->name(); + 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 @@ -374,6 +399,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { } } } + printChildrenInterfaces(*this, true, *current.creator()); // finally, store + announce new solutions to external interface for (const auto& solution : sorted) @@ -417,6 +443,7 @@ void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* fro } break; } + printChildrenInterfaces(*this, false, child); } SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {} diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 5ceb7614..93a1bf0e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -720,6 +720,8 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { } } } + std::cerr << name_ << ": "; + printPendingPairs(std::cerr); } // Check whether there are pending feasible states that could connect to source. @@ -759,6 +761,28 @@ void ConnectingPrivate::compute() { static_cast(me_)->compute(from, to); } +std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { + static const char* red = "\033[31m"; + static const char* reset = "\033[m"; + 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 << " "; + } + os << reset; + return os; +} + Connecting::Connecting(const std::string& name) : ComputeBase(new ConnectingPrivate(this, name)) {} void Connecting::reset() {