From ffb85983634ee80fda5473fd96b8ba2f5811eb23 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 20 Nov 2021 15:02:35 +0100 Subject: [PATCH] Recursively prune new CONNECT state if there is no enabled opposite This also requires to drop the assertion in SerialContainer::onNewSolution() that new solutions will have enabled start+end states (a CONNECT stage's solution might not). --- core/src/container.cpp | 4 ---- core/src/stage.cpp | 6 ++++++ 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index adb9f841..601244ba 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -443,10 +443,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // failures should never trigger this callback assert(!current.isFailure()); - // states of solution must be active, otherwise this would not have been computed - assert(current.start()->priority().enabled()); - assert(current.end()->priority().enabled()); - auto impl = pimpl(); const Stage* creator = current.creator(); auto& children = impl->children(); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ef711e73..010b712a 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -717,6 +717,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { assert(it->priority().enabled()); // new solutions are feasible, aren't they? auto parent_pimpl = parent()->pimpl(); InterfacePtr other_interface = pullInterface(dir); + bool have_enabled_opposites = false; for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { if (!static_cast(me_)->compatible(*it, *oit)) continue; @@ -725,10 +726,15 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { // https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 if (oit->priority().status() == InterfaceState::Status::ARMED) parent_pimpl->setStatus()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED); + if (oit->priority().enabled()) + have_enabled_opposites = true; // Remember all pending states, regardless of their status! pending.insert(make_pair(it, oit)); } + if (!have_enabled_opposites) // prune new state and associated branch if necessary + // pass creator=nullptr to skip hasPendingOpposites() check as we did this here already + parent_pimpl->setStatus(nullptr, nullptr, &*it, InterfaceState::Status::ARMED); } // std::cerr << name_ << ": "; // printPendingPairs(std::cerr);