mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Fix printChildrenInterfaces()
This commit is contained in:
parent
ed459702bd
commit
66e141db7b
@ -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.
|
||||
|
||||
@ -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())
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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