mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Debug evolution of interfaces
This commit is contained in:
parent
9466517f72
commit
f229743024
@ -328,6 +328,8 @@ public:
|
|||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
bool hasPendingOpposites(const InterfaceState* source) const;
|
bool hasPendingOpposites(const InterfaceState* source) const;
|
||||||
|
|
||||||
|
std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
|
// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
|
||||||
template <Interface::Direction other>
|
template <Interface::Direction other>
|
||||||
|
|||||||
@ -281,6 +281,31 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
|
|||||||
return os;
|
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();
|
||||||
|
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 */
|
/** Collect all partial solution sequences originating from start into given direction */
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
struct SolutionCollector
|
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
|
// finally, store + announce new solutions to external interface
|
||||||
for (const auto& solution : sorted)
|
for (const auto& solution : sorted)
|
||||||
@ -417,6 +443,7 @@ void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* fro
|
|||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
printChildrenInterfaces(*this, false, child);
|
||||||
}
|
}
|
||||||
|
|
||||||
SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {}
|
SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {}
|
||||||
|
|||||||
@ -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.
|
// Check whether there are pending feasible states that could connect to source.
|
||||||
@ -759,6 +761,28 @@ void ConnectingPrivate::compute() {
|
|||||||
static_cast<Connecting*>(me_)->compute(from, to);
|
static_cast<Connecting*>(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)) {}
|
Connecting::Connecting(const std::string& name) : ComputeBase(new ConnectingPrivate(this, name)) {}
|
||||||
|
|
||||||
void Connecting::reset() {
|
void Connecting::reset() {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user