diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3b58d0da..3f73ee02 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -148,9 +148,10 @@ protected: child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - /// Set ENABLED / PRUNED status of the solution tree starting from s into given direction + /// Set ENABLED/PRUNED/FAILED status of a solution branch starting from target into the given direction template - void setStatus(const InterfaceState* s, InterfaceState::Status status); + void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status); /// copy external_state to a child's interface and remember the link in internal_external map template diff --git a/core/src/container.cpp b/core/src/container.cpp index 391a4f17..57623b5d 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -118,32 +118,32 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState break; case PROPAGATE_FORWARDS: // mark from as failed (backwards) - setStatus(from, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, from, InterfaceState::Status::FAILED); break; case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - setStatus(to, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, to, InterfaceState::Status::FAILED); break; case CONNECT: - if (const Connecting* conn = dynamic_cast(&child)) { - // only prune if there are no opposite pending states - auto cimpl = conn->pimpl(); - if (!cimpl->hasPendingOpposites(to, from)) - setStatus(from, InterfaceState::Status::FAILED); - if (!cimpl->hasPendingOpposites(from, to)) - setStatus(to, InterfaceState::Status::FAILED); - } else { // other CONNECT-like stages, e.g. containers are always pruned - setStatus(from, InterfaceState::Status::FAILED); - setStatus(to, InterfaceState::Status::FAILED); - } + setStatus(&child, to, from, InterfaceState::Status::FAILED); + setStatus(&child, from, to, InterfaceState::Status::FAILED); break; } // printChildrenInterfaces(*this, false, child); } template -void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) { - if (s->priority().status() == status) +void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status) { + if (status != InterfaceState::Status::ENABLED && creator) { + if (const auto* conn = dynamic_cast(creator)) { + auto cimpl = conn->pimpl(); + // if creator is a Connecting stage and target has enabled opposite states (other than source) + if (cimpl->hasPendingOpposites(source, target)) + return; // don't prune + } + } + if (target->priority().status() == status) return; // nothing changing // Skip disabling the state, if there are alternative enabled solutions @@ -151,18 +151,18 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St auto solution_is_enabled = [](auto&& solution) { return state()>(*solution)->priority().enabled(); }; - const auto& alternatives = trajectories()>(*s); + const auto& alternatives = trajectories()>(*target); auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled); if (alternative_path != alternatives.cend()) return; } // actually enable/disable the state - const_cast(s)->updateStatus(status); + const_cast(target)->updateStatus(status); - // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface - if (parent() && trajectories(*s).empty()) { - auto external{ internalToExternalMap().find(s) }; + // if possible (i.e. if target has an external counterpart), escalate setStatus to external interface + if (parent() && trajectories(*target).empty()) { + auto external{ internalToExternalMap().find(target) }; if (external != internalToExternalMap().end()) { // do we have an external state? // only escalate if there is no other *enabled* internal state connected to the same external one // all internal states linked to external @@ -170,7 +170,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); }; auto other_path{ std::find_if(internals.first, internals.second, is_enabled) }; if (other_path == internals.second) - parent()->pimpl()->setStatus(external->get(), status); + parent()->pimpl()->setStatus(nullptr, nullptr, external->get(), status); return; } } @@ -185,18 +185,16 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED // traverse solution tree - for (const SolutionBase* successor : trajectories(*s)) - setStatus(state(*successor), status); + for (const SolutionBase* successor : trajectories(*target)) + setStatus(successor->creator(), target, state(*successor), status); } -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); template void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { - if (updated) { + if (updated) { // propagate external state update to internal copies auto internals{ externalToInternalMap().equal_range(&*external) }; for (auto& i = internals.first; i != internals.second; ++i) { - setStatus(i->second, external->priority().status()); + setStatus(nullptr, nullptr, i->second, external->priority().status()); } return; } diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 43f0b161..c82f9f35 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -46,7 +46,7 @@ TEST_F(Pruning, PruningMultiForward) { // The 2nd failing FW attempt would prune the path through CON, // but shouldn't because there exist two more GEN2 solutions -TEST_F(Pruning, DISABLED_NoPruningIfAlternativesExist) { +TEST_F(Pruning, NoPruningIfAlternativesExist) { add(t, new GeneratorMockup(PredefinedCosts({ 0.0 }))); add(t, new ConnectMockup()); add(t, new GeneratorMockup(std::list{ 0, 10, 20, 30 }, 2));