From 66e141db7b5118d638c39bdbbd55a9611b5f2db2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 15 Nov 2021 19:43:52 +0100 Subject: [PATCH] Fix printChildrenInterfaces() --- core/include/moveit/task_constructor/storage.h | 2 ++ core/src/container.cpp | 9 ++++++--- core/src/stage.cpp | 9 +++------ core/src/storage.cpp | 15 ++++++++------- 4 files changed, 19 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 2d3397bb..e71b3760 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. diff --git a/core/src/container.cpp b/core/src/container.cpp index 422dc92c..2ccc47ea 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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()) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2eaf6a43..09eaf517 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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; } 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; }