From 5c235ab580ce1d5e26da457aa08ea7a388463b3a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Sep 2021 11:51:04 +0200 Subject: [PATCH] debugging helper function --- core/include/moveit/task_constructor/container_p.h | 2 ++ core/src/container.cpp | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index e67e1f95..ab305a6f 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -272,6 +272,8 @@ private: template void onNewExternalState(Interface::iterator external, bool updated); 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) diff --git a/core/src/container.cpp b/core/src/container.cpp index c85304dc..904e0b5c 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -902,6 +902,15 @@ void FallbacksPrivate::computeGenerate() { (*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 void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) { if (updated) { @@ -911,6 +920,7 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd return; // already processed pending_states_.update(it); // update sorting pos of this single item + printPending("after update: "); // update prio of linked internal states as well ContainerBasePrivate::copyState(external, InterfacePtr(), updated); @@ -920,10 +930,13 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd // remember external state for later processing by children. // children().end() indicates that the states wasn't yet forwarded to any child pending_states_.push(ExternalState(external, dir, children().cend())); + printPending("after push: "); } void FallbacksPrivate::computePropagate(){ while (!pending_states_.empty()) { + printPending(); + auto current = pending_states_.begin(); if (!current->external_state->priority().enabled()) return;