mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Improve debug output
- printChildrenInterfaces(): fix/add usage - printPendingPairs(): full colorization according to status
This commit is contained in:
parent
0587129916
commit
4be448641f
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user