From 9466517f72aa8d49e1b4fc71da35761a54473988 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 7 Dec 2020 07:10:19 +0100 Subject: [PATCH] operator<< for Interface + Priority Co-authored by v4hn --- .../include/moveit/task_constructor/storage.h | 3 +++ core/src/storage.cpp | 19 +++++++++++++++---- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 249a7729..888b1f5b 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -205,6 +205,9 @@ private: using base_type::remove_if; }; +std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio); +std::ostream& operator<<(std::ostream& os, const Interface& interface); + class CostTerm; class StagePrivate; class ContainerBasePrivate; diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 5a3b1bfb..d370cd0c 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -81,10 +81,6 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) return depth() > other.depth(); // larger depth = smaller prio! } -std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& p) { - return os << "[depth: " << p.depth() << ", cost: " << p.cost() << "]"; -} - Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {} // Announce a new InterfaceState @@ -138,6 +134,21 @@ void Interface::updatePriority(InterfaceState* state, const InterfaceState::Prio notify_(it, true); // notify callback } +std::ostream& operator<<(std::ostream& os, const Interface& interface) { + if (interface.empty()) + os << "---"; + for (const auto& istate : interface) + os << istate->priority() << " "; + return os; +} +std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { + static const char* red = "\033[31m"; + static const char* green = "\033[32m"; + static const char* color_reset = "\033[m"; + os << (prio.enabled() ? green : red) << prio.depth() << ":" << prio.cost() << color_reset; + return os; +} + void SolutionBase::setCreator(Stage* creator) { assert(creator_ == nullptr || creator_ == creator); // creator must only set once creator_ = creator;