mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
debugging helper function
This commit is contained in:
parent
dcb6857f36
commit
5c235ab580
@ -272,6 +272,8 @@ private:
|
|||||||
template <typename Interface::Direction>
|
template <typename Interface::Direction>
|
||||||
void onNewExternalState(Interface::iterator external, bool updated);
|
void onNewExternalState(Interface::iterator external, bool updated);
|
||||||
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
|
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
|
||||||
|
// print pending states for debugging
|
||||||
|
void printPending(const char* comment = "pending: ") const;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Fallbacks)
|
PIMPL_FUNCTIONS(Fallbacks)
|
||||||
|
|
||||||
|
|||||||
@ -902,6 +902,15 @@ void FallbacksPrivate::computeGenerate() {
|
|||||||
(*current_generator_)->pimpl()->runCompute();
|
(*current_generator_)->pimpl()->runCompute();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
inline void FallbacksPrivate::printPending(const char* comment) const {
|
||||||
|
ROSCONSOLE_DEFINE_LOCATION(true, ::ros::console::levels::Debug, ROSCONSOLE_NAME_PREFIX ".Fallbacks");
|
||||||
|
if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) {
|
||||||
|
std::cout << name() << ": " << comment;
|
||||||
|
std::for_each(pending_states_.begin(), pending_states_.end(), [](const auto& e) { std::cout << e.external_state->priority() << " "; });
|
||||||
|
std::cout << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
template <typename Interface::Direction dir>
|
template <typename Interface::Direction dir>
|
||||||
void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) {
|
void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) {
|
||||||
if (updated) {
|
if (updated) {
|
||||||
@ -911,6 +920,7 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd
|
|||||||
return; // already processed
|
return; // already processed
|
||||||
|
|
||||||
pending_states_.update(it); // update sorting pos of this single item
|
pending_states_.update(it); // update sorting pos of this single item
|
||||||
|
printPending("after update: ");
|
||||||
|
|
||||||
// update prio of linked internal states as well
|
// update prio of linked internal states as well
|
||||||
ContainerBasePrivate::copyState<dir>(external, InterfacePtr(), updated);
|
ContainerBasePrivate::copyState<dir>(external, InterfacePtr(), updated);
|
||||||
@ -920,10 +930,13 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd
|
|||||||
// remember external state for later processing by children.
|
// remember external state for later processing by children.
|
||||||
// children().end() indicates that the states wasn't yet forwarded to any child
|
// children().end() indicates that the states wasn't yet forwarded to any child
|
||||||
pending_states_.push(ExternalState(external, dir, children().cend()));
|
pending_states_.push(ExternalState(external, dir, children().cend()));
|
||||||
|
printPending("after push: ");
|
||||||
}
|
}
|
||||||
|
|
||||||
void FallbacksPrivate::computePropagate(){
|
void FallbacksPrivate::computePropagate(){
|
||||||
while (!pending_states_.empty()) {
|
while (!pending_states_.empty()) {
|
||||||
|
printPending();
|
||||||
|
|
||||||
auto current = pending_states_.begin();
|
auto current = pending_states_.begin();
|
||||||
if (!current->external_state->priority().enabled())
|
if (!current->external_state->priority().enabled())
|
||||||
return;
|
return;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user